From 18ea6546a4cee277f440ad11570268b817014225 Mon Sep 17 00:00:00 2001 From: Reynaldo Morillo Date: Thu, 3 May 2018 00:08:30 -0400 Subject: [PATCH] The only steps left is giving better definitions to the cost functions of the Nodes and the finishing the A-Star implmentation to work with our graph data structure. --- AbstractNode.java | 349 +++++++++++++++++++++++++++++ pathfinding/Node.java => Node.java | 11 +- NodeFactory.java | 42 ++++ path.pde => Path.pde | 4 +- PathPlanner.java | 122 ++++++++++ RoadBuilder.java | 132 +++++++++++ vehicle.pde => Vehicle.pde | 0 pathfinding/Path.java | 7 - pathfinding/RoadBuilder.java | 92 -------- pathplanner.pde | 27 --- 10 files changed, 651 insertions(+), 135 deletions(-) create mode 100644 AbstractNode.java rename pathfinding/Node.java => Node.java (96%) create mode 100644 NodeFactory.java rename path.pde => Path.pde (94%) create mode 100644 PathPlanner.java create mode 100644 RoadBuilder.java rename vehicle.pde => Vehicle.pde (100%) delete mode 100644 pathfinding/Path.java delete mode 100644 pathfinding/RoadBuilder.java delete mode 100644 pathplanner.pde diff --git a/AbstractNode.java b/AbstractNode.java new file mode 100644 index 0000000..48ab821 --- /dev/null +++ b/AbstractNode.java @@ -0,0 +1,349 @@ +/* + Copyright (C) 2012 http://software-talk.org/ (developer@software-talk.org) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * This class represents an AbstractNode. It has all the appropriate fields as well + * as getter and setter to be used by the A* algorithm. + *

+ *

+ * An AbstractNode has x- and y-coordinates and can be walkable or not. + * A previous AbstractNode may be set, as well as the + * fCosts, gCosts and hCosts. + *

+ *

+ * fCosts: gCosts + hCosts + *

+ * gCosts: calculated costs from start AbstractNode to this AbstractNode + *

+ * hCosts: estimated costs to get from this AbstractNode to end AbstractNode + *

+ *

+ * A subclass has to override the heuristic function + *

+ * sethCosts(AbstractNode endAbstractNode) + *

+ * @see ExampleNode#sethCosts(AbstractNode endNode) example Implementation using manhatten method + *

+ * + * @version 1.0 + */ +public abstract class AbstractNode { + + /** costs to move sideways from one square to another. */ + protected static final float BASICMOVEMENTCOST = 10; + /** costs to move diagonally from one square to another. */ + protected static final float DIAGONALMOVEMENTCOST = 14; + + protected float xPosition; + protected float yPosition; + protected boolean walkable; + + // for pathfinding: + + /** the previous AbstractNode of this one on the currently calculated path. */ + protected AbstractNode previous; + + /** weather or not the move from previous to this AbstractNode is diagonally. */ + protected boolean diagonally; + + /** optional extra penalty. */ + protected float movementPanelty; + + //private float fCosts; // g + h costs + + /** calculated costs from start AbstractNode to this AbstractNode. */ + protected float gCosts; + + /** estimated costs to get from this AbstractNode to end AbstractNode. */ + protected float hCosts; + + /** + * constructs a walkable AbstractNode with given coordinates. + * + * @param xPosition + * @param yPosition + */ + public AbstractNode(float xPosition, float yPosition) { + this.xPosition = xPosition; + this.yPosition = yPosition; + this.walkable = true; + this.movementPanelty = 0; + } + + /** + * returns weather or not the move from the previousAbstractNode was + * diagonally. If it is not diagonal, it is sideways. + * + * @return + */ + public boolean isDiagonaly() { + return diagonally; + } + + /** + * sets weather or not the move from the previousAbstractNode was + * diagonally. If it is not diagonal, it is sideways. + * + * @param isDiagonaly + */ + public void setIsDiagonaly(boolean isDiagonaly) { + this.diagonally = isDiagonaly; + } + + /** + * sets x and y coordinates. + * + * @param x + * @param y + */ + public void setCoordinates(float x, float y) { + this.xPosition = x; + this.yPosition = y; + } + + /** + * @return the xPosition + */ + public float getxPosition() { + return xPosition; + } + + /** + * @return the yPosition + */ + public float getyPosition() { + return yPosition; + } + + /** + * @return the walkable + */ + public boolean isWalkable() { + return walkable; + } + + /** + * @param walkable the walkable to set + */ + public void setWalkable(boolean walkable) { + this.walkable = walkable; + } + + /** + * returns the node set as previous node on the current path. + * + * @return the previous + */ + public AbstractNode getPrevious() { + return previous; + } + + /** + * @param previous the previous to set + */ + public void setPrevious(AbstractNode previous) { + this.previous = previous; + } + + /** + * sets a general penalty for the movement on this node. + * + * @param movementPanelty the movementPanelty to set + */ + public void setMovementPanelty(float movementPanelty) { + this.movementPanelty = movementPanelty; + } + + /** + * returns gCosts + hCosts. + *

+ * + * + * @return the fCosts + */ + public float getfCosts() { + return gCosts + hCosts; + } + + /** + * returns the calculated costs from start AbstractNode to this AbstractNode. + * + * @return the gCosts + */ + public float getgCosts() { + return gCosts; + } + + /** + * sets gCosts to gCosts plus movementPanelty + * for this AbstractNode. + * + * @param gCosts the gCosts to set + */ + private void setgCosts(float gCosts) { + this.gCosts = gCosts + movementPanelty; + } + + /** + * sets gCosts to gCosts plus movementPanelty + * for this AbstractNode given the previous AbstractNode as well as the basic cost + * from it to this AbstractNode. + * + * @param previousAbstractNode + * @param basicCost + */ + public void setgCosts(AbstractNode previousAbstractNode, float basicCost) { + setgCosts(previousAbstractNode.getgCosts() + basicCost); + } + + /** + * sets gCosts to gCosts plus movementPanelty + * for this AbstractNode given the previous AbstractNode. + *

+ * It will assume BASICMOVEMENTCOST as the cost from + * previousAbstractNode to itself if the movement is not diagonally, + * otherwise it will assume DIAGONALMOVEMENTCOST. + * Weather or not it is diagonally is set in the Map class method which + * finds the adjacent AbstractNodes. + * + * @param previousAbstractNode + */ + public void setgCosts(AbstractNode previousAbstractNode) { + if (diagonally) { + setgCosts(previousAbstractNode, DIAGONALMOVEMENTCOST); + } else { + setgCosts(previousAbstractNode, BASICMOVEMENTCOST); + } + } + + /** + * calculates - but does not set - g costs. + *

+ * It will assume BASICMOVEMENTCOST as the cost from + * previousAbstractNode to itself if the movement is not diagonally, + * otherwise it will assume DIAGONALMOVEMENTCOST. + * Weather or not it is diagonally is set in the Map class method which + * finds the adjacent AbstractNodes. + * + * @param previousAbstractNode + * @return gCosts + */ + public float calculategCosts(AbstractNode previousAbstractNode) { + if (diagonally) { + return (previousAbstractNode.getgCosts() + + DIAGONALMOVEMENTCOST + movementPanelty); + } else { + return (previousAbstractNode.getgCosts() + + BASICMOVEMENTCOST + movementPanelty); + } + } + + /** + * calculates - but does not set - g costs, adding a movementPanelty. + * + * @param previousAbstractNode + * @param movementCost costs from previous AbstractNode to this AbstractNode. + * @return gCosts + */ + public float calculategCosts(AbstractNode previousAbstractNode, float movementCost) { + return (previousAbstractNode.getgCosts() + movementCost + movementPanelty); + } + + /** + * returns estimated costs to get from this AbstractNode to end AbstractNode. + * + * @return the hCosts + */ + public float gethCosts() { + return hCosts; + } + + /** + * sets hCosts. + * + * @param hCosts the hCosts to set + */ + protected void sethCosts(float hCosts) { + this.hCosts = hCosts; + } + + /** + * calculates hCosts for this AbstractNode to a given end AbstractNode. + * Uses Manhatten method. + * + * @param endAbstractNode + */ + public abstract void sethCosts(AbstractNode endAbstractNode); + + + /* + * @return the movementPanelty + */ + private float getMovementPanelty() { + return movementPanelty; + } + + /** + * returns a String containing the coordinates, as well as h, f and g + * costs. + * + * @return + */ + @Override + public String toString() { + return "(" + getxPosition() + ", " + getyPosition() + "): h: " + + gethCosts() + " g: " + getgCosts() + " f: " + getfCosts(); + } + + /** + * returns weather the coordinates of AbstractNodes are equal. + * + * @param obj + * @return + */ + @Override + public boolean equals(Object obj) { + if (obj == null) { + return false; + } + if (getClass() != obj.getClass()) { + return false; + } + final AbstractNode other = (AbstractNode) obj; + if (this.xPosition != other.xPosition) { + return false; + } + if (this.yPosition != other.yPosition) { + return false; + } + return true; + } + + /** + * returns hash code calculated with coordinates. + * + * @return + */ + @Override + public int hashCode() { + int hash = 3; + hash = 17 * hash + (int)this.xPosition; + hash = 17 * hash + (int)this.yPosition; + return hash; + } + +} diff --git a/pathfinding/Node.java b/Node.java similarity index 96% rename from pathfinding/Node.java rename to Node.java index 25abb3b..cc2c995 100644 --- a/pathfinding/Node.java +++ b/Node.java @@ -1,9 +1,6 @@ /** Our Implementation of an AbstractNode */ - -package pathfinding; - import java.util.ArrayList; /** @@ -56,19 +53,19 @@ public class Node extends AbstractNode { // Getting // -------------------------------------------------------- - public Node getLeft(Node aNode) { + public Node getLeft() { return neighbors.get(LEFT); } - public Node getStraight(Node aNode) { + public Node getStraight() { return neighbors.get(STRAIGHT); } - public Node getRight(Node aNode) { + public Node getRight() { return neighbors.get(RIGHT); } - public ArrayList get_neighbors() { + public ArrayList getNeighbors() { return neighbors; } diff --git a/NodeFactory.java b/NodeFactory.java new file mode 100644 index 0000000..99513d4 --- /dev/null +++ b/NodeFactory.java @@ -0,0 +1,42 @@ +/* + Copyright (C) 2012 http://software-talk.org/ (developer@software-talk.org) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * A Factory which creates new instances of an implementation of the + * AbstractNode at given coordinates. + *

+ * Must be implemented and given to Map instance on + * construction. + * + * @see AbstractNode + * @version 1.0 + */ +public interface NodeFactory { + + /** + * creates new instances of an implementation of the + * AbstractNode. + * In an implementation, it should return a new node with its position + * set to the given x and y values. + * + * @param x position on the x-axis + * @param y position on the y-axis + * @return + */ + public AbstractNode createNode(int x, int y); + +} diff --git a/path.pde b/Path.pde similarity index 94% rename from path.pde rename to Path.pde index 362f859..f04986f 100644 --- a/path.pde +++ b/Path.pde @@ -1,9 +1,9 @@ class Path { // A Path is an arraylist of points (PVector objects) - ArrayList points; + public ArrayList points; // A path has a radius, i.e how far is it ok for the boid to wander off - float radius; + public float radius; Path() { // Arbitrary radius of 30 diff --git a/PathPlanner.java b/PathPlanner.java new file mode 100644 index 0000000..d34dfaf --- /dev/null +++ b/PathPlanner.java @@ -0,0 +1,122 @@ +import java.util.ArrayList; + +class PathPlanner { + // Produces a path from a given track, startPoint, and EndPoint + RoadBuilder trackBuilder; + ArrayList> track; + + PathPlanner(Path skeleton, Node start, Node end, float straightDelta, float sideDelta, int numLanes) { + trackBuilder = new RoadBuilder(skeleton, start, end); + track = trackBuilder.buildRoad(straightDelta, sideDelta, numLanes); + } + + /** + * finds an allowed path from start to goal coordinates on this map. + *

+ * This method uses the A* algorithm. The hCosts value is calculated in + * the given Node implementation. + *

+ * This method will return a LinkedList containing the start node at the + * beginning followed by the calculated shortest allowed path ending + * with the end node. + *

+ * If no allowed path exists, an empty list will be returned. + *

+ *

+ * x/y must be bigger or equal to 0 and smaller or equal to width/hight. + * + * @param oldX + * @param oldY + * @param newX + * @param newY + * @return + */ + public final List findPath(int oldX, int oldY, int newX, int newY) { + // TODO check input + openList = new LinkedList(); + closedList = new LinkedList(); + openList.add(nodes[oldX][oldY]); // TODO: add starting node to open list + + done = false; + T current; + while (!done) { + current = lowestFInOpen(); // TODO: get node with lowest fCosts from openList + closedList.add(current); // add current node to closed list + openList.remove(current); // delete current node from open list + + if ((current.getxPosition() == newX) + && (current.getyPosition() == newY)) { // found goal + return calcPath(nodes[oldX][oldY], current); // NOTE: Looks like he back tracks from end to start t oproduce path + } + + // for all adjacent nodes: + List adjacentNodes = getAdjacent(current); // TODO: + for (int i = 0; i < adjacentNodes.size(); i++) { + T currentAdj = adjacentNodes.get(i); + if (!openList.contains(currentAdj)) { // node is not in openList + // TODO: Need to edit logic after this point + // NOTE: May not need any of this here + currentAdj.setPrevious(current); // set current node as previous for this node + currentAdj.sethCosts(nodes[newX][newY]); // set h costs of this node (estimated costs to goal) + currentAdj.setgCosts(current); // set g costs of this node (costs from start to this node) + // NOTE: This seems okay + openList.add(currentAdj); // add node to openList + } else { // node is in openList + // TODO: Implement these cost functions + if (currentAdj.getgCosts() > currentAdj.calculategCosts(current)) { // costs from current node are cheaper than previous costs + currentAdj.setPrevious(current); // set current node as previous for this node + currentAdj.setgCosts(current); // set g costs of this node (costs from start to this node) + } + } + } + + if (openList.isEmpty()) { // no path exists + return new LinkedList(); // return empty list + } + } + return null; // unreachable + } + + /** + * returns the node with the lowest fCosts. + * + * @return + */ + private T lowestFInOpen(LinkedList openList) { + // TODO currently, this is done by going through the whole openList! + T cheapest = openList.get(0); + for (int i = 0; i < openList.size(); i++) { + if (openList.get(i).getfCosts() < cheapest.getfCosts()) { + cheapest = openList.get(i); + } + } + return cheapest; + } + + /** + * calculates the found path between two points according to + * their given previousNode field. + * + * @param start + * @param goal + * @return + */ + private List calcPath(T start, T goal) { + // TODO if invalid nodes are given (eg cannot find from + // goal to start, this method will result in an infinite loop!) + LinkedList path = new LinkedList(); + + T curr = goal; + boolean done = false; + while (!done) { + path.addFirst(curr); + curr = (T) curr.getPrevious(); + + if (curr.equals(start)) { + done = true; + } + } + return path; + } + +} diff --git a/RoadBuilder.java b/RoadBuilder.java new file mode 100644 index 0000000..9fd1a97 --- /dev/null +++ b/RoadBuilder.java @@ -0,0 +1,132 @@ + +import java.util.ArrayList; +import java.lang.Math; + + +public class RoadBuilder implements NodeFactory { + + private ArrayList skeleton; + private Node startNode; + private Node endNode; + + public RoadBuilder(Path skeleton, Node start, Node end) { + this.skeleton = path2Nodes(skeleton); + // Extract start & end nodes + startNode = start; + endNode = end; + } + + public Node createNode(int x, int y) { + return new Node(x, y); + } + + public ArrayList pointInterpolation(Node node1, Node node2, float delta) { + // Extract coordinates + float x1 = node1.getxPosition(); + float y1 = node1.getyPosition(); + float x2 = node2.getxPosition(); + float y2 = node2.getyPosition(); + // Calculate some features of the line we're going to make + double distance = euclideanDistance(x1, y1, x2, y2); + // Rise/Run of slope + float rise = y2 - y1; + float run = x2 - x1; + float slopeDelta = (float) (Math.sqrt( Math.pow(rise, 2) + Math.pow(run, 2) )); + float proportion = delta/slopeDelta; + rise = (float) (rise * Math.sqrt(proportion)); + run = (float) (run * Math.sqrt(proportion)); + // Start filling in new points along the line connecting these two points + // Initialize loop variables + ArrayList points = new ArrayList(); + double distanceCovered = 0; + Node prevNode = node1; + float newX = x1; + float newY = y1; + // Interpolate until we have covered the distance of the line + while (distanceCovered < (distance-delta)) { + newX += run; + newY += rise; + Node newNode = new Node(newX, newY); + newNode.setParent(prevNode, Node.STRAIGHT); + points.add(newNode); + distanceCovered = euclideanDistance(x1, y1, newX, newY); + } + return points; + } + + public ArrayList createLayer(Node centerNode, float delta, int num) { + Node node1 = centerNode; + Node node2 = node1.getStraight(); + // Extract coordinates + float x1 = node1.getxPosition(); + float y1 = node1.getyPosition(); + float x2 = node1.getxPosition(); + float y2 = node1.getyPosition(); + // Rise/Run of slope (negative reciprical) + // Perpendicular slope + // https://www.mathsisfun.com/algebra/line-parallel-perpendicular.html + float rise = -1 * (x2 - x1); // NOTE: using X (because reciprical) + float run = y2 - y1; + float slopeDelta = (float) (Math.sqrt( Math.pow(rise, 2) + Math.pow(run, 2) )); + float proportion = delta/slopeDelta; + rise = (float) (rise * Math.sqrt(proportion)); + run = (float) (run * Math.sqrt(proportion)); + // Start creating layer + ArrayList points = new ArrayList(); + points.add(centerNode); + // new point coordinates + float currXleft = x1; + float currYleft = y1; + float currXright = x1; + float currYright = y1; + for (int i=1; i <= num; i++) { + // Left side + // calculate coordinates of new Node + currXleft += run * i; + currYleft += rise * i; + Node newNode = new Node(currXleft, currYleft); + points.add(newNode); + // Right side + // calculate coordinates of new Node + currXright += -run * i; + currYright += -rise * i; + newNode = new Node(currXright, currYright); + points.add(newNode); + } + return points; + } + + public ArrayList> buildRoad(float straightDelta, float sideDelta, int numLanes) { + ArrayList> road = new ArrayList>(); + // Line Interp center path (save as a linked list) + int pathLength = skeleton.size(); + for (int i=0; i < pathLength; i++) { + ArrayList interpPath = pointInterpolation(skeleton.get(i), + skeleton.get((i+1) % pathLength), + straightDelta); + int interpPathLength = interpPath.size(); + for (int j=0; j < interpPathLength; j++) { + ArrayList newLayer = createLayer(interpPath.get(j), sideDelta, numLanes/2); + road.add(newLayer); + } + } + return road; + } + + public ArrayList path2Nodes(Path path) { + ArrayList points = path.points; + int numPoints = points.size(); + ArrayList nodeList = new ArrayList(); + for (int i=0; i < numPoints; i++) { + PVector point = points.get(i); + Node newNode = new Node(point.x, point.y); + nodeList.add(newNode); + } + return nodeList; + } + + public double euclideanDistance(float x1, float y1, float x2, float y2) { + double squaredSum = Math.pow(x2-x1, 2) + Math.pow(y2-y1, 2); + return Math.sqrt(squaredSum); + } +} diff --git a/vehicle.pde b/Vehicle.pde similarity index 100% rename from vehicle.pde rename to Vehicle.pde diff --git a/pathfinding/Path.java b/pathfinding/Path.java deleted file mode 100644 index 355d1d7..0000000 --- a/pathfinding/Path.java +++ /dev/null @@ -1,7 +0,0 @@ -package pathfinding; - -public class Path { - public Path() { - - } -} diff --git a/pathfinding/RoadBuilder.java b/pathfinding/RoadBuilder.java deleted file mode 100644 index c018bd4..0000000 --- a/pathfinding/RoadBuilder.java +++ /dev/null @@ -1,92 +0,0 @@ - -package pathfinding; - -import java.util.ArrayList; -import java.lang.Math; - - -public class RoadBuilder implements NodeFactory { - - private ArrayList> road; - private Path skeleton; - private Node startNode; - private Node endNode; - - public RoadBuilder(Path skeleton) { - road = new ArrayList>(); - this.skeleton = skeleton; - // TODO: Extract start & end nodes - } - - public Node createNode(int x, int y) { - return new Node(x, y); - } - - public ArrayList pointInterpolation(Node node1, Node node2, float delta) { - // Extract coordinates - float x1 = node1.getxPosition(); - float y1 = node1.getyPosition(); - float x2 = node1.getxPosition(); - float y2 = node1.getyPosition(); - // Calculate some features of the line we're going to make - double distance = euclideanDistance(x1, y1, x2, y2); - // Rise/Run of slope - float rise = y2 - y1; - float run = x2 - x1; - // Start filling in new points along the line connecting these two points - // Initialize loop variables - ArrayList points = new ArrayList(); - double distanceCovered = 0; - Node prevNode = node1; - float newX = x1; - float newY = y1; - // Interpolate until we have covered the distance of the line - while (distanceCovered < distance) { - newX += run; - newY += rise; - Node newNode = new Node(newX, newY); - newNode.setParent(prevNode, Node.STRAIGHT); - points.add(newNode); - distanceCovered = euclideanDistance(x1, y1, newX, newY); - } - return points; - } - - // TODO - public ArrayList createLayer(Node centerNode, float delta, int num) { - ArrayList points = new ArrayList(); - // Left side - float currX = centerNode.getxPosition(); - float currY = centerNode.getyPosition(); - for (int i=0; i < num; i++) { - // calculate coordinates of new Node - // Node newNode = new Node(); - } - - // Right side - return null; - } - - // TODO - public void connectLayer(ArrayList layer1, ArrayList layer2) { - - } - - // TODO - public void buildRoad(float delta) { - // TODO: Line Interp center path (save as a linked list) - // TODO: Iterate over the center path nodes from start to ending - // --- TODO: Create a layer at each Node along the center Line - // ---- TODO: Connect the layers as you go - } - - // TODO: Returns the road as a Path - public Path convert2Path() { - return new Path(); - } - - public double euclideanDistance(float x1, float y1, float x2, float y2) { - double squaredSum = Math.pow(x2-x1, 2) + Math.pow(y2-y1, 2); - return Math.sqrt(squaredSum); - } -} diff --git a/pathplanner.pde b/pathplanner.pde deleted file mode 100644 index a6642a4..0000000 --- a/pathplanner.pde +++ /dev/null @@ -1,27 +0,0 @@ -class PathPlanner { - // Produces a path from a given track, startPoint, and EndPoint - - Path track = null; // Path of the track it's given - Path path = null; // Path it produces - PVector startPoint = null; // Start point on the path - PVector endPoint = null; // Destination point - - PathPlanner(Path atrack, PVector start, PVector destination) { - track = atrack; - startPoint = start; - endPoint = destination; - path = new Path(); - } - - void discritizeTrack() { - // Make the track a little more refined, to allow for more careful decisions - - } - - Path pathFinder() { - // Returns a Path through the track - - return null; - } - -}