Refactor body rotation and position

[Genbox]

When updating rotation or position of a body, we copy over more data than needed, as well as finding contacts multiple times. Using these properties are therefore slow compared to Get/SetTransform().

Refactoring this code into will yield a performance increase as well.