public void goToNextWaypoint(DebugInfo debugInfo) {
int from = getPath().getWaypoints().indexOf(nextWaypoint);
nextWaypoint = getPath().getWaypoints().get(from + 1);
// nextWaypoint = path.getFurthestVisibleWayPoint(nextWaypoint,
// debugInfo);//path.getOptimalVisibleWayPoint(nextWaypoint);
int to = getPath().getWaypoints().indexOf(nextWaypoint);
// Vector3f waypt = nextWaypoint.getPosition();
// currentPos3d.setX(waypt.getX());
// currentPos3d.setZ(waypt.getZ());
// currentPos.set(waypt.getX(), waypt.getZ());
currentCell = nextWaypoint.getCell();
// System.out.println("Going from WP idx "+from+" to "+to);
}
/** 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();
}