mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-02-23 18:20:47 +08:00
Quaternion could now map an array of 4 scalars :
new classes : * QuaternionBase * Map<Quaternion>
This commit is contained in:
Mathieu Gautier
parent
427f8a87d1
commit
611d2b0b1d
@ -129,6 +129,7 @@ template<typename Scalar> class PlanarRotation;
|
||||
// Geometry module:
|
||||
template<typename Derived, int _Dim> class RotationBase;
|
||||
template<typename Lhs, typename Rhs> class Cross;
|
||||
template<typename Derived> class QuaternionBase;
|
||||
template<typename Scalar> class Quaternion;
|
||||
template<typename Scalar> class Rotation2D;
|
||||
template<typename Scalar> class AngleAxis;
|
||||
|
||||
@ -507,4 +507,549 @@ struct ei_quaternion_assign_impl<Other,4,1>
|
||||
}
|
||||
};
|
||||
|
||||
/*###################################################################
|
||||
QuaternionBase and Map<Quaternion> and Quat
|
||||
###################################################################*/
|
||||
|
||||
template<typename Other,
|
||||
int OtherRows=Other::RowsAtCompileTime,
|
||||
int OtherCols=Other::ColsAtCompileTime>
|
||||
struct ei_quaternionbase_assign_impl;
|
||||
|
||||
template<typename Scalar> class Quat; // [XXX] => remove when Quat becomes Quaternion
|
||||
|
||||
template<typename Derived>
|
||||
struct ei_traits<QuaternionBase<Derived> >
|
||||
{
|
||||
typedef typename ei_traits<Derived>::Scalar Scalar;
|
||||
enum {
|
||||
PacketAccess = ei_traits<Derived>::PacketAccess
|
||||
};
|
||||
};
|
||||
|
||||
template<class Derived>
|
||||
class QuaternionBase : public RotationBase<Derived, 3> {
|
||||
typedef RotationBase<Derived, 3> Base;
|
||||
public:
|
||||
using Base::operator*;
|
||||
|
||||
typedef typename ei_traits<QuaternionBase<Derived> >::Scalar Scalar;
|
||||
typedef typename NumTraits<Scalar>::Real RealScalar;
|
||||
|
||||
// typedef typename Matrix<Scalar,4,1> Coefficients;
|
||||
/** the type of a 3D vector */
|
||||
typedef Matrix<Scalar,3,1> Vector3;
|
||||
/** the equivalent rotation matrix type */
|
||||
typedef Matrix<Scalar,3,3> Matrix3;
|
||||
/** the equivalent angle-axis type */
|
||||
typedef AngleAxis<Scalar> AngleAxisType;
|
||||
|
||||
/** \returns the \c x coefficient */
|
||||
inline Scalar x() const { return this->derived().coeffs().coeff(0); }
|
||||
/** \returns the \c y coefficient */
|
||||
inline Scalar y() const { return this->derived().coeffs().coeff(1); }
|
||||
/** \returns the \c z coefficient */
|
||||
inline Scalar z() const { return this->derived().coeffs().coeff(2); }
|
||||
/** \returns the \c w coefficient */
|
||||
inline Scalar w() const { return this->derived().coeffs().coeff(3); }
|
||||
|
||||
/** \returns a reference to the \c x coefficient */
|
||||
inline Scalar& x() { return this->derived().coeffs().coeffRef(0); }
|
||||
/** \returns a reference to the \c y coefficient */
|
||||
inline Scalar& y() { return this->derived().coeffs().coeffRef(1); }
|
||||
/** \returns a reference to the \c z coefficient */
|
||||
inline Scalar& z() { return this->derived().coeffs().coeffRef(2); }
|
||||
/** \returns a reference to the \c w coefficient */
|
||||
inline Scalar& w() { return this->derived().coeffs().coeffRef(3); }
|
||||
|
||||
/** \returns a read-only vector expression of the imaginary part (x,y,z) */
|
||||
inline const VectorBlock<typename ei_traits<Derived>::Coefficients,3,1> vec() const { return this->derived().coeffs().template start<3>(); }
|
||||
|
||||
/** \returns a vector expression of the imaginary part (x,y,z) */
|
||||
inline VectorBlock<typename ei_traits<Derived>::Coefficients,3,1> vec() { return this->derived().coeffs().template start<3>(); }
|
||||
|
||||
/** \returns a read-only vector expression of the coefficients (x,y,z,w) */
|
||||
inline const typename ei_traits<Derived>::Coefficients& coeffs() const { return this->derived().coeffs(); }
|
||||
|
||||
/** \returns a vector expression of the coefficients (x,y,z,w) */
|
||||
inline typename ei_traits<Derived>::Coefficients& coeffs() { return this->derived().coeffs(); }
|
||||
|
||||
template<class OtherDerived> QuaternionBase& operator=(const QuaternionBase<OtherDerived>& other);
|
||||
QuaternionBase& operator=(const AngleAxisType& aa);
|
||||
template<class OtherDerived>
|
||||
QuaternionBase& operator=(const MatrixBase<OtherDerived>& m);
|
||||
|
||||
/** \returns a quaternion representing an identity rotation
|
||||
* \sa MatrixBase::Identity()
|
||||
*/
|
||||
inline static Quat<Scalar> Identity() { return Quat<Scalar>(1, 0, 0, 0); }
|
||||
|
||||
/** \sa Quaternion2::Identity(), MatrixBase::setIdentity()
|
||||
*/
|
||||
inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; }
|
||||
|
||||
/** \returns the squared norm of the quaternion's coefficients
|
||||
* \sa Quaternion2::norm(), MatrixBase::squaredNorm()
|
||||
*/
|
||||
inline Scalar squaredNorm() const { return coeffs().squaredNorm(); }
|
||||
|
||||
/** \returns the norm of the quaternion's coefficients
|
||||
* \sa Quaternion2::squaredNorm(), MatrixBase::norm()
|
||||
*/
|
||||
inline Scalar norm() const { return coeffs().norm(); }
|
||||
|
||||
/** Normalizes the quaternion \c *this
|
||||
* \sa normalized(), MatrixBase::normalize() */
|
||||
inline void normalize() { coeffs().normalize(); }
|
||||
/** \returns a normalized version of \c *this
|
||||
* \sa normalize(), MatrixBase::normalized() */
|
||||
inline Quat<Scalar> normalized() const { return Quat<Scalar>(coeffs().normalized()); }
|
||||
|
||||
/** \returns the dot product of \c *this and \a other
|
||||
* Geometrically speaking, the dot product of two unit quaternions
|
||||
* corresponds to the cosine of half the angle between the two rotations.
|
||||
* \sa angularDistance()
|
||||
*/
|
||||
template<class OtherDerived> inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); }
|
||||
|
||||
template<class OtherDerived> inline Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const;
|
||||
|
||||
Matrix3 toRotationMatrix(void) const;
|
||||
|
||||
template<typename Derived1, typename Derived2>
|
||||
QuaternionBase& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
|
||||
|
||||
template<class OtherDerived> inline Quat<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const;
|
||||
template<class OtherDerived> inline QuaternionBase& operator*= (const QuaternionBase<OtherDerived>& q);
|
||||
|
||||
Quat<Scalar> inverse(void) const;
|
||||
Quat<Scalar> conjugate(void) const;
|
||||
|
||||
template<class OtherDerived> Quat<Scalar> slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const;
|
||||
|
||||
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
|
||||
* determined by \a prec.
|
||||
*
|
||||
* \sa MatrixBase::isApprox() */
|
||||
bool isApprox(const QuaternionBase& other, typename RealScalar prec = precision<Scalar>()) const
|
||||
{ return coeffs().isApprox(other.coeffs(), prec); }
|
||||
|
||||
Vector3 _transformVector(Vector3 v) const;
|
||||
|
||||
};
|
||||
|
||||
/* ########### Quat -> Quaternion */
|
||||
|
||||
template<typename _Scalar>
|
||||
struct ei_traits<Quat<_Scalar> >
|
||||
{
|
||||
typedef _Scalar Scalar;
|
||||
typedef Matrix<_Scalar,4,1> Coefficients;
|
||||
enum{
|
||||
PacketAccess = Aligned
|
||||
};
|
||||
};
|
||||
|
||||
template<typename _Scalar>
|
||||
class Quat : public QuaternionBase<Quat<_Scalar> >{
|
||||
typedef QuaternionBase<Quat<_Scalar> > Base;
|
||||
public:
|
||||
using Base::operator=;
|
||||
|
||||
typedef _Scalar Scalar;
|
||||
|
||||
typedef typename ei_traits<Quat<Scalar> >::Coefficients Coefficients;
|
||||
typedef typename Base::AngleAxisType AngleAxisType;
|
||||
|
||||
/** Default constructor leaving the quaternion uninitialized. */
|
||||
inline Quat() {}
|
||||
|
||||
/** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from
|
||||
* its four coefficients \a w, \a x, \a y and \a z.
|
||||
*
|
||||
* \warning Note the order of the arguments: the real \a w coefficient first,
|
||||
* while internally the coefficients are stored in the following order:
|
||||
* [\c x, \c y, \c z, \c w]
|
||||
*/
|
||||
inline Quat(Scalar w, Scalar x, Scalar y, Scalar z)
|
||||
{ coeffs() << x, y, z, w; }
|
||||
|
||||
/** Constructs and initialize a quaternion from the array data
|
||||
* This constructor is also used to map an array */
|
||||
inline Quat(const Scalar* data) : m_coeffs(data) {}
|
||||
|
||||
/** Copy constructor */
|
||||
// template<class Derived> inline Quat(const QuaternionBase<Derived>& other) { m_coeffs = other.coeffs(); } [XXX] redundant with 703
|
||||
|
||||
/** Constructs and initializes a quaternion from the angle-axis \a aa */
|
||||
explicit inline Quat(const AngleAxisType& aa) { *this = aa; }
|
||||
|
||||
/** Constructs and initializes a quaternion from either:
|
||||
* - a rotation matrix expression,
|
||||
* - a 4D vector expression representing quaternion coefficients.
|
||||
*/
|
||||
template<typename Derived>
|
||||
explicit inline Quat(const MatrixBase<Derived>& other) { *this = other; }
|
||||
|
||||
/** \returns \c *this with scalar type casted to \a NewScalarType
|
||||
*
|
||||
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
|
||||
* then this function smartly returns a const reference to \c *this.
|
||||
*/
|
||||
template<class Derived>
|
||||
inline typename ei_cast_return_type<Quat, QuaternionBase<Derived> >::type cast() const
|
||||
{ return typename ei_cast_return_type<Quat, QuaternionBase<Derived> >::type(*this); }
|
||||
|
||||
/** Copy constructor with scalar type conversion */
|
||||
template<class Derived>
|
||||
inline explicit Quat(const QuaternionBase<Derived>& other)
|
||||
{ m_coeffs = other.coeffs().template cast<Scalar>(); }
|
||||
|
||||
inline Coefficients& coeffs() { return m_coeffs;}
|
||||
inline const Coefficients& coeffs() const { return m_coeffs;}
|
||||
|
||||
protected:
|
||||
Coefficients m_coeffs;
|
||||
};
|
||||
|
||||
/* ########### Map<Quat> */
|
||||
|
||||
/** \class Map<Quat>
|
||||
* \nonstableyet
|
||||
*
|
||||
* \brief Expression of a quaternion
|
||||
*
|
||||
* \param Scalar the type of the vector of diagonal coefficients
|
||||
*
|
||||
* \sa class Quaternion, class QuaternionBase
|
||||
*/
|
||||
template<typename _Scalar, int _PacketAccess>
|
||||
struct ei_traits<Map<Quat<_Scalar>, _PacketAccess> >:
|
||||
ei_traits<Quat<_Scalar> >
|
||||
{
|
||||
typedef _Scalar Scalar;
|
||||
typedef Map<Matrix<_Scalar,4,1> > Coefficients;
|
||||
enum {
|
||||
PacketAccess = _PacketAccess
|
||||
};
|
||||
};
|
||||
|
||||
template<typename _Scalar, int PacketAccess>
|
||||
class Map<Quat<_Scalar>, PacketAccess > : public QuaternionBase<Map<Quat<_Scalar>, PacketAccess> >, ei_no_assignment_operator {
|
||||
public:
|
||||
|
||||
typedef _Scalar Scalar;
|
||||
|
||||
typedef typename ei_traits<Map<Quat<Scalar>, PacketAccess> >::Coefficients Coefficients;
|
||||
|
||||
inline Map<Quat<Scalar>, PacketAccess >(const Scalar* coeffs) : m_coeffs(coeffs) {}
|
||||
|
||||
inline Coefficients& coeffs() { return m_coeffs;}
|
||||
inline const Coefficients& coeffs() const { return m_coeffs;}
|
||||
|
||||
protected:
|
||||
Coefficients m_coeffs;
|
||||
};
|
||||
|
||||
typedef Map<Quat<double> > QuaternionMapd;
|
||||
typedef Map<Quat<float> > QuaternionMapf;
|
||||
typedef Map<Quat<double>, Aligned> QuaternionMapAlignedd;
|
||||
typedef Map<Quat<float>, Aligned> QuaternionMapAlignedf;
|
||||
|
||||
// Generic Quaternion * Quaternion product
|
||||
template<int Arch, class Derived, class OtherDerived, typename Scalar, int PacketAccess> struct ei_quat_product
|
||||
{
|
||||
inline static Quat<Scalar> run(const QuaternionBase<Derived>& a, const QuaternionBase<OtherDerived>& b){
|
||||
return Quat<Scalar>
|
||||
(
|
||||
a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
|
||||
a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
|
||||
a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
|
||||
a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
|
||||
);
|
||||
}
|
||||
};
|
||||
|
||||
/** \returns the concatenation of two rotations as a quaternion-quaternion product */
|
||||
template <class Derived>
|
||||
template <class OtherDerived>
|
||||
inline Quat<typename ei_traits<QuaternionBase<Derived> >::Scalar> QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const
|
||||
{
|
||||
EIGEN_STATIC_ASSERT((ei_is_same_type<typename Derived::Scalar, typename OtherDerived::Scalar>::ret),
|
||||
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
|
||||
return ei_quat_product<EiArch, Derived, OtherDerived,
|
||||
ei_traits<Derived>::Scalar,
|
||||
ei_traits<Derived>::PacketAccess && ei_traits<OtherDerived>::PacketAccess>::run(*this, other);
|
||||
}
|
||||
|
||||
/** \sa operator*(Quaternion) */
|
||||
template <class Derived>
|
||||
template <class OtherDerived>
|
||||
inline QuaternionBase<Derived>& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
|
||||
{
|
||||
return (*this = *this * other);
|
||||
}
|
||||
|
||||
/** Rotation of a vector by a quaternion.
|
||||
* \remarks If the quaternion is used to rotate several points (>1)
|
||||
* then it is much more efficient to first convert it to a 3x3 Matrix.
|
||||
* Comparison of the operation cost for n transformations:
|
||||
* - Quaternion2: 30n
|
||||
* - Via a Matrix3: 24 + 15n
|
||||
*/
|
||||
template <class Derived>
|
||||
inline typename QuaternionBase<Derived>::Vector3
|
||||
QuaternionBase<Derived>::_transformVector(Vector3 v) const
|
||||
{
|
||||
// Note that this algorithm comes from the optimization by hand
|
||||
// of the conversion to a Matrix followed by a Matrix/Vector product.
|
||||
// It appears to be much faster than the common algorithm found
|
||||
// in the litterature (30 versus 39 flops). It also requires two
|
||||
// Vector3 as temporaries.
|
||||
Vector3 uv = Scalar(2) * this->vec().cross(v);
|
||||
return v + this->w() * uv + this->vec().cross(uv);
|
||||
}
|
||||
|
||||
template<class Derived>
|
||||
template<class OtherDerived>
|
||||
inline QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
|
||||
{
|
||||
coeffs() = other.coeffs();
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** Set \c *this from an angle-axis \a aa and returns a reference to \c *this
|
||||
*/
|
||||
template<class Derived>
|
||||
inline QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
|
||||
{
|
||||
Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
|
||||
this->w() = ei_cos(ha);
|
||||
this->vec() = ei_sin(ha) * aa.axis();
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** Set \c *this from the expression \a xpr:
|
||||
* - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion
|
||||
* - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix
|
||||
* and \a xpr is converted to a quaternion
|
||||
*/
|
||||
|
||||
template<class Derived>
|
||||
template<class MatrixDerived>
|
||||
inline QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr)
|
||||
{
|
||||
EIGEN_STATIC_ASSERT((ei_is_same_type<typename Derived::Scalar, typename MatrixDerived::Scalar>::ret),
|
||||
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
|
||||
ei_quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** Convert the quaternion to a 3x3 rotation matrix. The quaternion is required to
|
||||
* be normalized, otherwise the result is undefined.
|
||||
*/
|
||||
template<class Derived>
|
||||
inline typename QuaternionBase<Derived>::Matrix3
|
||||
QuaternionBase<Derived>::toRotationMatrix(void) const
|
||||
{
|
||||
// NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
|
||||
// if not inlined then the cost of the return by value is huge ~ +35%,
|
||||
// however, not inlining this function is an order of magnitude slower, so
|
||||
// it has to be inlined, and so the return by value is not an issue
|
||||
Matrix3 res;
|
||||
|
||||
const Scalar tx = 2*this->x();
|
||||
const Scalar ty = 2*this->y();
|
||||
const Scalar tz = 2*this->z();
|
||||
const Scalar twx = tx*this->w();
|
||||
const Scalar twy = ty*this->w();
|
||||
const Scalar twz = tz*this->w();
|
||||
const Scalar txx = tx*this->x();
|
||||
const Scalar txy = ty*this->x();
|
||||
const Scalar txz = tz*this->x();
|
||||
const Scalar tyy = ty*this->y();
|
||||
const Scalar tyz = tz*this->y();
|
||||
const Scalar tzz = tz*this->z();
|
||||
|
||||
res.coeffRef(0,0) = 1-(tyy+tzz);
|
||||
res.coeffRef(0,1) = txy-twz;
|
||||
res.coeffRef(0,2) = txz+twy;
|
||||
res.coeffRef(1,0) = txy+twz;
|
||||
res.coeffRef(1,1) = 1-(txx+tzz);
|
||||
res.coeffRef(1,2) = tyz-twx;
|
||||
res.coeffRef(2,0) = txz-twy;
|
||||
res.coeffRef(2,1) = tyz+twx;
|
||||
res.coeffRef(2,2) = 1-(txx+tyy);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/** Sets \c *this to be a quaternion representing a rotation between
|
||||
* the two arbitrary vectors \a a and \a b. In other words, the built
|
||||
* rotation represent a rotation sending the line of direction \a a
|
||||
* to the line of direction \a b, both lines passing through the origin.
|
||||
*
|
||||
* \returns a reference to \c *this.
|
||||
*
|
||||
* Note that the two input vectors do \b not have to be normalized, and
|
||||
* do not need to have the same norm.
|
||||
*/
|
||||
template<class Derived>
|
||||
template<typename Derived1, typename Derived2>
|
||||
inline QuaternionBase<Derived>& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
|
||||
{
|
||||
Vector3 v0 = a.normalized();
|
||||
Vector3 v1 = b.normalized();
|
||||
Scalar c = v1.dot(v0);
|
||||
|
||||
// if dot == -1, vectors are nearly opposites
|
||||
// => accuraletly compute the rotation axis by computing the
|
||||
// intersection of the two planes. This is done by solving:
|
||||
// x^T v0 = 0
|
||||
// x^T v1 = 0
|
||||
// under the constraint:
|
||||
// ||x|| = 1
|
||||
// which yields a singular value problem
|
||||
if (c < Scalar(-1)+precision<Scalar>())
|
||||
{
|
||||
c = std::max<Scalar>(c,-1);
|
||||
Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
|
||||
SVD<Matrix<Scalar,2,3> > svd(m);
|
||||
Vector3 axis = svd.matrixV().col(2);
|
||||
|
||||
Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
|
||||
this->w() = ei_sqrt(w2);
|
||||
this->vec() = axis * ei_sqrt(Scalar(1) - w2);
|
||||
return *this;
|
||||
}
|
||||
Vector3 axis = v0.cross(v1);
|
||||
Scalar s = ei_sqrt((Scalar(1)+c)*Scalar(2));
|
||||
Scalar invs = Scalar(1)/s;
|
||||
this->vec() = axis * invs;
|
||||
this->w() = s * Scalar(0.5);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** \returns the multiplicative inverse of \c *this
|
||||
* Note that in most cases, i.e., if you simply want the opposite rotation,
|
||||
* and/or the quaternion is normalized, then it is enough to use the conjugate.
|
||||
*
|
||||
* \sa Quaternion2::conjugate()
|
||||
*/
|
||||
template <class Derived>
|
||||
inline Quat<typename ei_traits<QuaternionBase<Derived> >::Scalar> QuaternionBase<Derived>::inverse() const
|
||||
{
|
||||
// FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ??
|
||||
Scalar n2 = this->squaredNorm();
|
||||
if (n2 > 0)
|
||||
return Quat<Scalar>(conjugate().coeffs() / n2);
|
||||
else
|
||||
{
|
||||
// return an invalid result to flag the error
|
||||
return Quat<Scalar>(ei_traits<Derived>::Coefficients::Zero());
|
||||
}
|
||||
}
|
||||
|
||||
/** \returns the conjugate of the \c *this which is equal to the multiplicative inverse
|
||||
* if the quaternion is normalized.
|
||||
* The conjugate of a quaternion represents the opposite rotation.
|
||||
*
|
||||
* \sa Quaternion2::inverse()
|
||||
*/
|
||||
template <class Derived>
|
||||
inline Quat<typename ei_traits<QuaternionBase<Derived> >::Scalar> QuaternionBase<Derived>::conjugate() const
|
||||
{
|
||||
return Quat<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
|
||||
}
|
||||
|
||||
/** \returns the angle (in radian) between two rotations
|
||||
* \sa dot()
|
||||
*/
|
||||
template <class Derived>
|
||||
template <class OtherDerived>
|
||||
inline typename ei_traits<QuaternionBase<Derived> >::Scalar QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const
|
||||
{
|
||||
double d = ei_abs(this->dot(other));
|
||||
if (d>=1.0)
|
||||
return 0;
|
||||
return Scalar(2) * std::acos(d);
|
||||
}
|
||||
|
||||
/** \returns the spherical linear interpolation between the two quaternions
|
||||
* \c *this and \a other at the parameter \a t
|
||||
*/
|
||||
template <class Derived>
|
||||
template <class OtherDerived>
|
||||
Quat<typename ei_traits<QuaternionBase<Derived> >::Scalar> QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const
|
||||
{
|
||||
static const Scalar one = Scalar(1) - precision<Scalar>();
|
||||
Scalar d = this->dot(other);
|
||||
Scalar absD = ei_abs(d);
|
||||
if (absD>=one)
|
||||
return Quat<Scalar>(*this);
|
||||
|
||||
// theta is the angle between the 2 quaternions
|
||||
Scalar theta = std::acos(absD);
|
||||
Scalar sinTheta = ei_sin(theta);
|
||||
|
||||
Scalar scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta;
|
||||
Scalar scale1 = ei_sin( ( t * theta) ) / sinTheta;
|
||||
if (d<0)
|
||||
scale1 = -scale1;
|
||||
|
||||
return Quat<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
|
||||
}
|
||||
|
||||
// set from a rotation matrix
|
||||
template<typename Other>
|
||||
struct ei_quaternionbase_assign_impl<Other,3,3>
|
||||
{
|
||||
typedef typename Other::Scalar Scalar;
|
||||
template<class Derived> inline static void run(QuaternionBase<Derived>& q, const Other& mat)
|
||||
{
|
||||
// This algorithm comes from "Quaternion Calculus and Fast Animation",
|
||||
// Ken Shoemake, 1987 SIGGRAPH course notes
|
||||
Scalar t = mat.trace();
|
||||
if (t > 0)
|
||||
{
|
||||
t = ei_sqrt(t + Scalar(1.0));
|
||||
q.w() = Scalar(0.5)*t;
|
||||
t = Scalar(0.5)/t;
|
||||
q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
|
||||
q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
|
||||
q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
|
||||
}
|
||||
else
|
||||
{
|
||||
int i = 0;
|
||||
if (mat.coeff(1,1) > mat.coeff(0,0))
|
||||
i = 1;
|
||||
if (mat.coeff(2,2) > mat.coeff(i,i))
|
||||
i = 2;
|
||||
int j = (i+1)%3;
|
||||
int k = (j+1)%3;
|
||||
|
||||
t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
|
||||
q.coeffs().coeffRef(i) = Scalar(0.5) * t;
|
||||
t = Scalar(0.5)/t;
|
||||
q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
|
||||
q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
|
||||
q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// set from a vector of coefficients assumed to be a quaternion
|
||||
template<typename Other>
|
||||
struct ei_quaternionbase_assign_impl<Other,4,1>
|
||||
{
|
||||
typedef typename Other::Scalar Scalar;
|
||||
template<class Derived> inline static void run(QuaternionBase<Derived>& q, const Other& vec)
|
||||
{
|
||||
q.coeffs() = vec;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#endif // EIGEN_QUATERNION_H
|
||||
|
||||
@ -45,6 +45,27 @@ ei_quaternion_product<EiArch_SSE,float>(const Quaternion<float>& _a, const Quate
|
||||
return res;
|
||||
}
|
||||
|
||||
template<class Derived, class OtherDerived> struct ei_quat_product<EiArch_SSE, Derived, OtherDerived, float, Aligned>
|
||||
{
|
||||
inline static Quat<float> run(const QuaternionBase<Derived>& _a, const QuaternionBase<OtherDerived>& _b)
|
||||
{
|
||||
const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0,0,0,0x80000000));
|
||||
Quat<float> res;
|
||||
__m128 a = _a.coeffs().packet<Aligned>(0);
|
||||
__m128 b = _b.coeffs().packet<Aligned>(0);
|
||||
__m128 flip1 = _mm_xor_ps(_mm_mul_ps(ei_vec4f_swizzle1(a,1,2,0,2),
|
||||
ei_vec4f_swizzle1(b,2,0,1,2)),mask);
|
||||
__m128 flip2 = _mm_xor_ps(_mm_mul_ps(ei_vec4f_swizzle1(a,3,3,3,1),
|
||||
ei_vec4f_swizzle1(b,0,1,2,1)),mask);
|
||||
ei_pstore(&res.x(),
|
||||
_mm_add_ps(_mm_sub_ps(_mm_mul_ps(a,ei_vec4f_swizzle1(b,3,3,3,3)),
|
||||
_mm_mul_ps(ei_vec4f_swizzle1(a,2,0,1,0),
|
||||
ei_vec4f_swizzle1(b,1,2,0,0))),
|
||||
_mm_add_ps(flip1,flip2)));
|
||||
return res;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename VectorLhs,typename VectorRhs>
|
||||
struct ei_cross3_impl<EiArch_SSE,VectorLhs,VectorRhs,float,true> {
|
||||
inline static typename ei_plain_matrix_type<VectorLhs>::type
|
||||
|
||||
Loading…
Reference in New Issue
Block a user