Remove debug ouput.
Re-implement continuous collision checking

  - do not allow penetration anymore. tolerance_ now stands for the acceptable
    distance to collision of the end of the valid sub-path.