diff options
Diffstat (limited to 'apps/mathlib/math/Quaternion.cpp')
-rw-r--r-- | apps/mathlib/math/Quaternion.cpp | 59 |
1 files changed, 26 insertions, 33 deletions
diff --git a/apps/mathlib/math/Quaternion.cpp b/apps/mathlib/math/Quaternion.cpp index 68fe85300..4f07840c3 100644 --- a/apps/mathlib/math/Quaternion.cpp +++ b/apps/mathlib/math/Quaternion.cpp @@ -79,32 +79,34 @@ Quaternion::Quaternion(const Vector &v) : Quaternion::Quaternion(const Dcm &dcm) : Vector(4) { - setA(0.5f * sqrtf(1 + dcm(0, 0) + - dcm(1, 1) + dcm(2, 2))); - setB((dcm(2, 1) - dcm(1, 2)) / - (4 * getA())); - setC((dcm(0, 2) - dcm(2, 0)) / - (4 * getA())); - setD((dcm(1, 0) - dcm(0, 1)) / - (4 * getA())); + // avoiding singularities by not using + // division equations + setA(0.5 * sqrt(1.0 + + double( dcm(0, 0) + dcm(1, 1) + dcm(2, 2)))); + setB(0.5 * sqrt(1.0 + + double( dcm(0, 0) - dcm(1, 1) - dcm(2, 2)))); + setC(0.5 * sqrt(1.0 + + double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2)))); + setD(0.5 * sqrt(1.0 + + double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2)))); } Quaternion::Quaternion(const EulerAngles &euler) : Vector(4) { - float cosPhi_2 = cosf(euler.getPhi() / 2.0f); - float cosTheta_2 = cosf(euler.getTheta() / 2.0f); - float cosPsi_2 = cosf(euler.getPsi() / 2.0f); - float sinPhi_2 = sinf(euler.getPhi() / 2.0f); - float sinTheta_2 = sinf(euler.getTheta() / 2.0f); - float sinPsi_2 = sinf(euler.getPsi() / 2.0f); + double cosPhi_2 = cos(double(euler.getPhi()) / 2.0); + double sinPhi_2 = sin(double(euler.getPhi()) / 2.0); + double cosTheta_2 = cos(double(euler.getTheta()) / 2.0); + double sinTheta_2 = sin(double(euler.getTheta()) / 2.0); + double cosPsi_2 = cos(double(euler.getPsi()) / 2.0); + double sinPsi_2 = sin(double(euler.getPsi()) / 2.0); setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2); setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2); setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2); - setD(cosPhi_2 * cosTheta_2 * sinPsi_2 + + setD(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2); } @@ -147,33 +149,24 @@ int __EXPORT quaternionTest() ASSERT(equal(q.getC(), 0)); ASSERT(equal(q.getD(), 0)); // test float ctor - q = Quaternion(0, 1, 0, 0); - ASSERT(equal(q.getA(), 0)); - ASSERT(equal(q.getB(), 1)); - ASSERT(equal(q.getC(), 0)); - ASSERT(equal(q.getD(), 0)); + q = Quaternion(0.1825742, 0.3651484, 0.5477226, 0.7302967); + ASSERT(equal(q.getA(), 0.1825742)); + ASSERT(equal(q.getB(), 0.3651484)); + ASSERT(equal(q.getC(), 0.5477226)); + ASSERT(equal(q.getD(), 0.7302967)); // test euler ctor - q = Quaternion(EulerAngles(0, 0, 0)); - ASSERT(equal(q.getA(), 1)); - ASSERT(equal(q.getB(), 0)); - ASSERT(equal(q.getC(), 0)); - ASSERT(equal(q.getD(), 0)); + q = Quaternion(EulerAngles(0.1, 0.2, 0.3)); + ASSERT(vectorEqual(q, Quaternion(0.983347, 0.034271, 0.106021, 0.143572))); // test dcm ctor q = Quaternion(Dcm()); - ASSERT(equal(q.getA(), 1)); - ASSERT(equal(q.getB(), 0)); - ASSERT(equal(q.getC(), 0)); - ASSERT(equal(q.getD(), 0)); + ASSERT(vectorEqual(q, Quaternion(1, 0, 0, 0))); // TODO test derivative // test accessors q.setA(0.1); q.setB(0.2); q.setC(0.3); q.setD(0.4); - ASSERT(equal(q.getA(), 0.1)); - ASSERT(equal(q.getB(), 0.2)); - ASSERT(equal(q.getC(), 0.3)); - ASSERT(equal(q.getD(), 0.4)); + ASSERT(vectorEqual(q, Quaternion(0.1, 0.2, 0.3, 0.4))); printf("PASS\n"); return 0; } |