src/main/common/vector.h

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * INAV is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #pragma once
  19
  20 #include <stdint.h>
  21 #include <math.h>
  22
  23 #include "common/maths.h"
  24
  25 typedef union {
  26     float v[3];
  27     struct {
  28        float x,y,z;
  29     };
  30 } fpVector3_t;
  31
  32 typedef struct {
  33     float m[3][3];
  34 } fpMat3_t;
  35
  36 typedef struct {
  37     fpVector3_t axis;
  38     float angle;
  39 } fpAxisAngle_t;
  40
  41 void rotationMatrixFromAngles(fpMat3_t * rmat, const fp_angles_t * angles);
  42 void rotationMatrixFromAxisAngle(fpMat3_t * rmat, const fpAxisAngle_t * a);
  43
  44 static inline void vectorZero(fpVector3_t * v)
  45 {
  46     v->x = 0.0f;
  47     v->y = 0.0f;
  48     v->z = 0.0f;
  49 }
  50
  51 static inline fpVector3_t * rotationMatrixRotateVector(fpVector3_t * result, const fpVector3_t * a, const fpMat3_t * rmat)
  52 {
  53     fpVector3_t r;
  54
  55     r.x = rmat->m[0][0] * a->x + rmat->m[1][0] * a->y + rmat->m[2][0] * a->z;
  56     r.y = rmat->m[0][1] * a->x + rmat->m[1][1] * a->y + rmat->m[2][1] * a->z;
  57     r.z = rmat->m[0][2] * a->x + rmat->m[1][2] * a->y + rmat->m[2][2] * a->z;
  58
  59     *result = r;
  60     return result;
  61 }
  62
  63 static inline float vectorNormSquared(const fpVector3_t * v)
  64 {
  65     return sq(v->x) + sq(v->y) + sq(v->z);
  66 }
  67
  68 static inline fpVector3_t * vectorNormalize(fpVector3_t * result, const fpVector3_t * v)
  69 {
  70     float length = fast_fsqrtf(vectorNormSquared(v));
  71     if (length != 0) {
  72         result->x = v->x / length;
  73         result->y = v->y / length;
  74         result->z = v->z / length;
  75     }
  76     else {
  77         result->x = 0;
  78         result->y = 0;
  79         result->z = 0;
  80     }
  81     return result;
  82 }
  83
  84 static inline fpVector3_t * vectorCrossProduct(fpVector3_t * result, const fpVector3_t * a, const fpVector3_t * b)
  85 {
  86     fpVector3_t ab;
  87
  88     ab.x = a->y * b->z - a->z * b->y;
  89     ab.y = a->z * b->x - a->x * b->z;
  90     ab.z = a->x * b->y - a->y * b->x;
  91
  92     *result = ab;
  93     return result;
  94 }
  95
  96 static inline fpVector3_t * vectorAdd(fpVector3_t * result, const fpVector3_t * a, const fpVector3_t * b)
  97 {
  98     fpVector3_t ab;
  99
 100     ab.x = a->x + b->x;
 101     ab.y = a->y + b->y;
 102     ab.z = a->z + b->z;
 103
 104     *result = ab;
 105     return result;
 106 }
 107
 108 static inline fpVector3_t * vectorScale(fpVector3_t * result, const fpVector3_t * a, const float b)
 109 {
 110     fpVector3_t ab;
 111
 112     ab.x = a->x * b;
 113     ab.y = a->y * b;
 114     ab.z = a->z * b;
 115
 116     *result = ab;
 117     return result;
 118 }