inline int orient2d(const VecT1 & a, const VecT2 & b, const VecT3 & c) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT1,2)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT2,2)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT3,2)
PREC f_A = GeometryPredicates::orient2d(const_cast<double*>(a.data()),
const_cast<double*>(b.data()),
const_cast<double*>(c.data()));
return ( ( f_A < 0.0 )? -1 : ( (f_A > 0.0)? 1 : 0) );
}
void setRotFromQuaternion(const Eigen::MatrixBase<Derived>& quat, Eigen::MatrixBase<DerivedOther>& A)
{
typedef typename Derived::Scalar PREC;
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 4);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedOther, 3, 3);
// ASSERTMSG(quat.rows() == 4 && quat.cols()==1 && A.rows() == 3 && A.cols() == 3, "IN: "<<
// quat.rows()<<","<<quat.cols()<<"; OUT: "<<A.rows()<<","<<A.cols() );
// No check if quaternion is unit...(performance)
PREC fTx = 2.0 * quat(1);
PREC fTy = 2.0 * quat(2);
PREC fTz = 2.0 * quat(3);
PREC fTwx = fTx * quat(0);
PREC fTwy = fTy * quat(0);
PREC fTwz = fTz * quat(0);
PREC fTxx = fTx * quat(1);
PREC fTxy = fTy * quat(1);
PREC fTxz = fTz * quat(1);
PREC fTyy = fTy * quat(2);
PREC fTyz = fTz * quat(2);
PREC fTzz = fTz * quat(3);
A(0, 0) = 1.0f - (fTyy + fTzz);
A(0, 1) = fTxy - fTwz;
A(0, 2) = fTxz + fTwy;
A(1, 0) = fTxy + fTwz;
A(1, 1) = 1.0f - (fTxx + fTzz);
A(1, 2) = fTyz - fTwx;
A(2, 0) = fTxz - fTwy;
A(2, 1) = fTyz + fTwx;
A(2, 2) = 1.0f - (fTxx + fTyy);
}
/// \brief Exp: se3 -> SE3.
///
/// Return the integral of the input spatial velocity during time 1.
template <typename D> SE3Tpl<typename D::Scalar, D::Options>
exp6(const Eigen::MatrixBase<D> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,6);
MotionTpl<typename D::Scalar,D::Options> nu(v);
return exp6(nu);
}
/// \brief Exp: so3 -> SO3.
///
/// Return the integral of the input angular velocity during time 1.
template <typename D> Eigen::Matrix<typename D::Scalar,3,3,D::Options>
exp3(const Eigen::MatrixBase<D> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
typename D::Scalar nv = v.norm();
return Eigen::AngleAxis<typename D::Scalar>(nv, v / nv).matrix();
}
inline bool Transformation::setCoeffs(
const Eigen::MatrixBase<Derived_coeffs> & coeffs) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived_coeffs, 7);
parameters_ = coeffs;
updateC();
return true;
}
void setQuaternion(const Eigen::MatrixBase<Derived>& nc_quat,
const Eigen::MatrixBase<DerivedOther>& nc_n,
const typename Derived::Scalar angleRadian)
{
typedef typename Derived::Scalar PREC;
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 4);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedOther, 3);
Eigen::MatrixBase<Derived>& quat = const_cast<Eigen::MatrixBase<Derived>&>(nc_quat);
Eigen::MatrixBase<DerivedOther>& n = const_cast<Eigen::MatrixBase<DerivedOther>&>(nc_n);
n.normalize();
quat(0) = cos(angleRadian / 2);
quat.template tail<3>() = n * sin(angleRadian / 2);
}
inline Eigen::Matrix<typename Eigen::internal::traits<Derived_T>::Scalar, 3, 3> crossMx(
Eigen::MatrixBase<Derived_T> const & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived_T>, 3);
assert((v.cols()==3 && v.rows()==1)||(v.rows()==3 && v.cols()==1));
return crossMx(v(0, 0), v(1, 0), v(2, 0));
}
friend Motion operator* (const ConstraintPlanar &, const Eigen::MatrixBase<D> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
Motion result (Motion::Zero ());
result.linear ().template head<2> () = v.template topRows<2> ();
result.angular ().template tail<1> () = v.template bottomRows<1> ();
return result;
}
void setQuaternionZero(Eigen::MatrixBase<Derived>& quat)
{
typedef typename Derived::Scalar PREC;
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 4);
quat(0) = 1;
quat(1) = 0;
quat(2) = 0;
quat(3) = 0;
}
inline bool Transformation::oplus(
const Eigen::MatrixBase<Derived_delta> & delta,
const Eigen::MatrixBase<Derived_jacobian> & jacobian) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived_delta, 6);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived_jacobian, 7, 6);
if (!oplus(delta)) {
return false;
}
return oplusJacobian(jacobian);
}
Eigen::Matrix<double, 3, 3> getRotFromQuaternion(const Eigen::MatrixBase<Derived>& quat)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 4);
// ASSERTMSG(quat.rows() == 4 && quat.cols()==1, "IN: "<< quat.rows()<<","<<quat.cols());
Eigen::Matrix<double, 3, 3> A;
// No check if quaternion is unit...(performance)
setRotFromQuaternion(quat, A);
return A;
}
inline void addSkew(const Eigen::MatrixBase<Vector3Like> & v,
const Eigen::MatrixBase<Matrix3Like> & M)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3);
Matrix3Like & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,M);
M_(0,1) -= v[2]; M_(0,2) += v[1];
M_(1,0) += v[2]; M_(1,2) -= v[0];
M_(2,0) -= v[1]; M_(2,1) += v[0]; ;
}
inline void skew(const Eigen::MatrixBase<Vector3> & v,
const Eigen::MatrixBase<Matrix3> & M)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
Matrix3 & M_ = const_cast<Eigen::MatrixBase<Matrix3> &>(M).derived();
typedef typename Matrix3::RealScalar Scalar;
M_(0,0) = Scalar(0); M_(0,1) = -v[2]; M_(0,2) = v[1];
M_(1,0) = v[2]; M_(1,1) = Scalar(0); M_(1,2) = -v[0];
M_(2,0) = -v[1]; M_(2,1) = v[0]; M_(2,2) = Scalar(0);
}
inline bool Transformation::oplus(
const Eigen::MatrixBase<Derived_delta> & delta) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived_delta, 6);
r_ += delta.template head<3>();
Eigen::Vector4d dq;
double halfnorm = 0.5 * delta.template tail<3>().norm();
dq.template head<3>() = sinc(halfnorm) * 0.5 * delta.template tail<3>();
dq[3] = cos(halfnorm);
q_ = (Eigen::Quaterniond(dq) * q_);
q_.normalize();
updateC();
return true;
}
inline void unSkew(const Eigen::MatrixBase<Matrix3> & M,
const Eigen::MatrixBase<Vector3> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
assert((M + M.transpose()).isMuchSmallerThan(M));
Vector3 & v_ = const_cast<Eigen::MatrixBase<Vector3> &>(v).derived();
typedef typename Vector3::RealScalar Scalar;
v_[0] = Scalar(0.5) * (M(2,1) - M(1,2));
v_[1] = Scalar(0.5) * (M(0,2) - M(2,0));
v_[2] = Scalar(0.5) * (M(1,0) - M(0,1));
}
Eigen::Matrix<typename Derived::Scalar, 4, 1> quatMult(const Eigen::MatrixBase<Derived>& quat1,
const Eigen::MatrixBase<Derived>& quat2)
{
typedef typename Derived::Scalar PREC;
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 4);
Eigen::Matrix<PREC, 4, 1> ret;
ret(0) = quat1(0) * quat2(0) - quat1(1) * quat2(1) - quat1(2) * quat2(2) - quat1(3) * quat2(3);
ret(1) = quat1(0) * quat2(1) + quat1(1) * quat2(0) + quat1(2) * quat2(3) - quat1(3) * quat2(2);
ret(2) = quat1(0) * quat2(2) + quat1(2) * quat2(0) + quat1(3) * quat2(1) - quat1(1) * quat2(3);
ret(3) = quat1(0) * quat2(3) + quat1(3) * quat2(0) + quat1(1) * quat2(2) - quat1(2) * quat2(1);
return ret;
}
inline Eigen::Matrix<typename Derived::Scalar, 3, 3> mySkeww(const Eigen::MatrixBase<Derived> & vec)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
return (Eigen::Matrix<typename Derived::Scalar, 3, 3>() << 0.0, -vec[2], vec[1], vec[2], 0.0, -vec[0], -vec[1], vec[0], 0.0).finished();
}
