Clear distance result before distance computation.

  - result is not clear in function call.
Fix bug in object iterator

  if first body has no object, iterate on following joints.