Motion Planning Algorithms

A C++ implementation of motion planning algorithms with SFML visualization.

A* Search

Theta* (Any-Angle Planning)

D* Lite (Dynamic Replanning)

Reverse Resumable A* (Incremental Backward Search)

RRT (Rapidly-exploring Random Tree)

RRT* (Optimal RRT)

Informed RRT* (Focused Ellipsoidal Sampling)

RRT-Connect (Bidirectional RRT)

PRM (Probabilistic Roadmap)

Project Structure

MotionPlanning/
├── CMakeLists.txt
├── include/
│   ├── core/
│   │   ├── types.hpp              # Basic types (Vec2, Cell, AABB, etc.)
│   │   ├── grid.hpp               # Grid world representation
│   │   └── environment.hpp        # Continuous 2D space with geometric obstacles
│   ├── algorithms/
│   │   ├── search_algorithm.hpp   # Base class for grid search algorithms
│   │   ├── bfs.hpp                # Breadth-First Search
│   │   ├── dijkstra.hpp           # Dijkstra's algorithm
│   │   ├── astar.hpp              # A* with heuristics
│   │   ├── theta_star.hpp         # Theta* (Any-angle planning)
│   │   ├── d_star_lite.hpp        # D* Lite (Dynamic replanning)
│   │   ├── resumable_astar.hpp    # Reverse Resumable A* (Incremental backward A*)
│   │   ├── sampling_algorithm.hpp # Base class for sampling-based planners
│   │   ├── rrt.hpp                # Rapidly-exploring Random Tree
│   │   ├── rrt_star.hpp           # RRT* (Optimal RRT with rewiring)
│   │   ├── informed_rrt_star.hpp  # Informed RRT* (Ellipsoidal sampling)
│   │   ├── rrt_connect.hpp        # RRT-Connect (Bidirectional RRT)
│   │   └── prm.hpp                # PRM (Probabilistic Roadmap)
│   └── visualization/
│       ├── colors.hpp             # Color definitions
│       └── visualizer.hpp         # SFML visualization
├── src/
│   ├── core/
│   │   ├── grid.cpp
│   │   └── environment.cpp
│   ├── algorithms/
│   │   ├── search_algorithm.cpp
│   │   ├── bfs.cpp
│   │   ├── dijkstra.cpp
│   │   ├── astar.cpp
│   │   ├── theta_star.cpp
│   │   ├── d_star_lite.cpp
│   │   ├── resumable_astar.cpp
│   │   ├── rrt.cpp
│   │   ├── rrt_star.cpp
│   │   ├── informed_rrt_star.cpp
│   │   ├── rrt_connect.cpp
│   │   └── prm.cpp
│   ├── visualization/
│   │   └── visualizer.cpp
│   └── main.cpp
└── examples/
    ├── grid_search.cpp
    ├── rrt_demo.cpp
    ├── rrt_star_demo.cpp
    ├── informed_rrt_star_demo.cpp
    ├── rrt_connect_demo.cpp
    ├── prm_demo.cpp
    ├── theta_star_demo.cpp
    ├── d_star_lite_demo.cpp
    └── resumable_astar_demo.cpp

Requirements

• CMake 3.16+
• C++17 compiler
• SFML 3.0+

Installing SFML

Windows (vcpkg)

vcpkg install sfml:x64-windows

Ubuntu/Debian

sudo apt install libsfml-dev

macOS

brew install sfml

Building

mkdir build
cd build
cmake ..
cmake --build .

If using vcpkg:

cmake -DCMAKE_TOOLCHAIN_FILE=[vcpkg-root]/scripts/buildsystems/vcpkg.cmake ..

Running

./motion_planning      # Interactive grid search visualizer
./example_grid         # Grid search algorithm comparison
./example_rrt          # RRT demo with continuous space
./example_rrt_star     # RRT* demo with path optimization
./example_informed_rrt_star  # Informed RRT* with ellipsoidal sampling
./example_rrt_connect  # RRT-Connect demo with bidirectional trees
./example_prm          # PRM demo with probabilistic roadmap
./example_theta_star   # Theta* any-angle planning demo
./example_d_star_lite  # D* Lite dynamic replanning demo
./example_resumable_astar  # Reverse Resumable A* incremental replanning demo

Controls

Grid Search (motion_planning / example_grid)

Key	Action
Left Click	Draw obstacle / Set start/goal
Right Click	Erase obstacle
S	Set Start mode
G	Set Goal mode
D	Draw Obstacle mode
1	BFS algorithm
2	Dijkstra algorithm
3	A* algorithm
Space	Run search
R	Reset visualization
C	Clear all
Esc	Quit

RRT (example_rrt)

Key	Action
Left Click	Place obstacle / Set start/goal (depends on mode)
Right Click	Remove obstacle under cursor
S	Set Start mode
G	Set Goal mode
D	Draw Rectangle mode
O	Draw Circle mode
N	Normal mode (no drawing)
+/-	Adjust obstacle size (in draw mode) or step size
Space	Run RRT
R	Reset tree (keep obstacles)
C	Clear all obstacles
Esc	Quit

RRT* (example_rrt_star)

Same controls as RRT, plus:

Key	Action
T	Toggle continue-after-goal mode
Space	Run RRT*

Informed RRT* (example_informed_rrt_star)

Same controls as RRT*, plus ellipse visualization that shrinks as the path cost improves.

Key	Action
T	Toggle continue-after-goal mode
Space	Run Informed RRT*

RRT-Connect (example_rrt_connect)

Same controls as RRT.

PRM (example_prm)

Same controls as RRT, plus:

Key	Action
K	Decrease k (neighbor count)
L	Increase k (neighbor count)
+/-	Adjust sample count (in normal mode)
Space	Build roadmap and find path

Theta* (example_theta_star)

Key	Action
Space	Run Theta* search
R	Reset visualization
Esc	Quit

D* Lite (example_d_star_lite)

Key	Action
Space	Start planning / Toggle auto-move
N	Step robot one cell (manual mode)
Left Click	Add obstacle (triggers replan)
Right Click	Remove obstacle (triggers replan)
R	Reset everything
Esc	Quit

Reverse Resumable A* (example_resumable_astar)

Key	Action
Space	Start planning / Toggle auto-move
N	Step robot one cell (manual mode)
Left Click	Add obstacle (triggers replan)
Right Click	Remove obstacle (triggers replan)
R	Reset everything
Esc	Quit

Implemented Algorithms

Graph-Based Search (Discrete Grid)

• BFS (Breadth-First Search)
• Dijkstra's Algorithm
• A* with multiple heuristics (Manhattan, Euclidean, Chebyshev, Octile)
• Theta* (Any-angle planning with line-of-sight shortcuts)
• D* Lite (Dynamic replanning with incremental search)
• Reverse Resumable A* (Incremental backward A* with search resumption)

Sampling-Based Planning (Continuous Space)

• RRT (Rapidly-exploring Random Tree)
• RRT* (Optimal RRT with rewiring)
• Informed RRT* (Ellipsoidal informed sampling for faster convergence)
• RRT-Connect (Bidirectional RRT)
• PRM (Probabilistic Roadmap)