Clean up and simplify how rigid bodies are handled with rmfs

Again. Now external rigid bodies are (mostly) handled nicely and the internal coordinates are only stored once when they are used.