spatial-axis.hpp

//
// Copyright (c) 2017-2019 CNRS INRIA
//

#ifndef __pinocchio_spatial_axis_hpp__
#define __pinocchio_spatial_axis_hpp__

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/cartesian-axis.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/force.hpp"

namespace pinocchio
{
 template<int axis>
 struct SpatialAxis;

 template<int axis, typename MotionDerived>
 struct MotionAlgebraAction<SpatialAxis<axis>, MotionDerived>
 {
 typedef typename MotionDerived::MotionPlain ReturnType;
 };

 template<int _axis>
 struct SpatialAxis //: MotionBase< SpatialAxis<_axis> >
 {
 enum
 {
 axis = _axis,
 dim = 6
 };
 typedef CartesianAxis<_axis % 3> CartesianAxis3;

 enum
 {
 LINEAR = 0,
 ANGULAR = 3
 };

 template<typename Derived1, typename Derived2>
 inline static void cross(const MotionDense<Derived1> & min, const MotionDense<Derived2> & mout);

 template<typename Derived>
 static typename traits<Derived>::MotionPlain cross(const MotionDense<Derived> & min)
 {
 typename MotionDense<Derived>::MotionPlain res;
 cross(min, res);
 return res;
 }

 template<typename Derived1, typename Derived2>
 inline static void cross(const ForceDense<Derived1> & fin, const ForceDense<Derived2> & fout);

 template<typename Derived>
 static typename traits<Derived>::ForcePlain cross(const ForceDense<Derived> & fin)
 {
 typename ForceDense<Derived>::ForcePlain fout;
 cross(fin, fout);
 return fout;
 }

 template<typename Scalar>
 MotionTpl<Scalar> operator*(const Scalar & s) const
 {
 typedef MotionTpl<Scalar> ReturnType;
 ReturnType res;
 for (Eigen::DenseIndex i = 0; i < dim; ++i)
 res.toVector()[i] = i == axis ? s : Scalar(0);

 return res;
 }

 template<typename Scalar>
 friend inline MotionTpl<Scalar> operator*(const Scalar & s, const SpatialAxis &)
 {
 return SpatialAxis() * s;
 }

 template<typename Derived>
 friend Derived & operator<<(MotionDense<Derived> & min, const SpatialAxis &)
 {
 typedef typename traits<Derived>::Scalar Scalar;
 min.setZero();
 min.toVector()[axis] = Scalar(1);
 return min.derived();
 }

 template<typename MotionDerived>
 typename MotionDerived::MotionPlain motionAction(const MotionDense<MotionDerived> & m) const
 {
 typename MotionDerived::MotionPlain res;
 if ((LINEAR == 0 && axis < 3) || (LINEAR == 3 && axis >= 3))
 {
 res.angular().setZero();
 CartesianAxis3::cross(-m.angular(), res.linear());
 }
 else
 {
 CartesianAxis3::cross(-m.linear(), res.linear());
 CartesianAxis3::cross(-m.angular(), res.angular());
 }

 return res;
 }

 }; // struct SpatialAxis

 template<int axis>
 template<typename Derived1, typename Derived2>
 inline void
 SpatialAxis<axis>::cross(const MotionDense<Derived1> & min, const MotionDense<Derived2> & mout)
 {
 Derived2 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2, mout);

 if ((LINEAR == 0 && axis < 3) || (LINEAR == 3 && axis >= 3))
 {
 mout_.angular().setZero();
 CartesianAxis3::cross(min.angular(), mout_.linear());
 }
 else
 {
 CartesianAxis3::cross(min.linear(), mout_.linear());
 CartesianAxis3::cross(min.angular(), mout_.angular());
 }
 }

 template<int axis>
 template<typename Derived1, typename Derived2>
 inline void
 SpatialAxis<axis>::cross(const ForceDense<Derived1> & fin, const ForceDense<Derived2> & fout)
 {
 Derived2 & fout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2, fout);

 if ((LINEAR == 0 && axis < 3) || (LINEAR == 3 && axis >= 3))
 {
 fout_.linear().setZero();
 CartesianAxis3::cross(fin.linear(), fout_.angular());
 }
 else
 {
 CartesianAxis3::cross(fin.linear(), fout_.linear());
 CartesianAxis3::cross(fin.angular(), fout_.angular());
 }
 }

 typedef SpatialAxis<0> AxisVX;
 typedef SpatialAxis<1> AxisVY;
 typedef SpatialAxis<2> AxisVZ;

 typedef SpatialAxis<3> AxisWX;
 typedef SpatialAxis<4> AxisWY;
 typedef SpatialAxis<5> AxisWZ;
} // namespace pinocchio

#endif // __pinocchio_spatial_axis_hpp__