public Vector3 ComputeRequiredAngularAcceleration(Quaternion currentOrientation, Quaternion desiredOrientation, Vector3 currentAngularVelocity, float deltaTime)
{
Quaternion quaternion = QuaternionExtensions.RequiredRotation(currentOrientation, desiredOrientation);
Quaternion quaternion1 = Quaternion.identity.Subtract(quaternion);
Quaternion eulerAngleQuaternion = PidQuaternionController.ToEulerAngleQuaternion(currentAngularVelocity) * quaternion;
Matrix4x4 matrix4x4 = new Matrix4x4()
{
m00 = -quaternion.x * -quaternion.x + -quaternion.y * -quaternion.y + -quaternion.z * -quaternion.z,
m01 = -quaternion.x * quaternion.w + -quaternion.y * -quaternion.z + -quaternion.z * quaternion.y,
m02 = -quaternion.x * quaternion.z + -quaternion.y * quaternion.w + -quaternion.z * -quaternion.x,
m03 = -quaternion.x * -quaternion.y + -quaternion.y * quaternion.x + -quaternion.z * quaternion.w,
m10 = quaternion.w * -quaternion.x + -quaternion.z * -quaternion.y + quaternion.y * -quaternion.z,
m11 = quaternion.w * quaternion.w + -quaternion.z * -quaternion.z + quaternion.y * quaternion.y,
m12 = quaternion.w * quaternion.z + -quaternion.z * quaternion.w + quaternion.y * -quaternion.x,
m13 = quaternion.w * -quaternion.y + -quaternion.z * quaternion.x + quaternion.y * quaternion.w,
m20 = quaternion.z * -quaternion.x + quaternion.w * -quaternion.y + -quaternion.x * -quaternion.z,
m21 = quaternion.z * quaternion.w + quaternion.w * -quaternion.z + -quaternion.x * quaternion.y,
m22 = quaternion.z * quaternion.z + quaternion.w * quaternion.w + -quaternion.x * -quaternion.x,
m23 = quaternion.z * -quaternion.y + quaternion.w * quaternion.x + -quaternion.x * quaternion.w,
m30 = -quaternion.y * -quaternion.x + quaternion.x * -quaternion.y + quaternion.w * -quaternion.z,
m31 = -quaternion.y * quaternion.w + quaternion.x * -quaternion.z + quaternion.w * quaternion.y,
m32 = -quaternion.y * quaternion.z + quaternion.x * quaternion.w + quaternion.w * -quaternion.x,
m33 = -quaternion.y * -quaternion.y + quaternion.x * quaternion.x + quaternion.w * quaternion.w
};
Quaternion quaternion2 = this.ComputeOutput(quaternion1, eulerAngleQuaternion, deltaTime);
quaternion2 = PidQuaternionController.MultiplyAsVector(matrix4x4, quaternion2);
Quaternion quaternion3 = quaternion2.Multiply(-2f) * Quaternion.Inverse(quaternion);
return(new Vector3(quaternion3.x, quaternion3.y, quaternion3.z));
}
/// <summary>
/// Computes the angular acceleration required to rotate from the current orientation to
/// a desired orientation based on the specified current angular velocity for the current frame.
/// </summary>
/// <param name="currentOrientation">The current orientation.</param>
/// <param name="desiredOrientation">The desired orientation.</param>
/// <param name="currentAngularVelocity">The current angular velocity.</param>
/// <param name="deltaTime">The frame delta time.</param>
/// <returns>
/// The angular acceleration required to rotate from the current orientation to the desired orientation.
/// </returns>
public Vector3 ComputeRequiredAngularAcceleration(Quaternion currentOrientation, Quaternion desiredOrientation, Vector3 currentAngularVelocity, float deltaTime)
{
Quaternion requiredRotation = QuaternionExtensions.RequiredRotation(currentOrientation, desiredOrientation);
Quaternion error = Quaternion.identity.Subtract(requiredRotation);
Quaternion angularVelocity = ToEulerAngleQuaternion(currentAngularVelocity);
Quaternion delta = angularVelocity * requiredRotation;
var orthogonalizeMatrix = new Matrix4x4()
{
m00 =
-requiredRotation.x * -requiredRotation.x + -requiredRotation.y * -requiredRotation.y +
-requiredRotation.z * -requiredRotation.z,
m01 =
-requiredRotation.x * requiredRotation.w + -requiredRotation.y * -requiredRotation.z +
-requiredRotation.z * requiredRotation.y,
m02 =
-requiredRotation.x * requiredRotation.z + -requiredRotation.y * requiredRotation.w +
-requiredRotation.z * -requiredRotation.x,
m03 =
-requiredRotation.x * -requiredRotation.y + -requiredRotation.y * requiredRotation.x +
-requiredRotation.z * requiredRotation.w,
m10 =
requiredRotation.w * -requiredRotation.x + -requiredRotation.z * -requiredRotation.y +
requiredRotation.y * -requiredRotation.z,
m11 =
requiredRotation.w * requiredRotation.w + -requiredRotation.z * -requiredRotation.z +
requiredRotation.y * requiredRotation.y,
m12 =
requiredRotation.w * requiredRotation.z + -requiredRotation.z * requiredRotation.w +
requiredRotation.y * -requiredRotation.x,
m13 =
requiredRotation.w * -requiredRotation.y + -requiredRotation.z * requiredRotation.x +
requiredRotation.y * requiredRotation.w,
m20 =
requiredRotation.z * -requiredRotation.x + requiredRotation.w * -requiredRotation.y +
-requiredRotation.x * -requiredRotation.z,
m21 =
requiredRotation.z * requiredRotation.w + requiredRotation.w * -requiredRotation.z +
-requiredRotation.x * requiredRotation.y,
m22 =
requiredRotation.z * requiredRotation.z + requiredRotation.w * requiredRotation.w +
-requiredRotation.x * -requiredRotation.x,
m23 =
requiredRotation.z * -requiredRotation.y + requiredRotation.w * requiredRotation.x +
-requiredRotation.x * requiredRotation.w,
m30 =
-requiredRotation.y * -requiredRotation.x + requiredRotation.x * -requiredRotation.y +
requiredRotation.w * -requiredRotation.z,
m31 =
-requiredRotation.y * requiredRotation.w + requiredRotation.x * -requiredRotation.z +
requiredRotation.w * requiredRotation.y,
m32 =
-requiredRotation.y * requiredRotation.z + requiredRotation.x * requiredRotation.w +
requiredRotation.w * -requiredRotation.x,
m33 =
-requiredRotation.y * -requiredRotation.y + requiredRotation.x * requiredRotation.x +
requiredRotation.w * requiredRotation.w,
};
Quaternion neededAngularVelocity = ComputeOutput(error, delta, deltaTime);
neededAngularVelocity = MultiplyAsVector(orthogonalizeMatrix, neededAngularVelocity);
Quaternion doubleNegative = neededAngularVelocity.Multiply(-2.0f);
Quaternion result = doubleNegative * Quaternion.Inverse(requiredRotation);
return(new Vector3(result.x, result.y, result.z));
}
