refactored init and update in Rigid Body

This should probably move to helper of some kind. Note torque and
angular velocity not fully added.