SpatialMotionVector PositionRaw::changePointOf(const SpatialMotionVector & other) const
{
SpatialMotionVector result;
Eigen::Map<const Eigen::Vector3d> thisPos(this->data());
Eigen::Map<const Eigen::Vector3d> otherLinear(other.getLinearVec3().data());
Eigen::Map<const Eigen::Vector3d> otherAngular(other.getAngularVec3().data());
Eigen::Map<Eigen::Vector3d> resLinear(result.getLinearVec3().data());
Eigen::Map<Eigen::Vector3d> resAngular(result.getAngularVec3().data());
resLinear = otherLinear + thisPos.cross(otherAngular);
resAngular = otherAngular;
return result;
}
SpatialMotionVectorRaw SpatialMotionVectorRaw::cross(const SpatialMotionVectorRaw& other) const
{
SpatialMotionVectorRaw res;
Eigen::Map<const Eigen::Vector3d> op1Linear(this->data());
Eigen::Map<const Eigen::Vector3d> op1Angular(this->data()+3);
Eigen::Map<const Eigen::Vector3d> op2Linear(other.data());
Eigen::Map<const Eigen::Vector3d> op2Angular(other.data()+3);
Eigen::Map<Eigen::Vector3d> resLinear(res.data());
Eigen::Map<Eigen::Vector3d> resAngular(res.data()+3);
resLinear = op1Angular.cross(op2Linear) + op1Linear.cross(op2Angular);
resAngular = op1Angular.cross(op2Angular);
return res;
}
