[Spatial/Force] Improve efficiency of operator=

src/spatial/force-dense.hpp

@@ -48,6 +48,7 @@ namespace se3
     using Base::angular;
     using Base::derived;
     using Base::isApprox;
+    using Base::operator=;
     
     Derived & setZero() { linear().setZero(); angular().setZero(); return derived(); }
     Derived & setRandom() { linear().setRandom(); angular().setRandom(); return derived(); }
@@ -62,7 +63,7 @@ namespace se3
     
     // Arithmetic operators
     template<typename D2>
-    Derived & operator=(const ForceDense<D2> & other)
+    Derived & __equl__(const ForceDense<D2> & other)
     {
       linear() = other.linear();
       angular() = other.angular();
@@ -78,6 +79,12 @@ namespace se3
       return derived();
     }
     
+    template<typename D2>
+    Derived & operator=(const ForceDense<D2> & other)
+    {
+      return derived().__equl__(other.derived());
+    }
+    
     ForcePlain operator-() const { return derived().__opposite__(); }
     template<typename M1>
     ForcePlain operator+(const ForceDense<M1> & v) const { return derived().__plus__(v.derived()); }

src/spatial/force-tpl.hpp

@@ -85,6 +85,13 @@ namespace se3
     explicit ForceTpl(const ForceDense<M2> & clone)
     { linear() = clone.linear(); angular() = clone.angular(); }
     
+    template<int O2>
+    ForceTpl & __equl__(const ForceTpl<Scalar,O2> & other)
+    {
+      m_data = other.toVector();
+      return *this;
+    }
+    
     // initializers
     static ForceTpl Zero()   { return ForceTpl(Vector6::Zero());   }
     static ForceTpl Random() { return ForceTpl(Vector6::Random()); }