From 0f41af271a9998aa1656fb43c56ab8eef80941de Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 5 Jul 2016 20:59:42 -0400 Subject: [PATCH] Added axis angle attitude representation. (#25) --- CMakeLists.txt | 4 +- matrix/AxisAngle.hpp | 158 ++++++++++++++++++++++++++++++++++++++ matrix/Dcm.hpp | 19 +++++ matrix/Quaternion.hpp | 28 +++++++ matrix/math.hpp | 1 + test/attitude.cpp | 31 ++++++++ test/matrixAssignment.cpp | 4 + 7 files changed, 243 insertions(+), 2 deletions(-) create mode 100644 matrix/AxisAngle.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 206deffa5c..d3eaab047e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8) -set(VERSION_MAJOR "0") -set(VERSION_MINOR "1") +set(VERSION_MAJOR "1") +set(VERSION_MINOR "0") set(VERSION_PATCH "0") project(matrix CXX) diff --git a/matrix/AxisAngle.hpp b/matrix/AxisAngle.hpp new file mode 100644 index 0000000000..f52e331e5b --- /dev/null +++ b/matrix/AxisAngle.hpp @@ -0,0 +1,158 @@ +/** + * @file AxisAngle.hpp + * + * @author James Goppert + */ + +#pragma once + +#include "math.hpp" +#include "helper_functions.hpp" + +namespace matrix +{ + +template +class Dcm; + +template +class Euler; + +template +class AxisAngle; + +/** + * AxisAngle class + * + * The rotation between two coordinate frames is + * described by this class. + */ +template +class AxisAngle : public Vector +{ +public: + virtual ~AxisAngle() {}; + + typedef Matrix Matrix31; + + /** + * Constructor from array + * + * @param data_ array + */ + AxisAngle(const Type *data_) : + Vector(data_) + { + } + + /** + * Standard constructor + */ + AxisAngle() : + Vector() + { + } + + /** + * Constructor from Matrix31 + * + * @param other Matrix31 to copy + */ + AxisAngle(const Matrix31 &other) : + Vector(other) + { + } + + /** + * Constructor from quaternion + * + * This sets the instance from a quaternion representing coordinate transformation from + * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described + * by a 3-2-1 intrinsic Tait-Bryan rotation sequence. + * + * @param q quaternion + */ + AxisAngle(const Quaternion &q) : + Vector() + { + AxisAngle &v = *this; + Type angle = (Type)2.0f*acosf(q(0)); + Type mag = sinf(angle/2.0f); + v(0) = angle*q(1)/mag; + v(1) = angle*q(2)/mag; + v(2) = angle*q(3)/mag; + } + + /** + * Constructor from dcm + * + * Instance is initialized from a dcm representing coordinate transformation + * from frame 2 to frame 1. + * + * @param dcm dcm to set quaternion to + */ + AxisAngle(const Dcm &dcm) : + Vector() + { + AxisAngle &v = *this; + v = Quaternion(dcm); + } + + /** + * Constructor from euler angles + * + * This sets the instance to a quaternion representing coordinate transformation from + * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described + * by a 3-2-1 intrinsic Tait-Bryan rotation sequence. + * + * @param euler euler angle instance + */ + AxisAngle(const Euler &euler) : + Vector() + { + AxisAngle &v = *this; + v = Quaternion(euler); + } + + /** + * Constructor from 3 axis angle values (unit vector * angle) + * + * @param x r_x*angle + * @param y r_y*angle + * @param z r_z*angle + */ + AxisAngle(Type x, Type y, Type z) : + Vector() + { + AxisAngle &v = *this; + v(0) = x; + v(1) = y; + v(2) = z; + } + + /** + * Constructor from axis and angle + * + * @param x r_x*angle + * @param x r_x*angle + * @param y r_y*angle + * @param z r_z*angle + */ + AxisAngle(const Matrix31 & axis, Type angle) : + Vector() + { + AxisAngle &v = *this; + // make sure axis is a unit vector + Vector a = axis; + a = a.unit(); + v(0) = a(0)*angle; + v(1) = a(1)*angle; + v(2) = a(2)*angle; + } +}; + +typedef AxisAngle AxisAnglef; + +} // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/matrix/Dcm.hpp b/matrix/Dcm.hpp index 045ebd94fb..b7b7ac6f00 100644 --- a/matrix/Dcm.hpp +++ b/matrix/Dcm.hpp @@ -27,6 +27,10 @@ class Quaternion; template class Euler; +template +class AxisAngle; + + /** * Direction cosine matrix class * @@ -129,6 +133,21 @@ public: dcm(2, 2) = cosPhi * cosThe; } + + /** + * Constructor from axis angle + * + * This sets the transformation matrix from frame 2 to frame 1 where the rotation + * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence. + * + * + * @param euler euler angle instance + */ + Dcm(const AxisAngle & aa) { + Dcm &dcm = *this; + dcm = Quaternion(aa); + } + Vector vee() const { // inverse to Vector.hat() operation const Dcm &A(*this); Vector v; diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index fdb46c558f..2cadf6e2d6 100644 --- a/matrix/Quaternion.hpp +++ b/matrix/Quaternion.hpp @@ -30,6 +30,10 @@ class Dcm; template class Euler; +template +class AxisAngle; + + /** * Quaternion class * @@ -129,6 +133,30 @@ public: sinPhi_2 * sinTheta_2 * cosPsi_2; } + /** + * Quaternion from AxisAngle + * + * @param aa axis-angle vector + */ + Quaternion(const AxisAngle &aa) : + Vector() + { + Quaternion &q = *this; + Type angle = aa.norm(); + Vector axis = aa.unit(); + if (angle < (Type)1e-10) { + q(0) = (Type)1.0; + q(1) = q(2) = q(3) = 0; + } else { + Type magnitude = sinf(angle / 2.0f); + q(0) = cosf(angle / 2.0f); + q(1) = axis(0) * magnitude; + q(2) = axis(1) * magnitude; + q(3) = axis(2) * magnitude; + } + } + + /** * Constructor from quaternion values * diff --git a/matrix/math.hpp b/matrix/math.hpp index ef5facc74d..b69d2e0eec 100644 --- a/matrix/math.hpp +++ b/matrix/math.hpp @@ -12,3 +12,4 @@ #include "Dcm.hpp" #include "Scalar.hpp" #include "Quaternion.hpp" +#include "AxisAngle.hpp" diff --git a/test/attitude.cpp b/test/attitude.cpp index 50405b6003..e0a9642e3a 100644 --- a/test/attitude.cpp +++ b/test/attitude.cpp @@ -6,9 +6,16 @@ using namespace matrix; +// important to list all classes here for coverage template class Quaternion; template class Euler; template class Dcm; +template class AxisAngle; +template class Scalar; +template class SquareMatrix; +template class Vector; +template class Vector2; +template class Vector3; int main() { @@ -238,6 +245,30 @@ int main() for(int i = 0; i < 4; i++) TEST(fabsf(q_from_array(i) - q_array[i]) < eps); + // axis angle + AxisAnglef aa_true(Vector3f(1.0f, 2.0f, 3.0f)); + TEST(isEqual(aa_true, Vector3f(1.0f, 2.0f, 3.0f))); + AxisAnglef aa_empty; + TEST(isEqual(aa_empty, AxisAnglef(0.0f, 0.0f, 0.0f))); + float aa_data[] = {4.0f, 5.0f, 6.0f}; + AxisAnglef aa_data_init(aa_data); + TEST(isEqual(aa_data_init, AxisAnglef(4.0f, 5.0f, 6.0f))); + + q = Quatf(-0.29555112749297824f, 0.25532186f, 0.51064372f, 0.76596558f); + AxisAnglef aa_q_init(q); + TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f))); + + AxisAnglef aa_euler_init(Eulerf(0.0f, 0.0f, 0.0f)); + TEST(isEqual(aa_euler_init, Vector3f(0.0f, 0.0f, 1.0f))); + + Dcmf dcm_aa_check = AxisAnglef(dcm_check); + TEST(isEqual(dcm_aa_check, dcm_check)); + + AxisAnglef aa_axis_angle_init(Vector3f(1.0f, 2.0f, 3.0f), 3.0f); + TEST(isEqual(aa_axis_angle_init, Vector3f(0.80178373f, 1.60356745f, 2.40535118f))); + + TEST(isEqual(Quatf((AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), 0.0f))), + Quatf(1.0f, 0.0f, 0.0f, 0.0f))); }; /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/test/matrixAssignment.cpp b/test/matrixAssignment.cpp index 7a3aa56b8b..d6097526f1 100644 --- a/test/matrixAssignment.cpp +++ b/test/matrixAssignment.cpp @@ -90,7 +90,11 @@ int main() Scalar s; s = 1; + float const & s_float = (const Scalar)s; + const Vector & s_vect = s; TEST(fabs(s - 1) < 1e-5); + TEST(fabs(s_float - 1.0f) < 1e-5); + TEST(fabs(s_vect(0) - 1.0f) < 1e-5); Matrix m5 = s; TEST(fabs(m5(0,0) - s) < 1e-5);