From c84e934909148a6e8cc857b37d231f790628a882 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Wed, 4 Nov 2015 00:37:00 -0500 Subject: [PATCH] Updated vector class. --- doc/nasa_rotation_def.pdf | 2217 +++++++++++++++++++++++++++++++++++++ matrix/Quaternion.hpp | 2 +- matrix/Vector.hpp | 114 +- test/quaternion.cpp | 34 +- test/test_math.py | 66 ++ 5 files changed, 2418 insertions(+), 15 deletions(-) create mode 100644 doc/nasa_rotation_def.pdf create mode 100644 test/test_math.py diff --git a/doc/nasa_rotation_def.pdf b/doc/nasa_rotation_def.pdf new file mode 100644 index 0000000000..c97a741d0d --- /dev/null +++ b/doc/nasa_rotation_def.pdf @@ -0,0 +1,2217 @@ + + + + + + + + + + + + + Firmware/nasa_rotation_def.pdf at f8811649cb91fc88cef7b224b29c6c5f235f1d8d · PX4/Firmware + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Skip to content + + + + + + + + + + + + + + +
+ +
+
+ + +
+
+
+ +
+ +
+ + +
    + +
  • +
    + +
    + + + + Unwatch + + + + +
    + +
    +
    +
    +
    • + +
  • + +
    + +
    + + +
    +
    + + +
    + +
    • + +
  • + + + Fork + + + + + +
    • +
+ +

+ + /Firmware + + + + + +

+ +
+
+
+ +
+
+
+ + + +
+ +
+

HTTPS clone URL

+
+ + + + +
+
+ + +
+

SSH clone URL

+
+ + + + +
+
+ + +
+

Subversion checkout URL

