private BodyDescription CreateBoxDescription(float width, float height, float lenght)
{
var boxShape = new Box(width, height, lenght);
var boxShapeIndex = Simulation.Shapes.Add(boxShape);
boxShape.ComputeInertia(1, out var boxInertia);
Symmetric3x3.Scale(boxInertia.InverseInertiaTensor, .5f, out boxInertia.InverseInertiaTensor);
var boxDescription = new BodyDescription()
{
LocalInertia = boxInertia,
Pose = new RigidPose
{
Position = Vector3.Zero,
Orientation = Quaternion.Identity
},
Activity = new BodyActivityDescription {
MinimumTimestepCountUnderThreshold = 32, SleepThreshold = .01f
},
Collidable = new CollidableDescription {
Shape = boxShapeIndex, SpeculativeMargin = .1f
},
};
return(boxDescription);
}
public override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(-30, 8, -60);
camera.Yaw = MathHelper.Pi * 3f / 4;
camera.Pitch = 0;
_poseIntegrator = new DefaultPoseIntegratorCallbacks(BufferPool);
Simulation = Simulation.Create(BufferPool, new DefaultNarrowPhaseCallbacks(), _poseIntegrator);
var boxShape = new Box(1, 1, 1);
var baseShape = new Box(5, 1, 5);
boxShape.ComputeInertia(1000, out var boxInertia);
var boxShapeIndex = Simulation.Shapes.Add(boxShape);
var baseShapeIndex = Simulation.Shapes.Add(baseShape);
Symmetric3x3.Scale(boxInertia.InverseInertiaTensor, .5f, out boxInertia.InverseInertiaTensor);
var boxDescription = new BodyDescription
{
//Make the uppermost block kinematic to hold up the rest of the chain.
LocalInertia = boxInertia,
Pose = new RigidPose
{
Position = new Vector3(0, -2, 0),
Orientation = Quaternion.Identity
},
Activity = new BodyActivityDescription {
MinimumTimestepCountUnderThreshold = 32, SleepThreshold = .01f
},
Collidable = new CollidableDescription {
Shape = boxShapeIndex, SpeculativeMargin = .1f
},
};
var baseDescription = new BodyDescription
{
//Make the uppermost block kinematic to hold up the rest of the chain.
LocalInertia = new BodyInertia(),
Pose = new RigidPose
{
Position = new Vector3(0, 0, 0),
Orientation = Quaternion.Identity
},
Activity = new BodyActivityDescription {
MinimumTimestepCountUnderThreshold = 32, SleepThreshold = .01f
},
Collidable = new CollidableDescription {
Shape = baseShapeIndex, SpeculativeMargin = .1f
},
};
var boxIndex = Simulation.Bodies.Add(boxDescription);
var baseIndex = Simulation.Bodies.Add(baseDescription);
_boxReference = new BodyReference(boxIndex, Simulation.Bodies);
_baseReference = new BodyReference(baseIndex, Simulation.Bodies);
}
public static void Test()
{
var random = new Random(4);
var timer = new Stopwatch();
var symmetricVectorSandwichTime = 0.0;
var symmetricWideVectorSandwichTime = 0.0;
var triangularWideVectorSandwichTime = 0.0;
var symmetricWide2x3SandwichTime = 0.0;
var triangularWide2x3SandwichTime = 0.0;
var symmetricSkewSandwichTime = 0.0;
var symmetricWideSkewSandwichTime = 0.0;
var triangularWideSkewSandwichTime = 0.0;
var symmetricRotationSandwichTime = 0.0;
var symmetricWideRotationSandwichTime = 0.0;
var triangularWideRotationSandwichTime = 0.0;
var symmetricInvertTime = 0.0;
var symmetricWideInvertTime = 0.0;
var triangularWideInvertTime = 0.0;
for (int i = 0; i < 1000; ++i)
{
var axis = Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1));
Vector3Wide.Broadcast(axis, out var axisWide);
var rotation = Matrix3x3.CreateFromAxisAngle(axis, (float)random.NextDouble());
Matrix3x3Wide rotationWide;
Vector3Wide.Broadcast(rotation.X, out rotationWide.X);
Vector3Wide.Broadcast(rotation.Y, out rotationWide.Y);
Vector3Wide.Broadcast(rotation.Z, out rotationWide.Z);
var m2x3Wide = new Matrix2x3Wide()
{
X = axisWide, Y = new Vector3Wide {
X = -axisWide.Y, Y = axisWide.Z, Z = axisWide.X
}
};
var triangular = new Symmetric3x3
{
XX = (float)random.NextDouble() * 2 + 1,
YX = (float)random.NextDouble() * 1 + 1,
YY = (float)random.NextDouble() * 2 + 1,
ZX = (float)random.NextDouble() * 1 + 1,
ZY = (float)random.NextDouble() * 1 + 1,
ZZ = (float)random.NextDouble() * 2 + 1,
};
Symmetric3x3Wide triangularWide;
triangularWide.XX = new Vector <float>(triangular.XX);
triangularWide.YX = new Vector <float>(triangular.YX);
triangularWide.YY = new Vector <float>(triangular.YY);
triangularWide.ZX = new Vector <float>(triangular.ZX);
triangularWide.ZY = new Vector <float>(triangular.ZY);
triangularWide.ZZ = new Vector <float>(triangular.ZZ);
var symmetric = new Matrix3x3
{
X = new Vector3(triangular.XX, triangular.YX, triangular.ZX),
Y = new Vector3(triangular.YX, triangular.YY, triangular.ZY),
Z = new Vector3(triangular.ZX, triangular.ZY, triangular.ZZ),
};
Matrix3x3Wide symmetricWide;
Vector3Wide.Broadcast(symmetric.X, out symmetricWide.X);
Vector3Wide.Broadcast(symmetric.Y, out symmetricWide.Y);
Vector3Wide.Broadcast(symmetric.Z, out symmetricWide.Z);
var symmetricVectorSandwich = new SymmetricVectorSandwich()
{
v = axis, symmetric = symmetric
};
var symmetricWideVectorSandwich = new SymmetricWideVectorSandwich()
{
v = axisWide, symmetric = symmetricWide
};
var triangularWideVectorSandwich = new TriangularWideVectorSandwich()
{
v = axisWide, triangular = triangularWide
};
var symmetricWide2x3Sandwich = new SymmetricWide2x3Sandwich()
{
m = m2x3Wide, symmetric = symmetricWide
};
var triangularWide2x3Sandwich = new TriangularWide2x3Sandwich()
{
m = m2x3Wide, triangular = triangularWide
};
var symmetricSkewSandwich = new SymmetricSkewSandwich()
{
v = axis, symmetric = symmetric
};
var symmetricWideSkewSandwich = new SymmetricWideSkewSandwich()
{
v = axisWide, symmetric = symmetricWide
};
var triangularWideSkewSandwich = new TriangularWideSkewSandwich()
{
v = axisWide, triangular = triangularWide
};
var symmetricSandwich = new SymmetricRotationSandwich()
{
rotation = rotation, symmetric = symmetric
};
var symmetricWideSandwich = new SymmetricRotationSandwichWide()
{
rotation = rotationWide, symmetric = symmetricWide
};
var triangularWideSandwich = new TriangularRotationSandwichWide()
{
rotation = rotationWide, triangular = triangularWide
};
var symmetricInvert = new SymmetricInvert()
{
symmetric = symmetric
};
var symmetricWideInvert = new SymmetricInvertWide()
{
symmetric = symmetricWide
};
var triangularWideInvert = new TriangularInvertWide()
{
triangular = triangularWide
};
const int innerIterations = 100000;
symmetricVectorSandwichTime += TimeTest(innerIterations, ref symmetricVectorSandwich);
symmetricWideVectorSandwichTime += TimeTest(innerIterations, ref symmetricWideVectorSandwich);
triangularWideVectorSandwichTime += TimeTest(innerIterations, ref triangularWideVectorSandwich);
symmetricWide2x3SandwichTime += TimeTest(innerIterations, ref symmetricWide2x3Sandwich);
triangularWide2x3SandwichTime += TimeTest(innerIterations, ref triangularWide2x3Sandwich);
symmetricSkewSandwichTime += TimeTest(innerIterations, ref symmetricSkewSandwich);
symmetricWideSkewSandwichTime += TimeTest(innerIterations, ref symmetricWideSkewSandwich);
triangularWideSkewSandwichTime += TimeTest(innerIterations, ref triangularWideSkewSandwich);
symmetricRotationSandwichTime += TimeTest(innerIterations, ref symmetricSandwich);
symmetricWideRotationSandwichTime += TimeTest(innerIterations, ref symmetricWideSandwich);
triangularWideRotationSandwichTime += TimeTest(innerIterations, ref triangularWideSandwich);
symmetricInvertTime += TimeTest(innerIterations, ref symmetricInvert);
symmetricWideInvertTime += TimeTest(innerIterations, ref symmetricWideInvert);
triangularWideInvertTime += TimeTest(innerIterations, ref triangularWideInvert);
Compare(symmetricVectorSandwich.result, ref symmetricWideVectorSandwich.result);
Compare(symmetricVectorSandwich.result, ref triangularWideVectorSandwich.result);
Compare(ref symmetricWide2x3Sandwich.result, ref triangularWide2x3Sandwich.result);
Compare(ref symmetricSkewSandwich.result, ref symmetricWideSkewSandwich.result);
Compare(ref symmetricSkewSandwich.result, ref triangularWideSkewSandwich.result);
Compare(ref symmetricSandwich.result, ref symmetricWideSandwich.result);
Compare(ref symmetricSandwich.result, ref triangularWideSandwich.result);
Compare(ref symmetricInvert.result, ref symmetricWideInvert.result);
Compare(ref symmetricInvert.result, ref triangularWideInvert.result);
}
Console.WriteLine($"Symmetric vector sandwich: {symmetricVectorSandwichTime}");
Console.WriteLine($"Symmetric wide vector sandwich: {symmetricWideVectorSandwichTime}");
Console.WriteLine($"Triangular wide vector sandwich: {triangularWideVectorSandwichTime}");
Console.WriteLine($"Symmetric wide 2x3 sandwich: {symmetricWide2x3SandwichTime}");
Console.WriteLine($"Triangular wide 2x3 sandwich: {triangularWide2x3SandwichTime}");
Console.WriteLine($"Symmetric skew sandwich: {symmetricSkewSandwichTime}");
Console.WriteLine($"Symmetric wide skew sandwich: {symmetricWideSkewSandwichTime}");
Console.WriteLine($"Triangular wide skew sandwich: {triangularWideSkewSandwichTime}");
Console.WriteLine($"Symmetric rotation sandwich: {symmetricRotationSandwichTime}");
Console.WriteLine($"Symmetric wide rotation sandwich: {symmetricWideRotationSandwichTime}");
Console.WriteLine($"Triangular wide rotation sandwich: {triangularWideRotationSandwichTime}");
Console.WriteLine($"Symmetric invert: {symmetricInvertTime}");
Console.WriteLine($"Symmetric wide invert: {symmetricWideInvertTime}");
Console.WriteLine($"Triangular wide invert: {triangularWideInvertTime}");
}
public void ComputeAnalyticInertia(float mass, out BodyInertia inertia)
{
//Computing the inertia of a tetrahedron requires integrating across its volume.
//While it's possible to do so directly given arbitrary plane equations, it's more convenient to integrate over a normalized tetrahedron with coordinates
//at (0,0,0), (1,0,0), (0,1,0), and (0,0,1). The integration location can be transformed back to the original frame of reference using the tetrahedral edges.
//That is, (1,0,0) in normalized space transforms to B-A in world space.
//To make that explicit, we have an equation:
// [1,0,0] [ B - A ]
// [0,1,0] * Transform = [ C - A ]
// [0,0,1] [ D - A ]
//(Note that you could consider this to be an affine transform with a translation equal to A.)
//Since the normalized edge directions compose the identity matrix, the transform is just the edge directions.
//So, given function f that computes a point's contribution to the inertia tensor, the inertia tensor of the normalized tetrahedron with uniform unit density is:
//Integrate[Integrate[Integrate[f[{i, j, k}], {k, 0, 1-i-j}], {j, 0, 1-i}], {i, 0, 1}];
//Note the integration bounds- they are a result of the simple shape of the normalized tetrahedron making the plane equations easier to deal with.
//Now, to integrate over the true tetrahedron's shape, the normalized coordinates are transformed to world coordinates:
//Integrate[Integrate[Integrate[f[{i, j, k}.Transform + A] * Abs[Det[Transform]], {k, 0, 1-i-j}], {j, 0, 1-i}], {i, 0, 1}];
//One key difference is the inclusion of the transform's jacobian's determinant, which in this case is just the volume of the tetrahedron times six.
//For a geometric intuition for why that exists, consider that the normalized integration covers a tetrahedron with a volume of 1/6. The world space tetrahedron
//has a volume of Abs[Det[Transform]]/6. So, the volume changes by a factor of exactly Abs[Det[Transform]].
//That term compensates for the difference in integration domain.
//It's also constant over the integration, so you can just pull it out.
//Similarly, if you had a non-unit uniform density, you would multiply the integration by it too.
//So, putting that together and assuming the scaling term is pulled out, here's a chunk of code you can plop into wolfram cloud and whatnot to recreate the results:
//f[{x_, y_, z_}] := {{y^2 + z^2, -x * y, -x * z}, {-x * y, x^2 + z^2, -y * z}, {-x * z, -y * z, x^2 + y^2}}
//a = { AX, AY, AZ};
//b = { BX, BY, BZ};
//c = { CX, CY, CZ};
//d = { DX, DY, DZ};
//ab = b - a;
//ac = c - a;
//ad = d - a;
//A = { ab, ac, ad};
//Integrate[Integrate[Integrate[f[{ i, j, k}.A + a], {k, 0, 1-i-j}], {j, 0, 1-i}],{i, 0, 1}]
inertia.InverseMass = 1f / mass;
var ab = B - A;
var ac = C - A;
var ad = D - A;
//Revisiting the determinant, note that:
//density * abs(determinant) = density * volume * 6 = mass * 6
//So there's no need to actually compute the determinant/volume since we were given the mass directly.
var diagonalScaling = mass * (6f / 60f);
Symmetric3x3 inertiaTensor;
inertiaTensor.XX = diagonalScaling * (
A.Y * A.Y + A.Z * A.Z + B.Y * B.Y + B.Z * B.Z + C.Y * C.Y + C.Z * C.Z + D.Y * D.Y + D.Z * D.Z +
B.Y * C.Y + B.Z * C.Z +
(B.Y + C.Y) * D.Y + (B.Z + C.Z) * D.Z +
A.Y * (B.Y + C.Y + D.Y) + A.Z * (B.Z + C.Z + D.Z));
inertiaTensor.YY = diagonalScaling * (
A.X * A.X + A.Z * A.Z + B.X * B.X + B.Z * B.Z + C.X * C.X + C.Z * C.Z + D.X * D.X + D.Z * D.Z +
B.X * C.X + B.Z * C.Z +
(B.X + C.X) * D.X + (B.Z + C.Z) * D.Z +
A.X * (B.X + C.X + D.X) + A.Z * (B.Z + C.Z + D.Z));
inertiaTensor.ZZ = diagonalScaling * (
A.X * A.X + A.Y * A.Y + B.X * B.X + B.Y * B.Y + C.X * C.X + C.Y * C.Y + D.X * D.X + D.Y * D.Y +
B.X * C.X + B.Y * C.Y +
(B.X + C.X) * D.X + (B.Y + C.Y) * D.Y +
A.X * (B.X + C.X + D.X) + A.Y * (B.Y + C.Y + D.Y));
var offScaling = mass * (6f / 120f);
inertiaTensor.YX = offScaling * (
-2 * B.X * B.Y - 2 * C.X * C.Y -
B.Y * C.X - B.X * C.Y - B.Y * D.X - C.Y * D.X -
A.Y * (B.X + C.X + D.X) - (B.X + C.X + 2 * D.X) * D.Y - A.X * (2 * A.Y + B.Y + C.Y + D.Y));
inertiaTensor.ZX = offScaling * (
-2 * B.X * B.Z - 2 * C.X * C.Z -
B.Z * C.X - B.X * C.Z - B.Z * D.X - C.Z * D.X -
A.Z * (B.X + C.X + D.X) - (B.X + C.X + 2 * D.X) * D.Z - A.X * (2 * A.Z + B.Z + C.Z + D.Z));
inertiaTensor.ZY = offScaling * (
-2 * B.Y * B.Z - 2 * C.Y * C.Z -
B.Z * C.Y - B.Y * C.Z - B.Z * D.Y - C.Z * D.Y -
A.Z * (B.Y + C.Y + D.Y) - (B.Y + C.Y + 2 * D.Y) * D.Z - A.Y * (2 * A.Z + B.Z + C.Z + D.Z));
//TODO: Note that the above implementation isn't exactly optimal. Assuming for now that the performance isn't going to be relevant.
//That could change given certain convex hull use cases, but in that situation you should probably just jump to vectorizing over multiple tetrahedra at a time.
//(Plus some basic term caching.)
Symmetric3x3.Invert(inertiaTensor, out inertia.InverseInertiaTensor);
}
