private void SetBaseReference(MQuaternion baseGlobalRot, MVector3 baseGlobalPos)
{
this.isMapped = true;
this.targetBP = baseGlobalPos.Clone();
this.targetGBR = baseGlobalRot.Clone();
this.isBP = this.GetGlobalPosition();
this.isGBR = this.GetGlobalRotation();
this.invTargetGBR = MQuaternionExtensions.Inverse(this.targetGBR);
this.invIsGBR = MQuaternionExtensions.Inverse(this.isGBR);
this.isOffset = this.invIsGBR.Multiply(this.targetBP.Subtract(this.isBP));
this.targetOffset = this.invTargetGBR.Multiply(this.isBP.Subtract(this.targetBP));
this.inverseOffsetRotation = MQuaternionExtensions.Inverse(this.GetOffsetRotation());
}
public void RecomputeLocalTransformations()
{
MQuaternion parentRotation = new MQuaternion(0, 0, 0, 1);
MVector3 parentPosition = new MVector3(0, 0, 0);
if (this.parentJoint != null)
{
parentRotation = this.parentJoint.GetGlobalRotation();
parentPosition = this.parentJoint.GetGlobalPosition();
}
MQuaternion inverseParentRot = MQuaternionExtensions.Inverse(parentRotation);
if (!isMapped && this.parentJoint == null)
{
this.currentRotationValues = new MQuaternion(0, 0, 0, 1);
this.translation = this.globalPos;
}
else if (isMapped)
{
this.currentRotationValues = this.inverseOffsetRotation.Multiply(inverseParentRot).Multiply(this.globalRot);
MVector3 rotatedOffset = parentRotation.Multiply(this.joint.Position);
MVector3 rotatedTranslation = this.globalPos.Subtract(parentPosition).Subtract(rotatedOffset);
this.translation = this.inverseOffsetRotation.Multiply(inverseParentRot).Multiply(rotatedTranslation);
}
else
{
this.currentRotationValues = new MQuaternion(0, 0, 0, 1);
this.translation = new MVector3(0, 0, 0);
this.globalPos = null;
this.globalRot = null;
this.GetGlobalPosition();
this.GetGlobalRotation();
}
foreach (Joint c in this.children)
{
((RJoint)c).RecomputeLocalTransformations();
}
}
/// <summary>
/// Rotates the current transform around the specific point and axis
/// </summary>
/// <param name="center">The rotation center</param>
/// <param name="axis">The rotation axis</param>
/// <param name="angle">The angle to rotate</param>
private static MTransform RotateAround(MTransform transform, MVector3 center, MVector3 axis, float angle)
{
MTransform res = new MTransform()
{
ID = System.Guid.NewGuid().ToString()
};
MVector3 pos = transform.Position;
MQuaternion rot = MQuaternionExtensions.FromEuler(axis.Multiply(angle)); // get the desired rotation
MVector3 dir = pos.Subtract(center); // find current direction relative to center
dir = rot.Multiply(dir); // rotate the direction
res.Position = center.Add(dir); // define new position
MQuaternion myRot = transform.Rotation;
res.Rotation = transform.Rotation.Multiply(MQuaternionExtensions.Inverse(myRot).Multiply(rot).Multiply(myRot));
return(res);
}
/// <summary>
/// Transforms a rotation from global space into the local space of the MTransform
/// </summary>
/// <param name="transform"></param>
/// <param name="globalRotation"></param>
/// <returns></returns>
public static MQuaternion InverseTransformRotation(this MTransform transform, MQuaternion globalRotation)
{
return(MQuaternionExtensions.Inverse(transform.Rotation).Multiply(globalRotation));
}
/// <summary>
/// Transforms a point from the global space to the local space of the MTransform
/// </summary>
/// <param name="transform"></param>
/// <param name="globalPosition"></param>
/// <returns></returns>
public static MVector3 InverseTransformPoint(this MTransform transform, MVector3 globalPosition)
{
return(MQuaternionExtensions.Inverse(transform.Rotation).Multiply(globalPosition.Subtract(transform.Position)));
}