Improve Eigen integration

include/hpp/fcl/eigen/math_details.h

@@ -303,6 +303,133 @@ static inline bool equal(const eigen_wrapper_v4<T>& x, const eigen_wrapper_v4<T>
   return ((x.v - y.v).cwiseAbs ().array () < epsilon).all();
 }
 
+template <typename T>
+struct eigen_v3 :
+  Eigen::Matrix <T, 3, 1>
+{
+  typedef T meta_type;
+  typedef Eigen::Matrix <T, 3, 1> Base;
+
+  eigen_v3(void): Base () { this->setZero (); }
+
+  // This constructor allows you to construct MyVectorType from Eigen expressions
+  template<typename OtherDerived>
+    eigen_v3(const Eigen::MatrixBase<OtherDerived>& other)
+    : Base(other)
+  {}
+
+  // This method allows you to assign Eigen expressions to MyVectorType
+  template<typename OtherDerived>
+    eigen_v3& operator= (const Eigen::MatrixBase <OtherDerived>& other)
+    {
+      this->Base::operator=(other);
+      return *this;
+    }
+
+  eigen_v3(T x) :
+    Base (x, x, x)
+  {}
+
+  eigen_v3(T* x) :
+    Base (*x, *x, *x)
+  {}
+
+  eigen_v3(T x, T y, T z) :
+    Base (x, y, z)
+  {}
+
+  inline void setValue(T x, T y, T z)
+  {
+    this->operator[] (0) = x;
+    this->operator[] (1) = y;
+    this->operator[] (2) = z;
+  }
+
+  inline void setValue(T x)
+  {
+    this->setConstant (x);
+  }
+
+  inline void negate()
+  {
+    this->operator*= (-1);
+  }
+
+  template<typename OtherDerived>
+  inline eigen_v3<T>& ubound(const Eigen::MatrixBase<OtherDerived>& u)
+  {
+    *this = this->cwiseMin (u);
+    return *this;
+  }
+
+  template<typename OtherDerived>
+  inline eigen_v3<T>& lbound(const Eigen::MatrixBase<OtherDerived>& l)
+  {
+    *this = this->cwiseMax (l);
+    return *this;
+  }
+
+  // T operator [] (size_t i) const { return v[i]; }
+  // T& operator [] (size_t i) { return v[i]; }
+
+  // inline eigen_wrapper_v3<T> operator + (const eigen_wrapper_v3<T>& other) const { return eigen_wrapper_v3<T>(v + other.v); }
+  // inline eigen_wrapper_v3<T> operator - (const eigen_wrapper_v3<T>& other) const { return eigen_wrapper_v3<T>(v[0] - other.v[0], v[1] - other.v[1], v[2] - other.v[2]); }
+  // inline eigen_wrapper_v3<T> operator * (const eigen_wrapper_v3<T>& other) const { return eigen_wrapper_v3<T>(v[0] * other.v[0], v[1] * other.v[1], v[2] * other.v[2]); }
+  // inline eigen_v3<T> operator / (const eigen_v3<T>& other) const { return eigen_v3<T>(this->array().cwiseQuotient (other)); }
+  inline eigen_v3<T>& operator += (const eigen_v3<T>& other) { return static_cast<eigen_v3<T>&>(this->Base::operator+= (other)); }
+  inline eigen_v3<T>& operator -= (const eigen_v3<T>& other) { return static_cast<eigen_v3<T>&>(this->Base::operator-= (other)); }
+  inline eigen_v3<T>& operator *= (const eigen_v3<T>& other) { return static_cast<eigen_v3<T>&>(this->array().operator*=(other)); }
+  inline eigen_v3<T>& operator /= (const eigen_v3<T>& other) { return static_cast<eigen_v3<T>&>(this->array().operator/=(other)); }
+  // inline eigen_wrapper_v4<T>& operator /= (const eigen_wrapper_v4<T>& other) {  = d().cwiseQuotient (other.d()); return *this; }
+  // inline eigen_wrapper_v3<T>& operator *= (const eigen_wrapper_v3<T>& other) { return this->Base::operator*= (other); }
+  // inline eigen_wrapper_v3<T>& operator /= (const eigen_wrapper_v3<T>& other) { return this->Base::operator/= (other); }
+  // inline eigen_wrapper_v3<T> operator + (T t) const { return eigen_wrapper_v3<T>(v + t); }
+  // inline eigen_wrapper_v3<T> operator - (T t) const { return eigen_wrapper_v3<T>(v - t); }
+  // inline eigen_wrapper_v3<T> operator * (T t) const { return eigen_wrapper_v3<T>(v * t); }
+  // inline eigen_wrapper_v3<T> operator / (T t) const { return eigen_wrapper_v3<T>(v / t); }
+  inline eigen_v3<T>& operator += (T t) { this->array() += t; return *this; }
+  inline eigen_v3<T>& operator -= (T t) { this->array() -= t; return *this; }
+  inline eigen_v3<T>& operator *= (T t) { this->array() *= t; return *this; }
+  inline eigen_v3<T>& operator /= (T t) { this->array() /= t; return *this; }
+  // inline eigen_wrapper_v3<T> operator - () const { return eigen_wrapper_v3<T>(-v); }
+};
+
+
+template <typename T>
+static inline eigen_v3<T> cross_prod(const eigen_v3<T>& l, const eigen_v3<T>& r)
+{
+  return l.cross (r);
+}
+
+template <typename T>
+static inline T dot_prod3(const eigen_v3<T>& l, const eigen_v3<T>& r)
+{
+  return l.dot(r);
+}
+
+template <typename T>
+static inline eigen_v3<T> min(const eigen_v3<T>& x, const eigen_v3<T>& y)
+{
+  return x.cwiseMin (y);
+}
+
+template <typename T>
+static inline eigen_v3<T> max(const eigen_v3<T>& x, const eigen_v3<T>& y)
+{
+  return x.cwiseMax(y);
+}
+
+template <typename T>
+static inline eigen_v3<T> abs(const eigen_v3<T>& x)
+{
+  return x.cwiseAbs();
+}
+
+template <typename T>
+static inline bool equal(const eigen_v3<T>& x, const eigen_v3<T>& y, T epsilon)
+{
+  return ((x - y).cwiseAbs ().array () < epsilon).all();
+}
 
 template<typename T>
 struct eigen_wrapper_m3
