3D JPS Pathfinding (base on JPSVoxelEnv)

This document explains how to run JPS in 3D space using the JPSVoxelEnv class. JPSVoxelEnv rasterizes corridor boundaries and obstacle surfaces into a hash-indexed voxel grid, allowing each collision check to be completed in constant time O(1).

---

Features

JPSVoxelPathFinder Class

Implements the 3D JPS algorithm using surface rasterization optimized for UAV collision avoidance.

// JPSPathFinder2 extends the JPSVoxelEnv environment for 3D JPS pathfinding with rasterization
class JPSPathFinder2 : public JPSVoxelEnv
{
public:
    JPSPathFinder2(
        const std::unordered_map<size_t, SCData>& SCMap,
        const std::unordered_map<size_t, EFData>& EFMap
    );
    /**
     * Executes JPS graph search between two 3D points
     * @param startPoint  Starting position;
     * @param endPoint    Ending position;
     * @return A sequence of 3D integer grid coordinates representing the found path
     */
    std::vector<Eigen::RowVector3i> JPSGraphSearch(
        const Eigen::RowVector3d& startPoint, 
        const Eigen::RowVector3d& endPoint 
    );
    }

⚠️Critical Constraint:

1. Both startPoint and endPoint must lie within the navigable space defined by SCMap.
2. Neither startPoint nor endPoint must be located inside any obstacle volume defined by EFMap.

---

Example Usage

1. Data Preparation

This example uses testdata_simple.json from the repository.

Load the navigable‑corridor volumes (SCMap) and obstacle volumes (EFMap) from the JSON file.

// Load navigable‑corridor volumes (SCMap) and obstacle volumes (EFMap)
const auto& [SCMap,EFMap] = loadVolumeMaps("./testData/testdata_simple.json");

// Define start/end points within bounds and outside obstacles
Eigen::RowVector3d startPoint(748876.1520, 2564890.4544, 65.5); 
Eigen::RowVector3d endPoint(748678.2699, 2564651.6236, 50.0761);

×

2. Environment Initialization

The constructor voxelizes all obstacle faces and corridor boundaries, storing them in an efficient hash set called GridPointsSet (a private member of JPSVoxelPathFinder). This enables constant‑time O(1) collision checks during pathfinding.

// Build the rasterised environment (voxel hash) inside the path‑finder
JPSVoxelPathFinder _JPSVoxelPathFinder(SCMap, EFMap);

The rasterization result is visualized below:

×

3. Path Search Execution

With the voxel grid ready, execute the JPS algorithm to find a path between the two points.

const auto& bestPath = _JPSVoxelPathFinder.JPSGraphSearch(startPoint, endPoint);

4. Results

After the search completes, the algorithm returns a sequence of 3D grid coordinates representing the optimal path within the corridor.

The figure below shows the result:

×

• Obstacle‑surface voxels: Represented in red.
• Boundary voxels of the navigable corridor: Shown in green.
• JPS path: Displayed as a blue polyline

Complete Implementation

#include "Prepare.h"
#include "JPSVoxelEnv.h"
#include <chrono>  

int main() {
    // Load spatial data
    const auto& [SCMap,EFMap] = loadVolumeMaps("./testData/testdata_simple.json");

    // Define start / end points
    Eigen::RowVector3d startPoint(748876.1520, 2564890.4544, 65.5);
    Eigen::RowVector3d endPoint(748678.2699, 2564651.6236, 50.0761);

    // Initialise path‑finder (builds voxel hash)
    JPSVoxelPathFinder _JPSVoxelPathFinder(SCMap, EFMap);

    // Perform JPS search and measure execution time
    auto t0 = std::chrono::high_resolution_clock::now();
    const auto& bestPaths = _JPSVoxelPathFinder.JPSGraphSearch(startPoint, endPoint);
    auto t1 = std::chrono::high_resolution_clock::now();

    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0);
    std::cout << "JPSVoxelPathFinder execution time: " << duration.count() << " ms" << std::endl;

    return 0;
}