Aisha's Notes on Lab 7 (created Mar 16, 2005)
API of current code

Lab 7 Navigation
Assumptions: 
    1.) Obstacles are 2D convex polygons.
    2.) Using integers to represent coordinates, where 1 = 10^-2 meters, ie. 1 cm (sorry Nick)
    3.) Left-Handed coordinate system
Hints:  Create main program in each class that will test/verify the methods in that class. Think about your data structures. It's useful to take a look at java Collections. Be careful of the java drawPolygon method because you want a convex polygon, so you can run your Convex Hull to guarantee that java draws the right Polygon.

Steps To Create the Path Planner
1 Read a Map From a File and Store the Values
2 Convert Map to Configuration Space
3 Create a Grid (in progress)
4 Label the Grid Cells According to Their Distance From the Goal (in progress)
5 Walk along the grid cells from Start to Goal following the cells with the least distance (in progress)
7 View All Steps in the Lab 7 GUI (created by our spftware LA David Blau)

---

1 Map
The current implementation for the map of the world, the environment in which the robot navigates, is in Map.java

1.1 Read a map from an input  file (all input is specified in integers)
The input file format is specified below. There can be any number of convex 2D polygons, with one polygon a line:

( startPoint.X, startPoint.Y )	( goalPoint.X, goalPoint.Y )	robotLength x robotWidth
( Obstacle1Vertex1.X, Obstacle1Vertex1.Y )	( Obstacle1Vertex2.X, Obstacle1Vertex2.Y )	(Obstacle1Vertex3.X, Obstacle1Vertex3.Y ) ......
( Obstacle2Vertex1.X, Obstacle1Vertex1.Y )	( Obstacle2Vertex2.X, Obstacle2Vertex2.Y )	(Obstacle2Vertex3.X, Obstacle3Vertex3.Y )......

(note:there can be more than three points on each line that representa a polygon)
   
The first line contains the start and goal points of the robot, and the length and width of the robot. The start and goal points should refer to the center of the robot. The starting point of the robot is used as the reference point to compute the configuration space.  Below is an example input file.
(0,100) (500, 500)  25 x 30
(0,10) (0,20) (20,40) (50,40) (60,20) (60,10)

Currently the format is being interpreted by a regular expression defined in the ParseFile.java class . Below is an example of how to use this class. I assume that the students can use this to parse their input, or write their own parser, using the StringTokenizer or something of that sort.

import java.awt.*;
import java.io.*;
import java.util.*
.....
public class Map{
    private LinkedList obstaclePolygons;
    private Rectangle robotDimensions;
    private Rectangle robotBoundingSquare;
    private Point startPoint, goalPoint;
    ...
   public Map(String mapFile) {
        ParseFile parser = new ParseFile(mapFile);
        obstaclePolygons = new LinkedList();
        for(int i=0; i < parser.getNumberOfObstacles(); i++){
            obstaclePolygons.add(parser.getObstacle(i));
        }
        robotDimensions = new Rectangle(parser.getRobotDimension());
        int longestSide = Math.max(robotDimensions.height, robotDimensions.width);
        robotBoundingSquare = new Rectangle(longestSide, longestSide);
        Point startPoint = new Point(parser.getRobotStart());
        goalPoint = new Point(parser.getRobotGoal());       
    }
   
    public static void main(String[] args){
        Map m = new Map("C:\\cygwin\\home\\aisha\\rss\\rss_dev\\src\\lab7\\mapinput.txt");
        m.printMap();
    }
}

1.1 Storing the Obstacles and the Robot (ie. using  java data structures)
The obstacles are stored as Polygons using java's java.awt.Polygon class. The robot is stored as a Rectangle using the java.awt.Rectangle class. Another option is to store the robot is to use the java.awt.Dimension class. The robot start and goal points are stored as Points from java.awt.Point.

1.2 Creating a Bounding Box Around the Robot
To create a bounding box (square) that encompasses the robot including its rotation a very simple method is used. Take the length of the diagonal of the robot.  Then use the 1/2 the diagonal length to determine a circle of that radius and bound it with a box.

---

2 Configuration Space
The current implementation for the configuration space implementation is in ConfigurationSpace.java

2.1 Helper Methods
Depending on the students represent their map, they will probably need a few helper methods. That they can write or we can provide for them.  Below are some of the helper method prototypes that are used in the current Lab 7 solutions.
    public static Polygon[] makePolygonArray(Collection c) 
    public static Point[] makePointArray(Collection c)
    public static void printPointArray(Point[] srcArray)
    public static Polygon pointArrayToPolygon(Point []points)
    public static Point[] removeDuplicatePoints(Point []points)//this is required in order to use GeomUtils.java

2.2 Computing the Minkowski Sum
Computing the Minkowski sum in greatly simplified when using the bounding square of the robot. The students can compute the length of the diagonal of the robot's bonding square, and use polar coordinates to map out points from each vertex of an obstacle polygon. This will create four points for every vertex of an obstacle polygon including duplicate points which need to be thrown out prior to convex hull.

2.3 Computing the Convex Hull (reference Computational Geometry M. de Berg, M. van Krevald, M. Overmars, O. Schwarzkopf)
The current implementation support the algorithm presented in the reference given above. The algorithm is concise and  straight-forward and I think the students can understand it by tracing through the code using a set of points that they make-up. To sort an Array of points, I have defined a PointComparator class that compares two points. Here is an example of how it's used.

    public static Point[] convexHull( Point points[] ){
        if( points.length <= 2){
            System.out.println("Error: Not enough points to create a polygon.");
        }
        //1. Sort points by x-coords, if tie use highest y
        Arrays.sort( points, new PointComparator() );

There is one potentially difficult method required to implement the algorithm and that is to detecting with there has been a right turn between three points. Fortunately there is some code that can take care of that in GeomUtils.java . Below is an example of how to use it.
  Point p1 = new Point(5,5);
  Point p2 = new Point(10,0);
  Point p3 = new Point(10,10);
  if(GeomUtils.typeOfTurn(p1, p2, p3) == GeomUtils.TO_THE_RIGHT ){ ... }

---

7 Visualization with the GUI
David Blau has written a great GUI to visualize the steps involved in developing the lab 7 path planner, see Lab7GUI.java. Be careful of the java drawPolygon method because you want a convex polygon, so you can run your Convex Hull to guarantee that java draws the right Polygon.