Fixed optimized visibility graph!!!!

FRCTeam3044 · Jul 17, 2024 · 69e1c37 · 69e1c37
1 parent ecbc921
commit 69e1c37
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 7 deletions.
diff --git a/Pathfinding/build.gradle b/Pathfinding/build.gradle
@@ -6,7 +6,7 @@ plugins {
 }
 
 group 'me.nabdev.pathfinding'
-version '0.12.5-SNAPSHOT-OPTIMIZED'
+version '0.12.5-SNAPSHOT-OPTIMIZED2'
 
 java {
     withSourcesJar()

diff --git a/Pathfinding/src/main/java/me/nabdev/pathfinding/structures/Grid.java b/Pathfinding/src/main/java/me/nabdev/pathfinding/structures/Grid.java
@@ -111,8 +111,8 @@ public GridCellPair getCellPairOf(Vertex a, Vertex b, boolean forceSnapInField)
             y2 = (int) Math.floor(b.y * GridCell.ySizeDividend);
             int clampedX2 = MathUtil.clamp(x2, 0, cells.length - 1);
             int clampedY2 = MathUtil.clamp(y2, 0, cells[0].length - 1);
-            if ((!snapInField && !forceSnapInField) && clampedX2 != x || clampedY2 != y) {
-                throw new ImpossiblePathException("Vertex " + a + " is not in the field");
+            if ((!snapInField && !forceSnapInField) && clampedX2 != x2 || clampedY2 != y2) {
+                throw new ImpossiblePathException("Vertex " + b + " is not in the field");
             }
             x2 = clampedX2;
             y2 = clampedY2;

diff --git a/Pathfinding/src/main/java/me/nabdev/pathfinding/structures/Map.java b/Pathfinding/src/main/java/me/nabdev/pathfinding/structures/Map.java
@@ -115,6 +115,11 @@ public Map(ArrayList<Obstacle> obs, ArrayList<Vertex> obVertices, ArrayList<Edge
         // Uses vectors to make a list of points around the vertices of obstacles,
         // offset by the clearance parameter.
         pathVerticesStatic = calculateStaticPathVertices(clearance);
+
+        // Remove edges of obstacles that are completely outside of the field bounds.
+        validObstacleEdges = getValidObstacleEdges(obstacleEdges, obstacleVertices);
+
+        // Create a grid to speed up the visibility graph generation.
         precomputeGrid = new Grid(precomputeGridX, precomputeGridY, validObstacleEdges, obstacleVertices,
                 fieldx, fieldy, snapInField);
         for (Vertex v : pathVerticesStatic) {
@@ -123,7 +128,6 @@ public Map(ArrayList<Obstacle> obs, ArrayList<Vertex> obVertices, ArrayList<Edge
         }
         // Calculate the edges between these path vertices, so that the robot can't
         // phase through obstacles.
-        validObstacleEdges = getValidObstacleEdges(obstacleEdges, obstacleVertices);
         try {
             regenerateVisibilityGraph();
         } catch (ImpossiblePathException e) {
@@ -386,9 +390,11 @@ private boolean lineOfSight(Vertex v1, Vertex v2, ArrayList<Edge> obsEdges, Arra
             if (!e.isActive())
                 continue;
             if (Vector.dotIntersectFast(v1, v2, e.getVertexOne(obsVertices), e.getVertexTwo(obsVertices))) {
-                SmartDashboard.putNumberArray("intersect edge",
-                        new Double[] { e.getVertexOne(obstacleVertices).x, e.getVertexOne(obstacleVertices).y, 0.0,
-                                e.getVertexTwo(obstacleVertices).x, e.getVertexTwo(obstacleVertices).y, 0.0 });
+                // SmartDashboard.putNumberArray("intersect edge",
+                // new Double[] { e.getVertexOne(obstacleVertices).x,
+                // e.getVertexOne(obstacleVertices).y, 0.0,
+                // e.getVertexTwo(obstacleVertices).x, e.getVertexTwo(obstacleVertices).y, 0.0
+                // });
                 intersect = true;
                 break;
             }