Optimized visibility graph

Pathfinding/build.gradle

@@ -1,12 +1,12 @@
 plugins {
     id 'java-library'
-    id "edu.wpi.first.GradleRIO" version "2024.3.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.2"
     id 'com.github.johnrengelman.shadow' version '8.1.1'
     id 'maven-publish'
 }
 
 group 'me.nabdev.pathfinding'
-version '0.12.5-SNAPSHOT'
+version '0.12.5-SNAPSHOT-OPTIMIZED3'
 
 java {
     withSourcesJar()

Pathfinding/src/main/java/me/nabdev/pathfinding/Pathfinder.java

@@ -79,6 +79,22 @@ public class Pathfinder {
      */
     private SearchAlgorithmType searchAlgorithmType;
 
+    /**
+     * How many cells to use on the x axis of the grid when precomputing the edges
+     * for the dynamic visibility graph
+     */
+    public final int precomputeGridX;
+    /**
+     * How many cells to use on the y axis of the grid when precomputing the edges
+     * for the dynamic visibility graph
+     */
+    public final int precomputeGridY;
+
+    /**
+     * Whether or not to snap to the grid
+     */
+    private boolean snapInGrid;
+
     private double lastMatchTime = DriverStationWrapper.getMatchTime();
     private Optional<Alliance> lastAlliance = DriverStationWrapper.getAlliance();
     private boolean lastIsAuto = DriverStationWrapper.isAutonomous();
@@ -107,13 +123,21 @@ public class Pathfinder {
      * @param normalizeCorners    Whether or not to normalize distance between
      *                            corner points
      * @param searchAlgorithmType The search algorithm to use
+     * @param precomputeGridX     How many cells to use on the x axis of the grid
+     *                            when precomputing the edges for the dynamic
+     *                            visibility graph
+     * @param precomputeGridY     How many cells to use on the y axis of the grid
+     *                            when precomputing the edges for the dynamic
+     *                            visibility graph
+     * @param snapInField         Whether or not to snap to the field
      * @param profiling           Whether or not to profile the pathfinding process
      * @param endgameTime         The time in seconds when the robot should start to
      *                            consider endgame obstacles
      */
     public Pathfinder(FieldData field, double pointSpacing, double cornerPointSpacing, double cornerDist,
             double clearance, double cornerSplitPercent, boolean injectPoints, boolean normalizeCorners,
-            SearchAlgorithmType searchAlgorithmType, boolean profiling, double endgameTime) {
+            SearchAlgorithmType searchAlgorithmType, int precomputeGridX, int precomputeGridY, boolean snapInField,
+            boolean profiling, double endgameTime) {
         this.pointSpacing = pointSpacing;
         this.cornerPointSpacing = cornerPointSpacing;
         this.cornerDist = cornerDist;
@@ -122,6 +146,9 @@ public Pathfinder(FieldData field, double pointSpacing, double cornerPointSpacin
         this.injectPoints = injectPoints;
         this.normalizeCorners = normalizeCorners;
         this.searchAlgorithmType = searchAlgorithmType;
+        this.precomputeGridX = precomputeGridX;
+        this.precomputeGridY = precomputeGridY;
+        this.snapInGrid = snapInField;
         this.profiling = profiling;
         Pathfinder.endgameTime = endgameTime;
 
@@ -148,7 +175,8 @@ public Pathfinder(FieldData field, double pointSpacing, double cornerPointSpacin
         }
 
         // Create the map object
-        map = new Map(obstacles, obstacleVertices, edges, clearance, field.fieldX, field.fieldY);
+        map = new Map(obstacles, obstacleVertices, edges, clearance, field.fieldX, field.fieldY, precomputeGridX,
+                precomputeGridY, snapInField);
 
         for (Obstacle obs : obstacles) {
             obs.initialize(map.getPathVerticesStatic());
@@ -159,8 +187,10 @@ public Pathfinder(FieldData field, double pointSpacing, double cornerPointSpacin
     /**
      * Updates the modifier cache based on data from the driver station and wpilib.
      * Should be called in robotPeriodic.
+     * 
+     * @throws ImpossiblePathException If no path can be found
      */
-    public void periodic() {
+    public void periodic() throws ImpossiblePathException {
         boolean shouldInvalidate = false;
         if (DriverStationWrapper.getMatchTime() < endgameTime && !(lastMatchTime < endgameTime)) {
             shouldInvalidate = true;
@@ -456,6 +486,7 @@ private Path generatePathInner(Vertex start, Vertex target, PathfindSnapMode sna
         // long searchEndTime = System.nanoTime();
 
         path.setUnsnappedTarget(unsnappedTarget);
+
         if (processPath)
             path.processPath(snapMode);
         if (profiling) {
@@ -715,10 +746,20 @@ public boolean getProfiling() {
     };
 
     /**
-     * Time in seconds when the robot should start to consider endgame obstacles.
+     * Whether or not to snap to the grid
+     * 
+     * @return Whether or not to snap to the grid
+     */
+    public boolean getSnapInGrid() {
+        return snapInGrid;
+    };
+
+    /**
+     * Time in seconds when the robot should start to consider endgame
      * 
      * @return The time in seconds when the robot should start to consider endgame
      */
+
     public static double getEndgameTime() {
         return endgameTime;
     }
@@ -808,6 +849,15 @@ public void setProfiling(boolean newProfiling) {
         profiling = newProfiling;
     };
 
+    /**
+     * Whether or not to snap to the grid
+     * 
+     * @param newSnapInGrid Whether or not to snap to the grid
+     */
+    public void setSnapInGrid(boolean newSnapInGrid) {
+        snapInGrid = newSnapInGrid;
+    };
+
     /**
      * Time in seconds when the robot should start to consider endgame obstacles.
      * 

Pathfinding/src/main/java/me/nabdev/pathfinding/PathfinderBuilder.java

@@ -21,10 +21,13 @@ public class PathfinderBuilder {
     private boolean injectPoints = true;
     private boolean normalizeCorners = true;
     private SearchAlgorithmType searchAlgorithmType = SearchAlgorithmType.ASTAR;
+    private int precomputeGridX = 8;
+    private int precomputeGridY = 4;
     private double robotWidth = 0.7;
     private double robotLength = 0.7;
     private CornerCutting cornerCutting = CornerCutting.LINE;
     private boolean profiling = false;
+    private boolean snapInField = true;
     private double endgameTime = 25;
 
     /**
@@ -170,6 +173,29 @@ public PathfinderBuilder setSearchAlgorithmType(SearchAlgorithmType searchAlgori
     }
 
     /**
+     * Sets the number of cells on the x axis to use when precomputing the grid
+     * 
+     * @param precomputeGridX The number of cells on the x axis, default 8
+     * @return The builder
+     */
+    public PathfinderBuilder setPrecomputeGridX(int precomputeGridX) {
+        this.precomputeGridX = precomputeGridX;
+        return this;
+    }
+
+    /**
+     * Sets the number of cells on the y axis to use when precomputing the grid
+     * 
+     * @param precomputeGridY The number of cells on the y axis, default 4
+     * @return The builder
+     */
+    public PathfinderBuilder setPrecomputeGridY(int precomputeGridY) {
+        this.precomputeGridY = precomputeGridY;
+        return this;
+    }
+
+    /**
+     * Sets the corner cut distance
      * Sets the corner cutting type to use (See {@link CornerCutting} for more info)
      * 
      * @param cornerCutting The corner cutting type, default LINE
@@ -196,6 +222,18 @@ public PathfinderBuilder setProfiling(boolean profiling) {
         return this;
     }
 
+    /**
+     * Sets whether or not to snap given points inside the field grid if they are
+     * outside of field bounds
+     * 
+     * @param snapInField Whether or not to snap in field, default true
+     * @return The builder
+     */
+    public PathfinderBuilder setSnapInField(boolean snapInField) {
+        this.snapInField = snapInField;
+        return this;
+    }
+
     /**
      * Sets the endgame time (time in seconds when the robot should start to
      * consider endgame obstacles). Note that this uses
@@ -233,6 +271,7 @@ public Pathfinder build() {
         // clearance is the circumcircle radius of the robot
         double clearance = Math.sqrt(Math.pow(robotWidth, 2) + Math.pow(robotLength, 2)) / 2;
         return new Pathfinder(loadedField, pointSpacing, cornerPointSpacing, cornerDist, clearance, cornerSplitPercent,
-                injectPoints, normalizeCorners, searchAlgorithmType, profiling, endgameTime);
+                injectPoints, normalizeCorners, searchAlgorithmType, precomputeGridX, precomputeGridY, snapInField,
+                profiling, endgameTime);
     }
 }

Pathfinding/src/main/java/me/nabdev/pathfinding/structures/Grid.java

@@ -0,0 +1,140 @@
+package me.nabdev.pathfinding.structures;
+
+import java.util.ArrayList;
+
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * Represents a grid used to speed up the visibility graph generation. It
+ * generates a big list of possible edges that could be in the way between any
+ * two cells, and then when generating the visibility graph, it only checks
+ * those edges.
+ */
+public class Grid {
+    private GridCell[][] cells;
+    private GridCellPair[][][][] cellPairs;
+    private boolean snapInField = false;
+
+    /**
+     * Creates a new grid with the given number of cells on each dimension.
+     * 
+     * @param xCells      The number of cells on the x axis
+     * @param yCells      The number of cells on the y axis
+     * @param edges       The edges to use when generating the possible edges
+     * @param vertices    The vertices to use when generating the possible edges
+     * @param fieldx      The field's x dimension (meters)
+     * @param fieldy      The field's y dimension (meters)
+     * @param snapInField Whether to snap the vertices to inside the field
+     */
+    public Grid(int xCells, int yCells, ArrayList<Edge> edges, ArrayList<Vertex> vertices, double fieldx,
+            double fieldy, boolean snapInField) {
+        this.snapInField = snapInField;
+        GridCell.xSize = (fieldx - Map.originx) / (double) xCells;
+        GridCell.ySize = (fieldy - Map.originy) / (double) yCells;
+        GridCell.recomputeVectors();
+        cells = new GridCell[xCells][yCells];
+        cellPairs = new GridCellPair[xCells][yCells][xCells][yCells];
+        for (int x = 0; x < xCells; x++) {
+            for (int y = 0; y < yCells; y++) {
+                cells[x][y] = new GridCell(new Vertex((GridCell.xSize / 2) + x * GridCell.xSize,
+                        (GridCell.ySize / 2) + y * GridCell.ySize));
+            }
+        }
+
+        ArrayList<Vertex> centers = new ArrayList<>();
+        for (int x = 0; x < xCells; x++) {
+            for (int y = 0; y < yCells; y++) {
+                centers.add(cells[x][y].center);
+                for (int x2 = 0; x2 < xCells; x2++) {
+                    for (int y2 = 0; y2 < yCells; y2++) {
+                        GridCellPair pair = new GridCellPair(cells[x][y], cells[x2][y2]);
+                        pair.calculatePossibleEdges(edges, vertices);
+                        cellPairs[x][y][x2][y2] = pair;
+                        // if (!possibleEdgeLookup.containsKey(pair)) {
+                        // possibleEdgeLookup.put(pair,
+                        // pair.getPossibleEdges(edges, vertices));
+                        // }
+                    }
+                }
+            }
+        }
+    }
+
+    // long calculatetimeAverage = 0;
+    // long arrayLookupAverage = 0;
+    // long totalTimeAverage = 0;
+    // int iterations = 0;
+
+    /**
+     * Gets the {@link GridCellPair} for the given vertices and caches it
+     * 
+     * @param a The first vertex
+     * @param b The second vertex
+     * @return The {@link GridCellPair} for the given vertices
+     * @throws ImpossiblePathException If the vertices are not in the field
+     */
+    public GridCellPair getCellPairOf(Vertex a, Vertex b) throws ImpossiblePathException {
+        return getCellPairOf(a, b, false);
+    }
+
+    /**
+     * Gets the {@link GridCellPair} for the given vertices and caches it
+     * 
+     * @param a                The first vertex
+     * @param b                The second vertex
+     * @param forceSnapInField Whether to force the vertices to snap to the grid
+     * @return The {@link GridCellPair} for the given vertices
+     * @throws ImpossiblePathException If the vertices are not in the field
+     */
+    public GridCellPair getCellPairOf(Vertex a, Vertex b, boolean forceSnapInField) throws ImpossiblePathException {
+        // iterations++;
+        // long startTime = System.nanoTime();
+        int x = a.gridX;
+        int y = a.gridY;
+        int x2 = b.gridX;
+        int y2 = b.gridY;
+        if (a.gridX == -1) {
+            x = (int) Math.floor(a.x * GridCell.xSizeDividend);
+            y = (int) Math.floor(a.y * GridCell.ySizeDividend);
+            int clampedX = MathUtil.clamp(x, 0, cells.length - 1);
+            int clampedY = MathUtil.clamp(y, 0, cells[0].length - 1);
+            if ((!snapInField && !forceSnapInField) && clampedX != x || clampedY != y) {
+                throw new ImpossiblePathException("Vertex " + a + " is not in the field");
+            }
+            x = clampedX;
+            y = clampedY;
+            a.gridX = x;
+            a.gridY = y;
+        }
+        if (b.gridX == -1) {
+            x2 = (int) Math.floor(b.x * GridCell.xSizeDividend);
+            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 != x2 || clampedY2 != y2) {
+                throw new ImpossiblePathException("Vertex " + b + " is not in the field");
+            }
+            x2 = clampedX2;
+            y2 = clampedY2;
+            b.gridX = x2;
+            b.gridY = y2;
+        }
+        // long endTime = System.nanoTime();
+        // long arrayLookupStart = System.nanoTime();
+        return cellPairs[x][y][x2][y2];
+        // long arrayLookupEnd = System.nanoTime();
+        // calculatetimeAverage += endTime - startTime;
+        // arrayLookupAverage += arrayLookupEnd - arrayLookupStart;
+        // totalTimeAverage += arrayLookupEnd - startTime;
+
+        // double calcTime = calculatetimeAverage / iterations;
+        // double lookupTime = arrayLookupAverage / iterations;
+        // double totalTime = totalTimeAverage / iterations;
+        // // Print out the percentage of time spent calculating the cell pair
+        // System.out.println("Calculating: " + calcTime + " (" + (double) calcTime /
+        // totalTime * 100 + "%)");
+        // // Print out the percentage of time spent looking up the cell pair
+        // System.out.println("Looking up: " + lookupTime + " (" + (double) lookupTime /
+        // totalTime * 100 + "%)");
+    }
+}