Parameterized Better.

Actually working parameterizations. With class-like objects!
Rigid Body Tests & bugfixes.

Tests and bugfixes for rigid body warps. Parameterized stopping
condition better.