/// <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>
/// 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="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 body = obj as RigidBody;
TSVector temp;
TSVector.Multiply(ref body.linearVelocity, timestep, out temp);
TSVector.Add(ref temp, ref body.position, out body.position);
if (!(body.isParticle))
{
//exponential map
TSVector axis;
FP angle = body.angularVelocity.magnitude;
FP halfTimeStep = FP.Half * timestep;
FP halfTimeStepAngle = halfTimeStep * angle;
if (angle < FP.EN3)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
//TSVector.Multiply(ref body.angularVelocity, (halfTimeStep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis);
TSVector.Multiply(ref body.angularVelocity, halfTimeStep, out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
TSVector.Multiply(ref body.angularVelocity, (FP.Sin(halfTimeStepAngle) / angle), out axis);
}
TSQuaternion dorn = new TSQuaternion(axis.x, axis.y, axis.z, FP.Cos(halfTimeStepAngle));
TSQuaternion ornA;
TSQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
TSQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize();
TSMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
body.linearVelocity /= (1 + timestep * body.linearDrag);
body.angularVelocity /= (1 + timestep * body.angularDrag);
/*if ((body.Damping & RigidBody.DampingType.Linear) != 0)
* TSVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
*
* if ((body.Damping & RigidBody.DampingType.Angular) != 0)
* TSVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);*/
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
{
body.SweptExpandBoundingBox(timestep);
}
}
private void Integrate()
{
for (int index = 0, length = rigidBodies.Count; index < length; index++)
{
var body = rigidBodies[index];
if (body.isStatic || !body.IsActive)
{
continue;
}
body.position += body.linearVelocity * timestep;
if (!(body.isParticle))
{
//exponential map
TSVector axis;
FP angle = body.angularVelocity.magnitude;
if (angle < FP.EN3)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
TSVector.Multiply(body.angularVelocity, (FP.Half * timestep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
TSVector.Multiply(body.angularVelocity, (FP.Sin(FP.Half * angle * timestep) / angle), out axis);
}
TSQuaternion dorn = new TSQuaternion(axis.x, axis.y, axis.z, FP.Cos(angle * timestep * FP.Half));
TSQuaternion ornA; TSQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
TSQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize(); TSMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
body.linearVelocity *= 1 / (1 + timestep * body.linearDrag);
body.angularVelocity *= 1 / (1 + timestep * body.angularDrag);
/*if ((body.Damping & RigidBody.DampingType.Linear) != 0)
* TSVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
*
* if ((body.Damping & RigidBody.DampingType.Angular) != 0)
* TSVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);*/
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
{
body.SweptExpandBoundingBox(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));
}
