From 1f7e393e386ed3268e39c168355d53d69b2133e8 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sun, 12 Oct 2014 20:41:13 -0700 Subject: [PATCH] AP_Math: refactor quaternion library --- libraries/AP_Math/quaternion.cpp | 169 ++++++++++++++++++++++++++----- libraries/AP_Math/quaternion.h | 23 ++++- 2 files changed, 165 insertions(+), 27 deletions(-) diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp index 7d9a06e33f..670466ca59 100644 --- a/libraries/AP_Math/quaternion.cpp +++ b/libraries/AP_Math/quaternion.cpp @@ -32,15 +32,15 @@ void Quaternion::rotation_matrix(Matrix3f &m) const float q1q4 = q1 * q4; float q4q4 = q4 * q4; - m.a.x = 1-2*(q3q3 + q4q4); - m.a.y = 2*(q2q3 - q1q4); - m.a.z = 2*(q2q4 + q1q3); - m.b.x = 2*(q2q3 + q1q4); - m.b.y = 1-2*(q2q2 + q4q4); - m.b.z = 2*(q3q4 - q1q2); - m.c.x = 2*(q2q4 - q1q3); - m.c.y = 2*(q3q4 + q1q2); - m.c.z = 1-2*(q2q2 + q3q3); + m.a.x = 1.0f-2.0f*(q3q3 + q4q4); + m.a.y = 2.0f*(q2q3 - q1q4); + m.a.z = 2.0f*(q2q4 + q1q3); + m.b.x = 2.0f*(q2q3 + q1q4); + m.b.y = 1.0f-2.0f*(q2q2 + q4q4); + m.b.z = 2.0f*(q3q4 - q1q2); + m.c.x = 2.0f*(q2q4 - q1q3); + m.c.y = 2.0f*(q3q4 + q1q2); + m.c.z = 1.0f-2.0f*(q2q2 + q3q3); } // return the rotation matrix equivalent for this quaternion @@ -95,12 +95,8 @@ void Quaternion::from_rotation_matrix(const Matrix3f &m) void Quaternion::earth_to_body(Vector3f &v) const { Matrix3f m; - // we reverse z before and afterwards because of the differing - // quaternion conventions from APM conventions. - v.z = -v.z; rotation_matrix(m); v = m * v; - v.z = -v.z; } // create a quaternion from Euler angles @@ -119,21 +115,98 @@ void Quaternion::from_euler(float roll, float pitch, float yaw) q4 = cr2*cp2*sy2 - sr2*sp2*cy2; } +void Quaternion::from_axis_angle(Vector3f v) { + float theta = v.length(); + if(theta == 0.0f) { + q1 = 1.0f; + q2=q3=q4=0.0f; + } + v /= theta; + from_axis_angle(v,theta); +} + +void Quaternion::from_axis_angle(Vector3f axis, float theta) { + if(theta == 0.0f) { + q1 = 1.0f; + q2=q3=q4=0.0f; + } + float st2 = sinf(theta/2.0f); + + q1 = cos(theta/2.0f); + q2 = axis.x * st2; + q3 = axis.y * st2; + q4 = axis.z * st2; +} + +void Quaternion::rotate(Vector3f v) { + Quaternion r; + r.from_axis_angle(v); + (*this) *= r; +} + +void Quaternion::to_axis_angle(Vector3f &v) { + float l = sqrt(sq(q2)+sq(q3)+sq(q4)); + v = Vector3f(q2,q3,q4); + if(l != 0) { + v /= l; + v *= wrap_PI(2.0f * atan2(l,q1)); + } +} + +void Quaternion::from_axis_angle_fast(Vector3f v) { + float theta = v.length(); + if(theta == 0.0f) { + q1 = 1.0f; + q2=q3=q4=0.0f; + } + v /= theta; + from_axis_angle_fast(v,theta); +} + +void Quaternion::from_axis_angle_fast(Vector3f axis, float theta) { + float t2 = theta/2.0f; + float sqt2 = sq(t2); + float st2 = t2-sqt2*t2/6.0f; + + q1 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f; + q2 = axis.x * st2; + q3 = axis.y * st2; + q4 = axis.z * st2; +} + +void Quaternion::rotate_fast(const Vector3f &v) { + float theta = v.length(); + if(theta == 0.0f) return; + float t2 = theta/2.0f; + float sqt2 = sq(t2); + float st2 = t2-sqt2*t2/6.0f; + st2 /= theta; + + //"rotation quaternion" + float w2 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f; + float x2 = v.x * st2; + float y2 = v.y * st2; + float z2 = v.z * st2; + + //copy our quaternion + float w1 = q1; + float x1 = q2; + float y1 = q3; + float z1 = q4; + + //do the multiply into our quaternion + q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2; + q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2; + q3 = w1*y2 - x1*z2 + y1*w2 + z1*x2; + q4 = w1*z2 + x1*y2 - y1*x2 + z1*w2; +} + // create eulers from a quaternion -void Quaternion::to_euler(float *roll, float *pitch, float *yaw) const +void Quaternion::to_euler(float &roll, float &pitch, float &yaw) const { - if (roll) { - *roll = (atan2f(2.0f*(q1*q2 + q3*q4), - 1 - 2.0f*(q2*q2 + q3*q3))); - } - if (pitch) { - // we let safe_asin() handle the singularities near 90/-90 in pitch - *pitch = safe_asin(2.0f*(q1*q3 - q4*q2)); - } - if (yaw) { - *yaw = atan2f(2.0f*(q1*q4 + q2*q3), - 1 - 2.0f*(q3*q3 + q4*q4)); - } + roll = (atan2f(2.0f*(q1*q2 + q3*q4), 1 - 2.0f*(q2*q2 + q3*q3))); + pitch = safe_asin(2.0f*(q1*q3 - q4*q2)); + yaw = atan2f(2.0f*(q1*q4 + q2*q3), 1 - 2.0f*(q3*q3 + q4*q4)); } float Quaternion::length(void) const @@ -141,6 +214,11 @@ float Quaternion::length(void) const return sqrtf(sq(q1) + sq(q2) + sq(q3) + sq(q4)); } +Quaternion Quaternion::inverse(void) const +{ + return Quaternion(q1, -q2, -q3, -q4); +} + void Quaternion::normalize(void) { float quatMag = length(); @@ -153,3 +231,42 @@ void Quaternion::normalize(void) q4 *= quatMagInv; } } + +Quaternion Quaternion::operator*(Quaternion v) { + Quaternion ret; + float &w1 = q1; + float &x1 = q2; + float &y1 = q3; + float &z1 = q4; + + float &w2 = v.q1; + float &x2 = v.q2; + float &y2 = v.q3; + float &z2 = v.q4; + + ret.q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2; + ret.q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2; + ret.q3 = w1*y2 - x1*z2 + y1*w2 + z1*x2; + ret.q4 = w1*z2 + x1*y2 - y1*x2 + z1*w2; + + return ret; +} + +Quaternion &Quaternion::operator*=(Quaternion v) { + float w1 = q1; + float x1 = q2; + float y1 = q3; + float z1 = q4; + + float &w2 = v.q1; + float &x2 = v.q2; + float &y2 = v.q3; + float &z2 = v.q4; + + q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2; + q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2; + q3 = w1*y2 - x1*z2 + y1*w2 + z1*x2; + q4 = w1*z2 + x1*y2 - y1*x2 + z1*w2; + + return *this; +} diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h index d421a3d059..9442cabf32 100644 --- a/libraries/AP_Math/quaternion.h +++ b/libraries/AP_Math/quaternion.h @@ -15,6 +15,7 @@ */ // Copyright 2012 Andrew Tridgell, all rights reserved. +// Refactored by Jonathan Challinger #ifndef QUATERNION_H #define QUATERNION_H @@ -63,12 +64,29 @@ public: // create a quaternion from Euler angles void from_euler(float roll, float pitch, float yaw); + void to_axis_angle(Vector3f &v); + + void from_axis_angle(Vector3f v); + + void from_axis_angle(Vector3f axis, float theta); + + void rotate(Vector3f v); + + void from_axis_angle_fast(Vector3f v); + + void from_axis_angle_fast(Vector3f axis, float theta); + + void rotate_fast(const Vector3f &v); + + // create eulers from a quaternion - void to_euler(float *roll, float *pitch, float *yaw) const; + void to_euler(float &roll, float &pitch, float &yaw) const; float length(void) const; void normalize(); + Quaternion inverse(void) const; + // allow a quaternion to be used as an array, 0 indexed float & operator[](uint8_t i) { float *_v = &q1; @@ -85,5 +103,8 @@ public: #endif return _v[i]; } + + Quaternion operator*(Quaternion v); + Quaternion &operator*=(Quaternion v); }; #endif // QUATERNION_H