//==============================================================================
Eigen::Vector3d BallJoint::getPositionDifferencesStatic(
const Eigen::Vector3d& _q2, const Eigen::Vector3d& _q1) const
{
const Eigen::Matrix3d R1 = convertToRotation(_q1);
const Eigen::Matrix3d R2 = convertToRotation(_q2);
return convertToPositions(R1.transpose() * R2);
}
//==============================================================================
void BallJoint::integratePositions(double _dt)
{
Eigen::Matrix3d Rnext
= getR().linear() * convertToRotation(getVelocitiesStatic() * _dt);
setPositionsStatic(convertToPositions(Rnext));
}
//==============================================================================
void BallJoint::updateRelativeTransform() const
{
mR.linear() = convertToRotation(getPositionsStatic());
mT = Joint::mAspectProperties.mT_ParentBodyToJoint * mR
* Joint::mAspectProperties.mT_ChildBodyToJoint.inverse();
assert(math::verifyTransform(mT));
}
//==============================================================================
Eigen::Isometry3d BallJoint::convertToTransform(
const Eigen::Vector3d& _positions)
{
return Eigen::Isometry3d(convertToRotation(_positions));
}
//==============================================================================
Eigen::Matrix3d EulerJoint::convertToRotation(const Eigen::Vector3d& _positions)
const
{
return convertToRotation(_positions, getAxisOrder());
}
//==============================================================================
Eigen::Isometry3d EulerJoint::convertToTransform(
const Eigen::Vector3d& _positions, AxisOrder _ordering)
{
return Eigen::Isometry3d(convertToRotation(_positions, _ordering));
}