Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
pprz_algebra_double.c
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 
27 #include "pprz_algebra_double.h"
28 
29 void double_rmat_of_eulers_321(struct DoubleRMat *rm, struct DoubleEulers *e)
30 {
31  const double sphi = sin(e->phi);
32  const double cphi = cos(e->phi);
33  const double stheta = sin(e->theta);
34  const double ctheta = cos(e->theta);
35  const double spsi = sin(e->psi);
36  const double cpsi = cos(e->psi);
37 
38  RMAT_ELMT(*rm, 0, 0) = ctheta * cpsi;
39  RMAT_ELMT(*rm, 0, 1) = ctheta * spsi;
40  RMAT_ELMT(*rm, 0, 2) = -stheta;
41  RMAT_ELMT(*rm, 1, 0) = sphi * stheta * cpsi - cphi * spsi;
42  RMAT_ELMT(*rm, 1, 1) = sphi * stheta * spsi + cphi * cpsi;
43  RMAT_ELMT(*rm, 1, 2) = sphi * ctheta;
44  RMAT_ELMT(*rm, 2, 0) = cphi * stheta * cpsi + sphi * spsi;
45  RMAT_ELMT(*rm, 2, 1) = cphi * stheta * spsi - sphi * cpsi;
46  RMAT_ELMT(*rm, 2, 2) = cphi * ctheta;
47 }
48 
49 void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e)
50 {
51  const double phi2 = e->phi / 2.0;
52  const double theta2 = e->theta / 2.0;
53  const double psi2 = e->psi / 2.0;
54 
55  const double s_phi2 = sin(phi2);
56  const double c_phi2 = cos(phi2);
57  const double s_theta2 = sin(theta2);
58  const double c_theta2 = cos(theta2);
59  const double s_psi2 = sin(psi2);
60  const double c_psi2 = cos(psi2);
61 
62  q->qi = c_phi2 * c_theta2 * c_psi2 + s_phi2 * s_theta2 * s_psi2;
63  q->qx = -c_phi2 * s_theta2 * s_psi2 + s_phi2 * c_theta2 * c_psi2;
64  q->qy = c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2;
65  q->qz = c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2;
66 }
67 
68 void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q)
69 {
70  const double qx2 = q->qx * q->qx;
71  const double qy2 = q->qy * q->qy;
72  const double qz2 = q->qz * q->qz;
73  const double qiqx = q->qi * q->qx;
74  const double qiqy = q->qi * q->qy;
75  const double qiqz = q->qi * q->qz;
76  const double qxqy = q->qx * q->qy;
77  const double qxqz = q->qx * q->qz;
78  const double qyqz = q->qy * q->qz;
79  const double dcm00 = 1.0 - 2.*(qy2 + qz2);
80  const double dcm01 = 2.*(qxqy + qiqz);
81  const double dcm02 = 2.*(qxqz - qiqy);
82  const double dcm12 = 2.*(qyqz + qiqx);
83  const double dcm22 = 1.0 - 2.*(qx2 + qy2);
84 
85  e->phi = atan2(dcm12, dcm22);
86  e->theta = -asin(dcm02);
87  e->psi = atan2(dcm01, dcm00);
88 }
89 
90 void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in)
91 {
92  const double qi2_M1_2 = q->qi * q->qi - 0.5;
93  const double qiqx = q->qi * q->qx;
94  const double qiqy = q->qi * q->qy;
95  const double qiqz = q->qi * q->qz;
96  double m01 = q->qx * q->qy; /* aka qxqy */
97  double m02 = q->qx * q->qz; /* aka qxqz */
98  double m12 = q->qy * q->qz; /* aka qyqz */
99 
100  const double m00 = qi2_M1_2 + q->qx * q->qx;
101  const double m10 = m01 - qiqz;
102  const double m20 = m02 + qiqy;
103  const double m21 = m12 - qiqx;
104  m01 += qiqz;
105  m02 -= qiqy;
106  m12 += qiqx;
107  const double m11 = qi2_M1_2 + q->qy * q->qy;
108  const double m22 = qi2_M1_2 + q->qz * q->qz;
109  v_out->x = 2 * (m00 * v_in->x + m01 * v_in->y + m02 * v_in->z);
110  v_out->y = 2 * (m10 * v_in->x + m11 * v_in->y + m12 * v_in->z);
111  v_out->z = 2 * (m20 * v_in->x + m21 * v_in->y + m22 * v_in->z);
112 }
113 
114 void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b)
115 {
116  /*RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);*/
117  RMAT_ELMT(*m_b2a, 0, 1) = RMAT_ELMT(*m_a2b, 1, 0);
118  RMAT_ELMT(*m_b2a, 0, 2) = RMAT_ELMT(*m_a2b, 2, 0);
119  RMAT_ELMT(*m_b2a, 1, 0) = RMAT_ELMT(*m_a2b, 0, 1);
120  /*RMAT_ELMT(*m_b2a, 1, 1) = RMAT_ELMT(*m_a2b, 1, 1);*/
121  RMAT_ELMT(*m_b2a, 1, 2) = RMAT_ELMT(*m_a2b, 2, 1);
122  RMAT_ELMT(*m_b2a, 2, 0) = RMAT_ELMT(*m_a2b, 0, 2);
123  RMAT_ELMT(*m_b2a, 2, 1) = RMAT_ELMT(*m_a2b, 1, 2);
124  /*RMAT_ELMT(*m_b2a, 2, 2) = RMAT_ELMT(*m_a2b, 2, 2);*/
125 }
126 
130 void double_rmat_comp(struct DoubleRMat *m_a2c, struct DoubleRMat *m_a2b, struct DoubleRMat *m_b2c)
131 {
132  m_a2c->m[0] = m_b2c->m[0] * m_a2b->m[0] + m_b2c->m[1] * m_a2b->m[3] + m_b2c->m[2] * m_a2b->m[6];
133  m_a2c->m[1] = m_b2c->m[0] * m_a2b->m[1] + m_b2c->m[1] * m_a2b->m[4] + m_b2c->m[2] * m_a2b->m[7];
134  m_a2c->m[2] = m_b2c->m[0] * m_a2b->m[2] + m_b2c->m[1] * m_a2b->m[5] + m_b2c->m[2] * m_a2b->m[8];
135  m_a2c->m[3] = m_b2c->m[3] * m_a2b->m[0] + m_b2c->m[4] * m_a2b->m[3] + m_b2c->m[5] * m_a2b->m[6];
136  m_a2c->m[4] = m_b2c->m[3] * m_a2b->m[1] + m_b2c->m[4] * m_a2b->m[4] + m_b2c->m[5] * m_a2b->m[7];
137  m_a2c->m[5] = m_b2c->m[3] * m_a2b->m[2] + m_b2c->m[4] * m_a2b->m[5] + m_b2c->m[5] * m_a2b->m[8];
138  m_a2c->m[6] = m_b2c->m[6] * m_a2b->m[0] + m_b2c->m[7] * m_a2b->m[3] + m_b2c->m[8] * m_a2b->m[6];
139  m_a2c->m[7] = m_b2c->m[6] * m_a2b->m[1] + m_b2c->m[7] * m_a2b->m[4] + m_b2c->m[8] * m_a2b->m[7];
140  m_a2c->m[8] = m_b2c->m[6] * m_a2b->m[2] + m_b2c->m[7] * m_a2b->m[5] + m_b2c->m[8] * m_a2b->m[8];
141 }
142 
146 void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b, struct DoubleVect3 *va)
147 {
148  vb->x = m_a2b->m[0] * va->x + m_a2b->m[1] * va->y + m_a2b->m[2] * va->z;
149  vb->y = m_a2b->m[3] * va->x + m_a2b->m[4] * va->y + m_a2b->m[5] * va->z;
150  vb->z = m_a2b->m[6] * va->x + m_a2b->m[7] * va->y + m_a2b->m[8] * va->z;
151 }
152 
156 void double_rmat_transp_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_b2a, struct DoubleVect3 *va)
157 {
158  vb->x = m_b2a->m[0] * va->x + m_b2a->m[3] * va->y + m_b2a->m[6] * va->z;
159  vb->y = m_b2a->m[1] * va->x + m_b2a->m[4] * va->y + m_b2a->m[7] * va->z;
160  vb->z = m_b2a->m[2] * va->x + m_b2a->m[5] * va->y + m_b2a->m[8] * va->z;
161 }
162 
163 /* C n->b rotation matrix */
164 void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q)
165 {
166  const double _a = M_SQRT2 * q->qi;
167  const double _b = M_SQRT2 * q->qx;
168  const double _c = M_SQRT2 * q->qy;
169  const double _d = M_SQRT2 * q->qz;
170  const double a2_1 = _a * _a - 1;
171  const double ab = _a * _b;
172  const double ac = _a * _c;
173  const double ad = _a * _d;
174  const double bc = _b * _c;
175  const double bd = _b * _d;
176  const double cd = _c * _d;
177  RMAT_ELMT(*rm, 0, 0) = a2_1 + _b * _b;
178  RMAT_ELMT(*rm, 0, 1) = bc + ad;
179  RMAT_ELMT(*rm, 0, 2) = bd - ac;
180  RMAT_ELMT(*rm, 1, 0) = bc - ad;
181  RMAT_ELMT(*rm, 1, 1) = a2_1 + _c * _c;
182  RMAT_ELMT(*rm, 1, 2) = cd + ab;
183  RMAT_ELMT(*rm, 2, 0) = bd + ac;
184  RMAT_ELMT(*rm, 2, 1) = cd - ab;
185  RMAT_ELMT(*rm, 2, 2) = a2_1 + _d * _d;
186 }
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
DoubleEulers::phi
double phi
in radians
Definition: pprz_algebra_double.h:77
DoubleEulers::theta
double theta
in radians
Definition: pprz_algebra_double.h:78
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
DoubleQuat::qy
double qy
Definition: pprz_algebra_double.h:58
double_rmat_of_quat
void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q)
Definition: pprz_algebra_double.c:164
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
RMAT_ELMT
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:660
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
pprz_algebra_double.h
Paparazzi double precision floating point algebra.
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
M_SQRT2
#define M_SQRT2
Definition: pprz_algebra_float.h:46
DoubleQuat::qi
double qi
Definition: pprz_algebra_double.h:56
DoubleVect3
Definition: pprz_algebra_double.h:46
DoubleVect3::y
double y
Definition: pprz_algebra_double.h:48
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