Batch the adding of Jolt Physics bodies

[mihe]

Fixes #107366.
Supersedes #107396.

This defers the JPH::BodyInterface::AddBody call for as long as possible, by batching the JPH::BodyIDs for any objects that are added to a physics space, until the point where they must be added to the simulation, like when stepping it or performing physics queries against it, at which point we use JPH::BodyInterface::AddBodiesPrepare and JPH::BodyInterface::AddBodiesFinalize to add all pending objects as a single batch

This means that we (mostly) no longer risk certain problems with Jolt's broad phase, like running out of available nodes or getting a completely unbalanced quadtree, like we do when we add bodies one-by-one, which can cause a number of issues, like the crash seen in #107366.

This replaces the previous JoltSpace3D::try_optimize hack, which solved the unbalanced problem, but did so in a fairly crude way.

EDIT: This now also includes a partial cherrypick of jrouwe/JoltPhysics#1675.

[mihe]

@jrouwe I noticed that JPH::BodyInterface::RemoveBody blows up with a segfault if you pass a not-yet-added JPH::BodyID to it, which I guess is fair enough, and not something I intended to do of course. However, that begs the question, what else in JPH::BodyInterface will blow up if you pass a not-yet-added JPH::BodyID to it?

I'm assuming the JPH::Body is in some kind of limbo state before it's been properly added, as far as JPH::BodyInterface is concerned, so what can and can't I do with such a body?

Here's the call stack, if you're curious:

JPH::FixedSizeFreeList<JPH::QuadTree::Node>::GetStorage(JPH::FixedSizeFreeList<JPH::QuadTree::Node> * this, JPH::uint32 inObjectIndex) (thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.h:35)
JPH::FixedSizeFreeList<JPH::QuadTree::Node>::Get(JPH::FixedSizeFreeList<JPH::QuadTree::Node> * this, JPH::uint32 inObjectIndex) (thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.h:114)
JPH::QuadTree::GetBodyLocation(const JPH::QuadTree * this, const JPH::QuadTree::TrackingVector & inTracking, JPH::BodyID inBodyID, JPH::uint32 & outNodeIdx, JPH::uint32 & outChildIdx) (thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp:131)
JPH::QuadTree::RemoveBodies(JPH::QuadTree * this, const JPH::BodyVector & inBodies, JPH::QuadTree::TrackingVector & ioTracking, const JPH::BodyID * ioBodyIDs, int inNumber) (thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp:923)
JPH::BroadPhaseQuadTree::RemoveBodies(JPH::BroadPhaseQuadTree * this, JPH::BodyID * ioBodies, int inNumber) (thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp:318)
JPH::BodyInterface::RemoveBody(JPH::BodyInterface * this, const JPH::BodyID & inBodyID) (thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp:152)

[jrouwe]

Looking through your usages of the BodyInterface:

