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)
{
}
