Correctly flush dirty bodies - physics.

[AndreaCatania]

The lazy changes, implemented by this PR: #39726, was not flushing the dirty bodies when needed - causing different crashes.
This PR make sure to flush the dirty bodies, when is necessary do so, and not just during the physics step.

Note, the perf of this PR is not changed and is in line with the one measured here: #41082 (comment)

Test start
Added: 0
Added: 50
Added: 100
Added: 150
Added: 200
Added: 250
Added: 300
Added: 350
Added: 400
Added: 450
Added: 500
Added: 550
Added: 600
Added: 650
Added: 700
Added: 750
Added: 800
Added: 850
Added: 900
Added: 950
Added: 1000
Added: 1050
Added: 1100
Added: 1150
Added: 1200
Added: 1250
Added: 1300
Added: 1350
Added: 1400
Added: 1450
Added: 1500
Added: 1550
Added: 1600
Added: 1650
Added: 1700
Added: 1750
Added: 1800
Added: 1850
Added: 1900
Added: 1950
Frame 1: 33ms
Frame 2: 9146ms
Frame 3: 14503ms

Fixes: #40311
Fixes: #40840
Superseed: #41067

[briansemrau]

Just tested in the mini project where I first came across these issues, and not only are the crashes fixed, but performance when adding/removing bodies is noticeably smoother. I was skeptical of the original changes because of the number of bugs, but I think this is great progress.

[akien-mga]

CC @madmiraal

[madmiraal]

Personally, I think applying another big change on top of another big change (#40252) that already failed to resolve the crashing issues caused by the original big change #39726 that itself did not actually resolve #30027 (despite repeated assertions it only provides a moderate performance improvement as shown here) is the wrong approach.

Not only is it not guaranteed to resolve the crashing issues caused by #39726 (#40311 and #40840 are simple examples, so I wouldn't be surprised if there are others) but it makes backporting fixes (even simple fixes such as #41040 and #42061) difficult and error prone.

As stated in the Best practices for engine contributors changes should address specific problems using dedicated solutions. Since #39726 causes crashing issues, only provides a marginal performance improvement and didn't resolve #30027, I think the better approach is to revert #39726 (and #40252 that failed to fix it) as I have proposed in #41082.

[AndreaCatania]

Thanks to the opened issues, I was able to fix the bugs (introduced by #39726 and not fully solved by #40252) with this PR. If you can prove that this PR still doesn't solve the issues: #40311 #40840 (or any new regression) I'll be glad to keep working on it, otherwise I don't see why this should not be merged.

Regarding the marginal performance improvement, you may have miss the perf here: #41082 (comment)

[akien-mga]

Not relevant for the master branch anymore since Bullet was removed.

Could be redone for 3.x if wanted, though I don't really remember which PRs actually got backported / merged / reverted around this stuff.