Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
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
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
66}
67
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
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
115{
116 a2c->qi = a2b->qi * b2c->qi - a2b->qx * b2c->qx - a2b->qy * b2c->qy - a2b->qz * b2c->qz;
117 a2c->qx = a2b->qi * b2c->qx + a2b->qx * b2c->qi + a2b->qy * b2c->qz - a2b->qz * b2c->qy;
118 a2c->qy = a2b->qi * b2c->qy - a2b->qx * b2c->qz + a2b->qy * b2c->qi + a2b->qz * b2c->qx;
119 a2c->qz = a2b->qi * b2c->qz + a2b->qx * b2c->qy - a2b->qy * b2c->qx + a2b->qz * b2c->qi;
120}
121
122
124{
125 /*RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);*/
126 RMAT_ELMT(*m_b2a, 0, 1) = RMAT_ELMT(*m_a2b, 1, 0);
127 RMAT_ELMT(*m_b2a, 0, 2) = RMAT_ELMT(*m_a2b, 2, 0);
128 RMAT_ELMT(*m_b2a, 1, 0) = RMAT_ELMT(*m_a2b, 0, 1);
129 /*RMAT_ELMT(*m_b2a, 1, 1) = RMAT_ELMT(*m_a2b, 1, 1);*/
130 RMAT_ELMT(*m_b2a, 1, 2) = RMAT_ELMT(*m_a2b, 2, 1);
131 RMAT_ELMT(*m_b2a, 2, 0) = RMAT_ELMT(*m_a2b, 0, 2);
132 RMAT_ELMT(*m_b2a, 2, 1) = RMAT_ELMT(*m_a2b, 1, 2);
133 /*RMAT_ELMT(*m_b2a, 2, 2) = RMAT_ELMT(*m_a2b, 2, 2);*/
134}
135
140{
141 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];
142 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];
143 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];
144 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];
145 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];
146 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];
147 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];
148 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];
149 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];
150}
151
155void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b, struct DoubleVect3 *va)
156{
157 vb->x = m_a2b->m[0] * va->x + m_a2b->m[1] * va->y + m_a2b->m[2] * va->z;
158 vb->y = m_a2b->m[3] * va->x + m_a2b->m[4] * va->y + m_a2b->m[5] * va->z;
159 vb->z = m_a2b->m[6] * va->x + m_a2b->m[7] * va->y + m_a2b->m[8] * va->z;
160}
161
166{
167 vb->x = m_b2a->m[0] * va->x + m_b2a->m[3] * va->y + m_b2a->m[6] * va->z;
168 vb->y = m_b2a->m[1] * va->x + m_b2a->m[4] * va->y + m_b2a->m[7] * va->z;
169 vb->z = m_b2a->m[2] * va->x + m_b2a->m[5] * va->y + m_b2a->m[8] * va->z;
170}
171
172/* C n->b rotation matrix */
174{
175 const double _a = M_SQRT2 * q->qi;
176 const double _b = M_SQRT2 * q->qx;
177 const double _c = M_SQRT2 * q->qy;
178 const double _d = M_SQRT2 * q->qz;
179 const double a2_1 = _a * _a - 1;
180 const double ab = _a * _b;
181 const double ac = _a * _c;
182 const double ad = _a * _d;
183 const double bc = _b * _c;
184 const double bd = _b * _d;
185 const double cd = _c * _d;
186 RMAT_ELMT(*rm, 0, 0) = a2_1 + _b * _b;
187 RMAT_ELMT(*rm, 0, 1) = bc + ad;
188 RMAT_ELMT(*rm, 0, 2) = bd - ac;
189 RMAT_ELMT(*rm, 1, 0) = bc - ad;
190 RMAT_ELMT(*rm, 1, 1) = a2_1 + _c * _c;
191 RMAT_ELMT(*rm, 1, 2) = cd + ab;
192 RMAT_ELMT(*rm, 2, 0) = bd + ac;
193 RMAT_ELMT(*rm, 2, 1) = cd - ab;
194 RMAT_ELMT(*rm, 2, 2) = a2_1 + _d * _d;
195}
double psi
in radians
double theta
in radians
double phi
in radians
void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q)
void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in)
void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b, struct DoubleVect3 *va)
rotate 3D vector by rotation matrix.
void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b)
Inverse/transpose of a rotation matrix.
void double_quat_comp(struct DoubleQuat *a2c, struct DoubleQuat *a2b, struct DoubleQuat *b2c)
Composition (multiplication) of two quaternions.
void double_rmat_of_eulers_321(struct DoubleRMat *rm, struct DoubleEulers *e)
Rotation matrix from 321 Euler angles (double).
void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e)
void double_rmat_transp_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_b2a, struct DoubleVect3 *va)
rotate 3D vector by transposed rotation matrix.
void double_rmat_comp(struct DoubleRMat *m_a2c, struct DoubleRMat *m_a2b, struct DoubleRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q)
Roation quaternion.
rotation matrix
#define M_SQRT2
#define RMAT_ELMT(_rm, _row, _col)
uint16_t foo
Definition main_demo5.c:58
Paparazzi double precision floating point algebra.