src/main/common/quaternion.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  * Original implementation by HaukeRa
  18  * Refactored and adapted by DigitalEntity
  19  */
  20
  21 #pragma once
  22
  23 #include <stdint.h>
  24 #include <math.h>
  25
  26 #include "common/maths.h"
  27 #include "common/vector.h"
  28
  29 typedef struct {
  30     float q0, q1, q2, q3;
  31 } fpQuaternion_t;
  32
  33 static inline fpQuaternion_t * quaternionInitUnit(fpQuaternion_t * result)
  34 {
  35     result->q0 = 1.0f;
  36     result->q1 = 0.0f;
  37     result->q2 = 0.0f;
  38     result->q3 = 0.0f;
  39     return result;
  40 }
  41
  42 static inline fpQuaternion_t * quaternionInitFromVector(fpQuaternion_t * result, const fpVector3_t * v)
  43 {
  44     result->q0 = 0.0f;
  45     result->q1 = v->x;
  46     result->q2 = v->y;
  47     result->q3 = v->z;
  48     return result;
  49 }
  50
  51 static inline void quaternionToAxisAngle(fpAxisAngle_t * result, const fpQuaternion_t * q)
  52 {
  53     fpAxisAngle_t a = {.axis = {{1.0f, 0.0f, 0.0f}}, .angle = 0};
  54
  55     a.angle = 2.0f * acos_approx(constrainf(q->q0, -1.0f, 1.0f));
  56
  57     if (a.angle > M_PIf) {
  58         a.angle -= 2.0f * M_PIf;
  59     }
  60
  61     const float sinVal = sqrtf(1.0f - q->q0 * q->q0);
  62
  63     // Axis is only valid when rotation is large enough sin(0.0057 deg) = 0.0001
  64     if (sinVal > 1e-4f) {
  65         a.axis.x = q->q1 / sinVal;
  66         a.axis.y = q->q2 / sinVal;
  67         a.axis.z = q->q3 / sinVal;
  68     } else {
  69         a.angle = 0;
  70     }
  71
  72     *result = a;
  73 }
  74
  75 static inline fpQuaternion_t * axisAngleToQuaternion(fpQuaternion_t * result, const fpAxisAngle_t * a)
  76 {
  77   fpQuaternion_t q;
  78   const float s = sin_approx(a->angle / 2.0f);
  79
  80   q.q0 = cos_approx(a->angle / 2.0f);
  81   q.q1 = -a->axis.x * s;
  82   q.q2 = -a->axis.y * s;
  83   q.q3 = -a->axis.z * s;
  84
  85   *result = q;
  86   return result;
  87 }
  88
  89 static inline float quaternionNormSqared(const fpQuaternion_t * q)
  90 {
  91     return sq(q->q0) + sq(q->q1) + sq(q->q2) + sq(q->q3);
  92 }
  93
  94 static inline fpQuaternion_t * quaternionMultiply(fpQuaternion_t * result, const fpQuaternion_t * a, const fpQuaternion_t * b)
  95 {
  96   fpQuaternion_t p;
  97
  98   p.q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3;
  99   p.q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2;
 100   p.q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1;
 101   p.q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0;
 102
 103   *result = p;
 104   return result;
 105 }
 106
 107 static inline fpQuaternion_t * quaternionScale(fpQuaternion_t * result, const fpQuaternion_t * a, const float b)
 108 {
 109     fpQuaternion_t p;
 110
 111     p.q0 = a->q0 * b;
 112     p.q1 = a->q1 * b;
 113     p.q2 = a->q2 * b;
 114     p.q3 = a->q3 * b;
 115
 116     *result = p;
 117     return result;
 118 }
 119
 120 static inline fpQuaternion_t * quaternionAdd(fpQuaternion_t * result, const fpQuaternion_t * a, const fpQuaternion_t * b)
 121 {
 122     fpQuaternion_t p;
 123
 124     p.q0 = a->q0 + b->q0;
 125     p.q1 = a->q1 + b->q1;
 126     p.q2 = a->q2 + b->q2;
 127     p.q3 = a->q3 + b->q3;
 128
 129     *result = p;
 130     return result;
 131 }
 132
 133 static inline fpQuaternion_t * quaternionConjugate(fpQuaternion_t * result, const fpQuaternion_t * q)
 134 {
 135     result->q0 =  q->q0;
 136     result->q1 = -q->q1;
 137     result->q2 = -q->q2;
 138     result->q3 = -q->q3;
 139
 140     return result;
 141 }
 142
 143 static inline fpQuaternion_t * quaternionNormalize(fpQuaternion_t * result, const fpQuaternion_t * q)
 144 {
 145     float mod = fast_fsqrtf(quaternionNormSqared(q));
 146     if (mod < 1e-6f) {
 147         // Length is too small - re-initialize to zero rotation
 148         result->q0 = 1;
 149         result->q1 = 0;
 150         result->q2 = 0;
 151         result->q3 = 0;
 152     }
 153     else {
 154         result->q0 = q->q0 / mod;
 155         result->q1 = q->q1 / mod;
 156         result->q2 = q->q2 / mod;
 157         result->q3 = q->q3 / mod;
 158     }
 159
 160     return result;
 161 }
 162
 163 static inline fpVector3_t * quaternionRotateVector(fpVector3_t * result, const fpVector3_t * vect, const fpQuaternion_t * ref)
 164 {
 165     fpQuaternion_t vectQuat, refConj;
 166
 167     vectQuat.q0 = 0;
 168     vectQuat.q1 = vect->x;
 169     vectQuat.q2 = vect->y;
 170     vectQuat.q3 = vect->z;
 171
 172     quaternionConjugate(&refConj, ref);
 173     quaternionMultiply(&vectQuat, &refConj, &vectQuat);
 174     quaternionMultiply(&vectQuat, &vectQuat, ref);
 175
 176     result->x = vectQuat.q1;
 177     result->y = vectQuat.q2;
 178     result->z = vectQuat.q3;
 179     return result;
 180 }
 181
 182 static inline fpVector3_t * quaternionRotateVectorInv(fpVector3_t * result, const fpVector3_t * vect, const fpQuaternion_t * ref)
 183 {
 184     fpQuaternion_t vectQuat, refConj;
 185
 186     vectQuat.q0 = 0;
 187     vectQuat.q1 = vect->x;
 188     vectQuat.q2 = vect->y;
 189     vectQuat.q3 = vect->z;
 190
 191     quaternionConjugate(&refConj, ref);
 192     quaternionMultiply(&vectQuat, ref, &vectQuat);
 193     quaternionMultiply(&vectQuat, &vectQuat, &refConj);
 194
 195     result->x = vectQuat.q1;
 196     result->y = vectQuat.q2;
 197     result->z = vectQuat.q3;
 198     return result;
 199 }