Permalink
Browse files

Nathan is too tired to work commit to save work

  • Loading branch information...
1 parent f4ee02a commit a2ff022bc8956b8698d8f94d96f2293f5adb0ef2 @thenoviceoof thenoviceoof committed Nov 23, 2011
Showing with 88 additions and 16 deletions.
  1. +40 −0 hw4_world_small.txt
  2. +5 −0 path.txt
  3. +15 −8 src/Map.java
  4. +2 −2 src/PathPlanner.java
  5. +2 −1 src/Point.java
  6. +24 −5 src/Polygon.java
View
@@ -0,0 +1,40 @@
+5
+16
+7.734 1.458
+7.787 3.694
+-2.306 2.498
+-2.216 2.238
+-2.217 1.445
+-3.620 1.428
+-3.618 -3.690
+0.979 -3.686
+1.966 -3.240
+3.631 -2.620
+3.379 -1.891
+3.382 -1.141
+4.454 -1.139
+4.440 -0.914
+11.116 -0.916
+11.109 1.457
+6
+4.859620 1.190210
+4.859620 0.720210
+5.329620 0.720210
+5.304260 1.528800
+4.974260 1.528800
+4.974260 1.198800
+4
+5.329590 0.437658
+4.859590 0.437658
+4.859590 -0.032342
+5.329590 -0.032342
+4
+1.772600 -0.036179
+1.302600 -0.036179
+1.302600 -0.506179
+1.772600 -0.506179
+4
+0.467089 0.439399
+-0.002911 0.439399
+-0.002911 -0.030601
+0.467089 -0.030601
View
@@ -1 +1,6 @@
0.0 0.0
+2.9290890000000003 0.034398999999999846
+3.749089 0.034398999999999846
+7.791589999999999 -0.7873419999999995
+8.61159 -0.7873419999999995
+13.764 -0.61
View
@@ -30,7 +30,7 @@ public Map(String inputFile, String goalFile)
{
ArrayList<Point> points = processGoalFile(goalFile);
assert points != null;
- System.out.println(points);
+ /* System.out.println(points); */
Point start = points.get(0);
Point goal = points.get(1);
@@ -69,21 +69,26 @@ private void fillAdjacencyMatrix()
adjacencyMatrix[i][j] = begin.distFrom(end);
for(int k = 0; k < obstacles.size(); k++) {
Polygon poly = obstacles.get(k);
+ /* check for interior overlapping polygon crossings */ // !!!
+ /* if(k > 0 && (poly.interior(begin) || poly.interior(end)))
+ {
+ adjacencyMatrix[i][j] = Double.POSITIVE_INFINITY;
+ break;
+ } */
/* check for interior crossing of polygons */
int ind1 = poly.vertices.indexOf(begin);
int ind2 = poly.vertices.indexOf(end);
if(ind1 > -1 && ind2 > -1 &&
- (Math.abs(ind1-ind2) != 1 || Math.abs(ind1-ind2) != poly.vertices.size()-1))
+ (Math.abs(ind1-ind2) != 1 && Math.abs(ind1-ind2) != poly.vertices.size()-1))
{
adjacencyMatrix[i][j] = Double.POSITIVE_INFINITY;
break;
+ } else if(ind1 > -1 && ind2 > -1) {
+ System.out.print(ind1 + " ");
+ System.out.print(ind2 + " ");
+ System.out.print(poly.vertices.size() + " ");
+ System.out.println(poly.numVertices);
}
- /* check for interior overlapping polygon crossings */
- /* if(k != 0 && (poly.interior(begin) || poly.interior(end)))
- {
- adjacencyMatrix[i][j] = Double.POSITIVE_INFINITY;
- break;
- } */
/* do more mundane intersection checking */
if(poly.intersect(begin, end))
{
@@ -131,6 +136,8 @@ private void processFile(String inputFile)
polygons[i] = polygons[i].makeConvex();
polygons[i] = polygons[i].grow(ROBOT_SIZE/2);
}
+ System.out.println(polygons[i].vertices);
+
obstacles.add(polygons[i]);
addToMap(polygons[i]);
}
View
@@ -50,9 +50,9 @@
adj.dist = current.dist + map.adjacencyMatrix[j][ind];
adj.path = current;
}
- System.out.print(current + " ");
+ /* System.out.print(current + " ");
System.out.print(adj + " ");
- System.out.println(adj.dist);
+ System.out.println(adj.dist); */
pq.add(adj);
}
}
View
@@ -1,5 +1,6 @@
public class Point implements Comparable<Point>
{
+ private static final double THRESH_FLOAT = 0.001;
public double x;
public double y;
// below is mostly for the path finding algorithms
@@ -82,7 +83,7 @@ public Point clone()
public boolean equal(Point other)
{
- return this.x == other.x && this.y == other.y;
+ return this.distFrom(other) < THRESH_FLOAT;
}
@Override
View
@@ -52,6 +52,20 @@ public static Point intersectLineSegments(Point p1, Point p2, Point p3, Point p4
return p;
}
+ /* Ray starting at p1, in direction of p2, intersecting with segment p3/p4 */
+ public static Point intersectRay(Point p1, Point p2, Point p3, Point p4)
+ {
+ double d1, d2;
+ Point p = intersectLines(p1, p2, p3, p4);
+ if(p == null)
+ return null;
+ d2 = p3.distFrom(p4);
+ // check the line intersections themselves
+ if((d2 < p.distFrom(p3) || d2 < p.distFrom(p4)) || (p2.sub(p1).dot(p) < 0))
+ return null;
+ return p;
+ }
+
/* do we ever intersect the polygon? */
public boolean intersect(Point p1, Point p2)
{
@@ -62,10 +76,15 @@ public boolean intersect(Point p1, Point p2)
p3 = this.vertices.get(i % numVertices).clone();
p4 = this.vertices.get((i+1) % numVertices).clone();
inter = intersectLineSegments(p1,p2, p3,p4);
- if(inter != null)
- if(!(inter.equal(p1) || inter.equal(p2)))
+ if(inter != null){
+ if(!(inter.equal(p1) || inter.equal(p2))) {
+ /* System.out.print(p1 + " ");
+ System.out.print(p2 + " ");
+ System.out.println(inter); */
return true;
- /* allow movement along a polygon */
+ }
+ }
+ /* allow movement along a polygon */
if((p1.equal(p3) && p2.equal(p4)) || (p2.equal(p3) && p1.equal(p4))) {
return false;
}
@@ -85,12 +104,12 @@ public boolean interior(Point p)
for(int i = 0; i < numVertices; i++) {
p3 = this.vertices.get(i % numVertices).clone();
p4 = this.vertices.get((i+1) % numVertices).clone();
- inter = intersectLines(p,p2, p3,p4);
+ inter = intersectRay(p,p2, p3,p4);
if(inter != null && dir.dot(inter.sub(p)) > 0) {
countInter += 1;
}
}
- return countInter % 2 != 0;
+ return countInter % 2 == 1;
}
public Polygon grow(double amount)

0 comments on commit a2ff022

Please sign in to comment.