• JoltSoftBody3D::set_is_sleeping is calling ActivateBody/DeactivateBody which will not work, I think you want to implement this similar to JoltBody3D::set_is_sleeping
• JoltSoftBody3D::apply_central_force is unsafe as this internally calls ActivateBody
• JoltBody3D::is_ccd_enabled can be simplified from return space->get_body_iface().GetMotionQuality(jolt_body->GetID()) == JPH::EMotionQuality::LinearCast; to return !jolt_body->IsStatic() && jolt_body->GetMotionProperties()->GetMotionQuality() == JPH::EMotionQuality::LinearCast; (note that this is just a faster way of writing the same thing, it shouldn't cause any issues).
• SetPositionAndRotation, SetMotionQuality, SetUseManifoldReduction, SetObjectLayer and SetShape are safe.

[mihe]

JoltSoftBody3D::set_is_sleeping is calling ActivateBody/DeactivateBody which will not work, I think you want to implement this similar to JoltBody3D::set_is_sleeping

Oops. I forgot about that one.

JoltSoftBody3D::apply_central_force is unsafe as this internally calls ActivateBody

I suppose this should never have used JPH::BodyInterface::AddForce to begin with, since we use JPH::Body::AddForce for JoltBody3D, so this should be an easy fix.

JoltBody3D::is_ccd_enabled can be simplified

Will do.

SetPositionAndRotation, SetMotionQuality, SetUseManifoldReduction, SetObjectLayer and SetShape are safe.

I don't know if this is all considered too niche of a use-case, but it'd be great to have documented somewhere (either in the code docs or architecture docs) which methods are unsafe for not-yet-added bodies, for future reference.

[jrouwe]

I suppose this should never have used JPH::BodyInterface::AddForce to begin with, since we use JPH::Body::AddForce for JoltBody3D, so this should be an easy fix.

Make sure to wake up the body as in JoltBody3D. Note that it is tricky to know when to exactly wake up the body, this is why I added it as a parameter to the BodyInterface::AddForce call. Consider an object held up by constant forces. You'd want the body to be able to go to sleep if it stopped moving, but as soon as the force changes you would want to wake the object up again.

I don't know if this is all considered too niche of a use-case, but it'd be great to have documented somewhere (either in the code docs or architecture docs) which methods are unsafe for not-yet-added bodies, for future reference.

Everything should trigger an assert before crashing. I've fixed up some of the functions where I think it makes sense to call it when the body has not been added yet and added some documentation:
jrouwe/JoltPhysics@273d8ac (note that you don't need any of these changes for this PR)

[jrouwe]

Before I forget: There is still a case where this new code can fail. If you do:

for many objects do
   intersect_ray
   create_body

you would effectivly add single bodies, create a really inefficient broad phase and hit one of the asserts in QuadTree.

If you do:

for many objects do
   intersect_ray
   create_body
   destroy_body

you would be able to make it crash.

I will change the QuadTree code to remove the asserts and make it function inefficiently to fix the first case. I'm not sure if we should be fixing the 2nd case as it's a pretty stupid setup.

[mihe]

Make sure to wake up the body as in JoltBody3D

Yes, for sure.

Consider an object held up by constant forces. You'd want the body to be able to go to sleep if it stopped moving, but as soon as the force changes you would want to wake the object up again.

I'll admit I didn't consider this when I first added the waking up in JoltBody3D, but I've since left it as-is mostly to see if anyone would complain about it. The whole story of when dynamic bodies should wake up (in Godot in general) needs a bit of a rethink I feel like, as it's all fairly arbitrary right now, with plenty of performance footguns.

you would effectivly add single bodies, create a really inefficient broad phase and hit one of the asserts in QuadTree.
[...]
I will change the QuadTree code to remove the asserts and make it function inefficiently to fix the first case.

That's a good point. I could definitely see someone procedurally building out a level in this way.

Wouldn't this also be about correctness though, since you would presumably run into the exact problem that JoltSpace3D::try_optimize was meant to solve (godot-jolt/godot-jolt#833)?

If you do: [...] you would be able to make it crash.

I'm not sure I follow. Why would this cause a crash?

[jrouwe]

Wouldn't this also be about correctness though, since you would presumably run into the exact problem that JoltSpace3D::try_optimize was meant to solve (godot-jolt/godot-jolt#833)?

I was desperate not to do any dynamic memory allocations when querying, so that's why there is this assert:

https://github.com/jrouwe/JoltPhysics/blob/273d8acdf2415e5103c381feef8e2344c74a627c/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp#L1054-L1056

once you hit it, you skip testing some nodes in the tree which causes the issue mentioned in the ticket. If I were to fall back to a dynamic allocation for this case then there wouldn't be any correctness issues. It would only be very slow (until you step the world and the tree gets rebuilt).

I'm not sure I follow. Why would this cause a crash?

You would be running out of quad tree nodes just like the bug that triggered this PR. But now that I think of it, you'd actually need to ensure that the root node of the quad tree does not contain empty slots for this to trigger, so you'd need to destroy a body that was created more than 4 bodies ago (and all of this without ever stepping the world):

array = {}
for many objects do
   array.push_back(create_body)
   intersect_ray // Forces adding a single body
   if (array.size() > 4)
      destroy_body(array.pop_front()) // Removes a body that is not at the root of the quad tree, causing the number of quad tree nodes to grow on the next add

[mihe]

If I were to fall back to a dynamic allocation for this case then there wouldn't be any correctness issues.

Ah, you meant replace the assert with a functioning (but slow) implementation. I misunderstood.

so you'd need to destroy a body that was created more than 4 bodies ago (and all of this without ever stepping the world)

That sounds specific enough that I agree we can just ignore it for now.

[jrouwe]

See: jrouwe/JoltPhysics#1675

[jrouwe]

Looks good to me!

[jrouwe]

I've merged my change. You could consider taking QuadTree.cpp to improve the robustness of this PR.

[mihe]

I've run this through a couple of different projects now, and I can't spot any obvious regressions.

I also created a tool script to do something similar to the MRP in godot-jolt/godot-jolt#833, but with an intersect_ray after every added body, which ended up triggering an assert if I yanked out the current try_optimize calls, but which works fine with this PR, so the changes to Jolt's QuadTree.cpp seems to be working as expected.

[akien-mga]

Thanks!