diff options
author | jgoppert <james.goppert@gmail.com> | 2013-01-06 15:33:55 -0500 |
---|---|---|
committer | jgoppert <james.goppert@gmail.com> | 2013-01-06 15:33:55 -0500 |
commit | d9491b20cc5fc8b683eb0f60a50da6b322b55e57 (patch) | |
tree | 48d44accf2ccff88766bca351c78be06bb9fa4a0 /apps/systemlib/math/Quaternion.cpp | |
parent | 4f3b17f53b120cd54112097f4217a90863013c1f (diff) | |
download | px4-firmware-d9491b20cc5fc8b683eb0f60a50da6b322b55e57.tar.gz px4-firmware-d9491b20cc5fc8b683eb0f60a50da6b322b55e57.tar.bz2 px4-firmware-d9491b20cc5fc8b683eb0f60a50da6b322b55e57.zip |
Reformat of math library with astyle.
Diffstat (limited to 'apps/systemlib/math/Quaternion.cpp')
-rw-r--r-- | apps/systemlib/math/Quaternion.cpp | 189 |
1 files changed, 95 insertions, 94 deletions
diff --git a/apps/systemlib/math/Quaternion.cpp b/apps/systemlib/math/Quaternion.cpp index 79a913af6..12ae72c0e 100644 --- a/apps/systemlib/math/Quaternion.cpp +++ b/apps/systemlib/math/Quaternion.cpp @@ -48,68 +48,68 @@ namespace math { Quaternion::Quaternion() : - Vector(4) + Vector(4) { - setA(1.0f); - setB(0.0f); - setC(0.0f); - setD(0.0f); + setA(1.0f); + setB(0.0f); + setC(0.0f); + setD(0.0f); } Quaternion::Quaternion(float a, float b, - float c, float d) : - Vector(4) + float c, float d) : + Vector(4) { - setA(a); - setB(b); - setC(c); - setD(d); + setA(a); + setB(b); + setC(c); + setD(d); } -Quaternion::Quaternion(const float * data) : - Vector(4,data) +Quaternion::Quaternion(const float *data) : + Vector(4, data) { } -Quaternion::Quaternion(const Vector & v) : - Vector(v) +Quaternion::Quaternion(const Vector &v) : + Vector(v) { } -Quaternion::Quaternion(const Dcm & dcm) : - Vector(4) +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())); + 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())); } -Quaternion::Quaternion(const EulerAngles & euler) : - Vector(4) +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); - 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 + - sinPhi_2*sinTheta_2*cosPsi_2); + 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); + 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 + + sinPhi_2 * sinTheta_2 * cosPsi_2); } -Quaternion::Quaternion(const Quaternion & right) : - Vector(right) +Quaternion::Quaternion(const Quaternion &right) : + Vector(right) { } @@ -117,64 +117,65 @@ Quaternion::~Quaternion() { } -Vector Quaternion::derivative(const Vector & w) +Vector Quaternion::derivative(const Vector &w) { #ifdef QUATERNION_ASSERT - ASSERT(w.getRows()==3); + ASSERT(w.getRows() == 3); #endif - float dataQ[] = - {getA(), -getB(), -getC(), -getD(), - getB(), getA(), -getD(), getC(), - getC(), getD(), getA(), -getB(), - getD(), -getC(), getB(), getA()}; - Vector v(4); - v(0) = 0.0f; - v(1) = w(0); - v(2) = w(1); - v(3) = w(2); - Matrix Q(4,4,dataQ); - return Q*v*0.5f; + float dataQ[] = { + getA(), -getB(), -getC(), -getD(), + getB(), getA(), -getD(), getC(), + getC(), getD(), getA(), -getB(), + getD(), -getC(), getB(), getA() + }; + Vector v(4); + v(0) = 0.0f; + v(1) = w(0); + v(2) = w(1); + v(3) = w(2); + Matrix Q(4, 4, dataQ); + return Q * v * 0.5f; } int __EXPORT quaternionTest() { - printf("Test Quaternion\t\t: "); - // test default ctor - Quaternion q; - ASSERT(equal(q.getA(),1)); - ASSERT(equal(q.getB(),0)); - 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)); - // 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)); - // 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)); - // 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)); - printf("PASS\n"); - return 0; + printf("Test Quaternion\t\t: "); + // test default ctor + Quaternion q; + ASSERT(equal(q.getA(), 1)); + ASSERT(equal(q.getB(), 0)); + 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)); + // 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)); + // 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)); + // 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)); + printf("PASS\n"); + return 0; } } // namespace math |