rewrite from PathFinding.js