public static Vector3 GetFixedAngularError(
SimulationObject objectA,
SimulationObject objectB,
Quaternion relativeOrientation)
{
Quaternion currentRelativeOrientation = objectB.RotationStatus.Inverse () *
objectA.RotationStatus;
Quaternion relativeOrientationError = relativeOrientation.Inverse () *
currentRelativeOrientation;
var angularError = new Vector3 (
relativeOrientationError.b,
relativeOrientationError.c,
relativeOrientationError.d);
if (relativeOrientationError.a < 0.0)
{
angularError = new Vector3 (
-angularError.x,
-angularError.y,
-angularError.z);
}
return objectA.RotationMatrix * angularError;
}
public static double GetAngle(
SimulationObject simulationObjectA,
SimulationObject simulationObjectB,
Quaternion relativeRotation,
Vector3 rotationAxis)
{
Quaternion currentRelativeOrientation = simulationObjectA.RotationStatus.Inverse () *
simulationObjectB.RotationStatus;
Quaternion relativeOrientation = relativeRotation.Inverse () *
currentRelativeOrientation;
var quaternionVectorPart = new Vector3 (
relativeOrientation.b,
relativeOrientation.c,
relativeOrientation.d);
quaternionVectorPart = simulationObjectA.RotationMatrix * quaternionVectorPart;
return GetRotationAngle (
quaternionVectorPart,
relativeOrientation.a,
rotationAxis);
}