public override void CalculateMassInertia()
{
base.inertia = Matrix3.Zero;
base.mass = 0.0f;
for (int i = 0; i < Shapes.Length; i++)
{
Matrix3 currentInertia = Shapes[i].InverseOrientation * Shapes[i].Shape.Inertia * Shapes[i].Orientation;
Vector3 p = Shapes[i].Position * -1.0f;
float m = Shapes[i].Shape.Mass;
currentInertia.M11 += m * (p.Y * p.Y + p.Z * p.Z);
currentInertia.M22 += m * (p.X * p.X + p.Z * p.Z);
currentInertia.M33 += m * (p.X * p.X + p.Y * p.Y);
currentInertia.M12 += -p.X * p.Y * m;
currentInertia.M21 += -p.X * p.Y * m;
currentInertia.M31 += -p.X * p.Z * m;
currentInertia.M13 += -p.X * p.Z * m;
currentInertia.M32 += -p.Y * p.Z * m;
currentInertia.M23 += -p.Y * p.Z * m;
//base.inertia += currentInertia;
Matrix3.Add(ref base.inertia, ref currentInertia, out base.inertia);
base.mass += m;
}
}
public static void add()
{
var a = new Matrix3(_a);
var b = new Matrix3(_b);
var sum = a.Add(b);
Assert.Equal(_sum, sum);
Assert.Equal(_a, a);
Assert.Equal(_b, b);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
effectiveMass = Matrix3.Add(body1.invInertiaWorld, body2.invInertiaWorld);
softnessOverDt = softness / timestep;
effectiveMass.M11 += softnessOverDt;
effectiveMass.M22 += softnessOverDt;
effectiveMass.M33 += softnessOverDt;
Matrix3.Invert(ref effectiveMass, out effectiveMass);
Matrix3 orientationDifference;
Matrix3.Mult(ref initialOrientation1, ref initialOrientation2, out orientationDifference);
Matrix3.Transpose(ref orientationDifference, out orientationDifference);
Matrix3 q = orientationDifference * body2.invOrientation * body1.orientation;
Vector3 axis;
float x = q.M32 - q.M23;
float y = q.M13 - q.M31;
float z = q.M21 - q.M12;
float r = JMath.Sqrt(x * x + y * y + z * z);
float t = q.M11 + q.M22 + q.M33;
float angle = (float)Math.Atan2(r, t - 1);
axis = new Vector3(x, y, z) * angle;
if (r != 0.0f)
{
axis = axis * (1.0f / r);
}
bias = axis * biasFactor * (-1.0f / timestep);
// Apply previous frame solution as initial guess for satisfying the constraint.
if (!body1.IsStatic)
{
body1.angularVelocity += Vector3.Transform(accumulatedImpulse, body1.invInertiaWorld);
}
if (!body2.IsStatic)
{
body2.angularVelocity += Vector3.Transform(-1.0f * accumulatedImpulse, body2.invInertiaWorld);
}
}
public void Random_NByN_Symetric(int n)
{
Rectangular rand = new Rectangular();
Matrix3 A = rand.Randomfloat(n, n);
A = A.Add(Matrix3.Transpose(A));
/// Any matrix added to its transpose will be symetric
Assert.That(StandardMatrixTests.IsSymetric(A), Is.True);
EigenvalueDecomposition EofA = new EigenvalueDecomposition(A);
Matrix3 V = EofA.V;
// V is orthogonal V times V transpose is the identity
Assert.That(Matrix3.Mult(V, Matrix3.Transpose(V)).ToFloatArray(), Is.EqualTo(Matrix3.Identity.ToFloatArray()).Within(.0000001));
Matrix3 D = EofA.getD();
Matrix3 test = Matrix3.Mult(D, Matrix3.Transpose(V));
test = Matrix3.Mult(V, test);
Assert.That(test.ToFloatArray(), Is.EqualTo(A.ToFloatArray()).Within(.0000001).Percent);
}
/// <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 float CalculateMassInertia(Shape shape, out JVector centerOfMass, out JMatrix inertia)
public static float CalculateMassInertia(Shape shape, out Vector3 centerOfMass,
out Matrix3 inertia)
{
float mass = 0.0f;
centerOfMass = Vector3.Zero; inertia = Matrix3.Zero;
if (shape is Multishape)
{
throw new ArgumentException("Can't calculate inertia of multishapes.", "shape");
}
// build a triangle hull around the shape
List <Vector3> hullTriangles = new List <Vector3>();
shape.MakeHull(ref hullTriangles, 3);
// create inertia of tetrahedron with vertices at
// (0,0,0) (1,0,0) (0,1,0) (0,0,1)
float a = 1.0f / 60.0f, b = 1.0f / 120.0f;
Matrix3 C = new Matrix3(a, b, b, b, a, b, b, b, a);
for (int i = 0; i < hullTriangles.Count; i += 3)
{
Vector3 column0 = hullTriangles[i + 0];
Vector3 column1 = hullTriangles[i + 1];
Vector3 column2 = hullTriangles[i + 2];
Matrix3 A = new Matrix3(column0.X, column1.X, column2.X,
column0.Y, column1.Y, column2.Y,
column0.Z, column1.Z, column2.Z);
float detA = A.Determinant;
// now transform this canonical tetrahedron to the target tetrahedron
// inertia by a linear transformation A
var acta = A * C * Matrix3.Transpose(A);
Matrix3 tetrahedronInertia; Matrix3.Mult(ref acta, detA, out tetrahedronInertia);
Vector3 tetrahedronCOM = (1.0f / 4.0f) * (hullTriangles[i + 0] + hullTriangles[i + 1] + hullTriangles[i + 2]);
float tetrahedronMass = (1.0f / 6.0f) * detA;
inertia = Matrix3.Add(inertia, tetrahedronInertia);
centerOfMass += tetrahedronMass * tetrahedronCOM;
mass += tetrahedronMass;
}
//
Matrix3 tmpIn;
Matrix3.Mult(ref Matrix3.Identity, inertia.Trace, out tmpIn);
inertia = Matrix3.Sub(tmpIn, inertia);
//inertia = Matrix3.Multiply(Matrix3.Identity, inertia.Trace) - inertia;
centerOfMass = centerOfMass * (1.0f / mass);
float x = centerOfMass.X;
float y = centerOfMass.Y;
float z = centerOfMass.Z;
// now translate the inertia by the center of mass
Matrix3 t = new Matrix3(
-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));
Matrix3.Add(ref inertia, ref t, out inertia);
return(mass);
}
public static void add() {
var a = new Matrix3(_a);
var b = new Matrix3(_b);
var sum = a.Add(b);
Assert.Equal(_sum, sum);
Assert.Equal(_a, a);
Assert.Equal(_b, b);
}