Remove hash map allocation on every update step (backport #1005)

[mergify]

🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸

🎉 New feature

Part of gazebosim/gz-sim#3608
Improve performance by removing allocation + deallocation of a hash map for all link poses in each update step.

Summary

Guided by the profiling report by @caguero, Priority 2.
Building and destroying a hash map, where each node needs to call malloc / free, is an expensive operation.
I tried the quick recommendation to reserve the hash map but found out it didn't help much and I thought the "best" approach of not sending updates for static links might be a bit tricky (and still, it is not mutually exclusive, we can still do it in a followup).
Thus the approach is to remove the extra map and make sure we use the preexisting prevLinkPoses hash map instead.

Before there was an

declare empty map
if (pose changed)  -> add new pose to map
else -> add old pose to map

By caching the pose through iterations, we can get rid of the else statement (since it will be just set to the value in the previous iteration).
Finding out which links have been removed is a bit trickier. I thought since removing links is something that happens fairly rarely, we could do a fast check where, if at any point in time the size of our cache is larger than the size of our links map, we trigger a full traversal and delete stale links.
I believe this should be robust even to cases where we add and remove a link in the same iteration. Adding a link with a new ID will make the cache allocate a new element and thus it will become larger than the links array, triggering the reallocation.
At the same time, this approach will do virtually no work at all iterations, instead of having to do expensive lookups / traversal at every step for, again, something I believe happens very rarely (link deletion).

Backport Policy

• This is safe to backport to the following versions:
  • Jetty
  • Ionic
  • Harmonic
  • Fortress
• This should not be backported
• I am not sure
• Other (fill in yourself)

Test it

Run gz-sim with and without this PR:

$ gz sim -v3 3k_shapes.sdf -r --physics-engine gz-physics-dartsim-plugin -s
$ gz sim -v3 3k_shapes.sdf -r --physics-engine gz-physics-bullet-featherstone-plugin -s

On my machine I actually don't observe a noticeable improvement under dartsim since it's already very slow (~1% RTF, I'm open to revert the commit that makes this change there 1a78c62), but on bullet-featherstone the RTF goes from ~10-20% to 20-30%, so about 75% increase.

Checklist

• Signed all commits for DCO
• Added a screen capture or video to the PR description that demonstrates the feature
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• Updated Bazel files (if adding new files). Created an issue otherwise.
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by and Generated-by messages.

Backports: If this is a backport, please use Rebase and Merge instead.

---

This is an automatic backport of pull request #1005 done by Mergify.

[mergify]

Cherry-pick of 84c9ba1 has failed:

On branch mergify/bp/gz-physics9/pr-1005
Your branch is up to date with 'origin/gz-physics9'.

You are currently cherry-picking commit 84c9ba1.
  (fix conflicts and run "git cherry-pick --continue")
  (use "git cherry-pick --skip" to skip this patch)
  (use "git cherry-pick --abort" to cancel the cherry-pick operation)

Changes to be committed:
	modified:   bullet-featherstone/src/SimulationFeatures.cc
	modified:   bullet-featherstone/src/SimulationFeatures.hh
	modified:   dartsim/src/SimulationFeatures.cc

Unmerged paths:
  (use "git add <file>..." to mark resolution)
	both modified:   bullet-featherstone/src/Base.hh
	both modified:   dartsim/src/Base.hh
	both modified:   dartsim/src/SimulationFeatures.hh

To fix up this pull request, you can check it out locally. See documentation: https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/reviewing-changes-in-pull-requests/checking-out-pull-requests-locally

[luca-della-vedova]

Fixed conflicts on this (plus all others), would appreciate another pair of eyes!