Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
pprz_algebra_double.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2014 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
31 #ifndef PPRZ_ALGEBRA_DOUBLE_H
32 #define PPRZ_ALGEBRA_DOUBLE_H
33 
34 #ifdef __cplusplus
35 extern "C" {
36 #endif
37 
38 #include "pprz_algebra.h"
39 #include "pprz_algebra_float.h"
40 
41 struct DoubleVect2 {
42  double x;
43  double y;
44 };
45 
46 struct DoubleVect3 {
47  double x;
48  double y;
49  double z;
50 };
51 
55 struct DoubleQuat {
56  double qi;
57  double qx;
58  double qy;
59  double qz;
60 };
61 
62 struct DoubleMat33 {
63  double m[3 * 3];
64 };
65 
69 struct DoubleRMat {
70  double m[3 * 3];
71 };
72 
76 struct DoubleEulers {
77  double phi;
78  double theta;
79  double psi;
80 };
81 
85 struct DoubleRates {
86  double p;
87  double q;
88  double r;
89 };
90 
91 #define DOUBLE_VECT3_ROUND(_v) DOUBLE_VECT3_RINT(_v, _v)
92 
93 
94 #define DOUBLE_VECT3_RINT(_vout, _vin) { \
95  (_vout).x = rint((_vin).x); \
96  (_vout).y = rint((_vin).y); \
97  (_vout).z = rint((_vin).z); \
98  }
99 
100 static inline double double_vect3_norm(struct DoubleVect3 *v)
101 {
102  return sqrt(VECT3_NORM2(*v));
103 }
104 
106 static inline void double_vect3_normalize(struct DoubleVect3 *v)
107 {
108  const double n = double_vect3_norm(v);
109  if (n > 0) {
110  v->x /= n;
111  v->y /= n;
112  v->z /= n;
113  }
114 }
115 
116 
118 static inline void double_quat_identity(struct DoubleQuat *q)
119 {
120  q->qi = 1.0;
121  q->qx = 0;
122  q->qy = 0;
123  q->qz = 0;
124 }
125 
126 static inline double double_quat_norm(struct DoubleQuat *q)
127 {
128  return sqrt(SQUARE(q->qi) + SQUARE(q->qx) + SQUARE(q->qy) + SQUARE(q->qz));
129 }
130 
131 
132 static inline void double_quat_normalize(struct DoubleQuat *q)
133 {
134  double qnorm = double_quat_norm(q);
135  if (qnorm > FLT_MIN) {
136  q->qi = q->qi / qnorm;
137  q->qx = q->qx / qnorm;
138  q->qy = q->qy / qnorm;
139  q->qz = q->qz / qnorm;
140  }
141 }
142 
155 extern void double_rmat_of_eulers_321(struct DoubleRMat *rm, struct DoubleEulers *e);
156 extern void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e);
157 extern void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q);
158 extern void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in);
159 
161 static inline void double_rmat_identity(struct DoubleRMat *rm)
162 {
163  FLOAT_MAT33_DIAG(*rm, 1., 1., 1.);
164 }
165 
169 extern void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b);
170 
174 extern void double_rmat_comp(struct DoubleRMat *m_a2c, struct DoubleRMat *m_a2b,
175  struct DoubleRMat *m_b2c);
176 
180 extern void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b,
181  struct DoubleVect3 *va);
182 
186 extern void double_rmat_transp_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_b2a,
187  struct DoubleVect3 *va);
188 
189 extern void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q);
190 static inline void double_rmat_of_eulers(struct DoubleRMat *rm, struct DoubleEulers *e)
191 {
193 }
194 
195 /* defines for backwards compatibility */
196 #define DOUBLE_RMAT_OF_EULERS(_rm, _e) WARNING("DOUBLE_RMAT_OF_EULERS macro is deprecated, use the lower case function instead") double_rmat_of_eulers(&(_rm), &(_e))
197 #define DOUBLE_RMAT_OF_EULERS_321(_rm, _e) WARNING("DOUBLE_RMAT_OF_EULERS_321 macro is deprecated, use the lower case function instead") double_rmat_of_eulers(&(_rm), &(_e))
198 #define DOUBLE_QUAT_OF_EULERS(_q, _e) WARNING("DOUBLE_QUAT_OF_EULERS macro is deprecated, use the lower case function instead") double_quat_of_eulers(&(_q), &(_e))
199 #define DOUBLE_EULERS_OF_QUAT(_e, _q) WARNING("DOUBLE_EULERS_OF_QUAT macro is deprecated, use the lower case function instead") double_eulers_of_quat(&(_e), &(_q))
200 #define DOUBLE_QUAT_VMULT(v_out, q, v_in) WARNING("DOUBLE_QUAT_VMULT macro is deprecated, use the lower case function instead") double_quat_vmult(&(v_out), &(q), &(v_in))
201 
202 #ifdef __cplusplus
203 } /* extern "C" */
204 #endif
205 
206 #endif /* PPRZ_ALGEBRA_DOUBLE_H */
207 
double_rmat_of_eulers
static void double_rmat_of_eulers(struct DoubleRMat *rm, struct DoubleEulers *e)
Definition: pprz_algebra_double.h:190
DoubleQuat
Roation quaternion.
Definition: pprz_algebra_double.h:55
DoubleQuat::qz
double qz
Definition: pprz_algebra_double.h:59
double_rmat_inv
void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b)
Inverse/transpose of a rotation matrix.
Definition: pprz_algebra_double.c:114
DoubleRMat::m
double m[3 *3]
Definition: pprz_algebra_double.h:70
DoubleVect3::z
double z
Definition: pprz_algebra_double.h:49
double_quat_of_eulers
void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e)
Definition: pprz_algebra_double.c:49
SQUARE
#define SQUARE(_a)
Definition: pprz_algebra.h:48
DoubleEulers::phi
double phi
in radians
Definition: pprz_algebra_double.h:77
DoubleRates::q
double q
in rad/s^2
Definition: pprz_algebra_double.h:87
DoubleEulers::theta
double theta
in radians
Definition: pprz_algebra_double.h:78
double_vect3_norm
static double double_vect3_norm(struct DoubleVect3 *v)
Definition: pprz_algebra_double.h:100
DoubleRates::r
double r
in rad/s^2
Definition: pprz_algebra_double.h:88
DoubleVect2::y
double y
Definition: pprz_algebra_double.h:43
DoubleEulers::psi
double psi
in radians
Definition: pprz_algebra_double.h:79
double_eulers_of_quat
void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q)
Definition: pprz_algebra_double.c:68
pprz_algebra_float.h
Paparazzi floating point algebra.
VECT3_NORM2
#define VECT3_NORM2(_v)
Definition: pprz_algebra.h:252
FLOAT_MAT33_DIAG
#define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22)
Definition: pprz_algebra_float.h:234
DoubleQuat::qy
double qy
Definition: pprz_algebra_double.h:58
double_quat_identity
static void double_quat_identity(struct DoubleQuat *q)
initialises a quaternion to identity
Definition: pprz_algebra_double.h:118
double_vect3_normalize
static void double_vect3_normalize(struct DoubleVect3 *v)
normalize 3D vector in place
Definition: pprz_algebra_double.h:106
double_rmat_of_quat
void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q)
Definition: pprz_algebra_double.c:164
DoubleRates
angular rates
Definition: pprz_algebra_double.h:85
DoubleVect3::x
double x
Definition: pprz_algebra_double.h:47
double_quat_vmult
void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in)
Definition: pprz_algebra_double.c:90
double_rmat_of_eulers_321
void double_rmat_of_eulers_321(struct DoubleRMat *rm, struct DoubleEulers *e)
Rotation matrix from 321 Euler angles (double).
Definition: pprz_algebra_double.c:29
DoubleQuat::qx
double qx
Definition: pprz_algebra_double.h:57
double_rmat_comp
void double_rmat_comp(struct DoubleRMat *m_a2c, struct DoubleRMat *m_a2b, struct DoubleRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
Definition: pprz_algebra_double.c:130
DoubleMat33
Definition: pprz_algebra_double.h:62
double_rmat_vmult
void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b, struct DoubleVect3 *va)
rotate 3D vector by rotation matrix.
Definition: pprz_algebra_double.c:146
DoubleRMat
rotation matrix
Definition: pprz_algebra_double.h:69
DoubleQuat::qi
double qi
Definition: pprz_algebra_double.h:56
double_quat_normalize
static void double_quat_normalize(struct DoubleQuat *q)
Definition: pprz_algebra_double.h:132
DoubleVect2
Definition: pprz_algebra_double.h:41
DoubleVect3
Definition: pprz_algebra_double.h:46
pprz_algebra.h
Paparazzi generic algebra macros.
DoubleVect3::y
double y
Definition: pprz_algebra_double.h:48
double_quat_norm
static double double_quat_norm(struct DoubleQuat *q)
Definition: pprz_algebra_double.h:126
DoubleVect2::x
double x
Definition: pprz_algebra_double.h:42
DoubleMat33::m
double m[3 *3]
Definition: pprz_algebra_double.h:63
double_rmat_identity
static void double_rmat_identity(struct DoubleRMat *rm)
initialises a rotation matrix to identity
Definition: pprz_algebra_double.h:161
DoubleRates::p
double p
in rad/s^2
Definition: pprz_algebra_double.h:86
DoubleEulers
euler angles
Definition: pprz_algebra_double.h:76
double_rmat_transp_vmult
void double_rmat_transp_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_b2a, struct DoubleVect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition: pprz_algebra_double.c:156