Substituting MathLib::checkDistance with MathLib::sqrDist and a comparison.
Implemented a templated sqrDist() function, removed checkDistance().