Implemented rigid body + test