Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 04:45:37 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[python][math] some DoubleRMat additions

Browse Source
This commit is contained in:
Felix Ruess
2015-01-10 19:01:28 +01:00
parent 667397445c
commit 01f8065dd7
5 changed files with 136 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_algebra_double.c
+64
View File
@@ -110,3 +110,67 @@ void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct D
v_out->y = 2 * (m10 * v_in->x + m11 * v_in->y + m12 * v_in->z);
v_out->z = 2 * (m20 * v_in->x + m21 * v_in->y + m22 * v_in->z);
}
void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b)
{
/*RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);*/
RMAT_ELMT(*m_b2a, 0, 1) = RMAT_ELMT(*m_a2b, 1, 0);
RMAT_ELMT(*m_b2a, 0, 2) = RMAT_ELMT(*m_a2b, 2, 0);
RMAT_ELMT(*m_b2a, 1, 0) = RMAT_ELMT(*m_a2b, 0, 1);
/*RMAT_ELMT(*m_b2a, 1, 1) = RMAT_ELMT(*m_a2b, 1, 1);*/
RMAT_ELMT(*m_b2a, 1, 2) = RMAT_ELMT(*m_a2b, 2, 1);
RMAT_ELMT(*m_b2a, 2, 0) = RMAT_ELMT(*m_a2b, 0, 2);
RMAT_ELMT(*m_b2a, 2, 1) = RMAT_ELMT(*m_a2b, 1, 2);
/*RMAT_ELMT(*m_b2a, 2, 2) = RMAT_ELMT(*m_a2b, 2, 2);*/
}
/** Composition (multiplication) of two rotation matrices.
* m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b
*/
void double_rmat_comp(struct DoubleRMat *m_a2c, struct DoubleRMat *m_a2b, struct DoubleRMat *m_b2c)
{
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];
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];
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];
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];
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];
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];
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];
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];
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];
}
/** rotate 3D vector by rotation matrix.
* vb = m_a2b * va
*/
void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b, struct DoubleVect3 *va)
{
vb->x = m_a2b->m[0] * va->x + m_a2b->m[1] * va->y + m_a2b->m[2] * va->z;
vb->y = m_a2b->m[3] * va->x + m_a2b->m[4] * va->y + m_a2b->m[5] * va->z;
vb->z = m_a2b->m[6] * va->x + m_a2b->m[7] * va->y + m_a2b->m[8] * va->z;
}
/* C n->b rotation matrix */
void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q)
{
const double _a = M_SQRT2 * q->qi;
const double _b = M_SQRT2 * q->qx;
const double _c = M_SQRT2 * q->qy;
const double _d = M_SQRT2 * q->qz;
const double a2_1 = _a * _a - 1;
const double ab = _a * _b;
const double ac = _a * _c;
const double ad = _a * _d;
const double bc = _b * _c;
const double bd = _b * _d;
const double cd = _c * _d;
RMAT_ELMT(*rm, 0, 0) = a2_1 + _b * _b;
RMAT_ELMT(*rm, 0, 1) = bc + ad;
RMAT_ELMT(*rm, 0, 2) = bd - ac;
RMAT_ELMT(*rm, 1, 0) = bc - ad;
RMAT_ELMT(*rm, 1, 1) = a2_1 + _c * _c;
RMAT_ELMT(*rm, 1, 2) = cd + ab;
RMAT_ELMT(*rm, 2, 0) = bd + ac;
RMAT_ELMT(*rm, 2, 1) = cd - ab;
RMAT_ELMT(*rm, 2, 2) = a2_1 + _d * _d;
}
sw/airborne/math/pprz_algebra_double.h
+23
View File
@@ -153,6 +153,29 @@ extern void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e);
extern void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q);
extern void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in);
/** initialises a rotation matrix to identity */
static inline void double_rmat_identity(struct DoubleRMat *rm)
{
FLOAT_MAT33_DIAG(*rm, 1., 1., 1.);
}
/** Inverse/transpose of a rotation matrix.
* m_b2a = inv(_m_a2b) = transp(_m_a2b)
*/
extern void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b);
/** Composition (multiplication) of two rotation matrices.
* m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b
*/
extern void double_rmat_comp(struct DoubleRMat *m_a2c, struct DoubleRMat *m_a2b,
struct DoubleRMat *m_b2c);
/** rotate 3D vector by rotation matrix.
* vb = m_a2b * va
*/
extern void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b,
struct DoubleVect3 *va);
extern void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q);
static inline void double_rmat_of_eulers(struct DoubleRMat *rm, struct DoubleEulers *e)
{
double_rmat_of_eulers_321(rm, e);
sw/airborne/test/math/compare_utm_enu.py
+1
View File
@@ -8,6 +8,7 @@ PPRZ_SRC = os.getenv("PAPARAZZI_SRC", "../../../..")
sys.path.append(PPRZ_SRC + "/sw/lib/python")
from pprz_math.geodetic import *
from pprz_math.algebra import DoubleRMat
import matplotlib.pyplot as plt
import numpy as np
sw/lib/python/pprz_math/pprz_algebra_double.i
+45
View File
@@ -177,6 +177,51 @@ struct DoubleEulers {
}
};
%extend DoubleRMat {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"RMat[% .5f, % .5f, % .5f]\n [% .5f, % .5f, % .5f]\n [% .5f, % .5f, % .5f]",
$self->m[0], $self->m[1], $self->m[2], $self->m[3], $self->m[4],
$self->m[5], $self->m[6], $self->m[7], $self->m[8]);
return tmp;
}
DoubleRMat() {
struct DoubleRMat *rm = (struct DoubleRMat *) malloc(sizeof(struct DoubleRMat));
double_rmat_identity(rm);
return rm;
}
struct DoubleRMat __mul__(struct DoubleRMat *m_b2c) {
struct DoubleRMat m_a2c;
double_rmat_comp(&m_a2c, $self, m_b2c);
return m_a2c;
}
struct DoubleRMat __mul__(struct DoubleQuat *q) {
struct DoubleRMat m_b2c;
double_rmat_of_quat(&m_b2c, q);
struct DoubleRMat m_a2c;
double_rmat_comp(&m_a2c, $self, &m_b2c);
return m_a2c;
}
struct DoubleVect3 __mul__(struct DoubleVect3 *va) {
struct DoubleVect3 v;
double_rmat_vmult(&v, $self, va);
return v;
}
void set_identity() {
double_rmat_identity($self);
}
struct DoubleRMat inverse() {
struct DoubleRMat inv;
double_rmat_inv(&inv, $self);
return inv;
}
struct DoubleRMat transposed() {
struct DoubleRMat inv;
double_rmat_inv(&inv, $self);
return inv;
}
};
%extend DoubleEulers {
char *__str__() {
static char tmp[1024];
sw/lib/python/pprz_math/pprz_algebra_float.i
+3
View File
@@ -235,6 +235,9 @@ struct FloatEulers {
float_quat_of_rmat(&q, $self);
return q;
}
double reorthogonalize() {
return float_rmat_reorthogonalize($self);
}
};
%extend FloatEulers {