/** Build a navigation path using the provided points and the A* method */
  private boolean buildNavigationPath(
      Path navPath,
      Cell startCell,
      Vector3f startPos,
      Cell endCell,
      Vector3f endPos,
      float entityRadius,
      DebugInfo debugInfo) {

    // Increment our path finding session ID
    // This Identifies each pathfinding session
    // so we do not need to clear out old data
    // in the cells from previous sessions.
    sessionID++;

    // load our data into the Heap object
    // to prepare it for use.
    heap.initialize(sessionID, startPos);

    // We are doing a reverse search, from EndCell to StartCell.
    // Push our EndCell onto the Heap at the first cell to be processed
    endCell.queryForPath(heap, null, 0.0f);

    // process the heap until empty, or a path is found
    boolean foundPath = false;
    while (heap.isNotEmpty() && !foundPath) {

      // pop the top cell (the open cell with the lowest cost) off the
      // Heap
      Node currentNode = heap.getTop();

      // if this cell is our StartCell, we are done
      if (currentNode.cell.equals(startCell)) {
        foundPath = true;
      } else {
        // Process the Cell, Adding it's neighbors to the Heap as needed
        currentNode.cell.processCell(heap);
      }
    }

    Vector2f intersectionPoint = new Vector2f();

    // if we found a path, build a waypoint list
    // out of the cells on the path
    if (!foundPath) {
      return false;
    }

    // Setup the Path object, clearing out any old data
    navPath.initialize(navMesh, startPos, startCell, endPos, endCell);

    Vector3f lastWayPoint = startPos;

    // Step through each cell linked by our A* algorithm
    // from StartCell to EndCell
    Cell currentCell = startCell;
    while (currentCell != null && currentCell != endCell) {

      if (debugInfo != null) {
        debugInfo.addPlannedCell(currentCell);
      }

      // add the link point of the cell as a way point (the exit
      // wall's center)
      int linkWall = currentCell.getArrivalWall();
      Vector3f newWayPoint = currentCell.getWallMidpoint(linkWall).clone();

      Line2D wall = currentCell.getWall(linkWall);
      float length = wall.length();
      float distBlend = entityRadius / length;

      Line2D lineToGoal =
          new Line2D(
              new Vector2f(lastWayPoint.x, lastWayPoint.z), new Vector2f(endPos.x, endPos.z));
      LineIntersect result = lineToGoal.intersect(wall, intersectionPoint);
      switch (result) {
        case SegmentsIntersect:
          float d1 = wall.getPointA().distance(intersectionPoint);
          float d2 = wall.getPointB().distance(intersectionPoint);
          if (d1 > entityRadius && d2 > entityRadius) {
            // we can fit through the wall if we go
            // directly to the goal.
            newWayPoint = new Vector3f(intersectionPoint.x, 0, intersectionPoint.y);
          } else {
            // cannot fit directly.
            // try to find point where we can
            if (d1 < d2) {
              intersectionPoint.interpolate(wall.getPointA(), wall.getPointB(), distBlend);
              newWayPoint = new Vector3f(intersectionPoint.x, 0, intersectionPoint.y);
            } else {
              intersectionPoint.interpolate(wall.getPointB(), wall.getPointA(), distBlend);
              newWayPoint = new Vector3f(intersectionPoint.x, 0, intersectionPoint.y);
            }
          }
          currentCell.computeHeightOnCell(newWayPoint);
          break;
        case LinesIntersect:
        case ABisectsB:
        case BBisectsA:
          Vector2f lastPt2d = new Vector2f(lastWayPoint.x, lastWayPoint.z);
          Vector2f endPos2d = new Vector2f(endPos.x, endPos.z);

          Vector2f normalEnd = endPos2d.subtract(lastPt2d).normalizeLocal();
          Vector2f normalA = wall.getPointA().subtract(lastPt2d).normalizeLocal();
          Vector2f normalB = wall.getPointB().subtract(lastPt2d).normalizeLocal();
          if (normalA.dot(normalEnd) < normalB.dot(normalEnd)) {
            // choose point b
            intersectionPoint.interpolate(wall.getPointB(), wall.getPointA(), distBlend);
            newWayPoint = new Vector3f(intersectionPoint.x, 0, intersectionPoint.y);
          } else {
            // choose point a
            intersectionPoint.interpolate(wall.getPointA(), wall.getPointB(), distBlend);
            newWayPoint = new Vector3f(intersectionPoint.x, 0, intersectionPoint.y);
          }
          currentCell.computeHeightOnCell(newWayPoint);

          break;
        case CoLinear:
        case Parallel:
          System.out.println("## colinear or parallel");
          break;
      }

      if (debugInfo != null) {
        debugInfo.addPreOptWaypoints(newWayPoint.clone());
      }
      //                newWayPoint = snapPointToCell(currentCell, newWayPoint);
      lastWayPoint = newWayPoint.clone();

      navPath.addWaypoint(newWayPoint, currentCell);

      // get the next cell
      currentCell = currentCell.getLink(linkWall);
    }

    // cap the end of the path.
    navPath.finishPath();

    // remove optimization so it can be done as the actor moves
    // further: optimize the path
    List<Waypoint> newPath = new ArrayList<Waypoint>();
    Waypoint curWayPoint = navPath.getFirst();
    newPath.add(curWayPoint);
    while (curWayPoint != navPath.getLast()) {
      curWayPoint = navPath.getFurthestVisibleWayPoint(curWayPoint);
      newPath.add(curWayPoint);
    }

    navPath.initialize(navMesh, startPos, startCell, endPos, endCell);
    for (Waypoint newWayPoint : newPath) {
      navPath.addWaypoint(newWayPoint.getPosition(), newWayPoint.getCell());
    }
    navPath.finishPath();

    return true;
  }
 public Vector3f getDirectionToWaypoint() {
   Vector3f waypt = nextWaypoint.getPosition();
   return waypt.subtract(currentPos3d).normalizeLocal();
 }
 public float getDistanceToWaypoint() {
   return currentPos3d.distance(nextWaypoint.getPosition());
 }
 public Vector3f getWaypointPosition() {
   return nextWaypoint.getPosition();
 }