private void RandomizeConstraints(SimulationIsland island)
{
if (Simulation.Settings.Constraints.RandomizeConstraints)
{
// RNGs are stored per island because islands are processes in parallel (if multithreading
// is enabled) and this needs to be thread-safe and deterministic.
var random = island.Random;
if (random == null)
{
random = new Random(55711);
island.Random = random;
}
// Randomize constraints using the Fisher–Yates shuffle.
// (See http://en.wikipedia.org/wiki/Fisher-Yates_shuffle.)
for (int i = island.ContactConstraintsInternal.Count - 1; i >= 1; i--)
{
var contact = island.ContactConstraintsInternal[i];
//var j = NextRandom(i);
var j = random.NextInteger(0, i);
island.ContactConstraintsInternal[i] = island.ContactConstraintsInternal[j];
island.ContactConstraintsInternal[j] = contact;
}
for (int i = island.ConstraintsInternal.Count - 1; i >= 1; i--)
{
var constraint = island.ConstraintsInternal[i];
//var j = NextRandom(i);
var j = random.NextInteger(0, i);
island.ConstraintsInternal[i] = island.ConstraintsInternal[j];
island.ConstraintsInternal[j] = constraint;
}
}
}
/// <inheritdoc/>
public override void Solve(SimulationIsland island, float deltaTime)
{
// Abort if the island is empty.
int numberOfContacts = island.ContactConstraintsInternal.Count;
int numberOfConstraints = island.ConstraintsInternal.Count;
if (numberOfContacts == 0 && numberOfConstraints == 0)
{
return;
}
// We set stacking tolerance value for each contact constraint. Set value to 0 if there are
// not many objects. A non-zero value can create non-smooth rolling movement or additional unwanted torque.
float stackingTolerance = (island.RigidBodies.Count < 6) ? 0 : Simulation.Settings.Constraints.StackingTolerance;
// Setup contact constraints.
for (int i = 0; i < numberOfContacts; i++)
{
var contact = island.ContactConstraintsInternal[i];
contact.StackingTolerance = stackingTolerance;
contact.Setup();
}
// Setup other constraints.
for (int i = 0; i < numberOfConstraints; i++)
{
var constraint = island.ConstraintsInternal[i];
constraint.Setup();
}
// Randomly reorder constraints. This improves convergence of the solver but the reordering
// can cost a bit of performance and the reordered constraints could be less cache optimal.
RandomizeConstraints(island);
// Apply impulses iteratively.
int numberOfIterations = Simulation.Settings.Constraints.NumberOfConstraintIterations;
for (int iteration = 0; iteration < numberOfIterations; iteration++)
{
bool impulseWasApplied = false;
// Apply impulses at contacts.
for (int i = 0; i < numberOfContacts; i++)
{
var contact = island.ContactConstraintsInternal[i];
bool result = contact.ApplyImpulse();
impulseWasApplied = impulseWasApplied || result;
}
// Apply impulses of non-contact constraints.
for (int i = 0; i < numberOfConstraints; i++)
{
var constraint = island.ConstraintsInternal[i];
bool result = constraint.ApplyImpulse();
impulseWasApplied = impulseWasApplied || result;
}
// If no constraint in the island applied a significant impulse, we can abort and skip
// the rest of the iterations.
if (!impulseWasApplied)
{
// Note: If this early-out introduces problems we could early-out after
// n loops where no impulse was applied.
break;
}
}
}
/// <summary> /// Solves all constraints of the specified island. /// </summary> /// <param name="island">The simulation island.</param> /// <param name="deltaTime">The time step size.</param> public abstract void Solve(SimulationIsland island, float deltaTime);
/// <summary> /// Solves all constraints of the specified island. /// </summary> /// <param name="island">The simulation island.</param> /// <param name="deltaTime">The time step size.</param> public abstract void Solve(SimulationIsland island, float deltaTime);
/// <inheritdoc/>
public override void Solve(SimulationIsland island, float deltaTime)
{
// Abort if the island is empty.
int numberOfContacts = island.ContactConstraintsInternal.Count;
int numberOfConstraints = island.ConstraintsInternal.Count;
if (numberOfContacts == 0 && numberOfConstraints == 0)
return;
// We set stacking tolerance value for each contact constraint. Set value to 0 if there are
// not many objects. A non-zero value can create non-smooth rolling movement or additional unwanted torque.
float stackingTolerance = (island.RigidBodies.Count < 6) ? 0 : Simulation.Settings.Constraints.StackingTolerance;
// Setup contact constraints.
for (int i = 0; i < numberOfContacts; i++)
{
var contact = island.ContactConstraintsInternal[i];
contact.StackingTolerance = stackingTolerance;
contact.Setup();
}
// Setup other constraints.
for (int i = 0; i < numberOfConstraints; i++)
{
var constraint = island.ConstraintsInternal[i];
constraint.Setup();
}
// Randomly reorder constraints. This improves convergence of the solver but the reordering
// can cost a bit of performance and the reordered constraints could be less cache optimal.
RandomizeConstraints(island);
// Apply impulses iteratively.
int numberOfIterations = Simulation.Settings.Constraints.NumberOfConstraintIterations;
for (int iteration = 0; iteration < numberOfIterations; iteration++)
{
bool impulseWasApplied = false;
// Apply impulses at contacts.
for (int i = 0; i < numberOfContacts; i++)
{
var contact = island.ContactConstraintsInternal[i];
bool result = contact.ApplyImpulse();
impulseWasApplied = impulseWasApplied || result;
}
// Apply impulses of non-contact constraints.
for (int i = 0; i < numberOfConstraints; i++)
{
var constraint = island.ConstraintsInternal[i];
bool result = constraint.ApplyImpulse();
impulseWasApplied = impulseWasApplied || result;
}
// If no constraint in the island applied a significant impulse, we can abort and skip
// the rest of the iterations.
if (!impulseWasApplied)
{
// Note: If this early-out introduces problems we could early-out after
// n loops where no impulse was applied.
break;
}
}
}
private void RandomizeConstraints(SimulationIsland island)
{
if (Simulation.Settings.Constraints.RandomizeConstraints)
{
// RNGs are stored per island because islands are processes in parallel (if multithreading
// is enabled) and this needs to be thread-safe and deterministic.
var random = island.Random;
if (random == null)
{
random = new Random(55711);
island.Random = random;
}
// Randomize constraints using the Fisher–Yates shuffle.
// (See http://en.wikipedia.org/wiki/Fisher-Yates_shuffle.)
for (int i = island.ContactConstraintsInternal.Count - 1; i >= 1; i--)
{
var contact = island.ContactConstraintsInternal[i];
//var j = NextRandom(i);
var j = random.NextInteger(0, i);
island.ContactConstraintsInternal[i] = island.ContactConstraintsInternal[j];
island.ContactConstraintsInternal[j] = contact;
}
for (int i = island.ConstraintsInternal.Count - 1; i >= 1; i--)
{
var constraint = island.ConstraintsInternal[i];
//var j = NextRandom(i);
var j = random.NextInteger(0, i);
island.ConstraintsInternal[i] = island.ConstraintsInternal[j];
island.ConstraintsInternal[j] = constraint;
}
}
}
