Motion operator^ (const Motion & m1, const JointPlanar::MotionPlanar & m2) { Motion result; const Motion::Vector3 & m1_t = m1.linear(); const Motion::Vector3 & m1_w = m1.angular(); result.angular () << m1_w(1) * m2.theta_dot_, - m1_w(0) * m2.theta_dot_, 0.; result.linear () << m1_t(1) * m2.theta_dot_ - m1_w(2) * m2.y_dot_, - m1_t(0) * m2.theta_dot_ + m1_w(2) * m2.x_dot_, m1_w(0) * m2.y_dot_ - m1_w(1) * m2.x_dot_; return result; }
inline Motion operator^( const Motion& m1, const MotionRevoluteUnaligned & m2) { /* m1xm2 = [ v1xw2 + w1xv2; w1xw2 ] = [ v1xw2; w1xw2 ] */ const Motion::Vector3& v1 = m1.linear(); const Motion::Vector3& w1 = m1.angular(); const Motion::Vector3& w2 = m2.axis * m2.w ; return Motion( v1.cross(w2),w1.cross(w2)); }
friend Motion operator* (const ConstraintPlanar &, const Eigen::MatrixBase<D> & v) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3); Motion result (Motion::Zero ()); result.linear ().template head<2> () = v.template topRows<2> (); result.angular ().template tail<1> () = v.template bottomRows<1> (); return result; }
friend Motion operator+ (const MotionPlanar & m1, const Motion & m2) { Motion result (m2); result.linear ()[0] += m1.x_dot_; result.linear ()[1] += m1.y_dot_; result.angular ()[2] += m1.theta_dot_; return result; }
inline Motion operator+ (const MotionSpherical & m1, const Motion & m2) { return Motion( m2.linear(), m2.angular() + m1.w); }
inline Motion operator^ (const Motion & m1, const MotionSpherical & m2) { return Motion(m1.linear ().cross (m2.w), m1.angular ().cross (m2.w)); }
Motion operator+( const MotionPrismatic<axis>& m1, const Motion& m2) { return Motion( m2.linear()+typename prismatic::CartesianVector3<axis>(m1.v),m2.angular()); }
inline Motion operator+ (const MotionRevoluteUnaligned& m1, const Motion& m2) { return Motion( m2.linear(), m1.w*m1.axis+m2.angular() ); }