public static void SwapBodyLocation(Bodies bodies, 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. bodies.UpdateAttachedConstraintsForBodyMemoryMove(a, b); bodies.UpdateAttachedConstraintsForBodyMemoryMove(b, a); //Update the body locations. bodies.ActiveSet.Swap(a, b, ref bodies.HandleToLocation); }
public static void SwapBodyLocation(Bodies bodies, 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. bodies.UpdateAttachedConstraintsForBodyMemoryMove(a, b); bodies.UpdateAttachedConstraintsForBodyMemoryMove(b, a); //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]); }