package Algorithms; import edu.carleton.motion.*; import java.io.*; import java.util.*; import java.awt.*; import java.awt.geom.*; import java.math.*; /** Implementation of the line-of-sight motion planning algorithm. This * algorithm solves a motion planning problem in the following fashion: * for each vertex of each polygonal obstacle, a line is drawn to each * other vertex that it may "see." Seeing, in this sense, means any * vertex from which we can draw a line to the current vertex such that * this line intersects no other obstacle. Once all such lines have been * drawn (or at least calculated), we perform the same operation for the * start and the goal points of the Worldspace. Once this process is * complete, there must exist an uninterrupted path from start to goal * along these lines. * * @author Dave Flynn, Tim Whittemore, Jeremy Carr * @version 2/07/04 */ public class splines implements MPAlgorithm { private double[][] validLines; private Point[] reference; /** The motion planning method. Returns a vector of SolutionPath * objects, each of which corresponds to one distinct Robot * passed within the Scenario. */ public Vector solve(Scenario problem) { //build the vector of solution paths that we will pass back to //UserInterface.java. Initialize the vector to have one slot //for each robot in the Scenario. Vector paths = new Vector(problem.getNRobots()); //we set this to be twice the number of robots in the problem plus //the number of vertioes in the worldspace to account for the //robots' start and end points. int arraySize = GetTotalVertices(problem.getWorldspace()) + 2 * problem.getNRobots(); validLines = new double[arraySize][arraySize]; InitMatrix(arraySize, problem.getWorldspace()); reference = new Point[arraySize]; ReadPoints(problem.getWorldspace(), problem); BuildEdgeMatrix(problem.getWorldspace()); int startIndex = 0; int endIndex = 0; //perform these tasks for each robot, once the edge matrix has been //built. For each robot, find the index of its start and end point //in the reference array. Pass these indices to the shortestPath //algorithm, and record the shortest path for this robot--adding //it to the vector of solutionPath objects. for(int i = 0; i < problem.getNRobots(); i++) { //find the start and end indices in our array. SolutionPath sp = problem.getPath(i); for(int j = 0; j < arraySize; j++) { if(reference[j].equals(sp.getStart())) { startIndex = j; } if(reference[j].equals(sp.getEnd())) { endIndex = j; } } //find the shortest path through our edge matrix Vector v = shortest(arraySize, startIndex, endIndex); //build the solutionpath that we want to add to the vector of paths //SolutionPath sp2 = new SolutionPath(reference[startIndex]); /*for(int k = 1; k < v.size(); k++) { sp2.addPoint(reference[((Integer)v.elementAt(k)).intValue()]); }*/ //CHANGED Vector soluPoints = new Vector(); //soluPoints.addElement(reference[startIndex]); //Load soluPoints with solution path as points for(int k = 0; k < v.size(); k++) { soluPoints.addElement(reference[((Integer)v.elementAt(k)).intValue()]); } //System.out.println("1234356"); // buildSplines(soluPoints); //Put soluPoints into a solution path /*SolutionPath sp2 = new SolutionPath(reference[startIndex]); for (int k = 0; k < soluPoints.size(); k++) { sp2.addPoint((Point)soluPoints.elementAt(k)); }*/ SolutionPath sp2 = new SolutionPath(); for (int k = 1; k < v.size(); k++) { sp2.addPoint(reference[((Integer)v.elementAt(k)).intValue()]); } buildSplines(sp2); setOrientations(sp2); paths.add(sp2); } return paths; } /** The workhorse of the Line of Sight algorithm. Given a Scenario, * in particular the Worldspace, build a list of edges from one vertex * to another for each vertex of each obstacle. For vertices that we * have already connected, ignore the reverse edge to save time. */ private void BuildEdgeMatrix(Worldspace w) { int len = reference.length; double distance = 0; Point v1, v2; for(int i = 0; i < len; i++) { for(int j = i + 1; j < len; j++) { v1 = reference[i]; v2 = reference[j]; if(!intersect(v1, v2, w)) { int diffX = (int)(reference[i].getX() - reference[j].getX()); int diffY = (int)(reference[i].getY() - reference[j].getY()); distance = Math.sqrt(diffX * diffX + diffY * diffY); validLines[i][j] = distance; validLines[j][i] = distance; } } } } //THis algorithm is the suxor. /** Utility method: given two points and a worldspace, this method checks * to see if the two points are part of the same obstacle. If they are, we * do not perform the intersection computation on them, because such points * comprise a special case that we must deal with separately. */ /* private boolean SameObstacle(Point v1, Point v2, Worldspace w) { //for each obstacle in the worldspace for(int i = 0; i < w.numObstacles; i++) { Obstacle obs = w.getObstacle(i); //check to see if this obstacle contains both points if( obs.contains(v1) && obs.contains(v2) ) {//System.out.println("Two points detected as part of same polygon"); //System.out.println("Vertex one: (" + v1.x + ", " + v1.y + ")"); //System.out.println("Vertex two: (" + v2.x + ", " + v2.y + ")"); return true; } //if this obstacle contains only the first of the two points else if(obs.contains(v1)) return false; //if this obstacle contains only the second of the two points else if(obs.contains(v2)) return false; } //if we have not returned yet, then no obstacle in the worldspace contains //both of these points. Return false and exit. return false; } */ /** Line intersection algorithm. For each pair of vertices in the * Worldspace, we must attempt to draw a line between them and check * to see whether or not this line is legal; that is, if it intersects * any of the obstacles in the Worldspace. */ public boolean intersect(Point a, Point b, Worldspace w) { if(a.equals(b)) return false; Point half = new Point((int) (.5 * b.getX() + .5 * a.getX()), (int) (.5 * b.getY() + .5 * a.getY())); for(int j = 0; j < w.numObstacles; j++) { Obstacle ob = w.getObstacle(j); Polygon temp = (Polygon) ob.getPoly(); // if (temp.contains(closeToA) || temp.contains(closeToB)) // return true; if (temp.contains(half)) return true; for(int i = 0; i < ob.getNumVertices(); i++) { Point vertOne = ob.getPoint(i); Point vertTwo = ob.getPoint((i + 1) % ob.getNumVertices()); //special case: if any vertex in the worldspace lies directly on this line, //return true. Our general line intersection algorithm does not catch this. if(isOn(a, b, vertOne)) { // System.out.println("returning from isOn now...as true"); return true; } if (straddles(a,b,vertOne,vertTwo) && straddles(vertOne, vertTwo, a, b)) return true; } } return false; } private boolean isOn (Point A, Point B, Point C) { double TOLERANCE = .001; Point Left = new Point(); Point Right = new Point(); Point Middle = new Point(); //Put the points in order- catagorize A, B, and C as //Left, Right, and Middle if(A.equals(C) || B.equals(C)) return false; if (A.getX() < B.getX()) { if (C.getX() < A.getX()) { Left = C; Middle = A; Right = B; } else if (C.getX() < B.getX()){ Left = A; Middle = C; Right = B; } else { Left = A; Middle = B; Right = C; } } else { //B < A if (C.getX() < B.getX()) { Left = C; Middle = B; Right = A; } else if (C.getX() < A.getX()){ Left = B; Middle = C; Right = A; } else { Left = B; Middle = A; Right = C; } } //Infinite Slope case if (Left.getX() == Middle.getX() && Middle.getX() == Right.getX()) if ((Left.getY() > Middle.getY() && Right.getY() < Middle.getY()) || (Left.getY() < Middle.getY() && Right.getY() > Middle.getY())) return true; double slope = (double)(Left.getY() - Right.getY())/ (Left.getX() - Right.getX()); double projectedY = (double) (Left.getY() + slope*(Middle.getX() - Left.getX())); if (Math.abs(projectedY - Middle.getY()) < TOLERANCE) return true; return false; } /** Determines the total number of vertices in the worldspace. This means * that, for each obstacle present in the worldspace, we find out how * many vertices it has, and add them all up. This is neccesary to * create the 2D array that we will use to store the lines. */ private int GetTotalVertices(Worldspace w) { int vertexCount = 0; Obstacle obs; //for each obstacle in the worldspace, get the number of vertices //it has, and add them to a counter variable we keep track of. for(int i = 0; i < w.numObstacles; i++) { obs = w.getObstacle(i); vertexCount += obs.getNumVertices(); } return vertexCount; } /** Initialize the matrix that we need. This is a 2-dimensional * array used to store relationships between vertices, where the * relationship stored is the distance between two vertices. If a line * from one vertex to the other cannot be drawn without intersecting * an obstacle, then we set the value of the matrix[x,y] to be -1, * where x and y are the two vertices in question. Similarly, the matrix * element [y,x] is set to -1, since the two elements reference the same * line. If the line can be legally drawn, the matrix element is a double * indicating the distance from one point to the other. Finally, if * the two points are the same, the value of the element [x,x] is * defined to be 0. */ private void InitMatrix(int size, Worldspace w) { for(int i = 0; i < size; i++) { for(int j = 0; j < size; j++) { //if we're looking at the intersection in the matrix of //the same point, set this slot to zero. Otherwise, //set the elements [x,y] and [y,x] to -1 for now. if(i == j) validLines[i][j] = 0; else { validLines[i][j] = -1; validLines[j][i] = -1; } } } } /** Read each point of each obstacle in the worldspace in turn. Copy * this point into the reference array. This array is used to reference * the vertices of the matrix "validLines," which stores the distance * between each vertex. The index of the reference array corresponds * to the index of the matrix; the point at index i of the reference * array stores its relationship to other points in the Worldspace * in the row [i, x] of the matrix and the column [x,i] of the matrix, * where x represents the index of any other point in the reference * array. The reference array has already been set to the proper size. */ private void ReadPoints(Worldspace w, Scenario problem) { int pointCnt = reference.length; int curPoint = 0; int startObsPoint = 0; Obstacle obs; Point p = new Point(); SolutionPath sp = new SolutionPath(); Point v1, v2; //for each obstacle in w... for(int i = 0; i < w.numObstacles; i++) { obs = w.getObstacle(i); //for wraparound, we have to keep track of where in the matrix //the first (0 index) point of a particular obstacle is stored. startObsPoint = curPoint; //for each vertex of obs... for(int j = 0; j < obs.getNumVertices(); j++) { p = obs.getPoint(j); reference[curPoint] = new Point(p); //take care of the special case of edge detection within a //single obstacle. Adjacent vertices of an obstacle form valid //edges for the purposes of our edge matrix, as we would like //the algorithm to follow the edge of an obstacle if need be. v1 = obs.getPoint(j); //take care of wraparound, for last point of obs compared to first if((j+1) >= obs.getNumVertices()) v2 = obs.getPoint(0); else v2 = obs.getPoint(j+1); int diffX = (int)(v1.getX() - v2.getX()); int diffY = (int)(v1.getY() - v2.getY()); double distance = Math.sqrt(diffX * diffX + diffY * diffY); //second part of wraparound special case if((j+1) >= obs.getNumVertices()) { validLines[curPoint][startObsPoint] = distance; validLines[startObsPoint][curPoint] = distance; } else { validLines[curPoint][curPoint + 1] = distance; validLines[curPoint+1][curPoint] = distance; } curPoint++; } } for(int i = 0; i < problem.getNRobots(); i++) { sp = problem.getPath(i); reference[curPoint] = new Point(sp.getStart()); curPoint++; reference[curPoint] = new Point(sp.getEnd()); curPoint++; } } public boolean straddles(Point a, Point b, Point c, Point d) { Point v1 = new Point((int)(b.getX() - a.getX()), (int)(b.getY() - a.getY())); Point v2 = new Point((int)(c.getX() - a.getX()), (int)(c.getY() - a.getY())); Point v3 = new Point((int)(d.getX() - a.getX()), (int)(d.getY() - a.getY())); if (((v1.getX() * v2.getY()) - (v1.getY() * v2.getX())) * ((v1.getX() * v3.getY()) - (v1.getY() * v3.getX())) < 0) return true; else return false; } public Vector computePaths(int arraySize, Point[] reference, double[][] edges, Scenario problem) { System.out.println("in computePaths"); Vector pathsToReturn = new Vector(); for (int i = 0; i < problem.getNRobots(); i++) { Point start = (Point) ((SolutionPath) ((Vector) problem.getPathList()).get(i)).getStart(); Point end = (Point) ((SolutionPath) ((Vector) problem.getPathList()).get(i)).getEnd(); int startIndex = 0; int endIndex = 0; for (int j = 0; j < arraySize; j++) { Point temp = (Point) reference[j]; if (temp.equals(start)) startIndex = j; else if (temp.equals(end)) endIndex = j; } pathsToReturn.add((SolutionPath) onePath(arraySize, edges, startIndex, endIndex, reference)); } return pathsToReturn; } public SolutionPath onePath(int arraySize, double[][] edges, int startIndex, int endIndex, Point[] reference) { System.out.println("In OnePath"); SolutionPath computedPath = new SolutionPath(); computedPath.addPoint((Point) reference[startIndex]); computedPath.addPoint((Point) reference[startIndex]); computedPath.addOrientation(0); computedPath.addOrientation(0); int[] f = new int[2 * arraySize]; int lf = 0; //length of f double[] path = new double[arraySize]; //length of shortest path from startIndex to i int[] touch = new int[arraySize]; double[] length = new double[arraySize]; //initialize stuff... path[startIndex]=0; System.out.println("Entering For Loop 1"); for (int i = 0; i < arraySize; i++) { System.out.println("In two"); touch[i]=startIndex; length[i] = edges[startIndex][i]; } //for i System.out.println("Done For Loop 1"); boolean keepLooking = true; System.out.println("Entering While Loop 1"); for (int count = 0; count < arraySize && keepLooking; count++) { System.out.println("In three"); double min = 100000000; int vnear = startIndex; for (int i = 0; i < arraySize; i++) if (length[i]>0.0 && length[i]0 && length[vnear]+edges[vnear][i] 0 && shortestPath[k][j] > 0 && shortestPath[i][j] > 0 && shortestPath[i][k]+shortestPath[k][j] < shortestPath[i][j]) { //two edges are shorter than one, update the shortestPath matrix shortestPath[i][j] = shortestPath[i][k] + shortestPath[k][j]; shortestPath[j][i] = shortestPath[i][k] + shortestPath[k][j]; history[i][j] = k; // k is included in the shortest path history[j][i] = k; } } // Help for debugging: //Prints the history matrix /* for (int x=0; x -1) g.drawLine((int)start.getX(), (int)start.getY(), (int)end.getX(), (int)end.getY()); } } /* g.setColor(Color.green); for (int i = 0; i < len; i++) { String myNumber = "" + i; g.drawString( myNumber, (int)reference[i].getX(), (int)reference[i].getY()); }*/ } public static void setOrientations(SolutionPath solution) { Point temp1 = new Point(); Point temp2 = new Point(); double newOrientRad; int newOrient; for (int i = 0; i < solution.length(); i++) { temp1 = solution.getPoint(i); temp2 = solution.getPoint(i+1); newOrientRad = Math.atan((temp2.getX()-temp1.getX())/ (temp2.getY()-temp1.getY())); newOrient = 180*(int)(newOrientRad/Math.PI); solution.setOrient(newOrient, i); } } public static void buildSplines(SolutionPath solution) { Vector points = new Vector(); for (int i = 0; i < solution.length()+1; i++) { points.add(new Point(solution.getPoint(i))); } double MAGNITUDE = .35; double UNIT; Vector temp = new Vector(); //Need to check to see length of points if (points.size() == 2) return; //Deal with start point- Assumes at least 3 point solution path Point zero = new Point((Point)points.elementAt(0)); Point three = new Point((Point)points.elementAt(1)); //System.out.println("zero: " + zero.getX() + " y " + zero.getY()); //System.out.println("three: " + three.getX() + " y " + three.getY()); UNIT = Math.sqrt(Math.pow(three.getY()-zero.getY(),2.0)+ Math.pow(three.getX()-zero.getX(),2.0)); //CHANGE //UNIT*=2; double startX = MAGNITUDE*2*(three.getX() - zero.getX())/UNIT; double startY = MAGNITUDE*2*(three.getY() - zero.getY())/UNIT; double x1 = zero.getX()+startX; double y1 = zero.getY()+startY; Point one = new Point((int)x1, (int)y1); //System.out.println("one: " + one.getX() + " y " + one.getY()); //System.out.println("one: " + one.getX() + " y " + one.getY()); Point intermediatePoint = new Point((Point)(points.elementAt(2))); UNIT = Math.sqrt(Math.pow(intermediatePoint.getY()-zero.getY(),2.0)+ Math.pow(intermediatePoint.getX()-zero.getX(),2.0)); //CHANGE //UNIT=2; double endX = MAGNITUDE*(three.getX()-zero.getX())* (intermediatePoint.getX()-zero.getX())/UNIT; double endY = MAGNITUDE*(three.getY()-zero.getY())+ (intermediatePoint.getY()-zero.getY())/UNIT; double x2 = three.getX()-endX; double y2 = three.getY()-endY; Point two = new Point((int)x2, (int)y2); temp = splineIt(zero, one, two, three, 10); for (int i = 0; i < temp.size(); i++) { points.insertElementAt(((Point)temp.elementAt(i)), i+1); } Point tempPoint = new Point(); for (int i = 10; i < points.size() - 2; i++) { zero = new Point((Point)points.elementAt(i)); three = new Point((Point)points.elementAt(i+1)); x1 = x2+2*endX; y1 = y2+2*endY; one.setLocation(x1,y1); intermediatePoint = (Point)(points.elementAt(i+2)); UNIT = Math.sqrt(Math.pow(intermediatePoint.getY()-zero.getY(),2.0)+ Math.pow(intermediatePoint.getX()-zero.getX(),2.0)); //CHANGE //UNIT*=2; endX = MAGNITUDE*(three.getX()-zero.getX())* (intermediatePoint.getX()-zero.getX())/UNIT; endY = MAGNITUDE*(three.getY()-zero.getY())* (intermediatePoint.getY()-zero.getY())/UNIT; x2 = three.getX()-endX; y2 = three.getY()-endY; two.setLocation(x2, y2); temp = splineIt(zero, one, two, three, 10); int insertLocation = i; for (int j = 0; j < temp.size(); j++) { //System.out.println(j); points.insertElementAt((Point)temp.elementAt(j), insertLocation+j+1); i++; } } //Handle The last segment intermediatePoint = (Point)points.elementAt(points.size()-3); zero = new Point((Point)points.elementAt(points.size()-2)); three = new Point((Point)points.elementAt(points.size()-1)); //MIGHT BE UNECESSARY x1 = x2+endX*2; y1 = y2+endY*2; one.setLocation(x1,y1); x2 = three.getX() - endX; y2 = three.getY() - endY; two.setLocation(x2,y2); /* if ((left && above) || (!left && !above)) one.setLocation(x2-(vectorY*2), y2 +(vectorX*2)); else one.setLocation(x2+(vectorY*2), y2 -(vectorX*2)); */ temp = splineIt(zero, one, two, three, 10); for (int i = 0; i < temp.size(); i++) { points.insertElementAt((Point)temp.elementAt(i), points.size()-1); } solution.setPointPath(points); return;// points; } public static Vector splineIt(Point zero, Point one, Point two, Point three, int numSegs) { double x0 = zero.getX(); double y0 = zero.getY(); double x1 = one.getX(); double y1 = one.getY(); double x2 = two.getX(); double y2 = two.getY(); double x3 = three.getX(); double y3 = three.getY(); double A = x3 - 3*x2 + 3*x1 - x0; double B = 3*x2 - 6*x1 + 3*x0; double C = 3*x1 - 3*x0; double D = x0; double E = y3 - 3*y2 + 3*y1 - y0; double F = 3*y2 - 6*y1 + 3*y0; double G = 3*y1 - 3*y0; double H = y0; //Splines will just have the points that should be //inserted between point zero and three Vector splines = new Vector(); for (int curSeg = 1; curSeg < numSegs; curSeg++) { double count = (double)curSeg/(double)numSegs; double newX = A * Math.pow(count,3) + B * Math.pow(count,2) + C * count + D; double newY = E * Math.pow(count,3) + F * Math.pow(count,2) + G * count + H; splines.addElement(new Point((int)newX,(int)newY)); } return splines; } }