Verified Commit 79653d2d authored by Justin Carpentier's avatar Justin Carpentier Committed by Justin Carpentier
Browse files

[Joint/All/Motion] Use sparse operation for operator^

parent 2bd90652
......@@ -324,25 +324,10 @@ namespace se3
}
template<typename MotionDerived, typename S2, int O2>
inline typename MotionDerived::MotionPlain operator^ (const MotionDense<MotionDerived> & m1, const MotionPlanarTpl<S2,O2> & m2)
inline typename MotionDerived::MotionPlain
operator^(const MotionDense<MotionDerived> & m1, const MotionPlanarTpl<S2,O2> & m2)
{
typename MotionDerived::MotionPlain result;
typedef typename MotionDerived::Scalar Scalar;
const typename MotionDerived::ConstLinearType & m1_t = m1.linear();
const typename MotionDerived::ConstAngularType & m1_w = m1.angular();
result.angular()
<< m1_w(1) * m2.m_theta_dot
, - m1_w(0) * m2.m_theta_dot
, Scalar(0);
result.linear()
<< m1_t(1) * m2.m_theta_dot - m1_w(2) * m2.m_y_dot
, - m1_t(0) * m2.m_theta_dot + m1_w(2) * m2.m_x_dot
, m1_w(0) * m2.m_y_dot - m1_w(1) * m2.m_x_dot;
return result;
return m2.motionAction(m1);
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
......
......@@ -316,11 +316,7 @@ namespace se3
operator^(const MotionDense<MotionDerived> & m1,
const MotionPrismaticUnalignedTpl<S2,O2> & m2)
{
typedef typename MotionDerived::MotionPlain ReturnType;
/* m1xm2 = [ v1xw2 + w1xv2; w1xw2 ] = [ v1xw2; w1xw2 ] */
const typename MotionDerived::ConstAngularType & w1 = m1.angular();
const typename MotionPrismaticUnalignedTpl<S2,O2>::Vector3 & v2 = m2.axis * m2.rate;
return ReturnType(w1.cross(v2), ReturnType::Vector3::Zero());
return m2.motionAction(m1);
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
......
......@@ -327,11 +327,7 @@ namespace se3
operator^(const MotionDense<MotionDerived> & m1, const MotionRevoluteUnalignedTpl<S2,O2> & m2)
{
/* m1xm2 = [ v1xw2 + w1xv2; w1xw2 ] = [ v1xw2; w1xw2 ] */
typedef typename MotionDerived::MotionPlain ReturnType;
const typename MotionDerived::ConstLinearType v1 = m1.linear();
const typename MotionDerived::ConstAngularType w1 = m1.angular();
const typename ReturnType::Vector3 w2(m2.axis * m2.w);
return ReturnType(v1.cross(w2),w1.cross(w2));
return m2.motionAction(m1);
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
......
......@@ -302,8 +302,7 @@ namespace se3
operator^(const MotionDense<MotionDerived> & m1,
const MotionSphericalTpl<S2,O2> & m2)
{
return typename MotionDerived::MotionPlain(m1.template linear().cross(m2.w),
m1.template angular().cross(m2.w));
return m2.motionAction(m1);
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
......
......@@ -379,9 +379,7 @@ namespace se3
operator^(const MotionDense<MotionDerived> & m1,
const MotionTranslationTpl<S2,O2> & m2)
{
typedef typename MotionDerived::MotionPlain ReturnType;
return ReturnType(m1.angular().cross(m2.rate),
ReturnType::Vector3::Zero());
return m2.motionAction(m1);
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
......
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