Fix #133

This pull request also includes a change to `AllBodiesClosestDistanceConstraint` to allow for meaningful testing of the cpp constraint class, now that we no longer re-call `doKinematics` inside `collisionDetect`. Without that change, `testKinCnst` fails when testing `AllBodiesClosestDistanceConstraint` since the number of constraint values does not match between the MATLAB and mex versions.