void CFixedJoint::getLocalTransform(JointBody body, Vector3& position, Quaternion& rotation)
{
position = mPositions[(int)body];
rotation = mRotations[(int)body];
HRigidbody rigidbody = mBodies[(int)body];
if (rigidbody == nullptr) // Get world space transform if no relative to any body
{
Quaternion worldRot = SO()->getWorldRotation();
rotation = worldRot*rotation;
position = worldRot.rotate(position) + SO()->getWorldPosition();
}
else
{
// Find world space transform
Quaternion worldRot = rigidbody->SO()->getWorldRotation();
rotation = worldRot * rotation;
position = worldRot.rotate(position) + rigidbody->SO()->getWorldPosition();
// Get transform of the joint local to the object
Quaternion invRotation = rotation.inverse();
position = invRotation.rotate(SO()->getWorldPosition() - position);
rotation = invRotation * SO()->getWorldRotation();
}
}
void CSliderJoint::getLocalTransform(JointBody body, Vector3& position, Quaternion& rotation)
{
position = mPositions[(UINT32)body];
rotation = mRotations[(UINT32)body];
HRigidbody rigidbody = mBodies[(UINT32)body];
const Transform& tfrm = SO()->getTransform();
if (rigidbody == nullptr) // Get world space transform if no relative to any body
{
Quaternion worldRot = tfrm.getRotation();
rotation = worldRot*rotation;
position = worldRot.rotate(position) + tfrm.getPosition();
}
else
{
const Transform& rigidbodyTfrm = rigidbody->SO()->getTransform();
// Use only the offset for positioning, but for rotation use both the offset and target SO rotation.
// (Needed because we need to rotate the joint SO in order to orient the slider direction, so we need an
// additional transform that allows us to orient the object)
position = rotation.rotate(position);
rotation = (rigidbodyTfrm.getRotation()*rotation).inverse()*tfrm.getRotation();
}
}