add an AlignedVector3 module suitable for vectorization of 3D vectors

1 files changed, 191 insertions, 0 deletions

diff --git a/unsupported/Eigen/AlignedVector3 b/unsupported/Eigen/AlignedVector3
new file mode 100644
index 000000000..b4966f500
--- /dev/null
+++ b/unsupported/Eigen/AlignedVector3
@@ -0,0 +1,191 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// Alternatively, you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of
+// the License, or (at your option) any later version.
+//
+// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_ALIGNED_VECTOR3
+#define EIGEN_ALIGNED_VECTOR3
+
+#include <Eigen/Geometry>
+
+namespace Eigen {
+
+/** \ingroup Unsupported_modules
+  * \defgroup AlignedVector3_Module Aligned vector3 module
+  *
+  * \code
+  * #include <unsupported/Eigen/AlignedVector3>
+  * \endcode
+  */
+  //@{
+
+
+/** \class AlignedVector3
+  *
+  * \brief A vectorization frinedly 3D vector
+  *
+  * This class represents a 3D vector internally using a 4D vector
+  * such that vectorization can be seamlessly enabled. Of course,
+  * the same result can be achieved by directly using a 4D vector.
+  * This class makes this process simpler.
+  *
+  */
+// TODO specialize Cwise
+template<typename _Scalar> class AlignedVector3;
+
+template<typename _Scalar> struct ei_traits<AlignedVector3<_Scalar> >
+  : ei_traits<Matrix<_Scalar,3,1,0,4,1> >
+{
+};
+
+template<typename _Scalar> class AlignedVector3
+  : public MatrixBase<AlignedVector3<_Scalar> >
+{
+    typedef Matrix<_Scalar,4,1> CoeffType;
+    CoeffType m_coeffs;
+  public:
+  
+    EIGEN_GENERIC_PUBLIC_INTERFACE(AlignedVector3)
+    using Base::operator*;
+    
+    inline int rows() const { return 3; }
+    inline int cols() const { return 1; }
+    
+    inline const Scalar coeff(int row, int col) const
+    { return m_coeffs.coeff(row, col); }
+    
+    inline Scalar& coeffRef(int row, int col)
+    { return m_coeffs.coeffRef(row, col); }
+    
+    inline const Scalar coeff(int index) const
+    { return m_coeffs.coeff(index); }
+
+    inline Scalar& coeffRef(int index)
+    { return m_coeffs.coeffRef(index);}
+  
+  
+    inline AlignedVector3(const Scalar& x, const Scalar& y, const Scalar& z)
+      : m_coeffs(x, y, z, Scalar(0))
+    {}
+    
+    inline AlignedVector3(const AlignedVector3& other)
+      : m_coeffs(other.m_coeffs)
+    {}
+    
+    template<typename XprType, int Size=XprType::SizeAtCompileTime>
+    struct generic_assign_selector;
+    
+    template<typename XprType> struct generic_assign_selector<XprType,4>
+    {
+      inline static void run(AlignedVector3& dest, const XprType& src)
+      {
+        dest.m_coeffs = src;
+      }
+    };
+    
+    template<typename XprType> struct generic_assign_selector<XprType,3>
+    {
+      inline static void run(AlignedVector3& dest, const XprType& src)
+      {
+        dest.m_coeffs.template start<3>() = src;
+        dest.m_coeffs.w() = Scalar(0);
+      }
+    };
+    
+    template<typename Derived>
+    inline explicit AlignedVector3(const MatrixBase<Derived>& other)
+    {
+      generic_assign_selector<Derived>::run(*this,other.derived());
+    }
+    
+    inline AlignedVector3& operator=(const AlignedVector3& other)
+    { m_coeffs = other.m_coeffs; return *this; }
+    
+    
+    inline AlignedVector3 operator+(const AlignedVector3& other) const
+    { return AlignedVector3(m_coeffs + other.m_coeffs); }
+    
+    inline AlignedVector3& operator+=(const AlignedVector3& other)
+    { m_coeffs += other.m_coeffs; return *this; }
+    
+    inline AlignedVector3 operator-(const AlignedVector3& other) const
+    { return AlignedVector3(m_coeffs - other.m_coeffs); }
+    
+    inline AlignedVector3 operator-=(const AlignedVector3& other)
+    { m_coeffs -= other.m_coeffs; return *this; }
+    
+    inline AlignedVector3 operator*(const Scalar& s) const
+    { return AlignedVector3(m_coeffs * s); }
+    
+    inline friend AlignedVector3 operator*(const Scalar& s,const AlignedVector3& vec)
+    { return AlignedVector3(s * vec.m_coeffs); }
+    
+    inline AlignedVector3& operator*=(const Scalar& s)
+    { m_coeffs *= s; return *this; }
+    
+    inline AlignedVector3 operator/(const Scalar& s) const
+    { return AlignedVector3(m_coeffs / s); }
+    
+    inline AlignedVector3& operator/=(const Scalar& s)
+    { m_coeffs /= s; return *this; }
+    
+    inline Scalar dot(const AlignedVector3& other) const
+    {
+      ei_assert(m_coeffs.w()==Scalar(0));
+      ei_assert(other.m_coeffs.w()==Scalar(0));
+      return m_coeffs.dot(other.m_coeffs);
+    }
+    
+    inline Scalar sum() const
+    {
+      ei_assert(m_coeffs.w()==Scalar(0));
+      return m_coeffs.sum();
+    }
+    
+    inline Scalar squaredNorm() const
+    {
+      ei_assert(m_coeffs.w()==Scalar(0));
+      return m_coeffs.squaredNorm();
+    }
+    
+    inline Scalar norm() const
+    {
+      return ei_sqrt(squaredNorm());
+    }
+    
+    inline AlignedVector3 cross(const AlignedVector3& other) const
+    {
+      return AlignedVector3(m_coeffs.cross3(other.m_coeffs));
+    }
+    
+    template<typename Derived>
+    inline bool isApprox(const MatrixBase<Derived>& other, RealScalar eps=precision<Scalar>()) const
+    {
+      return m_coeffs.template start<3>().isApprox(other,eps);
+    }
+};
+
+//@}
+
+}
+
+#endif // EIGEN_ALIGNED_VECTOR3