src/main/common/vector.h

   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 /*
  23  * some functions are taken from https://github.com/iNavFlight/inav/
  24  */
  25
  26 #pragma once
  27
  28 #include <stdbool.h>
  29
  30 #include "common/maths.h"
  31
  32 typedef union vector2_u {
  33     float v[2];
  34     struct {
  35        float x, y;
  36     };
  37 } vector2_t;
  38
  39 typedef union vector3_u {
  40     float v[3];
  41     struct {
  42        float x, y, z;
  43     };
  44 } vector3_t;
  45
  46 typedef struct matrix33_s {
  47     float m[3][3];
  48 } matrix33_t;
  49
  50 bool vector2Equal(const vector2_t *a, const vector2_t *b);
  51 vector2_t *vector2Zero(vector2_t *v);
  52 vector2_t *vector2Add(vector2_t *result, const vector2_t *a, const vector2_t *b);
  53 vector2_t *vector2Sub(vector2_t *result, const vector2_t *a, const vector2_t *b);
  54 vector2_t *vector2Scale(vector2_t *result, const vector2_t *v, const float k);
  55 float vector2Dot(const vector2_t *a, const vector2_t *b);
  56 float vector2Cross(const vector2_t *a, const vector2_t *b);
  57 float vector2NormSq(const vector2_t *v);
  58 float vector2Norm(const vector2_t *v);
  59 vector2_t *vector2Normalize(vector2_t *result, const vector2_t *v);
  60 vector2_t *vector2Rotate(vector2_t *result, const vector2_t *v, const float angle);
  61
  62 bool vector3Equal(const vector3_t *a, const vector3_t *b);
  63 vector3_t *vector3Zero(vector3_t *v);
  64 vector3_t *vector3Add(vector3_t *result, const vector3_t *a, const vector3_t *b);
  65 vector3_t *vector3Sub(vector3_t *result, const vector3_t *a, const vector3_t *b);
  66 vector3_t *vector3Scale(vector3_t *result, const vector3_t *v, const float k);
  67 float vector3Dot(const vector3_t *a, const vector3_t *b);
  68 vector3_t *vector3Cross(vector3_t *result, const vector3_t *a, const vector3_t *b);
  69 float vector3NormSq(const vector3_t *v);
  70 float vector3Norm(const vector3_t *v);
  71 vector3_t *vector3Normalize(vector3_t *result, const vector3_t *v);
  72
  73 vector3_t *matrixVectorMul(vector3_t *result, const matrix33_t *mat, const vector3_t *v);
  74 vector3_t *matrixTrnVectorMul(vector3_t *result, const matrix33_t *mat, const vector3_t *v);
  75
  76 matrix33_t *buildRotationMatrix(matrix33_t *result, const fp_angles_t *rpy);
  77 vector3_t *applyRotationMatrix(vector3_t *v, const matrix33_t *rotationMatrix);
  78
  79 matrix33_t *yawToRotationMatrixZ(matrix33_t *result, const float yaw);