Added math library.

apps/systemlib/math/Quaternion.cpp

@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Quaternion.cpp
+ *
+ * math vector
+ */
+
+#include <systemlib/test/test.hpp>
+
+
+#include "Quaternion.hpp"
+#include "Dcm.hpp"
+#include "EulerAngles.hpp"
+
+namespace math
+{
+
+Quaternion::Quaternion() :
+    Vector(4)
+{
+    setA(1.0f);
+    setB(0.0f);
+    setC(0.0f);
+    setD(0.0f);
+}
+
+Quaternion::Quaternion(float a, float b,
+        float c, float d) :
+    Vector(4)
+{
+    setA(a);
+    setB(b);
+    setC(c);
+    setD(d);
+}
+
+Quaternion::Quaternion(const float * data) :
+    Vector(4,data)
+{
+}
+
+Quaternion::Quaternion(const Vector & v) :
+    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()));
+}
+
+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);
+}
+
+Quaternion::Quaternion(const Quaternion & right) :
+    Vector(right)
+{
+}
+
+Quaternion::~Quaternion()
+{
+}
+
+Vector Quaternion::derivative(const Vector & w)
+{
+#ifdef QUATERNION_ASSERT
+    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; 
+}
+
+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;
+}
+
+} // namespace math