mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-08-14 04:35:57 +08:00
* remove Cross product expression: MatrixBase::cross() now returns a temporary
which is even better optimized by the compiler. * Quaternion no longer inherits MatrixBase. Instead it stores the coefficients using a Matrix<> and provides only relevant methods.
This commit is contained in:
parent
6998037930
commit
eb7b7b2cfc
@ -558,7 +558,8 @@ template<typename Derived> class MatrixBase : public ArrayBase<Derived>
|
|||||||
/////////// Geometry module ///////////
|
/////////// Geometry module ///////////
|
||||||
|
|
||||||
template<typename OtherDerived>
|
template<typename OtherDerived>
|
||||||
const Cross<Derived,OtherDerived> cross(const MatrixBase<OtherDerived>& other) const;
|
typename ei_eval<Derived>::type
|
||||||
|
cross(const MatrixBase<OtherDerived>& other) const;
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -27,7 +27,7 @@
|
|||||||
#define EIGEN_REDUX_H
|
#define EIGEN_REDUX_H
|
||||||
|
|
||||||
template<typename BinaryOp, typename Derived, int Start, int Length>
|
template<typename BinaryOp, typename Derived, int Start, int Length>
|
||||||
struct ei_redux_unroller
|
struct ei_redux_impl
|
||||||
{
|
{
|
||||||
enum {
|
enum {
|
||||||
HalfLength = Length/2
|
HalfLength = Length/2
|
||||||
@ -38,13 +38,13 @@ struct ei_redux_unroller
|
|||||||
static Scalar run(const Derived &mat, const BinaryOp& func)
|
static Scalar run(const Derived &mat, const BinaryOp& func)
|
||||||
{
|
{
|
||||||
return func(
|
return func(
|
||||||
ei_redux_unroller<BinaryOp, Derived, Start, HalfLength>::run(mat, func),
|
ei_redux_impl<BinaryOp, Derived, Start, HalfLength>::run(mat, func),
|
||||||
ei_redux_unroller<BinaryOp, Derived, Start+HalfLength, Length - HalfLength>::run(mat, func));
|
ei_redux_impl<BinaryOp, Derived, Start+HalfLength, Length - HalfLength>::run(mat, func));
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
template<typename BinaryOp, typename Derived, int Start>
|
template<typename BinaryOp, typename Derived, int Start>
|
||||||
struct ei_redux_unroller<BinaryOp, Derived, Start, 1>
|
struct ei_redux_impl<BinaryOp, Derived, Start, 1>
|
||||||
{
|
{
|
||||||
enum {
|
enum {
|
||||||
col = Start / Derived::RowsAtCompileTime,
|
col = Start / Derived::RowsAtCompileTime,
|
||||||
@ -60,7 +60,7 @@ struct ei_redux_unroller<BinaryOp, Derived, Start, 1>
|
|||||||
};
|
};
|
||||||
|
|
||||||
template<typename BinaryOp, typename Derived, int Start>
|
template<typename BinaryOp, typename Derived, int Start>
|
||||||
struct ei_redux_unroller<BinaryOp, Derived, Start, Dynamic>
|
struct ei_redux_impl<BinaryOp, Derived, Start, Dynamic>
|
||||||
{
|
{
|
||||||
typedef typename ei_result_of<BinaryOp(typename Derived::Scalar)>::type Scalar;
|
typedef typename ei_result_of<BinaryOp(typename Derived::Scalar)>::type Scalar;
|
||||||
static Scalar run(const Derived& mat, const BinaryOp& func)
|
static Scalar run(const Derived& mat, const BinaryOp& func)
|
||||||
@ -91,7 +91,7 @@ MatrixBase<Derived>::redux(const BinaryOp& func) const
|
|||||||
const bool unroll = SizeAtCompileTime * CoeffReadCost
|
const bool unroll = SizeAtCompileTime * CoeffReadCost
|
||||||
+ (SizeAtCompileTime-1) * ei_functor_traits<BinaryOp>::Cost
|
+ (SizeAtCompileTime-1) * ei_functor_traits<BinaryOp>::Cost
|
||||||
<= EIGEN_UNROLLING_LIMIT;
|
<= EIGEN_UNROLLING_LIMIT;
|
||||||
return ei_redux_unroller<BinaryOp, Derived, 0,
|
return ei_redux_impl<BinaryOp, Derived, 0,
|
||||||
unroll ? int(SizeAtCompileTime) : Dynamic>
|
unroll ? int(SizeAtCompileTime) : Dynamic>
|
||||||
::run(derived(), func);
|
::run(derived(), func);
|
||||||
}
|
}
|
||||||
|
@ -26,7 +26,7 @@
|
|||||||
#define EIGEN_VISITOR_H
|
#define EIGEN_VISITOR_H
|
||||||
|
|
||||||
template<typename Visitor, typename Derived, int UnrollCount>
|
template<typename Visitor, typename Derived, int UnrollCount>
|
||||||
struct ei_visitor_unroller
|
struct ei_visitor_impl
|
||||||
{
|
{
|
||||||
enum {
|
enum {
|
||||||
col = (UnrollCount-1) / Derived::RowsAtCompileTime,
|
col = (UnrollCount-1) / Derived::RowsAtCompileTime,
|
||||||
@ -35,13 +35,13 @@ struct ei_visitor_unroller
|
|||||||
|
|
||||||
inline static void run(const Derived &mat, Visitor& visitor)
|
inline static void run(const Derived &mat, Visitor& visitor)
|
||||||
{
|
{
|
||||||
ei_visitor_unroller<Visitor, Derived, UnrollCount-1>::run(mat, visitor);
|
ei_visitor_impl<Visitor, Derived, UnrollCount-1>::run(mat, visitor);
|
||||||
visitor(mat.coeff(row, col), row, col);
|
visitor(mat.coeff(row, col), row, col);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
template<typename Visitor, typename Derived>
|
template<typename Visitor, typename Derived>
|
||||||
struct ei_visitor_unroller<Visitor, Derived, 1>
|
struct ei_visitor_impl<Visitor, Derived, 1>
|
||||||
{
|
{
|
||||||
inline static void run(const Derived &mat, Visitor& visitor)
|
inline static void run(const Derived &mat, Visitor& visitor)
|
||||||
{
|
{
|
||||||
@ -50,7 +50,7 @@ struct ei_visitor_unroller<Visitor, Derived, 1>
|
|||||||
};
|
};
|
||||||
|
|
||||||
template<typename Visitor, typename Derived>
|
template<typename Visitor, typename Derived>
|
||||||
struct ei_visitor_unroller<Visitor, Derived, Dynamic>
|
struct ei_visitor_impl<Visitor, Derived, Dynamic>
|
||||||
{
|
{
|
||||||
inline static void run(const Derived& mat, Visitor& visitor)
|
inline static void run(const Derived& mat, Visitor& visitor)
|
||||||
{
|
{
|
||||||
@ -85,7 +85,7 @@ void MatrixBase<Derived>::visit(Visitor& visitor) const
|
|||||||
const bool unroll = SizeAtCompileTime * CoeffReadCost
|
const bool unroll = SizeAtCompileTime * CoeffReadCost
|
||||||
+ (SizeAtCompileTime-1) * ei_functor_traits<Visitor>::Cost
|
+ (SizeAtCompileTime-1) * ei_functor_traits<Visitor>::Cost
|
||||||
<= EIGEN_UNROLLING_LIMIT;
|
<= EIGEN_UNROLLING_LIMIT;
|
||||||
return ei_visitor_unroller<Visitor, Derived,
|
return ei_visitor_impl<Visitor, Derived,
|
||||||
unroll ? int(SizeAtCompileTime) : Dynamic
|
unroll ? int(SizeAtCompileTime) : Dynamic
|
||||||
>::run(derived(), visitor);
|
>::run(derived(), visitor);
|
||||||
}
|
}
|
||||||
|
@ -25,78 +25,21 @@
|
|||||||
#ifndef EIGEN_CROSS_H
|
#ifndef EIGEN_CROSS_H
|
||||||
#define EIGEN_CROSS_H
|
#define EIGEN_CROSS_H
|
||||||
|
|
||||||
/** \class Cross
|
/** \returns the cross product of \c *this and \a other */
|
||||||
*
|
|
||||||
* \brief Expression of the cross product of two vectors
|
|
||||||
*
|
|
||||||
* \param Lhs the type of the left-hand side
|
|
||||||
* \param Rhs the type of the right-hand side
|
|
||||||
*
|
|
||||||
* This class represents an expression of the cross product of two 3D vectors.
|
|
||||||
* It is the return type of MatrixBase::cross(), and most
|
|
||||||
* of the time this is the only way it is used.
|
|
||||||
*/
|
|
||||||
template<typename Lhs, typename Rhs>
|
|
||||||
struct ei_traits<Cross<Lhs, Rhs> >
|
|
||||||
{
|
|
||||||
typedef typename Lhs::Scalar Scalar;
|
|
||||||
typedef typename ei_nested<Lhs,2>::type LhsNested;
|
|
||||||
typedef typename ei_nested<Rhs,2>::type RhsNested;
|
|
||||||
typedef typename ei_unref<LhsNested>::type _LhsNested;
|
|
||||||
typedef typename ei_unref<RhsNested>::type _RhsNested;
|
|
||||||
enum {
|
|
||||||
RowsAtCompileTime = 3,
|
|
||||||
ColsAtCompileTime = 1,
|
|
||||||
MaxRowsAtCompileTime = 3,
|
|
||||||
MaxColsAtCompileTime = 1,
|
|
||||||
Flags = ((_RhsNested::Flags | _LhsNested::Flags) & HereditaryBits)
|
|
||||||
| EvalBeforeAssigningBit,
|
|
||||||
CoeffReadCost = NumTraits<Scalar>::AddCost + 2 * NumTraits<Scalar>::MulCost
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
template<typename Lhs, typename Rhs> class Cross : ei_no_assignment_operator,
|
|
||||||
public MatrixBase<Cross<Lhs, Rhs> >
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
|
|
||||||
EIGEN_GENERIC_PUBLIC_INTERFACE(Cross)
|
|
||||||
typedef typename ei_traits<Cross>::LhsNested LhsNested;
|
|
||||||
typedef typename ei_traits<Cross>::RhsNested RhsNested;
|
|
||||||
|
|
||||||
Cross(const Lhs& lhs, const Rhs& rhs)
|
|
||||||
: m_lhs(lhs), m_rhs(rhs)
|
|
||||||
{
|
|
||||||
assert(lhs.isVector());
|
|
||||||
assert(rhs.isVector());
|
|
||||||
assert(lhs.size() == 3 && rhs.size() == 3);
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
int _rows() const { return 3; }
|
|
||||||
int _cols() const { return 1; }
|
|
||||||
|
|
||||||
Scalar _coeff(int i, int) const
|
|
||||||
{
|
|
||||||
return m_lhs[(i+1)%3]*m_rhs[(i+2)%3] - m_lhs[(i+2)%3]*m_rhs[(i+1)%3];
|
|
||||||
}
|
|
||||||
|
|
||||||
protected:
|
|
||||||
const LhsNested m_lhs;
|
|
||||||
const RhsNested m_rhs;
|
|
||||||
};
|
|
||||||
|
|
||||||
/** \returns an expression of the cross product of \c *this and \a other
|
|
||||||
*
|
|
||||||
* \sa class Cross
|
|
||||||
*/
|
|
||||||
template<typename Derived>
|
template<typename Derived>
|
||||||
template<typename OtherDerived>
|
template<typename OtherDerived>
|
||||||
const Cross<Derived,OtherDerived>
|
typename ei_eval<Derived>::type
|
||||||
MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const
|
inline MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const
|
||||||
{
|
{
|
||||||
return Cross<Derived,OtherDerived>(derived(),other.derived());
|
// Note that there is no need for an expression here since the compiler
|
||||||
|
// optimize such a small temporary very well (even within a complex expression)
|
||||||
|
const typename ei_nested<Derived,2>::type lhs(derived());
|
||||||
|
const typename ei_nested<OtherDerived,2>::type rhs(other.derived());
|
||||||
|
return typename ei_eval<Derived>::type(
|
||||||
|
lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1),
|
||||||
|
lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2),
|
||||||
|
lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)
|
||||||
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // EIGEN_CROSS_H
|
#endif // EIGEN_CROSS_H
|
||||||
|
@ -41,113 +41,80 @@
|
|||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
template<typename _Scalar>
|
template<typename _Scalar>
|
||||||
struct ei_traits<Quaternion<_Scalar> >
|
class Quaternion
|
||||||
{
|
{
|
||||||
typedef _Scalar Scalar;
|
typedef Matrix<_Scalar, 4, 1> Coefficients;
|
||||||
enum {
|
Coefficients m_coeffs;
|
||||||
RowsAtCompileTime = 4,
|
|
||||||
ColsAtCompileTime = 1,
|
|
||||||
MaxRowsAtCompileTime = 4,
|
|
||||||
MaxColsAtCompileTime = 1,
|
|
||||||
Flags = ei_corrected_matrix_flags<_Scalar, 4, 0>::ret,
|
|
||||||
CoeffReadCost = NumTraits<Scalar>::ReadCost
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
template<typename _Scalar>
|
|
||||||
class Quaternion : public MatrixBase<Quaternion<_Scalar> >
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
EIGEN_GENERIC_PUBLIC_INTERFACE(Quaternion)
|
/** the scalar type of the coefficients */
|
||||||
|
typedef _Scalar Scalar;
|
||||||
private:
|
|
||||||
|
|
||||||
EIGEN_ALIGN_128 Scalar m_data[4];
|
|
||||||
|
|
||||||
inline int _rows() const { return 4; }
|
|
||||||
inline int _cols() const { return 1; }
|
|
||||||
|
|
||||||
inline const Scalar& _coeff(int i, int) const { return m_data[i]; }
|
|
||||||
|
|
||||||
inline Scalar& _coeffRef(int i, int) { return m_data[i]; }
|
|
||||||
|
|
||||||
template<int LoadMode>
|
|
||||||
inline PacketScalar _packetCoeff(int row, int) const
|
|
||||||
{
|
|
||||||
ei_internal_assert(Flags & VectorizableBit);
|
|
||||||
if (LoadMode==Eigen::Aligned)
|
|
||||||
return ei_pload(&m_data[row]);
|
|
||||||
else
|
|
||||||
return ei_ploadu(&m_data[row]);
|
|
||||||
}
|
|
||||||
|
|
||||||
template<int StoreMode>
|
|
||||||
inline void _writePacketCoeff(int row, int , const PacketScalar& x)
|
|
||||||
{
|
|
||||||
ei_internal_assert(Flags & VectorizableBit);
|
|
||||||
if (StoreMode==Eigen::Aligned)
|
|
||||||
ei_pstore(&m_data[row], x);
|
|
||||||
else
|
|
||||||
ei_pstoreu(&m_data[row], x);
|
|
||||||
}
|
|
||||||
|
|
||||||
inline int _stride(void) const { return _rows(); }
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
typedef Matrix<Scalar,3,1> Vector3;
|
typedef Matrix<Scalar,3,1> Vector3;
|
||||||
typedef Matrix<Scalar,3,3> Matrix3;
|
typedef Matrix<Scalar,3,3> Matrix3;
|
||||||
|
|
||||||
|
inline Scalar x() const { return m_coeffs.coeff(0); }
|
||||||
|
inline Scalar y() const { return m_coeffs.coeff(1); }
|
||||||
|
inline Scalar z() const { return m_coeffs.coeff(2); }
|
||||||
|
inline Scalar w() const { return m_coeffs.coeff(3); }
|
||||||
|
|
||||||
|
inline Scalar& x() { return m_coeffs.coeffRef(0); }
|
||||||
|
inline Scalar& y() { return m_coeffs.coeffRef(1); }
|
||||||
|
inline Scalar& z() { return m_coeffs.coeffRef(2); }
|
||||||
|
inline Scalar& w() { return m_coeffs.coeffRef(3); }
|
||||||
|
|
||||||
|
/** \returns a read-only vector expression of the imaginary part (x,y,z) */
|
||||||
|
inline const Block<Coefficients,3,1> vec() const { return m_coeffs.template start<3>(); }
|
||||||
|
|
||||||
|
/** \returns a vector expression of the imaginary part (x,y,z) */
|
||||||
|
inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); }
|
||||||
|
|
||||||
|
/** \returns a read-only vector expression of the coefficients */
|
||||||
|
inline const Coefficients& _coeffs() const { return m_coeffs; }
|
||||||
|
|
||||||
|
/** \returns a vector expression of the coefficients */
|
||||||
|
inline Coefficients& _coeffs() { return m_coeffs; }
|
||||||
|
|
||||||
// FIXME what is the prefered order: w x,y,z or x,y,z,w ?
|
// FIXME what is the prefered order: w x,y,z or x,y,z,w ?
|
||||||
inline Quaternion(Scalar w = 1.0, Scalar x = 0.0, Scalar y = 0.0, Scalar z = 0.0)
|
inline Quaternion(Scalar w = 1.0, Scalar x = 0.0, Scalar y = 0.0, Scalar z = 0.0)
|
||||||
{
|
{
|
||||||
m_data[0] = x;
|
m_coeffs.coeffRef(0) = x;
|
||||||
m_data[1] = y;
|
m_coeffs.coeffRef(1) = y;
|
||||||
m_data[2] = z;
|
m_coeffs.coeffRef(2) = z;
|
||||||
m_data[3] = w;
|
m_coeffs.coeffRef(3) = w;
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Constructor copying the value of the expression \a other */
|
|
||||||
template<typename OtherDerived>
|
|
||||||
inline Quaternion(const Eigen::MatrixBase<OtherDerived>& other)
|
|
||||||
{
|
|
||||||
*this = other;
|
|
||||||
}
|
|
||||||
/** Copy constructor */
|
/** Copy constructor */
|
||||||
inline Quaternion(const Quaternion& other)
|
inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; }
|
||||||
: Base()
|
|
||||||
{
|
|
||||||
*this = other;
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Copies the value of the expression \a other into \c *this.
|
|
||||||
*/
|
|
||||||
template<typename OtherDerived>
|
|
||||||
inline Quaternion& operator=(const MatrixBase<OtherDerived>& other)
|
|
||||||
{
|
|
||||||
return Base::operator=(other.derived());
|
|
||||||
}
|
|
||||||
|
|
||||||
/** This is a special case of the templated operator=. Its purpose is to
|
/** This is a special case of the templated operator=. Its purpose is to
|
||||||
* prevent a default operator= from hiding the templated operator=.
|
* prevent a default operator= from hiding the templated operator=.
|
||||||
*/
|
*/
|
||||||
inline Quaternion& operator=(const Quaternion& other)
|
inline Quaternion& operator=(const Quaternion& other)
|
||||||
{
|
{
|
||||||
return operator=<Quaternion>(other);
|
m_coeffs = other.m_coeffs;
|
||||||
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
/** \overload
|
/** \returns a quaternion representing an identity rotation
|
||||||
* \returns a quaternion representing an identity rotation
|
* \sa MatrixBase::identity()
|
||||||
*/
|
*/
|
||||||
static Quaternion identity() { return Quaternion(1, 0, 0, 0); }
|
inline static Quaternion identity() { return Quaternion(1, 0, 0, 0); }
|
||||||
|
|
||||||
/** \overload
|
/** \sa Quaternion::identity(), MatrixBase::setIdentity()
|
||||||
* \sa Quaternion::identity()
|
|
||||||
*/
|
*/
|
||||||
Quaternion& setIdentity() { *this << 1, 0, 0, 0; return *this; }
|
inline Quaternion& setIdentity() { m_coeffs << 1, 0, 0, 0; return *this; }
|
||||||
|
|
||||||
|
/** \returns the squared norm of the quaternion's coefficients
|
||||||
|
* \sa Quaternion::norm(), MatrixBase::norm2()
|
||||||
|
*/
|
||||||
|
inline Scalar norm2() const { return m_coeffs.norm2(); }
|
||||||
|
|
||||||
|
/** \returns the norm of the quaternion's coefficients
|
||||||
|
* \sa Quaternion::norm2(), MatrixBase::norm()
|
||||||
|
*/
|
||||||
|
inline Scalar norm() const { return m_coeffs.norm(); }
|
||||||
|
|
||||||
template<typename Derived>
|
template<typename Derived>
|
||||||
Quaternion& fromRotationMatrix(const MatrixBase<Derived>& m);
|
Quaternion& fromRotationMatrix(const MatrixBase<Derived>& m);
|
||||||
@ -175,8 +142,23 @@ public:
|
|||||||
template<typename Derived>
|
template<typename Derived>
|
||||||
Vector3 operator* (const MatrixBase<Derived>& vec) const;
|
Vector3 operator* (const MatrixBase<Derived>& vec) const;
|
||||||
|
|
||||||
private:
|
protected:
|
||||||
// TODO discard here unreliable members.
|
|
||||||
|
/** Constructor copying the value of the expression \a other */
|
||||||
|
template<typename OtherDerived>
|
||||||
|
inline Quaternion(const Eigen::MatrixBase<OtherDerived>& other)
|
||||||
|
{
|
||||||
|
m_coeffs = other;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Copies the value of the expression \a other into \c *this.
|
||||||
|
*/
|
||||||
|
template<typename OtherDerived>
|
||||||
|
inline Quaternion& operator=(const MatrixBase<OtherDerived>& other)
|
||||||
|
{
|
||||||
|
m_coeffs = other.derived();
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
@ -217,8 +199,8 @@ Quaternion<Scalar>::operator* (const MatrixBase<Derived>& v) const
|
|||||||
// in the litterature (30 versus 39 flops). It also requires two
|
// in the litterature (30 versus 39 flops). It also requires two
|
||||||
// Vector3 as temporaries.
|
// Vector3 as temporaries.
|
||||||
Vector3 uv;
|
Vector3 uv;
|
||||||
uv = 2 * this->template start<3>().cross(v);
|
uv = 2 * this->vec().cross(v);
|
||||||
return v + this->w() * uv + this->template start<3>().cross(uv);
|
return v + this->w() * uv + this->vec().cross(uv);
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Convert the quaternion to a 3x3 rotation matrix */
|
/** Convert the quaternion to a 3x3 rotation matrix */
|
||||||
@ -264,38 +246,39 @@ Quaternion<Scalar>::toRotationMatrix(void) const
|
|||||||
*/
|
*/
|
||||||
template<typename Scalar>
|
template<typename Scalar>
|
||||||
template<typename Derived>
|
template<typename Derived>
|
||||||
Quaternion<Scalar>& Quaternion<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& m)
|
Quaternion<Scalar>& Quaternion<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
|
||||||
{
|
{
|
||||||
// FIXME maybe this function could accept 4x4 and 3x4 matrices as well ? (simply update the assert)
|
// FIXME maybe this function could accept 4x4 and 3x4 matrices as well ? (simply update the assert)
|
||||||
|
// FIXME this function could also be static and returns a temporary ?
|
||||||
ei_assert(Derived::RowsAtCompileTime==3 && Derived::ColsAtCompileTime==3);
|
ei_assert(Derived::RowsAtCompileTime==3 && Derived::ColsAtCompileTime==3);
|
||||||
// This algorithm comes from "Quaternion Calculus and Fast Animation",
|
// This algorithm comes from "Quaternion Calculus and Fast Animation",
|
||||||
// Ken Shoemake, 1987 SIGGRAPH course notes
|
// Ken Shoemake, 1987 SIGGRAPH course notes
|
||||||
Scalar t = m.trace();
|
Scalar t = mat.trace();
|
||||||
if (t > 0)
|
if (t > 0)
|
||||||
{
|
{
|
||||||
t = ei_sqrt(t + 1.0);
|
t = ei_sqrt(t + 1.0);
|
||||||
this->w() = 0.5*t;
|
this->w() = 0.5*t;
|
||||||
t = 0.5/t;
|
t = 0.5/t;
|
||||||
this->x() = (m.coeff(2,1) - m.coeff(1,2)) * t;
|
this->x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
|
||||||
this->y() = (m.coeff(0,2) - m.coeff(2,0)) * t;
|
this->y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
|
||||||
this->z() = (m.coeff(1,0) - m.coeff(0,1)) * t;
|
this->z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
int i = 0;
|
int i = 0;
|
||||||
if (m(1,1) > m(0,0))
|
if (mat.coeff(1,1) > mat.coeff(0,0))
|
||||||
i = 1;
|
i = 1;
|
||||||
if (m(2,2) > m(i,i))
|
if (mat.coeff(2,2) > mat.coeff(i,i))
|
||||||
i = 2;
|
i = 2;
|
||||||
int j = (i+1)%3;
|
int j = (i+1)%3;
|
||||||
int k = (j+1)%3;
|
int k = (j+1)%3;
|
||||||
|
|
||||||
t = ei_sqrt(m.coeff(i,i)-m.coeff(j,j)-m.coeff(k,k) + 1.0);
|
t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + 1.0);
|
||||||
this->coeffRef(i) = 0.5 * t;
|
m_coeffs.coeffRef(i) = 0.5 * t;
|
||||||
t = 0.5/t;
|
t = 0.5/t;
|
||||||
this->w() = (m.coeff(k,j)-m.coeff(j,k))*t;
|
this->w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
|
||||||
this->coeffRef(j) = (m.coeff(j,i)+m.coeff(i,j))*t;
|
m_coeffs.coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
|
||||||
this->coeffRef(k) = (m.coeff(k,i)+m.coeff(i,k))*t;
|
m_coeffs.coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
|
||||||
}
|
}
|
||||||
|
|
||||||
return *this;
|
return *this;
|
||||||
@ -313,7 +296,7 @@ inline Quaternion<Scalar>& Quaternion<Scalar>
|
|||||||
ei_assert(Derived::SizeAtCompileTime==3);
|
ei_assert(Derived::SizeAtCompileTime==3);
|
||||||
Scalar ha = 0.5*angle;
|
Scalar ha = 0.5*angle;
|
||||||
this->w() = ei_cos(ha);
|
this->w() = ei_cos(ha);
|
||||||
this->template start<3>() = ei_sin(ha) * axis;
|
this->vec() = ei_sin(ha) * axis;
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -327,7 +310,7 @@ void Quaternion<Scalar>::toAngleAxis(Scalar& angle, Vector3& axis) const
|
|||||||
// FIXME should we split this function to an "angle" and an "axis" functions ?
|
// FIXME should we split this function to an "angle" and an "axis" functions ?
|
||||||
// the drawbacks is that this approach would require to compute twice the norm of (x,y,z)...
|
// the drawbacks is that this approach would require to compute twice the norm of (x,y,z)...
|
||||||
// or we returns a Vector4, or a small AngleAxis object... ???
|
// or we returns a Vector4, or a small AngleAxis object... ???
|
||||||
Scalar n2 = this->template start<3>().norm2();
|
Scalar n2 = this->vec().norm2();
|
||||||
if (ei_isMuchSmallerThan(n2,Scalar(1)))
|
if (ei_isMuchSmallerThan(n2,Scalar(1)))
|
||||||
{
|
{
|
||||||
angle = 0;
|
angle = 0;
|
||||||
@ -336,7 +319,7 @@ void Quaternion<Scalar>::toAngleAxis(Scalar& angle, Vector3& axis) const
|
|||||||
else
|
else
|
||||||
{
|
{
|
||||||
angle = 2*std::acos(this->w());
|
angle = 2*std::acos(this->w());
|
||||||
axis = this->template start<3>() / ei_sqrt(n2);
|
axis = this->vec() / ei_sqrt(n2);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -394,11 +377,11 @@ inline Quaternion<Scalar>& Quaternion<Scalar>::fromTwoVectors(const MatrixBase<D
|
|||||||
if (ei_isApprox(c,Scalar(1)))
|
if (ei_isApprox(c,Scalar(1)))
|
||||||
{
|
{
|
||||||
// set to identity
|
// set to identity
|
||||||
this->w() = 1; this->template start<3>().setZero();
|
this->w() = 1; this->vec().setZero();
|
||||||
}
|
}
|
||||||
Scalar s = ei_sqrt((1+c)*2);
|
Scalar s = ei_sqrt((1+c)*2);
|
||||||
Scalar invs = 1./s;
|
Scalar invs = 1./s;
|
||||||
this->template start<3>() = axis * invs;
|
this->vec() = axis * invs;
|
||||||
this->w() = s * 0.5;
|
this->w() = s * 0.5;
|
||||||
|
|
||||||
return *this;
|
return *this;
|
||||||
@ -416,11 +399,11 @@ inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const
|
|||||||
// FIXME should this funtion be called multiplicativeInverse and conjugate() be called inverse() or opposite() ??
|
// FIXME should this funtion be called multiplicativeInverse and conjugate() be called inverse() or opposite() ??
|
||||||
Scalar n2 = this->norm2();
|
Scalar n2 = this->norm2();
|
||||||
if (n2 > 0)
|
if (n2 > 0)
|
||||||
return conjugate() / n2;
|
return conjugate()._coeffs() / n2;
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
// return an invalid result to flag the error
|
// return an invalid result to flag the error
|
||||||
return this->zero();
|
return Coefficients::zero();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
x
Reference in New Issue
Block a user