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

[Spatial] Add alphaCross operations to CartesianAxis3

parent e9c8403c
......@@ -127,9 +127,8 @@ namespace se3
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
CartesianAxis3::cross(v.angular(),mout.linear());
mout.linear() *= -rate;
CartesianAxis3::alphaCross(-rate,v.angular(),mout.linear());
// Angular
mout.angular().setZero();
}
......
......@@ -130,12 +130,10 @@ namespace se3
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
CartesianAxis3::cross(v.linear(),mout.linear());
mout.linear() *= -w;
CartesianAxis3::alphaCross(-w,v.linear(),mout.linear());
// Angular
CartesianAxis3::cross(v.angular(),mout.angular());
mout.angular() *= -w;
CartesianAxis3::alphaCross(-w,v.angular(),mout.angular());
}
template<typename M1>
......
......@@ -40,6 +40,20 @@ namespace se3
return res;
}
template<typename Scalar, typename V3_in, typename V3_out>
inline static void alphaCross(const Scalar & s,
const Eigen::MatrixBase<V3_in> & vin,
const Eigen::MatrixBase<V3_out> & vout);
template<typename Scalar, typename V3>
static typename EIGEN_PLAIN_TYPE(V3) alphaCross(const Scalar & s,
const Eigen::MatrixBase<V3> & vin)
{
typename EIGEN_PLAIN_TYPE(V3) res;
alphaCross(s,vin,res);
return res;
}
template<typename Scalar>
Eigen::Matrix<Scalar,dim,1> operator*(const Scalar & s) const
{
......@@ -92,6 +106,42 @@ namespace se3
V3_out & vout_ = const_cast<Eigen::MatrixBase<V3_out> &>(vout).derived();
vout_[0] = -vin[1]; vout_[1] = vin[0]; vout_[2] = 0.;
}
template<>
template<typename Scalar, typename V3_in, typename V3_out>
inline void CartesianAxis<0>::alphaCross(const Scalar & s,
const Eigen::MatrixBase<V3_in> & vin,
const Eigen::MatrixBase<V3_out> & vout)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
V3_out & vout_ = const_cast<Eigen::MatrixBase<V3_out> &>(vout).derived();
vout_[0] = 0.; vout_[1] = -s*vin[2]; vout_[2] = s*vin[1];
}
template<>
template<typename Scalar, typename V3_in, typename V3_out>
inline void CartesianAxis<1>::alphaCross(const Scalar & s,
const Eigen::MatrixBase<V3_in> & vin,
const Eigen::MatrixBase<V3_out> & vout)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
V3_out & vout_ = const_cast<Eigen::MatrixBase<V3_out> &>(vout).derived();
vout_[0] = s*vin[2]; vout_[1] = 0.; vout_[2] = -s*vin[0];
}
template<>
template<typename Scalar, typename V3_in, typename V3_out>
inline void CartesianAxis<2>::alphaCross(const Scalar & s,
const Eigen::MatrixBase<V3_in> & vin,
const Eigen::MatrixBase<V3_out> & vout)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
V3_out & vout_ = const_cast<Eigen::MatrixBase<V3_out> &>(vout).derived();
vout_[0] = -s*vin[1]; vout_[1] = s*vin[0]; vout_[2] = 0.;
}
typedef CartesianAxis<0> AxisX;
typedef CartesianAxis<1> AxisY;
......
......@@ -828,10 +828,15 @@ BOOST_AUTO_TEST_CASE(test_cartesian_axis)
using namespace Eigen;
using namespace se3;
Vector3d v(Vector3d::Random());
const double alpha = 3;
Vector3d v2(alpha*v);
BOOST_CHECK(AxisX::cross(v).isApprox(Vector3d::Unit(0).cross(v)));
BOOST_CHECK(AxisY::cross(v).isApprox(Vector3d::Unit(1).cross(v)));
BOOST_CHECK(AxisZ::cross(v).isApprox(Vector3d::Unit(2).cross(v)));
BOOST_CHECK(AxisX::alphaCross(alpha,v).isApprox(Vector3d::Unit(0).cross(v2)));
BOOST_CHECK(AxisY::alphaCross(alpha,v).isApprox(Vector3d::Unit(1).cross(v2)));
BOOST_CHECK(AxisZ::alphaCross(alpha,v).isApprox(Vector3d::Unit(2).cross(v2)));
test_scalar_multiplication_cartesian_axis<0>::run();
test_scalar_multiplication_cartesian_axis<1>::run();
......
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