Changed the algorithm for finding closest point on rectangle to a super fast one.

Now only do calculations for closest point if the agent has a chance of
being in interaction range with the obstacle (huge improvement in
calculation time). Also made some better time tests for the obstacles.