Verified Commit 2c4e1845 authored by Justin Carpentier's avatar Justin Carpentier Committed by Justin Carpentier
Browse files

[Spatial/Inertia] Add cast method

parent 37ecc81d
......@@ -164,6 +164,7 @@ namespace se3
public:
friend class InertiaBase< InertiaTpl< _Scalar, _Options > >;
SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl);
enum { Options = _Options };
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare;
......@@ -480,6 +481,15 @@ namespace se3
<< " c = " << lever().transpose() << "\n"
<< " I = \n" << inertia().matrix() << "";
}
/// \returns An expression of *this with the Scalar type casted to NewScalar.
template<typename NewScalar>
InertiaTpl<NewScalar,Options> cast() const
{
return InertiaTpl<NewScalar,Options>(static_cast<NewScalar>(mass()),
lever().template cast<NewScalar>(),
inertia().template cast<NewScalar>());
}
protected:
Scalar m_mass;
......
......@@ -619,6 +619,15 @@ BOOST_AUTO_TEST_CASE ( test_Inertia )
}
BOOST_AUTO_TEST_CASE(cast_inertia)
{
using namespace se3;
Inertia Y(Inertia::Random());
BOOST_CHECK(Y.cast<double>() == Y);
BOOST_CHECK(Y.cast<long double>().cast<double>() == Y);
}
BOOST_AUTO_TEST_CASE ( test_ActOnSet )
{
using namespace se3;
......
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