Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: add quaternion::rotation_matrix_norm

Browse Source
This commit is contained in:
Leonard Hall 2016-06-24 16:10:35 +09:00 committed by Randy Mackay
parent a1548405c1
commit 8afd29cc85
2 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Math/quaternion.cpp
View File

@ -45,6 +45,32 @@ void Quaternion::rotation_matrix(Matrix3f &m) const
m.c.z = 1.0f-2.0f*(q2q2 + q3q3);
}
// return the rotation matrix equivalent for this quaternion after normalization
void Quaternion::rotation_matrix_norm(Matrix3f &m) const
{
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
float invs = 1.0f / (q1q1 + q2q2 + q3q3 + q4q4);
m.a.x = ( q2q2 - q3q3 - q4q4 + q1q1)*invs;
m.a.y = 2.0f*(q2q3 - q1q4)*invs;
m.a.z = 2.0f*(q2q4 + q1q3)*invs;
m.b.x = 2.0f*(q2q3 + q1q4)*invs;
m.b.y = (-q2q2 + q3q3 - q4q4 + q1q1)*invs;
m.b.z = 2.0f*(q3q4 - q1q2)*invs;
m.c.x = 2.0f*(q2q4 - q1q3)*invs;
m.c.y = 2.0f*(q3q4 + q1q2)*invs;
m.c.z = (-q2q2 - q3q3 + q4q4 + q1q1)*invs;
}
// return the rotation matrix equivalent for this quaternion
// Thanks to Martin John Baker
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm

3
libraries/AP_Math/quaternion.h
View File

@ -59,6 +59,9 @@ public:
// return the rotation matrix equivalent for this quaternion
void rotation_matrix(Matrix3f &m) const;
// return the rotation matrix equivalent for this quaternion after normalization
void rotation_matrix_norm(Matrix3f &m) const;
void from_rotation_matrix(const Matrix3f &m);
// convert a vector from earth to body frame