void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
v.angular().noalias() = m.rotation().col(2) * m_theta_dot;
v.linear().noalias() = m.translation().cross(v.angular());
v.linear() += m.rotation().col(0) * m_x_dot;
v.linear() += m.rotation().col(1) * m_y_dot;
}
EIGEN_STRONG_INLINE
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
CartesianAxis3::alphaCross(-w,v.linear(),mout.linear());
// Angular
CartesianAxis3::alphaCross(-w,v.angular(),mout.angular());
}
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
// Linear
// TODO: use v.angular() as temporary variable
Vector3 v3_tmp;
CartesianAxis3::alphaCross(w,m.translation(),v3_tmp);
v.linear().noalias() = m.rotation().transpose() * v3_tmp;
// Angular
v.angular().noalias() = m.rotation().transpose().col(axis) * w;
}
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
// Linear
// TODO: use v.angular() as temporary variable
Vector3 v3_tmp;
AxisZ::alphaCross(m_theta_dot,m.translation(),v3_tmp);
v3_tmp[0] += m_x_dot; v3_tmp[1] += m_y_dot;
v.linear().noalias() = m.rotation().transpose() * v3_tmp;
// Angular
v.angular().noalias() = m.rotation().transpose().col(2) * m_theta_dot;
}
DenseBase motionAction(const MotionDense<MotionDerived> & m) const
{
const typename MotionDerived::ConstLinearType v = m.linear();
const typename MotionDerived::ConstAngularType w = m.angular();
DenseBase res(DenseBase::Zero());
res(0,1) = -w[2]; res(0,2) = v[1];
res(1,0) = w[2]; res(1,2) = -v[0];
res(2,0) = -w[1]; res(2,1) = w[0];
res(3,2) = w[1];
res(4,2) = -w[0];
return res;
}
DenseBase motionAction(const MotionDense<MotionDerived> & m) const
{
DenseBase res;
MotionRef<DenseBase> v(res);
v = m.cross(Axis());
return res;
}
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
AxisZ::alphaCross(-m_theta_dot,v.linear(),mout.linear());
typename M1::ConstAngularType w_in = v.angular();
typename M2::LinearType v_out = mout.linear();
v_out[0] -= w_in[2] * m_y_dot;
v_out[1] += w_in[2] * m_x_dot;
v_out[2] += -w_in[1] * m_x_dot + w_in[0] * m_y_dot ;
// Angular
AxisZ::alphaCross(-m_theta_dot,v.angular(),mout.angular());
}
void setTo(MotionDense<MotionDerived> & other) const
{
other.linear() << m_x_dot, m_y_dot, (Scalar)0;
other.angular() << (Scalar)0, (Scalar)0, m_theta_dot;
}
void addTo(MotionDense<Derived> & other) const
{
other.linear()[0] += m_x_dot;
other.linear()[1] += m_y_dot;
other.angular()[2] += m_theta_dot;
}
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
v.angular().noalias() = m.rotation().col(axis) * w;
v.linear().noalias() = m.translation().cross(v.angular());
}
void addTo(MotionDense<MotionDerived> & v) const
{
typedef typename MotionDense<MotionDerived>::Scalar OtherScalar;
v.angular()[axis] += (OtherScalar)w;
}
void setTo(MotionDense<MotionDerived> & m) const
{
m.linear().setZero();
for(Eigen::DenseIndex k = 0; k < 3; ++k)
m.angular()[k] = k == axis ? w : (Scalar)0;
}