Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

more rotation matrices functions

Browse Source
This commit is contained in:
Antoine Drouin
2009-05-21 14:07:29 +00:00
parent 365f5ab105
commit 848fc0d5a6
3 changed files with 91 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/pprz_algebra_float.h
+27 -15
View File
@@ -90,6 +90,11 @@ struct FloatRates {
vo.z = v1.x*v2.y - v1.y*v2.x; \
}
#define FLOAT_VECT3_NORMALIZE(_v) { \
const float n = FLOAT_VECT3_NORM(_v); \
FLOAT_VECT3_SMUL(_v, 1./n, _v); \
}
/*
* Rotation Matrices
*/
@@ -111,26 +116,26 @@ struct FloatRates {
}
/* initialises a rotation matrix from unit vector axis and angle */
#define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _ax, _an) { \
#define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an) { \
\
const float ux2 = _rm.x*_rm.x; \
const float uy2 = _rm.y*_rm.y; \
const float uz2 = _rm.z*_rm.z; \
const float uxuy = _rm.x*_rm.y; \
const float uyuz = _rm.y*_rm.z; \
const float uxuz = _rm.x*_rm.z; \
const float can = cosf(_am); \
const float san = sinf(_am); \
const float ux2 = _uv.x*_uv.x; \
const float uy2 = _uv.y*_uv.y; \
const float uz2 = _uv.z*_uv.z; \
const float uxuy = _uv.x*_uv.y; \
const float uyuz = _uv.y*_uv.z; \
const float uxuz = _uv.x*_uv.z; \
const float can = cosf(_an); \
const float san = sinf(_an); \
const float one_m_can = (1. - can); \
\
RMAT_ELMT(_rm, 0, 0) = ux2 + (1.-ux2)*can; \
RMAT_ELMT(_rm, 0, 1) = uxuy*one_m_can - uz*san; \
RMAT_ELMT(_rm, 0, 2) = uxuz*one_m_can + uy*san; \
RMAT_ELMT(_rm, 1, 0) = uxuy*one_m_can + uz*san; \
RMAT_ELMT(_rm, 0, 1) = uxuy*one_m_can + _uv.z*san; \
RMAT_ELMT(_rm, 0, 2) = uxuz*one_m_can - _uv.y*san; \
RMAT_ELMT(_rm, 1, 0) = uxuy*one_m_can - _uv.z*san; \
RMAT_ELMT(_rm, 1, 1) = uy2 + (1.-uy2)*can; \
RMAT_ELMT(_rm, 1, 2) = uyuz*one_m_can-ux*san; \
RMAT_ELMT(_rm, 2, 0) = uxuz*one_m_can-uy*san; \
RMAT_ELMT(_rm, 2, 1) = uyuz*one_m_can+ux*san; \
RMAT_ELMT(_rm, 1, 2) = uyuz*one_m_can + _uv.x*san; \
RMAT_ELMT(_rm, 2, 0) = uxuz*one_m_can + _uv.y*san; \
RMAT_ELMT(_rm, 2, 1) = uyuz*one_m_can - _uv.x*san; \
RMAT_ELMT(_rm, 2, 2) = uz2 + (1.-uz2)*can; \
\
}
@@ -334,6 +339,13 @@ struct FloatRates {
\
}
#define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) { \
const float san = sinf(_an/2.); \
_q.qi = cosf(_an/2.); \
_q.qx = san * _uv.x; \
_q.qy = san * _uv.y; \
_q.qz = san * _uv.z; \
}
/*
* Euler angles
sw/airborne/test/test_algebra.c
+63 -3
View File
@@ -26,6 +26,7 @@ float test_quat_comp(struct FloatQuat qa2b_f, struct FloatQuat qb2c_f, int displ
float test_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int display);
float test_quat_comp_inv(struct FloatQuat qa2c_f, struct FloatQuat qb2c_f, int display);
void test_of_axis_angle(void);
int main(int argc, char** argv) {
@@ -38,9 +39,10 @@ int main(int argc, char** argv) {
// test_4_float();
printf("\n");
test_5();
test_6();
test_7();
// test_5();
//test_6();
//test_7();
test_of_axis_angle();
return 0;
}
@@ -478,3 +480,61 @@ float test_quat_comp_inv(struct FloatQuat qa2c_f, struct FloatQuat qb2c_f, int d
return norm_err;
}
void test_of_axis_angle(void) {
struct FloatVect3 axis = { 0., 1., 0.};
FLOAT_VECT3_NORMALIZE(axis);
DISPLAY_FLOAT_VECT3("axis", axis);
const float angle = RadOfDeg(30.);
printf("angle %f\n", DegOfRad(angle));
struct FloatQuat my_q;
FLOAT_QUAT_OF_AXIS_ANGLE(my_q, axis, angle);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("quat", my_q);
struct FloatMat33 my_r1;
FLOAT_RMAT_OF_QUAT(my_r1, my_q);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", my_r1);
DISPLAY_FLOAT_RMAT("rmat1", my_r1);
struct FloatMat33 my_r;
FLOAT_RMAT_OF_AXIS_ANGLE(my_r, axis, angle);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", my_r);
DISPLAY_FLOAT_RMAT("rmat", my_r);
printf("\n");
struct FloatEulers eul = {RadOfDeg(30.), RadOfDeg(30.), 0.};
struct FloatVect3 uz = { 0., 0., 1.};
struct FloatMat33 r_yaw;
FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);
struct FloatVect3 uy = { 0., 1., 0.};
struct FloatMat33 r_pitch;
FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);
struct FloatVect3 ux = { 1., 0., 0.};
struct FloatMat33 r_roll;
FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);
struct FloatMat33 r_yaw_pitch;
FLOAT_RMAT_COMP(r_yaw_pitch, r_yaw, r_pitch);
struct FloatMat33 r_yaw_pitch_roll;
FLOAT_RMAT_COMP(r_yaw_pitch_roll, r_yaw_pitch, r_roll);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", r_yaw_pitch_roll);
DISPLAY_FLOAT_RMAT("rmat", r_yaw_pitch_roll);
DISPLAY_FLOAT_EULERS_DEG("eul", eul);
struct FloatMat33 rmat1;
FLOAT_RMAT_OF_EULERS(rmat1, eul);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1);
DISPLAY_FLOAT_RMAT("rmat1", rmat1);
}
sw/logalizer/Makefile
+1 -1
View File
@@ -137,7 +137,7 @@ ahrs2fg: ahrs2fg.c network.c flight_gear.c utils.c
$(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS)
test_samere: test_samere.c network.c flight_gear.c utils.c
$(CC) $(CFLAGS) -I../airborne -g -o $@ $^ $(LDFLAGS)
$(CC) $(CFLAGS) -I../airborne -I../airborne/test -g -o $@ $^ $(LDFLAGS)
ivy_example: ivy_example.c
gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy