Pathfinding on Mesh

So I implemented a 2D navigation mesh and it works so far.

Applet using the navigation mesh (you can just drag a path with the mouse)

But I don't really like the code (see below). 600 lines seems rather heavyweight and the way I construct the navigation graph is computationally very expensive. So maybe I'm doing something needlessly complicated.

The navigation graph is generated by first finding all the outer edges of the mesh by checking if an edge belongs to exactly one triangle (outer edge) or not. Also for each point on the "perimeter" of the graph I determine if the angle formed by it's edges is below 180° (in which case it's never part of an optimal path). Then I iterate over all pairs of points in the mesh and check if they intersect any of the outer edges. This step has potentially O(n^3) complexity . Is there an easier way to do this?

Also I had all sorts of problems with edge intersections which all had to do with the treatment of edges that "touch". So the basic questions are:
1. do I consider edges that share an endpoint to be intersecting?
2. do I consider edges to be intersecting if and endpoint of one lies exactly on the other?

Especially the second one is unclear. If I don't consider them to be intersecting, then there are special cases where lines going exactly through an outer point won't intersect with the "outline". If I do consider them to be intersecting weird things happen if a path ends or starts somewhere on an outer edge because any directions going into the mesh are illegal (since they intersect) except ones that are parallel to that edge. So the generated path goes from the starting point along the edge to the next mesh point and then on from there. For example if you want to move from the border of a triangle to its center the pathfinder will start by moving into a corner first, which is obviously not the shortest path.

