Refactor Spatial Utilities to avoid Dynamic Allocations

[pwensing]

All of these utilities could be more efficient if they didn't create temporaries for the outputs. Especially since we are working with large matrices. Worth capturing in an issue for later.

Originally posted by @pwensing in #100

[pwensing]

Also consider inverseTransformForceSubspace and inverseTransformSpatialInertia