public static void SwapBodyLocation(Bodies bodies, Solver solver, int a, int b)
{
Debug.Assert(a != b, "Swapping a body with itself isn't meaningful. Whaddeyer doin?");
//Enumerate the bodies' current set of constraints, changing the reference in each to the new location.
//Note that references to both bodies must be changed- both bodies moved!
//This function does not update the actual position of the list in the graph, so we can modify both without worrying about invalidating indices.
solver.UpdateForBodyMemorySwap(a, b);
//Update the body locations.
bodies.ActiveSet.Swap(a, b, ref bodies.HandleToLocation);
//TODO: If the body layout optimizer occurs before or after all other stages, this swap isn't required. If we move it in between other stages though, we need to keep the inertia
//coherent with the other body properties.
//Helpers.Swap(ref bodies.Inertias[a], ref bodies.Inertias[b]);
}
public static void SwapBodyLocation(Bodies bodies, Solver solver, int a, int b)
{
Debug.Assert(a != b, "Swapping a body with itself isn't meaningful. Whaddeyer doin?");
//Enumerate the bodies' current set of constraints, changing the reference in each to the new location.
//Note that references to both bodies must be changed- both bodies moved!
//This function does not update the actual position of the list in the graph, so we can modify both without worrying about invalidating indices.
solver.UpdateForBodyMemorySwap(a, b);
//Update the body locations.
bodies.ActiveSet.Swap(a, b, ref bodies.HandleToLocation);
//Note that the active set does not contain the world inertia. While this isn't actually a problem for correctness so long as the body layout optimizer
//takes places before the pose integrator or after all stages, it does create a pretty confusing dependency which is easy to forget.
//Given how cheap it is, we're just going to swap the world inertia too.
Helpers.Swap(ref bodies.Inertias[a], ref bodies.Inertias[b]);
}