+
+ + + + +
+
+ + + +
You can clone with +
,
, or
. + + + +
+ + + + Download ZIP + +
+
+
+ + + + + + + +
+ +
+ + + +
+ +
+ + + + +
+ +
+ Firmware/apps/mathlib/math/nasa_rotation_def.pdf +
+
+ + +
+ + + 464c524 + + + + + +
+ + 1 + contributor + + +
+ + +
+ +
+
+
+ +
+ Raw + History +
+ + + +
+ +
+ 693 KB +
+
+ + + +
+ +
+
+ +
Sorry, something went wrong. Reload?
+
Sorry, we cannot display this file.
+
Sorry, this file is invalid so it cannot be displayed.
+ +
+
+ +
+ +
+ +Jump to Line + + +
+
+
+
+
+ + +
+ +
+ +
+ + + + + + + +
+ + + Something went wrong with that request. Please try again. +
+ + + + + + + + + + diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index d36c25e81d..ca142a4696 100644 --- a/matrix/Quaternion.hpp +++ b/matrix/Quaternion.hpp @@ -64,7 +64,7 @@ public: cosPhi_2 * sinTheta_2 * sinPsi_2; q(2) = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; - q(3) = cosPhi_2 * cosTheta_2 * sinPsi_2 + + q(3) = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; } diff --git a/matrix/Vector.hpp b/matrix/Vector.hpp index 7bd792ef1e..ff175d194d 100644 --- a/matrix/Vector.hpp +++ b/matrix/Vector.hpp @@ -12,25 +12,25 @@ namespace matrix { -template -class Vector : public Matrix +template +class Vector : public Matrix { public: virtual ~Vector() {}; - Vector() : Matrix() + Vector() : Matrix() { } inline Type operator()(size_t i) const { - const Matrix &v = *this; + const Matrix &v = *this; return v(i, 0); } inline Type &operator()(size_t i) { - Matrix &v = *this; + Matrix &v = *this; return v(i, 0); } @@ -47,6 +47,110 @@ public: Vector &a(*this); return sqrt(a.dot(a)); } + + /** + * Vector Operations + */ + + Vector operator+(const Vector &other) const + { + Vector res; + const Vector &self = *this; + + for (size_t i = 0; i < M; i++) { + res(i) = self(i) + other(i); + } + + return res; + } + + bool operator==(const Vector &other) const + { + Vector res; + const Vector &self = *this; + + for (size_t i = 0; i < M; i++) { + if (self(i) != other(i)) { + return false; + } + } + + return true; + } + + Vector operator-(const Vector &other) const + { + Vector res; + const Vector &self = *this; + + for (size_t i = 0; i < M; i++) { + res(i) = self(i) - other(i); + } + + return res; + } + + void operator+=(const Vector &other) + { + Vector &self = *this; + self = self + other; + } + + void operator-=(const Vector &other) + { + Vector &self = *this; + self = self - other; + } + + void operator*=(const Vector &other) + { + Vector &self = *this; + self = self * other; + } + + /** + * Scalar Operations + */ + + Vector operator*(Type scalar) const + { + Vector res; + const Vector &self = *this; + + for (size_t i = 0; i < M; i++) { + res(i) = self(i) * scalar; + } + + return res; + } + + Vector operator+(Type scalar) const + { + Vector res; + Vector &self = *this; + + for (size_t i = 0; i < M; i++) { + res(i) = self(i) + scalar; + } + + return res; + } + + void operator*=(Type scalar) + { + Vector &self = *this; + + for (size_t i = 0; i < M; i++) { + self(i) = self(i) * scalar; + } + } + + void operator/=(Type scalar) + { + Vector &self = *this; + self = self * (1.0f / scalar); + } + }; }; // namespace matrix diff --git a/test/quaternion.cpp b/test/quaternion.cpp index 89c0db2063..35a2f6c7fc 100644 --- a/test/quaternion.cpp +++ b/test/quaternion.cpp @@ -6,16 +6,32 @@ using namespace matrix; int main() { - Quatf p(1, 2, 3, 4); - assert(p(0) == 1); - assert(p(1) == 2); - assert(p(2) == 3); - assert(p(3) == 4); + // test default ctor + Quatf q; + assert(q(0) == 1); + assert(q(1) == 0); + assert(q(2) == 0); + assert(q(3) == 0); + + q = Quatf(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); + assert(q(0) == 0.1825742f); + assert(q(1) == 0.3651484f); + assert(q(2) == 0.5477226f); + assert(q(3) == 0.7302967f); - Quatf q(0, 1, 0, 0); - Quatf r = p*q; - Dcmf dcm = Dcmf(p); - Eulerf e = Eulerf(p); + // test euler ctor + q = Quatf(Eulerf(0.1f, 0.2f, 0.3f)); + assert((q - Quatf(0.983347f, 0.034271f, 0.106021f, 0.143572f)).norm() < 1e-3); + // test dcm ctor + //q = Quatf(Dcmf()); + //assert(q == Quatf(1.0f, 0.0f, 0.0f, 0.0f)); + // TODO test derivative + // test accessors + //q(0) = 0.1f; + //q(1) = 0.2f; + //q(2) = 0.3f; + //q(3) = 0.4f; + //assert(q == Quatf(0.1f, 0.2f, 0.3f, 0.4f)); return 0; } diff --git a/test/test_math.py b/test/test_math.py new file mode 100644 index 0000000000..d5d24da60c --- /dev/null +++ b/test/test_math.py @@ -0,0 +1,66 @@ +from __future__ import print_function +from pylab import * +from pprint import pprint + +#pylint: disable=all + +phi = 0.1 +theta = 0.2 +psi = 0.3 + +cosPhi = cos(phi) +cosPhi_2 = cos(phi/2) +sinPhi = sin(phi) +sinPhi_2 = sin(phi/2) + +cosTheta = cos(theta) +cosTheta_2 = cos(theta/2) +sinTheta = sin(theta) +sinTheta_2 = sin(theta/2) + +cosPsi = cos(psi) +cosPsi_2 = cos(psi/2) +sinPsi = sin(psi) +sinPsi_2 = sin(psi/2) + +C_nb = array([ + [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi], + [cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi], + [-sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]]) + +print('\nC_nb') +pprint(C_nb) + +theta = arcsin(-C_nb[2,0]) +phi = arctan(C_nb[2,1]/ C_nb[2,2]) +psi = arctan(C_nb[1,0]/ C_nb[0,0]) +print('\nphi {:f}, theta {:f}, psi {:f}\n'.format(phi, theta, psi)) + +q = array([ + cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2, + sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2, + cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2, + cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2]) + +a = q[0] +b = q[1] +c = q[2] +d = q[3] + +print('\nq') +pprint(q.T) + +a2 = a*a +b2 = b*b +c2 = c*c +d2 = d*d + +C2_nb = array([ + [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c)], + [2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b)], + [2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]]) + +print('\nC2_nb') +pprint(C2_nb) + +# vim: set et ft=python fenc=utf-8 ff=unix sts=4 sw=4 ts=8 :