Verified Commit 8b6203c6 authored by Justin Carpentier's avatar Justin Carpentier Committed by Justin Carpentier
Browse files

[All/Motion] Add setTo method to each sparse Motion class

parent 17c8ea8a
......@@ -98,12 +98,11 @@ namespace se3
other.angular()[2] += m_theta_dot;
}
template<typename Derived>
void addTo(MotionDense<Derived> & v) const
template<typename MotionDerived>
void setTo(MotionDense<MotionDerived> & other) const
{
v.linear()[0] += m_x_dot;
v.linear()[1] += m_y_dot;
v.angular()[2] += m_theta_dot;
other.linear() << m_x_dot, m_y_dot, 0;
other.angular() << 0, 0, m_theta_dot;
}
template<typename S2, int O2, typename D2>
......
......@@ -88,16 +88,23 @@ namespace se3
// { return MotionPlain(axis*rate,MotionPlain::Vector3::Zero());}
template<typename Derived>
void addTo(MotionDense<Derived> & v) const
void addTo(MotionDense<Derived> & other) const
{
v.linear() += axis * rate;
other.linear() += axis * rate;
}
template<typename Derived>
void setTo(MotionDense<Derived> & other) const
{
other.linear().noalias() = axis*rate;
other.angular().setZero();
}
template<typename S2, int O2, typename D2>
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
v.angular().setZero();
v.linear().noalias() = rate * (m.rotation() * axis); // TODO: check efficiency
v.angular().setZero();
}
template<typename S2, int O2>
......
......@@ -85,10 +85,18 @@ namespace se3
// inline operator MotionPlain() const { return Axis() * rate; }
template<typename Derived>
void addTo(MotionDense<Derived> & v_) const
void addTo(MotionDense<Derived> & other) const
{
typedef typename MotionDense<Derived>::Scalar OtherScalar;
v_.linear()[_axis] += (OtherScalar) rate;
other.linear()[_axis] += (OtherScalar) rate;
}
template<typename MotionDerived>
void setTo(MotionDense<MotionDerived> & other) const
{
for(Eigen::DenseIndex k = 0; k < 3; ++k)
other.linear()[k] = k == axis ? rate : 0;
other.angular().setZero();
}
template<typename S2, int O2, typename D2>
......
......@@ -93,6 +93,13 @@ namespace se3
{
v.angular() += axis*w;
}
template<typename Derived>
void setTo(MotionDense<Derived> & other) const
{
other.linear().setZero();
other.angular().noalias() = axis*w;
}
template<typename S2, int O2, typename D2>
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
......
......@@ -97,10 +97,11 @@ namespace se3
other.angular() += w;
}
template<typename MotionDerived>
void addTo(MotionDense<MotionDerived> & v) const
template<typename Derived>
void setTo(MotionDense<Derived> & other) const
{
v.angular() += w;
other.linear().setZero();
other.angular() = w;
}
template<typename S2, int O2, typename D2>
......
......@@ -101,9 +101,16 @@ namespace se3
}
template<typename Derived>
void addTo(MotionDense<Derived> & v_) const
void addTo(MotionDense<Derived> & other) const
{
v_.linear() += rate;
other.linear() += rate;
}
template<typename Derived>
void setTo(MotionDense<Derived> & other) const
{
other.linear() = rate;
other.angular().setZero();
}
template<typename S2, int O2, typename D2>
......
......@@ -91,6 +91,13 @@ namespace se3
return derived();
}
template<typename D2>
Derived & operator=(const MotionBase<D2> & other)
{
other.derived().setTo(derived());
return derived();
}
template<typename V6>
Derived & operator=(const Eigen::MatrixBase<V6> & v)
{
......
......@@ -93,6 +93,10 @@ namespace se3
explicit MotionTpl(const MotionDense<M2> & clone)
{ linear() = clone.linear(); angular() = clone.angular(); }
template<typename M2>
explicit MotionTpl(const MotionBase<M2> & clone)
{ *this = clone; }
// initializers
static MotionTpl Zero() { return MotionTpl(Vector6::Zero()); }
static MotionTpl Random() { return MotionTpl(Vector6::Random()); }
......
......@@ -74,7 +74,13 @@ namespace se3
{ return other.linear().isZero() && other.angular().isZero(); }
template<typename D2>
static void addTo(const MotionDense<D2> &) {}
static void addTo(const MotionBase<D2> &) {}
template<typename D2>
static void setTo(MotionBase<D2> & other)
{
other.setZero();
}
template<typename M1>
BiasZeroTpl motionAction(const MotionBase<M1> &) const
......
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