Right now I solved that by just checking if a starting or endpoint is on the edge and return no path if one of them does. So basically I defined the edges to be "unwalkable". This again is slightly strange since the pathfinder will return paths that walk on the edges (that's why there is the "offsetPoint" thingy, see below).

Here is an example to give you an idea how the class is used (geometry from the applet):

NavigationMesh nm = new NavigationMesh();

//insert points
nm.addPoint(250,0);
nm.addPoint(0,450);
nm.addPoint(240,450);
nm.addPoint(200,350);
nm.addPoint(300,350);
nm.addPoint(260,450);
nm.addPoint(500,450);
nm.addPoint(250, 100);
nm.addPoint(240, 300);
nm.addPoint(380, 300);

//define triangles
nm.addTriangle(0,1,8);
nm.addTriangle(0,8,7);
nm.addTriangle(0,7,9);
nm.addTriangle(0,9,6);
nm.addTriangle(1,8,3);
nm.addTriangle(1,3,2);
nm.addTriangle(4,5,6);
nm.addTriangle(4,6,9);
nm.addTriangle(3,8,9);
nm.addTriangle(3,9,4);

//get path from (x1, y1) to (x2, y2). result does not contain start and end point
ArrayList<Integer> path = nm.getPath(x1, y1, x2, y2);

//get coordinates of i-th point on path
Point2D.Double p = nm.getPoint(path.get(i));

//get coordinates of i-th point on path offset by 10 into the mesh
Point2D.Double p = nm.getOffsetPoint(path.get(i), 10);


The class:

package net.progsch;

import java.awt.Color;
import java.awt.Graphics;
import java.awt.Polygon;
import java.awt.geom.Point2D;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.LinkedList;
import java.util.PriorityQueue;
import java.util.TreeSet;

public class NavigationMesh {
private class ProxyComparator implements Comparator<Integer>
{//used by A* to build the priority queue sorted by some external score array
double [] c;
public ProxyComparator(double [] pc)
{ c = pc; }
public int compare(Integer a, Integer b)
{ return (int)(c[a] - c); }
}
private ArrayList<Double> x = new ArrayList<Double>(), y = new ArrayList<Double>();
private ArrayList<Integer> triangleVertices = new ArrayList<Integer>();
private ArrayList<LinkedList<Integer>> vertexTriangles = new ArrayList<LinkedList<Integer>>();
private ArrayList<Integer> outline, internal;
private ArrayList<LinkedList<Integer>> pathGraph;
private boolean [] reflexAngle;
private Point2D.Double [] offsetDirection;
private boolean ready = false;

private final double tol = 1.e-12;

public int addPoint(double px, double py)
{
x.add(px); y.add(py);
vertexTriangles.add(new LinkedList<Integer>());
ready = false;
return x.size()-1;
}

public void addTriangle(int a, int b, int c)
{
int n = triangleVertices.size();

Point2D.Double ap = getPoint(a);
Point2D.Double bp = getPoint(b);
Point2D.Double cp = getPoint(c);
Point2D.Double ndir = new Point2D.Double();
ndir.x = (ap.y-bp.y);
ndir.y = (-ap.x+bp.x);
//make sure all triangles are equally oriented
if(ndir.x * (cp.x-ap.x) + ndir.y * (cp.y-ap.y) < 0)
{
triangleVertices.add(a);
triangleVertices.add(b);
triangleVertices.add(c);
}
else
{
triangleVertices.add(a);
triangleVertices.add(c);
triangleVertices.add(b);
}
vertexTriangles.get(a).add(n);
vertexTriangles.get(b).add(n);
vertexTriangles.get(c).add(n);

ready = false;
}

private void calculateInternalConnections()
{
/*
* calculates the waypoint/edge graph by checking for every pair of points
* in the mesh whether their connecting line intersects the outline or not.
* O(n^3) complexity <img src='http://public.gamedev.net/public/style_emoticons/<#EMO_DIR#>/sad.gif' class='bbc_emoticon' alt=':(' />
*/

outline = new ArrayList<Integer>();
internal = new ArrayList<Integer>();
calculateOutline();
int n = x.size();

internal.clear();
for(int i = 0;i<n;++i)
{
if(!reflexAngle) //skip corners
continue;
for(int j = i+1;j<n;++j)
{
if(!reflexAngle[j]) //skip corners
continue;

if(!legalDirection(x.get(j), y.get(j), i) || !legalDirection(x.get(i), y.get(i), j))
continue;

boolean intersects = false;
for(int k = 0;k<outline.size();k+=2)
if(intersectLines(i, j, outline.get(k), outline.get(k+1)))
{
intersects = true;
break;
}
if(!intersects)
{
internal.add(i);
internal.add(j);
}
}
}
// fill pathGraph with lists of neighbors for every node
pathGraph = new ArrayList<LinkedList<Integer>>(n);
for(int i = 0;i<n;++i)
{
pathGraph.add(new LinkedList<Integer>());
}

for(int i = 0;i<internal.size();i+=2)
{
pathGraph.get(internal.get(i)).add(internal.get(i+1));
pathGraph.get(internal.get(i+1)).add(internal.get(i));
}

ready = true;
}


private void calculateOutline()
{
/*
* calculates the "edges" of the mesh by counting how many triangles
* share the edge. If an edge belongs to exactly one triangle
* it's and outer edge.
* O(n^2) complexity
*/
int n = vertexTriangles.size();
outline.clear();
reflexAngle = new boolean[n];
offsetDirection = new Point2D.Double[x.size()];

int [] firstEdge = new int[n];

for(int i = 0;i<n;++i)
{
offsetDirection = new Point2D.Double();
firstEdge = -1;
}
for(int i = 0;i<n;++i)
for(int j = i+1;j<n;++j)
{
int count = 0;
int triangle = 0;
for(int tria : vertexTriangles.get(i))
{
if(vertexTriangles.get(j).contains(tria))
{
count++;
triangle = tria;
}

if(count>1)
break;
}

if(count == 1) //is outer edge
{
outline.add(i);
outline.add(j);

Point2D.Double a = getPoint(i);
Point2D.Double b = getPoint(j);
Point2D.Double c = null;
for(int k = 0;k<3;++k)
if(triangleVertices.get(triangle+k) != i && triangleVertices.get(triangle+k) != j)
c = getPoint(triangleVertices.get(triangle+k));
Point2D.Double normal = new Point2D.Double();
double l = distance(i, j);
normal.x = (a.y-b.y)/l;
normal.y = (-a.x+b.x)/l;
//make sure normal points into the triangle
if(normal.x * (c.x-a.x) + normal.y * (c.y-a.y) < 0)
{
normal.x = -normal.x;
normal.y = -normal.y;
}
//calculates offset vector for each point so it can be "offset into the mesh"
if(offsetDirection.x == 0 && offsetDirection.y == 0)
{
offsetDirection.x += normal.x;
offsetDirection.y += normal.y;
}
else
{
offsetDirection.x += normal.x;
offsetDirection.y += normal.y;
double f = 2./(offsetDirection.x*offsetDirection.x
+ offsetDirection.y*offsetDirection.y);
offsetDirection.x *= f;
offsetDirection.y *= f;
}

if(offsetDirection[j].x == 0 && offsetDirection[j].y == 0)
{
offsetDirection[j].x += normal.x;
offsetDirection[j].y += normal.y;
}
else
{
offsetDirection[j].x += normal.x;
offsetDirection[j].y += normal.y;
double f = 2./(offsetDirection[j].x*offsetDirection[j].x
+ offsetDirection[j].y*offsetDirection[j].y);
offsetDirection[j].x *= f;
offsetDirection[j].y *= f;
}

//determine if angle between edges is > 180°
if(firstEdge == -1)
firstEdge = j;
else if(normal.x * (getPoint(firstEdge).x - getPoint(i).x) +
normal.y * (getPoint(firstEdge).y - getPoint(i).y) < 0)
{
reflexAngle = true;
}

if(firstEdge[j] == -1)
firstEdge[j] = i;
else if(normal.x * (getPoint(firstEdge[j]).x - getPoint(j).x) +
normal.y * (getPoint(firstEdge[j]).y - getPoint(j).y) < 0)
{
reflexAngle[j] = true;
}
}
}
}

private double distance(int a, int b)
{
return Math.sqrt((x.get(a)-x.get(b))*(x.get(a)-x.get(b)) + (y.get(a)-y.get(b))*(y.get(a)-y.get(b)));
}

public void draw(Graphics g)
{//draws the mesh, it's outline and edges in the graph for testing purposes.
if(!ready)
calculateInternalConnections();

for(int i = 0;i<triangleVertices.size();i+=3)
{
Polygon p = new Polygon();
p.addPoint((int)x.get(triangleVertices.get(i+0)).doubleValue(), (int)y.get(triangleVertices.get(i+0)).doubleValue());
p.addPoint((int)x.get(triangleVertices.get(i+1)).doubleValue(), (int)y.get(triangleVertices.get(i+1)).doubleValue());
p.addPoint((int)x.get(triangleVertices.get(i+2)).doubleValue(), (int)y.get(triangleVertices.get(i+2)).doubleValue());

g.setColor(new Color(255,200,200));
g.fillPolygon(p);
}

g.setColor(new Color(0,255,0));
for(int i = 0;i<internal.size();i+=2)
{
g.drawLine((int)x.get(internal.get(i)).doubleValue(), (int)y.get(internal.get(i)).doubleValue(),
(int)x.get(internal.get(i+1)).doubleValue(), (int)y.get(internal.get(i+1)).doubleValue());
}

g.setColor(new Color(255,100,100));
for(int i = 0;i<outline.size();i+=2)
{
g.drawLine((int)x.get(outline.get(i)).doubleValue(), (int)y.get(outline.get(i)).doubleValue(),
(int)x.get(outline.get(i+1)).doubleValue(), (int)y.get(outline.get(i+1)).doubleValue());
}

}

private boolean freePath(double x1, double y1, double x2, double y2)
{ //check if the direct line from (x1, y1) to (x2, y2) is a legal path within the mesh
//assumes that starting and end point are legal
for(int i = 0;i<outline.size();i+=2)
{
if(intersectLines(x1, y1, x2, y2, outline.get(i), outline.get(i+1)))
return false;
}
return true;
}

private boolean freePath(double x1, double y1, int i)
{
if(!legalDirection(x1, y1, i))
return false;

return freePath(x1, y1, x.get(i), y.get(i));
}

public ArrayList<Integer> getPath(double xs, double ys, double xd, double yd)
{
/*
* calculate optimal path using A*. The result is null in case no path exists or an ArrayList of indices
* to the waypoints (source and destination are NOT in the list).
*/

if(!ready)
calculateInternalConnections();

if(!(isLegal(xs, ys) && isLegal(xd, yd)))
return null; //if one point is outside the mesh there is no legal path

ArrayList<Integer> path = new ArrayList<Integer>();

if(freePath(xs, ys, xd, yd)) // nothing in the way
return path;

//temporarily add source and destination
int s = this.addPoint(xs, ys);
int d = this.addPoint(xd, yd);

int n = x.size();

ArrayList<Integer> sv = getVisiblePoints(xs, ys);
sv.remove(Integer.valueOf(s));
ArrayList<Integer> dv = getVisiblePoints(xd, yd);
dv.remove(Integer.valueOf(d));

LinkedList<Integer> snode = new LinkedList<Integer>();
LinkedList<Integer> dnode = new LinkedList<Integer>();
pathGraph.add(snode);
pathGraph.add(dnode);
//add edges connecting start-/end-point with mesh points to the graph
for(int i : sv)
if(reflexAngle)
{
pathGraph.get(i).add(s);
pathGraph.get(s).add(i);
}

for(int i : dv)
if(reflexAngle)
{
pathGraph.get(i).add(d);
pathGraph.get(d).add(i);
}


//A* (straight adaption of the pseudocode from wikipedia)
int start = s;
int goal = d;

double [] g_score = new double[n];
double [] h_score = new double[n];
double [] f_score = new double[n];

// The set of nodes already evaluated.
TreeSet<Integer> closedset = new TreeSet<Integer>();
// The set of tentative nodes to be evaluated.
PriorityQueue<Integer> openset = new PriorityQueue<Integer>(1, new ProxyComparator(f_score));
openset.add(s);

// The map of navigated nodes.
int [] came_from = new int[n];
for(int i=0;i<n;++i)
came_from = -1;
// Distance from start along optimal path.
g_score[start] = 0;
h_score[start] = distance(start, goal);
// Estimated total distance from start to goal through y.
f_score[start] = h_score[start];
boolean success = false;
while(!openset.isEmpty())
{
int x = openset.remove();

if(x == goal)
{
success = true;
break;
}

openset.remove(Integer.valueOf(x));
closedset.add(x);
for(int y : pathGraph.get(x))
{

boolean tentative_is_better;
if(closedset.contains(y))
continue;

double tentative_g_score = g_score[x] + distance(x,y);

if(!openset.contains(y))
{
openset.add(y);
tentative_is_better = true;
}
else if(tentative_g_score < g_score[y])
tentative_is_better = true;
else
tentative_is_better = false;
if(tentative_is_better == true)
{
came_from[y] = x;

g_score[y] = tentative_g_score;
h_score[y] = distance(y, goal);
f_score[y] = g_score[y] + h_score[y];
openset.remove(y);
openset.add(y);
}
}
}

//cleanup and remove the stuff that was only temporarily inserted
x.remove(d);
x.remove(s);
y.remove(d);
y.remove(s);
vertexTriangles.remove(d);
vertexTriangles.remove(s);

for(int i : dv)
{
pathGraph.get(i).remove(Integer.valueOf(d));
}

for(int i : sv)
{
pathGraph.get(i).remove(Integer.valueOf(s));
}

pathGraph.remove(snode);
pathGraph.remove(dnode);

ready = true;

if(success == false)
return null;//no path found

//retrace optimal path and build the path list
reconstruct_path(came_from, came_from[goal], path);

return path;
}

public Point2D.Double getPoint(int i)
{
return new Point2D.Double(x.get(i), y.get(i));
}

public Point2D.Double getOffsetPoint(int i, double offset)
{ //get an offset point so lines connecting mesh points will stay at a distance offset from the edges
return new Point2D.Double(x.get(i)+offsetDirection.x*offset, y.get(i)+offsetDirection.y*offset);
}

private ArrayList<Integer> getVisiblePoints(double px, double py)
{ //get all points of the mesh that can be reached directly from (px, py)
ArrayList<Integer> result = new ArrayList<Integer>();
for(int i = 0;i<x.size();++i)
{
if(freePath(px, py, i))
result.add(i);
}
return result;
}

private boolean intersectLines(double x1, double y1, double x2, double y2,
double x3, double y3, double x4, double y4)
{//consinders lines that are on top of each other to be intersecting
double d = (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1);
if(Math.abs(d)<tol)
{
return false;
}
double ua = ((x4-x3)*(y1-y3)-(y4-y3)*(x1-x3))/d;
double ub = ((x2-x1)*(y1-y3)-(y2-y1)*(x1-x3))/d;
if(-tol<ua && ua<1+tol && 0<ub && ub<1)
return true;
else if(0<ua && ua<1 && -tol<ub && ub<1+tol)
return true;
else
return false;
}


private boolean intersectLines(double x1, double y1, double x2, double y2, int c, int d)
{
return intersectLines(x1, y1, x2, y2 ,x.get(c), y.get(c),x.get(d), y.get(d));
}

private boolean intersectLines(int a, int b, int c, int d)
{
return intersectLines(x.get(a), y.get(a),x.get(b), y.get(b),x.get(c), y.get(c),x.get(d), y.get(d));
}

private boolean inTriangle(double xp, double yp, int triang)
{ //check if (xp, yp) is in a triangle
int t0 = triangleVertices.get(triang);
int t1 = triangleVertices.get(triang+1);
int t2 = triangleVertices.get(triang+2);
double diffxp = xp - x.get(t0);
double diffyp = yp - y.get(t0);

double diffxa = x.get(t1) - x.get(t0);
double diffya = y.get(t1) - y.get(t0);

double diffxb = x.get(t2) - x.get(t0);
double diffyb = y.get(t2) - y.get(t0);

double dotaa = diffxa*diffxa + diffya*diffya;
double dotab = diffxa*diffxb + diffya*diffyb;
double dotap = diffxa*diffxp + diffya*diffyp;
double dotbb = diffxb*diffxb + diffyb*diffyb;
double dotbp = diffxb*diffxp + diffyb*diffyp;

// barycentric coordinates
double invDenom = 1 / (dotaa * dotbb - dotab * dotab);
double u = (dotbb * dotap - dotab * dotbp) * invDenom;
double v = (dotaa * dotbp - dotab * dotap) * invDenom;

// is in triangle
return (u > -tol) && (v > -tol) && (u + v < 1+tol);
}

public boolean isLegal(double xp, double yp)
{ //check if (xp, yp) is in the "walkable" area
for(int i = 0;i<outline.size();i+=2)
{
if(onLine(xp, yp,
x.get(outline.get(i)), y.get(outline.get(i)),
x.get(outline.get(i+1)), y.get(outline.get(i+1))))
return false;
}

for(int i = 0;i<triangleVertices.size();i+=3)
if(inTriangle(xp, yp, i))
return true;

return false;
}

private boolean legalDirection(double xp, double yp, int source)
{ //check if source -> (xp, yp) doesn't instantly leave the mesh
for(int i : vertexTriangles.get(source))
{
int pos = 0;
while(triangleVertices.get(i+pos)!=source)
++pos;
if(legalDirection(xp, yp, i, pos))
return true;
}
return false;
}

private boolean legalDirection(double xp, double yp, int triang, int source)
{ //check if the line connecting source and (xp, yp) points "into" the triangle or out
int t0 = triangleVertices.get(triang+source);
int t1 = triangleVertices.get(triang+(1+source)%3);
int t2 = triangleVertices.get(triang+(2+source)%3);

double diffxp = xp - x.get(t0);
double diffyp = yp - y.get(t0);

double diffxa = x.get(t1) - x.get(t0);
double diffya = y.get(t1) - y.get(t0);

double diffxb = x.get(t2) - x.get(t0);
double diffyb = y.get(t2) - y.get(t0);

double dotaa = diffxa*diffxa + diffya*diffya;
double dotab = diffxa*diffxb + diffya*diffyb;
double dotap = diffxa*diffxp + diffya*diffyp;
double dotbb = diffxb*diffxb + diffyb*diffyb;
double dotbp = diffxb*diffxp + diffyb*diffyp;

double u = (dotbb * dotap - dotab * dotbp);
double v = (dotaa * dotbp - dotab * dotap);

// is in cone
return (u > -tol) && (v > -tol);
}

private boolean onLine(double xp, double yp, double x1, double y1, double x2, double y2)
{ //check if (xp, yp) lies on the line connecting (x1, y1) and (x2, y2)
if((xp > x1 && xp > x2) || (xp < x1 && xp < x2)
|| (yp > y1 && yp > y2) || (yp < y1 && yp < y2))
return false;

if(Math.abs((x2-x1)*(yp-y1) - (y2-y1)*(xp-x1)) < tol*Math.abs((x2-x1)+(y2-y1))*((xp-x1)+(yp-y1)))
return true;
else
return false;
}

private void reconstruct_path(int [] came_from, int current_node, ArrayList<Integer> path)
{//helper function to retrace A* results
if(came_from[current_node] != -1)
{
reconstruct_path(came_from, came_from[current_node], path);
path.add(current_node);
}
}
}