const Transform & FixedJoint::getTransform(const VectorDynSize & jntPos, const LinkIndex child, const LinkIndex parent) const
{
if( child == this->link1 )
{
iDynTreeAssert(p_linkB == this->link2);
return this->link1_X_link2;
}
else
{
iDynTreeAssert(child == this->link2);
iDynTreeAssert(parent == this->link1);
return this->link2_X_link1;
}
}
// Geometric operations
AngularForceVector3 AngularForceVector3::changePoint(const Position & newPoint,
const LinearForceVector3 & otherLinear) const
{
AngularForceVector3 resultAngular;
iDynTreeAssert(semantics.changePoint(newPoint.getSemantics(), otherLinear.getSemantics(), resultAngular.semantics));
Eigen::Map<const Vector3d> newPointMap(newPoint.data());
Eigen::Map<const Vector3d> otherLinearMap(otherLinear.data());
Eigen::Map<const Vector3d> thisMap(this->data());
Eigen::Map<Vector3d> resultAngularMap(resultAngular.data());
resultAngularMap = thisMap + newPointMap.cross(otherLinearMap);
return resultAngular;
}