aboutsummaryrefslogtreecommitdiff
path: root/apps/systemlib/math/EulerAngles.cpp
diff options
context:
space:
mode:
authorjgoppert <james.goppert@gmail.com>2013-01-06 15:33:55 -0500
committerjgoppert <james.goppert@gmail.com>2013-01-06 15:33:55 -0500
commitd9491b20cc5fc8b683eb0f60a50da6b322b55e57 (patch)
tree48d44accf2ccff88766bca351c78be06bb9fa4a0 /apps/systemlib/math/EulerAngles.cpp
parent4f3b17f53b120cd54112097f4217a90863013c1f (diff)
downloadpx4-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/EulerAngles.cpp')
-rw-r--r--apps/systemlib/math/EulerAngles.cpp102
1 files changed, 50 insertions, 52 deletions
diff --git a/apps/systemlib/math/EulerAngles.cpp b/apps/systemlib/math/EulerAngles.cpp
index b530f172d..259c866c7 100644
--- a/apps/systemlib/math/EulerAngles.cpp
+++ b/apps/systemlib/math/EulerAngles.cpp
@@ -48,48 +48,46 @@ namespace math
{
EulerAngles::EulerAngles() :
- Vector(3)
+ Vector(3)
{
- setPhi(0.0f);
- setTheta(0.0f);
- setPsi(0.0f);
+ setPhi(0.0f);
+ setTheta(0.0f);
+ setPsi(0.0f);
}
EulerAngles::EulerAngles(float phi, float theta, float psi) :
- Vector(3)
+ Vector(3)
{
- setPhi(phi);
- setTheta(theta);
- setPsi(psi);
+ setPhi(phi);
+ setTheta(theta);
+ setPsi(psi);
}
-EulerAngles::EulerAngles(const Quaternion & q) :
- Vector(3)
+EulerAngles::EulerAngles(const Quaternion &q) :
+ Vector(3)
{
- (*this) = EulerAngles(Dcm(q));
+ (*this) = EulerAngles(Dcm(q));
}
-EulerAngles::EulerAngles(const Dcm & dcm) :
- Vector(3)
+EulerAngles::EulerAngles(const Dcm &dcm) :
+ Vector(3)
{
- setTheta(asinf(-dcm(2,0)));
- if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f)
- {
- setPhi(0.0f);
- setPsi(atan2f(dcm(1,2) - dcm(0,1),
- dcm(0,2) + dcm(1,1)) + getPhi());
- }
- else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f)
- {
- setPhi(0.0f);
- setPsi(atan2f(dcm(1,2) - dcm(0,1),
- dcm(0,2) + dcm(1,1)) - getPhi());
- }
- else
- {
- setPhi(atan2f(dcm(2,1),dcm(2,2)));
- setPsi(atan2f(dcm(1,0),dcm(0,0)));
- }
+ setTheta(asinf(-dcm(2, 0)));
+
+ if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) {
+ setPhi(0.0f);
+ setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
+ dcm(0, 2) + dcm(1, 1)) + getPhi());
+
+ } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) {
+ setPhi(0.0f);
+ setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
+ dcm(0, 2) + dcm(1, 1)) - getPhi());
+
+ } else {
+ setPhi(atan2f(dcm(2, 1), dcm(2, 2)));
+ setPsi(atan2f(dcm(1, 0), dcm(0, 0)));
+ }
}
EulerAngles::~EulerAngles()
@@ -98,27 +96,27 @@ EulerAngles::~EulerAngles()
int __EXPORT eulerAnglesTest()
{
- printf("Test EulerAngles\t: ");
- EulerAngles euler(1,2,3);
-
- // test ctor
- ASSERT(vectorEqual(Vector3(1,2,3),euler));
- ASSERT(equal(euler.getPhi(),1));
- ASSERT(equal(euler.getTheta(),2));
- ASSERT(equal(euler.getPsi(),3));
-
- // test dcm ctor
-
- // test assignment
- euler.setPhi(4);
- ASSERT(equal(euler.getPhi(),4));
- euler.setTheta(5);
- ASSERT(equal(euler.getTheta(),5));
- euler.setPsi(6);
- ASSERT(equal(euler.getPsi(),6));
-
- printf("PASS\n");
- return 0;
+ printf("Test EulerAngles\t: ");
+ EulerAngles euler(1, 2, 3);
+
+ // test ctor
+ ASSERT(vectorEqual(Vector3(1, 2, 3), euler));
+ ASSERT(equal(euler.getPhi(), 1));
+ ASSERT(equal(euler.getTheta(), 2));
+ ASSERT(equal(euler.getPsi(), 3));
+
+ // test dcm ctor
+
+ // test assignment
+ euler.setPhi(4);
+ ASSERT(equal(euler.getPhi(), 4));
+ euler.setTheta(5);
+ ASSERT(equal(euler.getTheta(), 5));
+ euler.setPsi(6);
+ ASSERT(equal(euler.getPsi(), 6));
+
+ printf("PASS\n");
+ return 0;
}
} // namespace math