Clean up matrices in physical entities
Add test setup for steering and targeting

* improve access to uniforms
* fix debug code for velocity and position