Cleaned up some timing and mathematics bugs
Missed in earlier commit
Test case for 3D rigid body physics