static void TryConnectTo(int sliceIndex, int rowIndex, int columnIndex,
ref BodyDescription bodyDescription,
ref LatticeBodyGetter ids,
ref ConstraintAdder constraintAdder)
{
if (ids.GetBody(columnIndex, rowIndex, sliceIndex, out var otherHandle, out var otherDescription) &&
(bodyDescription.LocalInertia.InverseMass > 0 || otherDescription.LocalInertia.InverseMass > 0))
{
CreateBallSocket(ref bodyDescription.Pose, ref otherDescription.Pose, out var description);
constraintAdder.Add(ref description, otherHandle);
}
}
public static void BuildLattice <TBodyBuilder, TConstraintBuilder>(TBodyBuilder bodyBuilder, TConstraintBuilder constraintBuilder, int width, int height, int length, Simulation simulation,
out int[] bodyHandles, out int[] constraintHandles) where TBodyBuilder : IBodyBuilder where TConstraintBuilder : IConstraintBuilder
{
var bodyCount = width * height * length;
bodyHandles = new int[bodyCount];
var bodyGetter = new LatticeBodyGetter(width, height, length, bodyHandles, simulation.Bodies);
for (int sliceIndex = 0; sliceIndex < length; ++sliceIndex)
{
for (int rowIndex = 0; rowIndex < height; ++rowIndex)
{
for (int columnIndex = 0; columnIndex < width; ++columnIndex)
{
bodyBuilder.Build(columnIndex, rowIndex, sliceIndex, out var bodyDescription);
bodyGetter.TryGetId(columnIndex, rowIndex, sliceIndex, out var id);
bodyHandles[id] = simulation.Bodies.Add(bodyDescription);
}
}
}
var constraintAdder = new ConstraintAdder(simulation, new List <int>(width * height * length * 3));
for (int sliceIndex = 0; sliceIndex < length; ++sliceIndex)
{
//The bottom rows are all kinematic, so don't create connections between them.
for (int rowIndex = 0; rowIndex < height; ++rowIndex)
{
for (int columnIndex = 0; columnIndex < width; ++columnIndex)
{
bodyGetter.GetBody(columnIndex, rowIndex, sliceIndex, out constraintAdder.LocalBodyHandle, out var bodyDescription);
constraintBuilder.BuildConstraintsForBody(sliceIndex, rowIndex, columnIndex, ref bodyDescription, ref bodyGetter, ref constraintAdder);
}
}
}
constraintHandles = constraintAdder.ConstraintHandles.ToArray();
}