Commit 2e414ab5 authored by jcarpent's avatar jcarpent
Browse files

[Spatial] Add scalar multiplication of a motion vector

parent 5170c96b
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
......@@ -65,6 +65,7 @@ namespace se3
Derived_t operator-(const Derived_t & v2) const { return derived().__minus__(v2); }
Derived_t & operator+=(const Derived_t & v2) { return derived().__pequ__(v2); }
Derived_t & operator-=(const Derived_t & v2) { return derived().__mequ__(v2); }
Derived_t operator*(const Scalar alpha) const { return derived().__mult__(alpha); }
bool isApprox (const Derived_t & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{ return derived().isApprox_impl(other, prec);}
......@@ -206,6 +207,7 @@ namespace se3
MotionTpl __minus__(const MotionTpl & v2) const { return MotionTpl(data - v2.data); }
MotionTpl& __pequ__(const MotionTpl & v2) { data += v2.data; return *this; }
MotionTpl& __mequ__(const MotionTpl & v2) { data -= v2.data; return *this; }
MotionTpl __mult__(const Scalar alpha) const { return MotionTpl(alpha*data); }
Scalar dot(const Force & f) const { return data.dot(f.toVector()); }
......@@ -269,6 +271,10 @@ namespace se3
MotionTpl<S,O> operator^( const MotionTpl<S,O> &m1, const MotionTpl<S,O> &m2 ) { return m1.cross(m2); }
template<typename S,int O>
ForceTpl<S,O> operator^( const MotionTpl<S,O> &m, const ForceTpl<S,O> &f ) { return m.cross(f); }
template<typename S,int O>
MotionTpl<S,O> operator*(const S alpha, const MotionTpl<S,O> & m )
{ return m*alpha; }
/////////////// BiasZero ///////////////
......
......@@ -112,6 +112,10 @@ BOOST_AUTO_TEST_CASE ( test_Motion )
// Test .=V6
bv3 = bv2_vec;
BOOST_CHECK( bv3.toVector().isApprox(bv2_vec, 1e-12));
// Test scalar*M6
Motion twicebv(2.*bv);
BOOST_CHECK(twicebv.isApprox(Motion(2.*bv.toVector())));
// Test constructor from V6
Motion bv4(bv2_vec);
......
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