Add inter-body distance computations for mesh models

  These computations are very time consuming, but they are useful to debug
  direct path validation.