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]); }