SpatialMotionVectorRaw SpatialMotionVectorRaw::inverse(const SpatialMotionVectorRaw& op) { SpatialMotionVectorRaw result; Eigen::Map<const Vector6d> opData(op.data()); Eigen::Map<Vector6d> resultData(result.data()); resultData = -opData; return result; }
SpatialMotionVectorRaw SpatialMotionVectorRaw::compose(const SpatialMotionVectorRaw& op1, const SpatialMotionVectorRaw& op2) { SpatialMotionVectorRaw result; Eigen::Map<const Vector6d> op1Data(op1.data()); Eigen::Map<const Vector6d> op2Data(op2.data()); Eigen::Map<Vector6d> resultData(result.data()); resultData = op1Data + op2Data; return result; }
SpatialMotionVectorRaw PositionRaw::changePointOf(const SpatialMotionVectorRaw & other) const { SpatialMotionVectorRaw result; Eigen::Map<const Eigen::Vector3d> thisPos(this->data()); Eigen::Map<const Vector6d> otherTwist(other.data()); Eigen::Map<Vector6d> resTwist(result.data()); resTwist.segment<3>(0) = otherTwist.segment<3>(0)+thisPos.cross(otherTwist.segment<3>(3)); resTwist.segment<3>(3) = otherTwist.segment<3>(3); 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; }
double SpatialForceVectorRaw::dot(const SpatialMotionVectorRaw& other) const { Eigen::Map<const Vector6d> otherData(other.data()); Eigen::Map<const Vector6d> thisData(this->data()); return thisData.dot(otherData); }
SpatialMotionVectorRaw::SpatialMotionVectorRaw(const SpatialMotionVectorRaw& other): Vector6(other.data(),6) { }