/// <summary>
/// Creates the transposed matrix.
/// </summary>
/// <param name="matrix">The matrix which should be transposed.</param>
/// <returns>The transposed JMatrix.</returns>
#region public static JMatrix Transpose(JMatrix matrix)
public static TSMatrix Transpose(TSMatrix matrix)
{
TSMatrix result;
TSMatrix.Transpose(ref matrix, 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>
/// Creates a new instance of the TransformedShape struct.
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="orientation">The orientation this shape should have.</param>
/// <param name="position">The position this shape should have.</param>
public TransformedShape(Shape shape, TSMatrix orientation, TSVector position)
{
this.position = position;
this.orientation = orientation;
TSMatrix.Transpose(ref orientation, out invOrientation);
this.shape = shape;
this.boundingBox = new TSBBox();
UpdateBoundingBox();
}
/// <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);
}