@@ -474,6 +601,177 @@ eigen_wrapper_m3<T> inverse(const eigen_wrapper_m3<T>& m)
   return eigen_wrapper_m3<T> (m.m.inverse().eval ());
 }
 
+template<typename T>
+struct eigen_m3 :
+  Eigen::Matrix <T, 3, 3>
+{
+  typedef T meta_type;
+  typedef eigen_v3<T> vector_type;
+  typedef Eigen::Matrix <T, 3, 3> Base;
+
+  typedef typename Base::ColXpr ColXpr;
+  typedef typename Base::ConstColXpr ConstColXpr;
+  typedef typename Base::RowXpr RowXpr;
+  typedef typename Base::ConstRowXpr ConstRowXpr;
+
+  eigen_m3(void): Base () { }
+
+  // This constructor allows you to construct MyVectorType from Eigen expressions
+  template<typename OtherDerived>
+    eigen_m3(const Eigen::MatrixBase<OtherDerived>& other)
+    : Base(other)
+  {}
+
+  // This method allows you to assign Eigen expressions to MyVectorType
+  template<typename OtherDerived>
+    eigen_m3& operator= (const Eigen::MatrixBase <OtherDerived>& other)
+    {
+      this->Base::operator=(other);
+      return *this;
+    }
+
+  eigen_m3(T xx, T xy, T xz,
+           T yx, T yy, T yz,
+           T zx, T zy, T zz)
+  {
+    setValue(xx, xy, xz,
+             yx, yy, yz,
+             zx, zy, zz);
+  }
+
+  eigen_m3(const vector_type& v1, const vector_type& v2, const vector_type& v3)
+  {
+    this->row(1) = v1;
+    this->row(2) = v2;
+    this->row(3) = v3;
+  }
+
+  inline ColXpr getColumn(size_t i) { return this->col (i); }
+  inline RowXpr getRow(size_t i)    { return this->row (i); }
+  inline ConstColXpr getColumn(size_t i) const { return this->col (i); }
+  inline ConstRowXpr getRow(size_t i) const    { return this->row (i); }
+
+  inline eigen_m3<T>& operator *= (const eigen_m3<T>& other) { return static_cast<eigen_m3<T>&>(this->operator*=(other)); }
+  inline eigen_m3<T>& operator += (const eigen_m3<T>& other) { return static_cast<eigen_m3<T>&>(this->operator+=(other)); }
+  inline eigen_m3<T>& operator -= (const eigen_m3<T>& other) { return static_cast<eigen_m3<T>&>(this->operator-=(other)); }
+
+  inline eigen_m3<T>& operator += (T c) { return static_cast<eigen_m3<T>&>(this->operator+=(c)); }
+  inline eigen_m3<T>& operator -= (T c) { return static_cast<eigen_m3<T>&>(this->operator-=(c)); }
+  inline eigen_m3<T>& operator *= (T c) { return static_cast<eigen_m3<T>&>(this->operator*=(c)); }
+  inline eigen_m3<T>& operator /= (T c) { return static_cast<eigen_m3<T>&>(this->operator/=(c)); }
+
+  static const eigen_m3<T>& getIdentity()
+  {
+    static const eigen_m3<T> I(Base::Identity ());
+    return I;
+  }
+
+  eigen_m3<T>& transpose() { this->transposeInPlace (); return *this; }
+
+  eigen_m3<T>& inverse()
+  {
+    this->Base::operator=(this->inverse ().eval());
+    // m = m.inverse().eval();
+    return *this;
+  }
+
+  template <typename OtherDerived>
+    const Eigen::ProductReturnType<eigen_m3<T>, OtherDerived>::Type
+    transposeTimes(const Eigen::MatrixBase<OtherDerived>& other) const
+  {
+    return this->Base::transpose() * other;
+  }
+
+  template <typename OtherDerived>
+    const Eigen::ProductReturnType<eigen_m3<T>, OtherDerived>::Type
+    timesTranspose(const Eigen::MatrixBase<OtherDerived>& other) const
+  {
+    return this->operator* (other.Base::transpose());
+  }
+
+  template <typename OtherDerived>
+  inline T transposeDotX(const Eigen::MatrixBase<OtherDerived>& other) const
+  {
+    return this->col(0).dot(other);
+  }
+
+  template <typename OtherDerived>
+  inline T transposeDotY(const Eigen::MatrixBase<OtherDerived>& other) const
+  {
+    return this->col(1).dot(other);
+  }
+
+  template <typename OtherDerived>
+  inline T transposeDotZ(const Eigen::MatrixBase<OtherDerived>& other) const
+  {
+    return this->col(2).dot(other);
+  }
+
+  template <typename OtherDerived>
+  inline T transposeDot(size_t i, const Eigen::MatrixBase<OtherDerived>& other) const
+  {
+    return this->col(i).dot(other);
+  }
+
+  template <typename OtherDerived>
+  inline T dotX(const Eigen::MatrixBase<OtherDerived>& other) const
+  {
+    return this->row(0).dot(other);
+  }
+
+  template <typename OtherDerived>
+  inline T dotY(const Eigen::MatrixBase<OtherDerived>& other) const
+  {
+    return this->row(1).dot(other);
+  }
+
+  template <typename OtherDerived>
+  inline T dotZ(const Eigen::MatrixBase<OtherDerived>& other) const
+  {
+    return this->row(2).dot(other);
+  }
+
+  template <typename OtherDerived>
+  inline T dot(size_t i, const Eigen::MatrixBase<OtherDerived>& other) const
+  {
+    return this->row(i).dot(other);
+  }
+
+  inline void setValue(T xx, T xy, T xz,
+                       T yx, T yy, T yz,
+                       T zx, T zy, T zz)
+  {
+    *this << xx, xy, xz,
+             yx, yy, yz,
+             zx, zy, zz;
+  }
+
+  inline void setValue(T x)
+  {
+    this->setConstant (x);
+  }
+};
+
+
+template<typename T>
+eigen_m3<T> abs(const eigen_m3<T>& m)
+{
+  return eigen_m3<T>(m.cwiseAbs ());
+}
+
+template<typename T>
+eigen_m3<T> transpose(const eigen_m3<T>& m)
+{
+  return eigen_m3<T>(m.eigen_m3<T>::Base::transpose ());
+}
+
+
+template<typename T>
+eigen_m3<T> inverse(const eigen_m3<T>& m)
+{
+  return eigen_m3<T> (m.eigen_m3<T>::Base::inverse().eval ());
+}
+
 }
 
 }