Skip to content

Instantly share code, notes, and snippets.

@darkwave
Last active August 29, 2015 14:12
Show Gist options
  • Save darkwave/4442ae06465adf15fc1b to your computer and use it in GitHub Desktop.
Save darkwave/4442ae06465adf15fc1b to your computer and use it in GitHub Desktop.
Simple Path Finding using NavMesh for Point and Click Adventure Games for Processing. It depends on AI for Games library by Peter Lager and it uses Polygon class by Roman Kushnarenko. SVG file mesh.svg included as source for the navmesh (add it to the /data folder).
class Agent {
PVector pos, currentTarget;
ArrayList<PVector> path = new ArrayList<PVector>();
ArrayList<PVector> newPath;
int pathIndex = 0;
Agent(PVector _pos) {
pos = _pos;
currentTarget = pos;
newPath = path;
}
void setPath(ArrayList<PVector> _newPath) {
newPath = _newPath;
}
void update() {
if (newPath != path) {
path = newPath;
pathIndex = -1;
currentTarget = path.get(0);
}
float distance = PVector.dist(pos, currentTarget);
if (distance < 3) {
if (path.size() > 0) { //check for path
pathIndex++;
if (pathIndex == path.size()) { //arrived
path.clear();
pathIndex = -1;
return;
}
currentTarget = path.get(pathIndex).copy();
}
return;
}
float deltaX = currentTarget.x - pos.x;
float deltaY = currentTarget.y - pos.y;
float angle = atan2(deltaX, deltaY);
pos.x += sin(angle) * 3;
pos.y += cos(angle) * 3;
}
void display() {
pushMatrix();
translate(pos.x, pos.y);
ellipse(0, 0, 20, 20);
popMatrix();
}
}
/**
* Line is defined by starting point and ending point on 2D dimension.<br>
*
* @author Roman Kushnarenko (sromku@gmail.com)
*/
public class Line
{
private final Point _start;
private final Point _end;
private float _a = Float.NaN;
private float _b = Float.NaN;
private boolean _vertical = false;
public Line(Point start, Point end)
{
_start = start;
_end = end;
if (_end.x - _start.x != 0)
{
_a = ((_end.y - _start.y) / (_end.x - _start.x));
_b = _start.y - _a * _start.x;
}
else
{
_vertical = true;
}
}
/**
* Indicate whereas the point lays on the line.
*
* @param point
* - The point to check
* @return <code>True</code> if the point lays on the line, otherwise return <code>False</code>
*/
public boolean isInside(Point point)
{
float maxX = _start.x > _end.x ? _start.x : _end.x;
float minX = _start.x < _end.x ? _start.x : _end.x;
float maxY = _start.y > _end.y ? _start.y : _end.y;
float minY = _start.y < _end.y ? _start.y : _end.y;
if ((point.x >= minX && point.x <= maxX) && (point.y >= minY && point.y <= maxY))
{
return true;
}
return false;
}
/**
* Indicate whereas the line is vertical. <br>
* For example, line like x=1 is vertical, in other words parallel to axis Y. <br>
* In this case the A is (+/-)infinite.
*
* @return <code>True</code> if the line is vertical, otherwise return <code>False</code>
*/
public boolean isVertical()
{
return _vertical;
}
/**
* y = <b>A</b>x + B
*
* @return The <b>A</b>
*/
public float getA()
{
return _a;
}
/**
* y = Ax + <b>B</b>
*
* @return The <b>B</b>
*/
public float getB()
{
return _b;
}
/**
* Get start point
*
* @return The start point
*/
public Point getStart()
{
return _start;
}
/**
* Get end point
*
* @return The end point
*/
public Point getEnd()
{
return _end;
}
@Override
public String toString()
{
return String.format("%s-%s", _start.toString(), _end.toString());
}
}
Display the source blob
Display the rendered blob
Raw
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
import java.util.LinkedList;
import java.util.ArrayList;
import processing.core.PShape;
import processing.core.PVector;
import processing.core.PConstants;
import processing.core.PApplet;
import game2dai.graph.*;
public class NavMesh {
IGraphSearch pf;
PShape base;
ArrayList<PVector> innerPoints = new ArrayList<PVector>();
float interval = 19;
Graph graph;
public NavMesh(PShape _base, float _interval) {
if (_interval < 15)
interval = 15;
else
interval = _interval;
base = _base.getChild(1).getChild(0);
Polygon.Builder builder = Polygon.Builder();
float minX = PConstants.MAX_FLOAT;
float minY = PConstants.MAX_FLOAT;
float maxX = PConstants.MIN_FLOAT;
float maxY = PConstants.MIN_FLOAT;
for (int i = 0; i < base.getVertexCount (); i++) {
builder.addVertex(new Point(base.getVertex(i).x, base.getVertex(i).y));
if (base.getVertex(i).x < minX)
minX = base.getVertex(i).x;
if (base.getVertex(i).y < minY)
minY = base.getVertex(i).y;
if (base.getVertex(i).x > maxX)
maxX = base.getVertex(i).x;
if (base.getVertex(i).y > maxY)
maxY = base.getVertex(i).y;
}
Polygon poly = builder.build();
int counter = 0;
for (float y = minY; y <= maxY; y += interval)
for (float x = minX; x <= maxX; x += interval) {
if (poly.contains(new Point(x, y))) {
innerPoints.add(new PVector(x, y));
counter++;
//println(counter++);
}
}
System.out.println(counter);
//ArrayList<Triangle> triangles = Triangulate.triangulate(innerPoints);
graph = getGraph(innerPoints);
pf = new GraphSearch_Astar(graph, new AshCrowFlight());
}
public Graph getGraph(ArrayList<PVector> points) {
Graph graph = new Graph();
GraphNode node;
int indexCounter = 0;
for (PVector point : points) {
node = new GraphNode(indexCounter, point.x, point.y);
graph.addNode(node);
indexCounter++;
}
GraphNode[] nodes = graph.getNodeArray();
int edgeCounter = 0;
for (GraphNode n : nodes) {
for (GraphNode on : nodes) {
if (PApplet.dist(n.xf(), n.yf(), on.xf(), on.yf()) <= PApplet.sqrt(interval * interval * 2)) {
graph.addEdge(n.id(), on.id(), 0, 0);
edgeCounter++;
}
}
}
System.out.println(edgeCounter);
return graph;
}
public ArrayList<PVector> getPath(PVector start, PVector end) {
ArrayList<PVector> result = new ArrayList<PVector>();
LinkedList<GraphNode> solution = pf.search(nearestNode(start), nearestNode(end));
for (GraphNode node : solution)
result.add(new PVector(node.xf(), node.yf()));
return result;
}
public int nearestNode(PVector v) {
return nearestNode(v.x, v.y);
}
public int nearestNode(float x, float y) {
float distance = PConstants.MAX_FLOAT;
GraphNode result = null;
GraphNode[] nodes = graph.getNodeArray();
for (int i = 0; i < nodes.length; i++) {
if (PApplet.dist(x, y, nodes[i].xf(), nodes[i].yf()) < distance) {
result = nodes[i];
distance = PApplet.dist(x, y, nodes[i].xf(), nodes[i].yf());
//println(distance);
}
}
return result.id();
}
}
/**
* Point on 2D landscape
*
* @author Roman Kushnarenko (sromku@gmail.com)</br>
*/
public class Point
{
public Point(float x, float y)
{
this.x = x;
this.y = y;
}
public float x;
public float y;
@Override
public String toString()
{
return String.format("(%.2f,%.2f)", x, y);
}
}
NavMesh mesh;
PVector start;
PVector nearest;
ArrayList<PVector> solution;
Agent player;
void setup() {
size(1024, 768);
PShape aShape = loadShape("mesh.svg");
mesh = new NavMesh(aShape, 1);
player = new Agent(new PVector());
}
void draw() {
background(#333333);
shape(mesh.base);
fill(255, 128, 0);
noStroke();
if (nearest != null)
ellipse(nearest.x, nearest.y, 30, 30);
if (start != null)
ellipse(start.x, start.y, 30, 30);
fill(99, 0, 0);
if (solution != null) {
player.setPath(solution);
for (PVector n : solution) {
fill(255, 200, 100);
ellipse(n.x, n.y, 10, 10);
fill(255, 0, 0);
}
}
player.update();
player.display();
}
void mousePressed() {
start = new PVector(mouseX, mouseY);
}
void mouseDragged() {
nearest = new PVector(mouseX, mouseY);
if (start != null) {
solution = mesh.getPath(start, nearest);
}
}
import java.util.ArrayList;
import java.util.List;
/**
* The 2D polygon. <br>
*
* @see {@link Builder}
* @author Roman Kushnarenko (sromku@gmail.com)
*/
public class Polygon
{
private final BoundingBox _boundingBox;
private final List<Line> _sides;
private Polygon(List<Line> sides, BoundingBox boundingBox)
{
_sides = sides;
_boundingBox = boundingBox;
}
/**
* Get the builder of the polygon
*
* @return The builder
*/
public static Builder Builder()
{
return new Builder();
}
/**
* Builder of the polygon
*
* @author Roman Kushnarenko (sromku@gmail.com)
*/
public static class Builder
{
private List<Point> _vertexes = new ArrayList<Point>();
private List<Line> _sides = new ArrayList<Line>();
private BoundingBox _boundingBox = null;
private boolean _firstPoint = true;
private boolean _isClosed = false;
/**
* Add vertex points of the polygon.<br>
* It is very important to add the vertexes by order, like you were drawing them one by one.
*
* @param point
* The vertex point
* @return The builder
*/
public Builder addVertex(Point point)
{
if (_isClosed)
{
// each hole we start with the new array of vertex points
_vertexes = new ArrayList<Point>();
_isClosed = false;
}
updateBoundingBox(point);
_vertexes.add(point);
// add line (edge) to the polygon
if (_vertexes.size() > 1)
{
Line Line = new Line(_vertexes.get(_vertexes.size() - 2), point);
_sides.add(Line);
}
return this;
}
/**
* Close the polygon shape. This will create a new side (edge) from the <b>last</b> vertex point to the <b>first</b> vertex point.
*
* @return The builder
*/
public Builder close()
{
validate();
// add last Line
_sides.add(new Line(_vertexes.get(_vertexes.size() - 1), _vertexes.get(0)));
_isClosed = true;
return this;
}
/**
* Build the instance of the polygon shape.
*
* @return The polygon
*/
public Polygon build()
{
validate();
// in case you forgot to close
if (!_isClosed)
{
// add last Line
_sides.add(new Line(_vertexes.get(_vertexes.size() - 1), _vertexes.get(0)));
}
Polygon polygon = new Polygon(_sides, _boundingBox);
return polygon;
}
/**
* Update bounding box with a new point.<br>
*
* @param point
* New point
*/
private void updateBoundingBox(Point point)
{
if (_firstPoint)
{
_boundingBox = new BoundingBox();
_boundingBox.xMax = point.x;
_boundingBox.xMin = point.x;
_boundingBox.yMax = point.y;
_boundingBox.yMin = point.y;
_firstPoint = false;
}
else
{
// set bounding box
if (point.x > _boundingBox.xMax)
{
_boundingBox.xMax = point.x;
}
else if (point.x < _boundingBox.xMin)
{
_boundingBox.xMin = point.x;
}
if (point.y > _boundingBox.yMax)
{
_boundingBox.yMax = point.y;
}
else if (point.y < _boundingBox.yMin)
{
_boundingBox.yMin = point.y;
}
}
}
private void validate()
{
if (_vertexes.size() < 3)
{
throw new RuntimeException("Polygon must have at least 3 points");
}
}
}
/**
* Check if the the given point is inside of the polygon.<br>
*
* @param point
* The point to check
* @return <code>True</code> if the point is inside the polygon, otherwise return <code>False</code>
*/
public boolean contains(Point point)
{
if (inBoundingBox(point))
{
Line ray = createRay(point);
int intersection = 0;
for (Line side : _sides)
{
if (intersect(ray, side))
{
// System.out.println("intersection++");
intersection++;
}
}
/*
* If the number of intersections is odd, then the point is inside the polygon
*/
if (intersection % 2 == 1)
{
return true;
}
}
return false;
}
public List<Line> getSides()
{
return _sides;
}
/**
* By given ray and one side of the polygon, check if both lines intersect.
*
* @param ray
* @param side
* @return <code>True</code> if both lines intersect, otherwise return <code>False</code>
*/
private boolean intersect(Line ray, Line side)
{
Point intersectPoint = null;
// if both vectors aren't from the kind of x=1 lines then go into
if (!ray.isVertical() && !side.isVertical())
{
// check if both vectors are parallel. If they are parallel then no intersection point will exist
if (ray.getA() - side.getA() == 0)
{
return false;
}
float x = ((side.getB() - ray.getB()) / (ray.getA() - side.getA())); // x = (b2-b1)/(a1-a2)
float y = side.getA() * x + side.getB(); // y = a2*x+b2
intersectPoint = new Point(x, y);
}
else if (ray.isVertical() && !side.isVertical())
{
float x = ray.getStart().x;
float y = side.getA() * x + side.getB();
intersectPoint = new Point(x, y);
}
else if (!ray.isVertical() && side.isVertical())
{
float x = side.getStart().x;
float y = ray.getA() * x + ray.getB();
intersectPoint = new Point(x, y);
}
else
{
return false;
}
// System.out.println("Ray: " + ray.toString() + " ,Side: " + side);
// System.out.println("Intersect point: " + intersectPoint.toString());
if (side.isInside(intersectPoint) && ray.isInside(intersectPoint))
{
return true;
}
return false;
}
/**
* Create a ray. The ray will be created by given point and on point outside of the polygon.<br>
* The outside point is calculated automatically.
*
* @param point
* @return
*/
private Line createRay(Point point)
{
// create outside point
float epsilon = (_boundingBox.xMax - _boundingBox.xMin) / 100f;
Point outsidePoint = new Point(_boundingBox.xMin - epsilon, _boundingBox.yMin);
Line vector = new Line(outsidePoint, point);
return vector;
}
/**
* Check if the given point is in bounding box
*
* @param point
* @return <code>True</code> if the point in bounding box, otherwise return <code>False</code>
*/
private boolean inBoundingBox(Point point)
{
if (point.x < _boundingBox.xMin || point.x > _boundingBox.xMax || point.y < _boundingBox.yMin || point.y > _boundingBox.yMax)
{
return false;
}
return true;
}
private static class BoundingBox
{
public float xMax = Float.NEGATIVE_INFINITY;
public float xMin = Float.NEGATIVE_INFINITY;
public float yMax = Float.NEGATIVE_INFINITY;
public float yMin = Float.NEGATIVE_INFINITY;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment