/// <summary>
/// Scale a quaternion
/// </summary>
/// <param name="quaternion1">The quaternion to scale.</param>
/// <param name="scaleFactor">Scale factor.</param>
/// <returns>The scaled quaternion.</returns>
#region public static JQuaternion Multiply(JQuaternion quaternion1, FP scaleFactor)
public static TSQuaternion Multiply(TSQuaternion quaternion1, FP scaleFactor)
{
TSQuaternion result;
TSQuaternion.Multiply(ref quaternion1, scaleFactor, out result);
return(result);
}
/// <summary>
/// Multiply two quaternions.
/// </summary>
/// <param name="value1">The first quaternion.</param>
/// <param name="value2">The second quaternion.</param>
/// <returns>The product of both quaternions.</returns>
#region public static FP operator *(JQuaternion value1, JQuaternion value2)
public static TSQuaternion operator *(TSQuaternion value1, TSQuaternion value2)
{
TSQuaternion result;
TSQuaternion.Multiply(ref value1, ref value2, out result);
return(result);
}
/// <summary>
/// Multiply two quaternions.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <returns>The product of both quaternions.</returns>
#region public static JQuaternion Multiply(JQuaternion quaternion1, JQuaternion quaternion2)
public static TSQuaternion Multiply(TSQuaternion quaternion1, TSQuaternion quaternion2)
{
TSQuaternion result;
TSQuaternion.Multiply(ref quaternion1, ref quaternion2, out result);
return(result);
}
public static TSQuaternion LerpUnclamped(TSQuaternion a, TSQuaternion b, FP t)
{
TSQuaternion result = TSQuaternion.Multiply(a, 1 - t) + TSQuaternion.Multiply(b, t);
result.Normalize();
return(result);
}
private void IntegrateCallback(object obj)
{
RigidBody rigidBody = obj as RigidBody;
TSVector tSVector;
TSVector.Multiply(ref rigidBody.linearVelocity, this.timestep, out tSVector);
TSVector.Add(ref tSVector, ref rigidBody.position, out rigidBody.position);
bool flag = !rigidBody.isParticle;
if (flag)
{
FP magnitude = rigidBody.angularVelocity.magnitude;
bool flag2 = magnitude < FP.EN3;
TSVector tSVector2;
if (flag2)
{
TSVector.Multiply(ref rigidBody.angularVelocity, FP.Half * this.timestep - this.timestep * this.timestep * this.timestep * (2082 * FP.EN6) * magnitude * magnitude, out tSVector2);
}
else
{
TSVector.Multiply(ref rigidBody.angularVelocity, FP.Sin(FP.Half * magnitude * this.timestep) / magnitude, out tSVector2);
}
TSQuaternion tSQuaternion = new TSQuaternion(tSVector2.x, tSVector2.y, tSVector2.z, FP.Cos(magnitude * this.timestep * FP.Half));
TSQuaternion tSQuaternion2;
TSQuaternion.CreateFromMatrix(ref rigidBody.orientation, out tSQuaternion2);
TSQuaternion.Multiply(ref tSQuaternion, ref tSQuaternion2, out tSQuaternion);
tSQuaternion.Normalize();
TSMatrix.CreateFromQuaternion(ref tSQuaternion, out rigidBody.orientation);
}
bool flag3 = (rigidBody.Damping & RigidBody.DampingType.Linear) > RigidBody.DampingType.None;
if (flag3)
{
TSVector.Multiply(ref rigidBody.linearVelocity, this.currentLinearDampFactor, out rigidBody.linearVelocity);
}
bool flag4 = (rigidBody.Damping & RigidBody.DampingType.Angular) > RigidBody.DampingType.None;
if (flag4)
{
TSVector.Multiply(ref rigidBody.angularVelocity, this.currentAngularDampFactor, out rigidBody.angularVelocity);
}
rigidBody.Update();
bool flag5 = this.CollisionSystem.EnableSpeculativeContacts || rigidBody.EnableSpeculativeContacts;
if (flag5)
{
rigidBody.SweptExpandBoundingBox(this.timestep);
}
}
public static TSQuaternion Inverse(TSQuaternion rotation)
{
FP invNorm = FP.One / ((rotation.x * rotation.x) + (rotation.y * rotation.y) + (rotation.z * rotation.z) + (rotation.w * rotation.w));
return(TSQuaternion.Multiply(TSQuaternion.Conjugate(rotation), invNorm));
}
public static TSQuaternion Inverse(TSQuaternion rotation)
{
FP scaleFactor = FP.One / (rotation.x * rotation.x + rotation.y * rotation.y + rotation.z * rotation.z + rotation.w * rotation.w);
return(TSQuaternion.Multiply(TSQuaternion.Conjugate(rotation), scaleFactor));
}