Fix path snapping bug

FRCTeam3044 · Feb 28, 2024 · a5be7c3 · a5be7c3
1 parent 018e6f6
commit a5be7c3
Show file tree

Hide file tree

Showing 4 changed files with 5 additions and 14 deletions.
diff --git a/Pathfinding/build.gradle b/Pathfinding/build.gradle
@@ -6,7 +6,7 @@ plugins {
 }
 
 group 'me.nabdev.pathfinding'
-version '0.9.19SNAPSHOT'
+version '0.9.20'
 
 java {
     withSourcesJar()

diff --git a/Pathfinding/src/main/java/me/nabdev/pathfinding/Pathfinder.java b/Pathfinding/src/main/java/me/nabdev/pathfinding/Pathfinder.java
@@ -180,7 +180,6 @@ public void periodic() {
         if (shouldInvalidate) {
             for (Obstacle obs : obstacles) {
                 obs.modifiers.invalidateCache();
-                obs.iHateProgramming();
             }
             map.calculateStaticNeighbors();
         }
@@ -419,6 +418,7 @@ private Path generatePathInner(Vertex start, ArrayList<Vertex> waypoints, Pathfi
     private Path generatePathInner(Vertex start, Vertex target, PathfindSnapMode snapMode,
             ArrayList<Vertex> dynamicVertices, boolean processPath) throws ImpossiblePathException {
         long startTime = System.nanoTime();
+        periodic();
         // Snapping is done because the center of the robot can be inside of the
         // inflated obstacle edges
         // In the case where this happened the start needs to be snapped outside
@@ -492,8 +492,7 @@ private Path generatePathInner(Vertex start, Vertex target, PathfindSnapMode sna
      * @return
      */
     private Vertex snap(Vertex point) throws ImpossiblePathException {
-        ArrayList<Obstacle> targetObs = Obstacle.isRobotInObstacle(obstacles, point);
-        targetObs.removeIf(obs -> obs.modifiers.hasModifier(ObstacleModifierTypes.ZONE_MODIFIER));
+        ArrayList<Obstacle> targetObs = Obstacle.isRobotInObstacle(obstacles, point, true);
         Vertex tempNearestVertex = point;
         int i = 0;
         while (targetObs.size() > 0) {
@@ -503,7 +502,7 @@ private Vertex snap(Vertex point) throws ImpossiblePathException {
             for (Obstacle obs : targetObs) {
                 tempNearestVertex = obs.calculateNearestPointFromInside(tempNearestVertex);
             }
-            targetObs = Obstacle.isRobotInObstacle(obstacles, tempNearestVertex);
+            targetObs = Obstacle.isRobotInObstacle(obstacles, tempNearestVertex, true);
             i++;
         }
         return tempNearestVertex;

diff --git a/Pathfinding/src/main/java/me/nabdev/pathfinding/modifiers/ModifierCollection.java b/Pathfinding/src/main/java/me/nabdev/pathfinding/modifiers/ModifierCollection.java
@@ -85,7 +85,7 @@ public boolean isActive() {
             boolean hasRequired = true;
             for (ObstacleModifier mod : requiredModifiers) {
                 if (!mod.isActive())
-                    isActive = false;
+                    hasRequired = false;
             }
             boolean hasOptional = false;
             for (ObstacleModifier mod : optionalModifiers) {

diff --git a/Pathfinding/src/main/java/me/nabdev/pathfinding/structures/Obstacle.java b/Pathfinding/src/main/java/me/nabdev/pathfinding/structures/Obstacle.java
@@ -4,7 +4,6 @@
 
 import org.json.JSONArray;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import me.nabdev.pathfinding.modifiers.ModifierCollection;
 import me.nabdev.pathfinding.modifiers.ObstacleModifier.ObstacleModifierTypes;
 
@@ -300,11 +299,4 @@ public ArrayList<Vertex> getVertices() {
     public ArrayList<Edge> getEdges() {
         return edges;
     }
-
-    public void iHateProgramming() {
-        if (id.contains("Barrier")) {
-            System.out.println("I hate programming");
-            SmartDashboard.putBoolean(id, modifiers.isActive());
-        }
-    }
 }