src/main/common/vector.c

   1 /*
   2  * This file is part of Betaflight.
   3  *
   4  * Betaflight is free software. You can redistribute this software
   5  * and/or modify this software under the terms of the GNU General
   6  * Public License as published by the Free Software Foundation,
   7  * either version 3 of the License, or (at your option) any later
   8  * version.
   9  *
  10  * Betaflight is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  *
  14  * See the GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public
  17  * License along with this software.
  18  *
  19  * If not, see <http://www.gnu.org/licenses/>.
  20  */
  21
  22 #include "platform.h"
  23
  24 #include <math.h>
  25
  26 #include "common/axis.h"
  27 #include "common/maths.h"
  28
  29 #include "vector.h"
  30
  31 bool vector2Equal(const vector2_t *a, const vector2_t *b)
  32 {
  33     return (a->x == b->x) && (a->y == b->y);
  34 }
  35
  36 vector2_t *vector2Zero(vector2_t *v)
  37 {
  38     v->x = 0.0f;
  39     v->y = 0.0f;
  40
  41     return v;
  42 }
  43
  44 vector2_t *vector2Add(vector2_t *result, const vector2_t *a, const vector2_t *b)
  45 {
  46     result->x = a->x + b->x;
  47     result->y = a->y + b->y;
  48
  49     return result;
  50 }
  51
  52 vector2_t *vector2Sub(vector2_t *result, const vector2_t *a, const vector2_t *b)
  53 {
  54     result->x = a->x - b->x;
  55     result->y = a->y - b->y;
  56
  57     return result;
  58 }
  59
  60 vector2_t *vector2Scale(vector2_t *result, const vector2_t *v, const float k)
  61 {
  62     result->x = v->x * k;
  63     result->y = v->y * k;
  64
  65     return result;
  66 }
  67
  68 float vector2Dot(const vector2_t *a, const vector2_t *b)
  69 {
  70     return a->x * b->x + a->y * b->y;
  71 }
  72
  73 float vector2Cross(const vector2_t *a, const vector2_t *b)
  74 {
  75     return a->x * b->y - a->y * b->x;
  76 }
  77
  78 float vector2NormSq(const vector2_t *v)
  79 {
  80     return vector2Dot(v, v);
  81 }
  82
  83 float vector2Norm(const vector2_t *v)
  84 {
  85     return sqrtf(vector2NormSq(v));
  86 }
  87
  88 vector2_t *vector2Normalize(vector2_t *result, const vector2_t *v)
  89 {
  90     const float normSq = vector2NormSq(v);
  91
  92     if (normSq > 0.0f) {
  93         return vector2Scale(result, v, 1.0f / sqrtf(normSq));
  94     } else {
  95         return vector2Zero(result);
  96     }
  97 }
  98
  99 // rotate 2d vector by angle
 100 // angle is in radians and positive means counterclockwise
 101 vector2_t *vector2Rotate(vector2_t *result, const vector2_t *v, const float angle)
 102 {
 103     vector2_t tmp;
 104     tmp.x = v->x * cos_approx(angle) - v->y * sin_approx(angle);
 105     tmp.y = v->x * sin_approx(angle) + v->y * cos_approx(angle);
 106     *result = tmp;
 107     return result;
 108 }
 109
 110 bool vector3Equal(const vector3_t *a, const vector3_t *b)
 111 {
 112     return (a->x == b->x) && (a->y == b->y) && (a->z == b->z);
 113 }
 114
 115 vector3_t *vector3Zero(vector3_t *v)
 116 {
 117     v->x = 0.0f;
 118     v->y = 0.0f;
 119     v->z = 0.0f;
 120
 121     return v;
 122 }
 123
 124 vector3_t *vector3Add(vector3_t *result, const vector3_t *a, const vector3_t *b)
 125 {
 126     result->x = a->x + b->x;
 127     result->y = a->y + b->y;
 128     result->z = a->z + b->z;
 129
 130     return result;
 131 }
 132
 133 vector3_t *vector3Sub(vector3_t *result, const vector3_t *a, const vector3_t *b)
 134 {
 135     result->x = a->x - b->x;
 136     result->y = a->y - b->y;
 137     result->z = a->z - b->z;
 138
 139     return result;
 140 }
 141
 142 vector3_t *vector3Scale(vector3_t *result, const vector3_t *v, const float k)
 143 {
 144     result->x = v->x * k;
 145     result->y = v->y * k;
 146     result->z = v->z * k;
 147
 148     return result;
 149 }
 150
 151 float vector3Dot(const vector3_t *a, const vector3_t *b)
 152 {
 153     return a->x * b->x + a->y * b->y + a->z * b->z;
 154 }
 155
 156 vector3_t *vector3Cross(vector3_t *result, const vector3_t *a, const vector3_t *b)
 157 {
 158     vector3_t tmp;
 159
 160     tmp.x = a->y * b->z - a->z * b->y;
 161     tmp.y = a->z * b->x - a->x * b->z;
 162     tmp.z = a->x * b->y - a->y * b->x;
 163
 164     *result = tmp;
 165
 166     return result;
 167 }
 168
 169 float vector3NormSq(const vector3_t *v)
 170 {
 171     return vector3Dot(v, v);
 172 }
 173
 174 float vector3Norm(const vector3_t *v)
 175 {
 176     return sqrtf(vector3NormSq(v));
 177 }
 178
 179 vector3_t *vector3Normalize(vector3_t *result, const vector3_t *v)
 180 {
 181     const float normSq = vector3NormSq(v);
 182
 183     if (normSq > 0) {  // Hopefully sqrt(nonzero) is quite large
 184         return vector3Scale(result, v, 1.0f / sqrtf(normSq));
 185     } else {
 186         return vector3Zero(result);
 187     }
 188 }
 189
 190 vector3_t *matrixVectorMul(vector3_t * result, const matrix33_t *mat, const vector3_t *v)
 191 {
 192     const vector3_t tmp = *v;
 193
 194     result->x = mat->m[0][0] * tmp.x + mat->m[0][1] * tmp.y + mat->m[0][2] * tmp.z;
 195     result->y = mat->m[1][0] * tmp.x + mat->m[1][1] * tmp.y + mat->m[1][2] * tmp.z;
 196     result->z = mat->m[2][0] * tmp.x + mat->m[2][1] * tmp.y + mat->m[2][2] * tmp.z;
 197
 198     return result;
 199 }
 200
 201 vector3_t *matrixTrnVectorMul(vector3_t *result, const matrix33_t *mat, const vector3_t *v)
 202 {
 203     const vector3_t tmp = *v;
 204
 205     result->x = mat->m[0][0] * tmp.x + mat->m[1][0] * tmp.y + mat->m[2][0] * tmp.z;
 206     result->y = mat->m[0][1] * tmp.x + mat->m[1][1] * tmp.y + mat->m[2][1] * tmp.z;
 207     result->z = mat->m[0][2] * tmp.x + mat->m[1][2] * tmp.y + mat->m[2][2] * tmp.z;
 208
 209     return result;
 210 }
 211
 212 matrix33_t *buildRotationMatrix(matrix33_t *result, const fp_angles_t *rpy)
 213 {
 214     const float cosx = cos_approx(rpy->angles.roll);
 215     const float sinx = sin_approx(rpy->angles.roll);
 216     const float cosy = cos_approx(rpy->angles.pitch);
 217     const float siny = sin_approx(rpy->angles.pitch);
 218     const float cosz = cos_approx(rpy->angles.yaw);
 219     const float sinz = sin_approx(rpy->angles.yaw);
 220
 221     result->m[0][X] = cosz * cosy;
 222     result->m[0][Y] = -cosy * sinz;
 223     result->m[0][Z] = siny;
 224     result->m[1][X] = sinz * cosx + sinx * cosz * siny;
 225     result->m[1][Y] = cosz * cosx - sinx * sinz * siny;
 226     result->m[1][Z] = -sinx * cosy;
 227     result->m[2][X] = sinx * sinz - cosz * cosx * siny;
 228     result->m[2][Y] = sinx * cosz + sinz * cosx * siny;
 229     result->m[2][Z] = cosy * cosx;
 230
 231     return result;
 232 }
 233
 234 vector3_t *applyRotationMatrix(vector3_t *v, const matrix33_t *rotationMatrix)
 235 {
 236     return matrixTrnVectorMul(v, rotationMatrix, v);
 237 }
 238
 239 matrix33_t *yawToRotationMatrixZ(matrix33_t *result, const float yaw)
 240 {
 241     const float sinYaw = sin_approx(yaw);
 242     const float cosYaw = cos_approx(yaw);
 243
 244     result->m[0][0] = cosYaw;
 245     result->m[0][1] = -sinYaw;
 246     result->m[0][2] = 0.0f;
 247     result->m[1][0] = sinYaw;
 248     result->m[1][1] = cosYaw;
 249     result->m[1][2] = 0.0f;
 250     result->m[2][0] = 0.0f;
 251     result->m[2][1] = 0.0f;
 252     result->m[2][2] = 1.0f;
 253
 254     return result;
 255 }