public RegularGridBuilder(Vector3 spacing, Vector3 origin, BodyInertia localInertia, TypedIndex shapeIndex = new TypedIndex())
{
Spacing = spacing;
Origin = origin;
LocalInertia = localInertia;
ShapeIndex = shapeIndex;
}
public override void ProcessBlock(float deltaTime, BlockAccessor block)
{
var entities = block.GetEntityData();
var collider = block.GetSharedComponentData <MeshCollider>();
var hasRigidBody = block.TryGetComponentData(out Span <RigidBody> rbs);
var hasScale = block.TryGetComponentData(out Span <Scale> scales);
Vector3 defaultScale = Vector3.One;
for (int i = 0; i < block.length; i++)
{
Vector3 scale = defaultScale;
if (hasScale)
{
scale = new Vector3(scales[i].value.x, scales[i].value.y, scales[i].value.z);
}
Mesh mesh = new Mesh(collider.GetTriangles(), scale, PhysicsSystem.BufferPool);
TypedIndex shapeIdx = PhysicsSystem.Simulation.Shapes.Add(mesh);
BodyInertia inertia = new BodyInertia();
if (hasRigidBody)
{
mesh.ComputeOpenInertia(rbs[i].mass, out inertia);
}
afterUpdateCommands.AddComponent(entities[i],
new InternalColliderHandle()
{
inertia = inertia,
shapeIdx = shapeIdx
});
}
}
public override void ProcessBlock(float deltaTime, BlockAccessor block)
{
var entities = block.GetEntityData();
var colliders = block.GetReadOnlyComponentData <BoxCollider>();
var hasRigidBody = block.TryGetComponentData(out Span <RigidBody> rbs);
var hasScale = block.TryGetComponentData(out Span <Scale> scales);
vec3 defaultScale = vec3.Ones;
for (int i = 0; i < block.length; i++)
{
vec3 scale = hasScale ? scales[i].value : defaultScale;
Box box = new Box(colliders[i].width * scale.x, colliders[i].height * scale.y, colliders[i].length * scale.z);
TypedIndex shapeIdx = PhysicsSystem.Simulation.Shapes.Add(box);
BodyInertia inertia = new BodyInertia();
if (hasRigidBody)
{
box.ComputeInertia(rbs[i].mass, out inertia);
}
afterUpdateCommands.AddComponent(entities[i],
new InternalColliderHandle()
{
inertia = inertia,
shapeIdx = shapeIdx
});
}
}
protected override void SetBody(TypedIndex type, float speculativeMargin, BodyInertia inertia, Vector3 offset)
{
var physicsSystem = GameObject.CurrentScene.GetOrCreateSystem <PhysicsSystem>();
if (_voxelStatic.Exists)
{
physicsSystem.RemoveStatic(_voxelStatic);
}
var transformedOffset = Vector3.Transform(offset, GameObject.Transform.WorldOrientation);
_voxelStatic = physicsSystem.AddStatic(new StaticDescription(GameObject.Transform.WorldPosition + transformedOffset, new CollidableDescription(type, speculativeMargin)), this);
}
protected override void OnAttach()
{
base.OnAttach();
rigidBody = Entity.Get <RigidbodyComponent>();
BodyInertia interia = new BodyInertia {
InverseMass = 1f / Mass
};
rigidBody.GetBodyReference().SetLocalInertia(interia);
ref var character = ref Simulation.AllocateCharacter(rigidBody.BodyHandle);
public void ComputeAnalyticInertia(float mass, out BodyInertia inertia)
{
Box.ComputeInertia(mass, out inertia);
}
public override void GetShapeInteria(float mass, out BodyInertia inertia)
{
((Cylinder)InternalShape).ComputeInertia(mass, out inertia);
}
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(25, 4, 40);
camera.Yaw = 0;
Simulation = Simulation.Create(BufferPool, new TestCallbacks());
Simulation.PoseIntegrator.Gravity = new Vector3(0, -10, 0);
var shapeA = new Box(.75f, 1, .5f);
var shapeIndexA = Simulation.Shapes.Add(shapeA);
var collidableA = new CollidableDescription(shapeIndexA, 0.1f);
var shapeB = new Box(.75f, 1, .5f);
var shapeIndexB = Simulation.Shapes.Add(shapeB);
var collidableB = new CollidableDescription(shapeIndexB, 0.1f);
var activity = new BodyActivityDescription(0.01f);
shapeA.ComputeInertia(1, out var inertiaA);
shapeA.ComputeInertia(1, out var inertiaB);
var nextX = -10f;
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new AngularHinge {
LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new Hinge
{
LocalOffsetA = new Vector3(0, 1, 0),
LocalHingeAxisA = new Vector3(0, 1, 0),
LocalOffsetB = new Vector3(0, -1, 0),
LocalHingeAxisB = new Vector3(0, 1, 0),
SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new AngularSwivelHinge {
LocalSwivelAxisA = new Vector3(1, 0, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new SwingLimit {
AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new SwivelHinge
{
LocalOffsetA = new Vector3(0, 1, 0),
LocalSwivelAxisA = new Vector3(1, 0, 0),
LocalOffsetB = new Vector3(0, -1, 0),
LocalHingeAxisB = new Vector3(0, 1, 0),
SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new SwingLimit {
AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new TwistServo
{
LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)),
LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)),
TargetAngle = MathHelper.PiOver4,
SpringSettings = new SpringSettings(30, 1),
ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new TwistLimit
{
LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)),
LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)),
MinimumAngle = MathHelper.Pi * -0.5f,
MaximumAngle = MathHelper.Pi * 0.95f,
SpringSettings = new SpringSettings(30, 1),
});
Simulation.Solver.Add(a, b, new AngularHinge {
LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new TwistMotor
{
LocalAxisA = new Vector3(0, 1, 0),
LocalAxisB = new Vector3(0, 1, 0),
TargetVelocity = MathHelper.Pi * 2,
Settings = new MotorSettings(float.MaxValue, 0.1f)
});
Simulation.Solver.Add(a, b, new AngularHinge {
LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new AngularServo
{
TargetRelativeRotationLocalA = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2),
ServoSettings = new ServoSettings(float.MaxValue, 0, 12f),
SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new AngularMotor {
TargetVelocityLocalA = new Vector3(0, 1, 0), Settings = new MotorSettings(15, 0.0001f)
});
}
{
var x = GetNextPosition(ref nextX);
var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity);
var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity);
//aDescription.Velocity.Angular = new Vector3(0, 0, 5);
var a = Simulation.Bodies.Add(aDescription);
var b = Simulation.Bodies.Add(bDescription);
Simulation.Solver.Add(a, b, new Weld {
LocalOffset = new Vector3(0, 2, 0), LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var sphere = new Sphere(0.125f);
//Treat each vertex as a point mass that cannot rotate.
var sphereInertia = new BodyInertia {
InverseMass = 1
};
var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f);
var a = new Vector3(x, 3, 0);
var b = new Vector3(x, 4, 0);
var c = new Vector3(x, 3, 1);
var d = new Vector3(x + 1, 3, 0);
var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity);
var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity);
var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity);
var dDescription = BodyDescription.CreateDynamic(d, sphereInertia, sphereCollidable, activity);
var aHandle = Simulation.Bodies.Add(aDescription);
var bHandle = Simulation.Bodies.Add(bDescription);
var cHandle = Simulation.Bodies.Add(cDescription);
var dHandle = Simulation.Bodies.Add(dDescription);
var distanceSpringiness = new SpringSettings(3f, 1);
Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness));
Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness));
Simulation.Solver.Add(aHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(a, d), distanceSpringiness));
Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness));
Simulation.Solver.Add(bHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(b, d), distanceSpringiness));
Simulation.Solver.Add(cHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(c, d), distanceSpringiness));
Simulation.Solver.Add(aHandle, bHandle, cHandle, dHandle, new VolumeConstraint(a, b, c, d, new SpringSettings(30, 1)));
}
{
var x = GetNextPosition(ref nextX);
var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity);
var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity);
var a = Simulation.Bodies.Add(aDescription);
var b = Simulation.Bodies.Add(bDescription);
Simulation.Solver.Add(a, b, new DistanceServo(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1.9f, new SpringSettings(30, 1), ServoSettings.Default));
}
{
var x = GetNextPosition(ref nextX);
var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity);
var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity);
var a = Simulation.Bodies.Add(aDescription);
var b = Simulation.Bodies.Add(bDescription);
Simulation.Solver.Add(a, b, new DistanceLimit(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1f, 3, new SpringSettings(30, 1)));
}
{
var x = GetNextPosition(ref nextX);
var sphere = new Sphere(0.125f);
//Treat each vertex as a point mass that cannot rotate.
var sphereInertia = new BodyInertia {
InverseMass = 1
};
var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f);
var a = new Vector3(x, 3, 0);
var b = new Vector3(x, 4, 0);
var c = new Vector3(x + 1, 3, 0);
var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity);
var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity);
var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity);
var aHandle = Simulation.Bodies.Add(aDescription);
var bHandle = Simulation.Bodies.Add(bDescription);
var cHandle = Simulation.Bodies.Add(cDescription);
var distanceSpringiness = new SpringSettings(3f, 1);
Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness));
Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness));
Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness));
Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a, b, c, new SpringSettings(30, 1)));
}
{
var x = GetNextPosition(ref nextX);
var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), default, collidableA, activity);
internal CompoundPhysicsMesh(ICompoundShape shape, TypedIndex meshIndex, BodyInertia inertia)
{
this.CompoundShape = shape;
this.MeshIndex = meshIndex;
this.Inertia = inertia;
}
public abstract void GetShapeInteria(float mass, out BodyInertia inertia);
private PhyObject CreateVanilla(ObjectState state, CollidableDescription collidableDescription, BodyInertia bodyInertia)
{
PhyObject phy;
if (state.mass != 0)
{
BodyDescription boxDescription = BodyDescription.CreateDynamic(state.position, bodyInertia,
collidableDescription,
new BodyActivityDescription(0.01f));
boxDescription.Pose = new RigidPose(state.position, state.quaternion);
var bodyHandle = Simulation.Bodies.Add(boxDescription);
phy = SetUpPhyObject(bodyHandle, state);
objectsHandlers.Add(bodyHandle, phy);
}
else
{
StaticDescription description = new StaticDescription(state.position, state.quaternion, collidableDescription);
StaticHandle handle = Simulation.Statics.Add(description);
//collidableMaterials.Allocate(handle) = new SimpleMaterial { FrictionCoefficient = 1, MaximumRecoveryVelocity = float.MaxValue, SpringSettings = new SpringSettings(1f, 1f) };
phy = SetUpPhyObject(handle, state);
staticObjectsHandlers.Add(handle, (StaticPhyObject)phy);
//objectsHandlers.Add(handle,phy);
}
return(phy);
}
public override void GetShapeInteria(float mass, out BodyInertia inertia)
{
((Mesh)InternalShape).ComputeClosedInertia(mass, out inertia);
}
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);
}
