2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
22 * some functions are taken from https://github.com/iNavFlight/inav/
27 #include "common/maths.h"
48 static inline fpVector3_t
* vectorZero(fpVector3_t
*v
)
56 static inline float vectorNormSquared(const fpVector3_t
* v
)
58 return sq(v
->x
) + sq(v
->y
) + sq(v
->z
);
61 static inline float vectorNorm(const fpVector3_t
* v
)
63 return sqrtf(vectorNormSquared(v
));
66 static inline fpVector3_t
* vectorCrossProduct(fpVector3_t
*result
, const fpVector3_t
*a
, const fpVector3_t
*b
)
70 ab
.x
= a
->y
* b
->z
- a
->z
* b
->y
;
71 ab
.y
= a
->z
* b
->x
- a
->x
* b
->z
;
72 ab
.z
= a
->x
* b
->y
- a
->y
* b
->x
;
78 static inline fpVector3_t
* vectorAdd(fpVector3_t
*result
, const fpVector3_t
*a
, const fpVector3_t
*b
)
90 static inline fpVector3_t
* vectorScale(fpVector3_t
*result
, const fpVector3_t
*a
, const float b
)
102 static inline fpVector3_t
* vectorNormalize(fpVector3_t
*result
, const fpVector3_t
*v
)
104 float normSq
= vectorNormSquared(v
);
105 if (normSq
> 0) { // Hopefully sqrt(nonzero) is quite large
106 return vectorScale(result
, v
, 1.0f
/ sqrtf(normSq
));
108 return vectorZero(result
);
112 static inline fpVector3_t
* matrixVectorMul(fpVector3_t
* result
, const fpMat33_t
* mat
, const fpVector3_t
* a
)
116 r
.x
= mat
->m
[0][0] * a
->x
+ mat
->m
[0][1] * a
->y
+ mat
->m
[0][2] * a
->z
;
117 r
.y
= mat
->m
[1][0] * a
->x
+ mat
->m
[1][1] * a
->y
+ mat
->m
[1][2] * a
->z
;
118 r
.z
= mat
->m
[2][0] * a
->x
+ mat
->m
[2][1] * a
->y
+ mat
->m
[2][2] * a
->z
;
124 static inline fpVector3_t
* matrixTrnVectorMul(fpVector3_t
* result
, const fpMat33_t
* mat
, const fpVector3_t
* a
)
128 r
.x
= mat
->m
[0][0] * a
->x
+ mat
->m
[1][0] * a
->y
+ mat
->m
[2][0] * a
->z
;
129 r
.y
= mat
->m
[0][1] * a
->x
+ mat
->m
[1][1] * a
->y
+ mat
->m
[2][1] * a
->z
;
130 r
.z
= mat
->m
[0][2] * a
->x
+ mat
->m
[1][2] * a
->y
+ mat
->m
[2][2] * a
->z
;
136 static inline float vector2Cross(const fpVector2_t
*a
, const fpVector2_t
*b
)
138 return a
->x
* b
->y
- a
->y
* b
->x
;
141 static inline float vector2Dot(const fpVector2_t
*a
, const fpVector2_t
*b
)
143 return a
->x
* b
->x
+ a
->y
* b
->y
;
146 static inline float vector2Mag(const fpVector2_t
*a
)
148 return sqrtf(sq(a
->x
) + sq(a
->y
));