Fix bug.
Added rigid body api and tests.