/// <summary>
/// Multiply a matrix by a scalefactor.
/// </summary>
/// <param name="matrix1">The matrix.</param>
/// <param name="scaleFactor">The scale factor.</param>
/// <returns>A JMatrix multiplied by the scale factor.</returns>
#region public static JMatrix Multiply(JMatrix matrix1, FP scaleFactor)
public static TSMatrix Multiply(TSMatrix matrix1, FP scaleFactor)
{
TSMatrix result;
TSMatrix.Multiply(ref matrix1, scaleFactor, out result);
return(result);
}
/// <summary>
/// Subtracts two matrices.
/// </summary>
/// <param name="value1">The first matrix.</param>
/// <param name="value2">The second matrix.</param>
/// <returns>The difference of both values.</returns>
#region public static JMatrix operator -(JMatrix value1, JMatrix value2)
public static TSMatrix operator -(TSMatrix value1, TSMatrix value2)
{
TSMatrix result; TSMatrix.Multiply(ref value2, -FP.One, out value2);
TSMatrix.Add(ref value1, ref value2, out result);
return(result);
}
/// <summary>
/// Gets the determinant of the matrix.
/// </summary>
/// <returns>The determinant of the matrix.</returns>
#region public FP Determinant()
//public FP Determinant()
//{
// return M11 * M22 * M33 -M11 * M23 * M32 -M12 * M21 * M33 +M12 * M23 * M31 + M13 * M21 * M32 - M13 * M22 * M31;
//}
#endregion
/// <summary>
/// Multiply two matrices. Notice: matrix multiplication is not commutative.
/// </summary>
/// <param name="matrix1">The first matrix.</param>
/// <param name="matrix2">The second matrix.</param>
/// <returns>The product of both matrices.</returns>
#region public static JMatrix Multiply(JMatrix matrix1, JMatrix matrix2)
public static TSMatrix Multiply(TSMatrix matrix1, TSMatrix matrix2)
{
TSMatrix result;
TSMatrix.Multiply(ref matrix1, ref matrix2, out result);
return(result);
}
/// <summary>
/// Recalculates the axis aligned bounding box and the inertia
/// values in world space.
/// </summary>
public virtual void Update()
{
if (isParticle)
{
this.inertia = TSMatrix.Zero;
this.invInertia = this.invInertiaWorld = TSMatrix.Zero;
this.invOrientation = this.orientation = TSMatrix.Identity;
this.boundingBox = shape.boundingBox;
TSVector.Add(ref boundingBox.min, ref this.position, out boundingBox.min);
TSVector.Add(ref boundingBox.max, ref this.position, out boundingBox.max);
angularVelocity.MakeZero();
}
else
{
// Given: Orientation, Inertia
TSMatrix.Transpose(ref orientation, out invOrientation);
this.Shape.GetBoundingBox(ref orientation, out boundingBox);
TSVector.Add(ref boundingBox.min, ref this.position, out boundingBox.min);
TSVector.Add(ref boundingBox.max, ref this.position, out boundingBox.max);
if (!isStatic)
{
TSMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld);
TSMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld);
}
}
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
effectiveMass = body1.invInertiaWorld + body2.invInertiaWorld;
softnessOverDt = softness / timestep;
effectiveMass.M11 += softnessOverDt;
effectiveMass.M22 += softnessOverDt;
effectiveMass.M33 += softnessOverDt;
TSMatrix.Inverse(ref effectiveMass, out effectiveMass);
TSMatrix orientationDifference;
TSMatrix.Multiply(ref initialOrientation1, ref initialOrientation2, out orientationDifference);
TSMatrix.Transpose(ref orientationDifference, out orientationDifference);
TSMatrix q = orientationDifference * body2.invOrientation * body1.orientation;
TSVector axis;
FP x = q.M32 - q.M23;
FP y = q.M13 - q.M31;
FP z = q.M21 - q.M12;
FP r = TSMath.Sqrt(x * x + y * y + z * z);
FP t = q.M11 + q.M22 + q.M33;
FP angle = FP.Atan2(r, t - 1);
axis = new TSVector(x, y, z) * angle;
if (r != FP.Zero)
{
axis = axis * (FP.One / r);
}
bias = axis * biasFactor * (-FP.One / timestep);
// Apply previous frame solution as initial guess for satisfying the constraint.
if (!body1.IsStatic)
{
body1.angularVelocity += TSVector.Transform(accumulatedImpulse, body1.invInertiaWorld);
}
if (!body2.IsStatic)
{
body2.angularVelocity += TSVector.Transform(-FP.One * accumulatedImpulse, body2.invInertiaWorld);
}
}
/// <summary>
/// Calculates the inertia of the shape relative to the center of mass.
/// </summary>
/// <param name="shape"></param>
/// <param name="centerOfMass"></param>
/// <param name="inertia">Returns the inertia relative to the center of mass, not to the origin</param>
/// <returns></returns>
#region public static FP CalculateMassInertia(Shape shape, out JVector centerOfMass, out JMatrix inertia)
public static FP CalculateMassInertia(Shape shape, out TSVector centerOfMass,
out TSMatrix inertia)
{
FP mass = FP.Zero;
centerOfMass = TSVector.zero; inertia = TSMatrix.Zero;
if (shape is Multishape)
{
throw new ArgumentException("Can't calculate inertia of multishapes.", "shape");
}
// build a triangle hull around the shape
List <TSVector> hullTriangles = new List <TSVector>();
shape.MakeHull(ref hullTriangles, 3);
// create inertia of tetrahedron with vertices at
// (0,0,0) (1,0,0) (0,1,0) (0,0,1)
FP a = FP.One / (60 * FP.One), b = FP.One / (120 * FP.One);
TSMatrix C = new TSMatrix(a, b, b, b, a, b, b, b, a);
for (int i = 0; i < hullTriangles.Count; i += 3)
{
TSVector column0 = hullTriangles[i + 0];
TSVector column1 = hullTriangles[i + 1];
TSVector column2 = hullTriangles[i + 2];
TSMatrix A = new TSMatrix(column0.x, column1.x, column2.x,
column0.y, column1.y, column2.y,
column0.z, column1.z, column2.z);
FP detA = A.Determinant();
// now transform this canonical tetrahedron to the target tetrahedron
// inertia by a linear transformation A
TSMatrix tetrahedronInertia = TSMatrix.Multiply(A * C * TSMatrix.Transpose(A), detA);
TSVector tetrahedronCOM = (FP.One / (4 * FP.One)) * (hullTriangles[i + 0] + hullTriangles[i + 1] + hullTriangles[i + 2]);
FP tetrahedronMass = (FP.One / (6 * FP.One)) * detA;
inertia += tetrahedronInertia;
centerOfMass += tetrahedronMass * tetrahedronCOM;
mass += tetrahedronMass;
}
inertia = TSMatrix.Multiply(TSMatrix.Identity, inertia.Trace()) - inertia;
centerOfMass = centerOfMass * (FP.One / mass);
FP x = centerOfMass.x;
FP y = centerOfMass.y;
FP z = centerOfMass.z;
// now translate the inertia by the center of mass
TSMatrix t = new TSMatrix(
-mass * (y * y + z * z), mass * x * y, mass * x * z,
mass * y * x, -mass * (z * z + x * x), mass * y * z,
mass * z * x, mass * z * y, -mass * (x * x + y * y));
TSMatrix.Add(ref inertia, ref t, out inertia);
return(mass);
}
/// <summary>
/// Multiplies two matrices.
/// </summary>
/// <param name="value1">The first matrix.</param>
/// <param name="value2">The second matrix.</param>
/// <returns>The product of both values.</returns>
#region public static JMatrix operator *(JMatrix value1,JMatrix value2)
public static TSMatrix operator *(TSMatrix value1, TSMatrix value2)
{
TSMatrix result; TSMatrix.Multiply(ref value1, ref value2, out result);
return(result);
}
private void UpdateInvInertiaWorld()
{
TSMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld);
TSMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld);
}