Fixed the distance formula and tolerance levels.

Fixed it!