Commit 4b170af5 authored by jcarpent's avatar jcarpent
Browse files

[Spatial] Correct operators

parent 30d96bac
......@@ -71,6 +71,10 @@ namespace se3
template<typename D2>
bool isEqual_impl(const MotionDense<D2> & other) const
{ return other.linear().isZero() && other.angular() == c_J; }
template<typename D2>
void addTo(MotionDense<D2> & other) const
{ other.angular() += c_J; }
}; // struct BiasSphericalTpl
template <typename S2, int O2>
......
......@@ -84,18 +84,18 @@ namespace se3
MotionPlain operator-() const { return derived().__minus__(); }
template<typename M1>
MotionPlain operator+(const MotionDense<M1> & v) const { return derived().__plus__(v); }
MotionPlain operator+(const MotionDense<M1> & v) const { return derived().__plus__(v.derived()); }
template<typename M1>
MotionPlain operator-(const MotionDense<M1> & v) const { return derived().__minus__(v); }
MotionPlain operator-(const MotionDense<M1> & v) const { return derived().__minus__(v.derived()); }
template<typename M1>
Derived & operator+=(const MotionDense<M1> & v) { return derived().__pequ__(v); }
Derived & operator+=(const MotionDense<M1> & v) { return derived().__pequ__(v.derived()); }
template<typename M1>
Derived & operator+=(const MotionBase<M1> & v)
{ v.derived().addTo(derived()); return derived(); }
template<typename M1>
Derived & operator-=(const MotionDense<M1> & v) { return derived().__mequ__(v); }
Derived & operator-=(const MotionDense<M1> & v) { return derived().__mequ__(v.derived()); }
MotionPlain operator*(const Scalar alpha) const { return derived().__mult__(alpha); }
MotionPlain __minus__() const { return MotionPlain(-linear(),-angular()); }
......
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