Implemented basic Eigen Quaternion class

jeljaik · Oct 26, 2015 · cce1e43 · cce1e43
1 parent 65321b8
commit cce1e43
Show file tree

Hide file tree

Showing 3 changed files with 144 additions and 4 deletions.
diff --git a/src/wrappers/matrix/quaternion_wrapper.h b/src/wrappers/matrix/quaternion_wrapper.h
@@ -64,5 +64,6 @@ namespace MatrixWrapper{
 
 // Include implementations
 #include "vector_BOOST.h"
+#include "vector_EIGEN.h"
 
 #endif // _QUATERNION_WRAPPER_
diff --git a/src/wrappers/matrix/vector_EIGEN.cpp b/src/wrappers/matrix/vector_EIGEN.cpp
@@ -364,16 +364,14 @@ double MyRowVector::operator* (const MyColumnVector &a) const
   return (op1 * op2)(0,0);
 }
 
-MyRowVector&
-MyRowVector::operator=(const MyRowVector &a)
+MyRowVector& MyRowVector::operator=(const MyRowVector &a)
 {
   EigenRowVector& op1 = *this;
   op1 = (EigenRowVector)a;
   return *this;
 }
 
-MyRowVector&
-MyRowVector::operator=(double a)
+MyRowVector& MyRowVector::operator=(double a)
 {
   EigenRowVector& op1 = *this;
   op1.setConstant(a);
@@ -386,4 +384,115 @@ MyRowVector MyRowVector::sub(int j_start , int j_end) const
   return MyRowVector(op1.segment(j_start-1,j_end-j_start+1));
 }
 
+///////////////////////////////////////////////////////////////////////
+////////////////////////////// QUATERNION /////////////////////////////
+///////////////////////////////////////////////////////////////////////
+
+// Constructors
+MyQuaternion::Quaternion() :  EigenQuaternion() { }
+
+MyQuaternion::Quaternion(double real, double i, double j, double z) : EigenQuaternion( (const double) real, (const double) i, (const double) j, (const double) z) { }
+
+MyQuaternion::Quaternion(MyColumnVector col) : EigenQuaternion( col(1), col(2), col(3), col(4) ) { }
+
+// Destructor
+MyQuaternion::~Quaternion() { }
+
+// Copy constructor
+MyQuaternion::Quaternion( const MyQuaternion & q ) : EigenQuaternion( q ) { }
+MyQuaternion::Quaternion( const EigenQuaternion & q ) : EigenQuaternion( q ) { }
+
+// Operators
+double MyQuaternion::operator()( unsigned int i )
+{
+    EigenQuaternion & op1 = *(this);
+    switch (i) {
+        case 1:
+            return op1.w();
+            break;
+        case 2:
+            return op1.x();
+            break;
+        case 3:
+            return op1.y();
+            break;
+        case 4:
+            return op1.z();
+            break;
+        default:
+            std::cout << "[BFLERR] Index out of boundaries." << std::endl;
+            break;
+    }
+}
+
+double MyQuaternion::operator()( unsigned int i ) const
+{
+    const EigenQuaternion & op1 = *(this);
+    switch (i) {
+        case 1:
+            return op1.w();
+            break;
+        case 2:
+            return op1.x();
+            break;
+        case 3:
+            return op1.y();
+            break;
+        case 4:
+            return op1.z();
+            break;
+        default:
+            std::cout << "[BFLERR] Index out of boundaries." << std::endl;
+            break;
+    }
+}
+
+MyQuaternion& MyQuaternion::operator =(const MyQuaternion &q)
+{
+    EigenQuaternion & op1 = *this;
+    op1 = (EigenQuaternion)q;
+    return *this;
+}
+
+
+MyQuaternion MyQuaternion::operator+ (const MyQuaternion &q) const
+{
+    const EigenQuaternion & op1 = *(this);
+    const EigenQuaternion & op2 = q;
+    Eigen::Vector4d tmp(4); tmp.setZero();
+    tmp[1] = op1.w() + op2.w();                // real part sum
+    tmp.bottomRows(3) = op1.vec() + op2.vec(); // im part
+    EigenQuaternion result = EigenQuaternion(tmp.data());
+    return (MyQuaternion) result;
+}
+
+MyQuaternion MyQuaternion::operator- (const MyQuaternion &q) const
+{
+    const EigenQuaternion & op1 = *(this);
+    const EigenQuaternion & op2 = q;
+    Eigen::Vector4d tmp(4); tmp.setZero();
+    tmp[1] = op1.w() - op2.w();                // real part sum
+    tmp.bottomRows(3) = op1.vec() - op2.vec(); // im part
+    EigenQuaternion result = EigenQuaternion(tmp.data());
+    return (MyQuaternion) result;
+}
+
+MyQuaternion MyQuaternion::normalize()
+{
+    EigenQuaternion& op1 = *(this);
+    op1.normalize();
+}
+
+bool MyQuaternion::conjugate(MyQuaternion& output)
+{
+    bool ret = false;
+    const EigenQuaternion &q = *(this);
+    q.conjugate();
+    ret = true;
+    return ret;
+}
+
+
+
+
 #endif
diff --git a/src/wrappers/matrix/vector_EIGEN.h b/src/wrappers/matrix/vector_EIGEN.h
@@ -6,11 +6,14 @@
 
 #include "matrix_wrapper.h"
 #include "vector_wrapper.h"
+#include "quaternion_wrapper.h"
 
 #include <Eigen/Core>
+#include <Eigen/Geometry>
 
 typedef Eigen::VectorXd EigenColumnVector;
 typedef Eigen::RowVectorXd EigenRowVector;
+typedef Eigen::Quaterniond EigenQuaternion;
 
 
 namespace MatrixWrapper
@@ -158,6 +161,33 @@ class RowVector : public EigenRowVector, public RowVector_Wrapper
   virtual double operator*(const MyColumnVector& a) const;
 
 };
+
+/// Wrapper class for Quaternion (Eigen implementation)
+class Quaternion : public EigenQuaternion, public Quaternion_Wrapper
+{
+public:
+    // Constructors
+    Quaternion();
+    Quaternion(double real, double i, double j, double z);
+    Quaternion(MyColumnVector);
+    Quaternion(const MyQuaternion& q);
+    // Copy constructor for boost
+    Quaternion(const EigenQuaternion& q);
+
+    // Destructor
+    virtual ~Quaternion();
+
+    /// Operators
+    double operator()(unsigned int i);
+    double operator()(unsigned int i) const;
+    virtual MyQuaternion& operator =(const MyQuaternion &q);
+    virtual MyQuaternion operator+ (const MyQuaternion &q) const;
+    virtual MyQuaternion operator- (const MyQuaternion &q) const;
+    Quaternion normalize();
+    virtual bool conjugate(MyQuaternion& output);
+};
+
+
 
 }