optimized pathfinder

no longer users sqrt function
the fix applied for the gcc prevents it from compiling on msvc (change
need to be tested in order to find out if it works on gcc right now)
small fix