Skip to content
Snippets Groups Projects
Commit e516fa72 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Improve Eigen integration

parent 0d64ce1e
No related branches found
No related tags found
No related merge requests found
......@@ -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 ());
}
}
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment