[Test] Added tests for the physics manager and matrix