public void calcHCost(KNode endNode) { hCost = point.distance(endNode.getPoint()); }
double getDistToParent() { return point.distance(getParent().getPoint()); }