Debug.Assert(!float.IsNaN(costChange) && !float.IsInfinity(costChange),
"If the change in heuristic cost hits invalid values, it's likely that there are invalid poses or velocities. A bugged velocity input or constraint triggering an explosion is likely.");
Something has gone wrong and NaN values have infected the simulation to the point where the broad phase is now turning into a NaN mess. As the assert says, there are likely invalid body poses or velocities. It's hard to say where it's coming from, but good first places to check are:
1) Any direct modifications to poses or velocities, especially those involving potential divisions by zero.
2) Simulation.Timestep dt values- passing 0 or very large values will cause problems.
3) Constraint configuration. Most constraints have validation checks, but it's possible to sometimes provide values that are invalid but are not caught.
4) Race conditions. If any thread acts on the simulation while it's running, it will cause corruption.