Commit 68982317 authored by jcarpent's avatar jcarpent
Browse files

[Spatial] Add new skewSquare linear operator from two 3d vectors

parent af186283
......@@ -77,6 +77,30 @@ namespace se3
m(2,0) = - m(0,2); m(2,1) = - m(1,2); m(2,2) = 0;
return m;
}
///
/// \brief Computes the square cross product linear operator C(u,v) such that for any vector w, \f$ u \times ( v \times w ) = C(u,v) w \f$.
///
/// \param[in] u A 3 dimensional vector.
/// \param[in] v A 3 dimensional vector.
///
/// \return The square cross product C matrix.
///
template <typename D1, typename D2>
inline Eigen::Matrix<typename D1::Scalar,3,3,D1::Options>
skewSquare(const Eigen::MatrixBase<D1> & u, const Eigen::MatrixBase<D2> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D1,3);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D2,3);
typedef Eigen::DiagonalMatrix<typename D1::Scalar,3> DiagonalMatrix;
const typename D1::Scalar udotv(u.dot(v));
Eigen::Matrix<typename D1::Scalar,3,3,D1::Options> res(v*u.transpose());
res -= DiagonalMatrix(udotv,udotv,udotv);
return res;
}
template <typename V,typename M>
inline Eigen::Matrix<typename M::Scalar,3,M::ColsAtCompileTime,M::Options>
......
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -371,4 +371,20 @@ BOOST_AUTO_TEST_CASE(test_skew)
}
BOOST_AUTO_TEST_CASE(test_skew_square)
{
using namespace se3;
typedef SE3::Vector3 Vector3;
typedef SE3::Matrix3 Matrix3;
Vector3 u(Vector3::Random());
Vector3 v(Vector3::Random());
Matrix3 ref = skew(u) * skew(v);
Matrix3 res = skewSquare(u,v);
BOOST_CHECK(res.isApprox(ref));
}
BOOST_AUTO_TEST_SUITE_END ()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment