package Algorithms; import edu.carleton.motion.*; import java.io.*; import java.util.*; import java.awt.*; import java.awt.geom.*; /** Implementation of the wall crawing algorithm. The algorithm works by * drawing by a straight line from start to finish, and then * traversing it until it hits an obsticle It then traces the outline of * the obstacle until it hits the original line again. * * * @author Robert Hirai * @version 1/22/04 */ public class Crawaller implements MPAlgorithm { public void draw(Graphics g) { } /** 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) { Worldspace w = problem.getWorldspace(); // Load the worldspace int obsCount = w.getNumObstacles(); // Load the number of obstacles Vector pointPath; Vector ipts = new Vector(); Vector tempPts = new Vector(); SolutionPath tempPath; Point start, end; Point2D.Double tempPt, checkPt; int nextPt = 0; Vector paths = new Vector(problem.getNRobots()); for(int i = 0; i < problem.getNRobots(); i++) { paths.add(i, new SolutionPath()); } // For each robot, solve it. for (int i=0; i < problem.getNRobots(); i++) { pointPath = new Vector(2); tempPath = (SolutionPath) problem.getPath(i); start = tempPath.getStart(); pointPath.add(0, start); // Add the start point to the path end = tempPath.getEnd(); // This is calculates to see if the line enters the bounding box // of the polygon. If it does, calculate any intersection points Line2D.Double masterLine = new Line2D.Double(start, end); for( int j = 0; j < obsCount; j++ ){ Obstacle o = w.getObstacle(j); Polygon p = o.getPoly(); Rectangle2D r = p.getBounds2D(); if( r.intersectsLine( masterLine ) ) { tempPts = CalculatePts( o, masterLine ); if( !tempPts.isEmpty() ) ipts.add( tempPts ); } } while(!ipts.isEmpty()) { int k; tempPt = new Point2D.Double(end.getX(),end.getY()); for( k = 0; k < ipts.size(); k++) { tempPts = (Vector) ipts.get(k); checkPt = (Point2D.Double) tempPts.get(0); if( checkPt.distance(start) < tempPt.distance(start) ) { tempPt = checkPt; nextPt = k; } } tempPts = (Vector) ipts.get(nextPt); Point first = new Point ((int)((Point2D.Double)tempPts.get(0)).getX(),(int)((Point2D.Double)tempPts.get(0)).getY()); Point last = new Point ((int)((Point2D.Double)tempPts.get(tempPts.size()-1)).getX(),(int)((Point2D.Double)tempPts.get(tempPts.size()-1)).getY()); pointPath.add(first); for(int m = 1; m < tempPts.size()-1; m++) { pointPath.add(tempPts.get(m)); } pointPath.add(last); ipts.removeElementAt(nextPt); } // System.out.println("0) Adding point " + start); // ***** pointPath.add(end); // System.out.println("pointPath=" + pointPath); // ***** ((SolutionPath)paths.get(i)).setPath(pointPath); // Put this vector into the vector of paths.. } // ((SolutionPath)paths.get(0)).showValues(); return paths; } // Stolen from // http://www.webreference.com/programming/java/beginning/chap5/3/3.html private Point2D.Double getIntersectPt(Line2D line1, Line2D line2) { Point2D.Double localPoint = new Point2D.Double(0, 0); double num = (line2.getY2()-line2.getY1())*(line2.getX1()-line1.getX1()) - (line2.getX2()-line2.getX1())*(line2.getY1()-line1.getY1()); double denom = (line2.getY2()-line2.getY1())*(line1.getX2()-line1.getX1()) - (line2.getX2()-line2.getX1())*(line1.getY2()-line1.getY1()); localPoint.x = line1.getX1() + (line1.getX2()-line1.getX1())*num/denom; localPoint.y = line1.getY1() + (line1.getY2()-line1.getY1())*num/denom; return localPoint; } // This calculates the verticies to add to the point path. The it follows the // polygon all the way around, meaning it won't jump across convex sections // if it comes to it. private Vector CalculatePts( Obstacle o, Line2D.Double l ) { Vector pointPath = new Vector(); Line2D.Double tempLine = new Line2D.Double(); Vector possiblePts = new Vector(); Point2D.Double start = (Point2D.Double) l.getP2(); Point2D.Double end = (Point2D.Double) l.getP1(); Point2D.Double p1 = new Point2D.Double(); Point2D.Double p2 = new Point2D.Double(); Point2D.Double l1 = new Point2D.Double(); Point2D.Double l2 = new Point2D.Double(); l1.setLocation( l.getP1() ); l2.setLocation( l.getP2() ); int vertCount = o.getNumVertices(); int firstVert = 0; int lastVert = 0; for( int i = 1; i < vertCount; i++ ) { p1.setLocation( o.getPoint(i-1) ); p2.setLocation( o.getPoint(i) ); //System.out.println("Checking " + (i-1) + " to " + i); tempLine = new Line2D.Double( p1, p2); Point2D.Double intersect = getIntersectPt( l, tempLine ); if( inBounds(intersect, p1, p2 ) && inBounds( intersect, l1, l2 )) { Vector iSide = new Vector(); iSide.add(intersect); iSide.add(new Integer(i-1)); iSide.add(new Integer(i)); possiblePts.add(iSide); // System.out.println("1) addin pt " + iSide);// ***** } } p1.setLocation( o.getPoint(o.getNumVertices()-1) ); p2.setLocation( o.getPoint(0) ); // System.out.println("Checking " + (o.getNumVertices()-1) + " to " + 0); tempLine = new Line2D.Double( p1, p2); Point2D.Double intersect = getIntersectPt( l, tempLine ); if( inBounds(intersect, p1, p2 ) && inBounds( intersect, l1, l2 )) { Vector iSide = new Vector(); iSide.add(intersect); iSide.add(new Integer(o.getNumVertices()-1)); iSide.add(new Integer(0)); possiblePts.add(iSide); // System.out.println("2) addin pt " + iSide);// ***** } for( int i = 0; i < possiblePts.size(); i++ ) { // System.out.println("Checking possiblePt["+i+"]"); Integer tempInt = new Integer(0); Vector iSide = (Vector) possiblePts.get(i); Point2D.Double currentPt = (Point2D.Double) iSide.get(0); // System.out.println("Distance for currentPt to start is " + currentPt.distance(l.getP1())); // System.out.println("Distance for start to start is " + start.distance(l.getP1())); if( currentPt.distance(l.getP1()) < start.distance(l.getP1()) ) { start = currentPt; tempInt = (Integer)iSide.get(2); firstVert = tempInt.intValue(); // System.out.println("firstVert "+firstVert); // System.out.println("firstVert "+currentPt+" possiblePts["+i+"]"); } // System.out.println("Distance for currentPt to end is " + currentPt.distance(l.getP2())); // System.out.println("Distance for end to end is " + end.distance(l.getP2())); if( currentPt.distance(l.getP2()) < end.distance(l.getP2()) ) { end = currentPt; tempInt = (Integer)iSide.get(1); lastVert = tempInt.intValue(); // System.out.println("lastVert "+lastVert); // System.out.println("lastVert "+currentPt+" possiblePts["+i+"]"); } } if( !start.equals(l.getP1()) && !start.equals(l.getP2()) && !end.equals(l.getP1()) && !end.equals(l.getP2()) ) { // System.out.println("l.getP1() = "+ l.getP1() + " and l.getP2() = " + l.getP2()); // System.out.println("firstVert="+firstVert+" lastVert="+lastVert); // System.out.println("Adding start " + start); pointPath.add(start); if( firstVert <= lastVert ) { for( int i = firstVert; i <= lastVert; i++) { pointPath.add(o.getPoint(i)); // System.out.println("adding middle " +o.getPoint(i)); } } else { for( int i = firstVert - 1; i > lastVert; i--) { pointPath.add(o.getPoint(i)); // System.out.println("adding middle " +o.getPoint(i)); } } // System.out.println("Adding end " + end);// ***** pointPath.add(end); } return pointPath; } /** * Method that checks to see if P is within the bounding box of the rectangle * with corners a and b */ private boolean inBounds( Point2D.Double p, Point2D.Double a, Point2D.Double b) { boolean x = false; boolean y = false; // System.out.println("p = (" + p.x + ", " + p.y + ") a = (" + a.x + ", " + a.y + ") b = (" + b.x + ", " + b.y + ")"); if(a.x > b.x) // a is right of b { if( p.x >= b.x && p.x <= a.x ) x = true; } else // b is right of a { if( p.x <= b.x && p.x >= a.x ) x = true; } if(a.y > b.y) // a is up from b { if( p.y >= b.y && p.y <= a.y ) y = true; } else // b is up from a { if( p.y <= b.y && p.y >= a.y ) y = true; } return (x && y); } }