Fix #122: Incorrect integration of BallJoint and FreeJoint

* Fix #122 and add an unit test
* Clean up `BallJoint` and `FreeJoint`