A C++17 library and simulator for calculating intercept trajectories of moving targets in 2D and 3D space, with support for dynamic constraints (max speed, turning radius) for boats, drones, and other vehicles.
- 2D and 3D Intercept Calculations: Quadratic intercept solver for optimal pursuit trajectories
- Guidance Laws: Pure Pursuit and Line-of-Sight (LOS) guidance for path following
- Dynamic Constraints: Support for max speed, turning radius, acceleration, and angular velocity limits
- Real-time Simulation: Time-stepped simulator with trajectory recording
- Visualization: ASCII art rendering and CSV export for plotting
- Comprehensive Testing: 75+ unit tests with Google Test
- Well-Documented: Extensive inline documentation and examples
mkdir build && cd build
cmake ..
make -j$(nproc)./run_all_tests./basic_2d_intercept
./basic_3d_intercept
./boat_intercept
./drone_intercept
./simulation_demointercept_3d/
├── include/intercept/ # Public API headers
│ ├── types.h # Common types and utility functions
│ ├── vector_math.h # 2D/3D vector operations
│ ├── intercept_solver.h # Core intercept algorithms
│ ├── guidance.h # Guidance laws (Pure Pursuit, LOS)
│ └── constraints.h # Dynamic constraints handling
├── src/ # Implementation files
├── simulator/ # Simulation engine and visualization
├── examples/ # Example programs
├── tests/ # Unit tests
└── docs/ # Additional documentation
The fundamental intercept algorithm solves the problem: Given a pursuer at position P with max speed s, and a target at position T moving with velocity V, find when the pursuer can reach the target.
Mathematical Formulation:
The intercept occurs when the distance traveled by the pursuer equals the distance to the target's future position:
|T + t·V - P| = s·t
Expanding this equation yields a quadratic:
a·t² + b·t + c = 0
where:
a = |V|² - s²
b = 2·(T - P)·V
c = |T - P|²
Solution:
- Use the quadratic formula to solve for t
- Choose the smallest positive root
- Handle edge cases: no solution (target too fast), negative time (already passed), etc.
Implementation:
InterceptResult result = InterceptSolver::solveQuadraticIntercept2D(
pursuer_pos, // Vec2: Current position
pursuer_max_speed, // double: Maximum speed
target_pos, // Vec2: Target position
target_velocity // Vec2: Target velocity
);
if (result.feasible) {
std::cout << "Intercept in " << result.time_to_intercept << " seconds\n";
std::cout << "At position: " << result.intercept_point << "\n";
std::cout << "Required heading: " << result.required_heading << " radians\n";
}Pure Pursuit is a geometric path tracking algorithm that calculates steering commands to reach a lookahead point. It's particularly effective for vehicles with turning constraints.
Key Concepts:
- Lookahead Distance: L_d = k · speed (where k is the lookahead gain, typically 0.5-2.0)
- Cross-Track Angle: α = angle from current heading to lookahead point
- Steering Command: Smooth path toward the target
Implementation:
PurePursuitGuidance guidance(1.0); // lookahead gain = 1.0
double desired_heading = guidance.calculateDesiredHeading(
current_pos,
current_heading,
target_pos,
current_speed
);Tuning:
- Smaller k (0.5-1.0): Tighter tracking, more aggressive turns
- Larger k (1.5-2.5): Smoother paths, better for vehicles with turning constraints
LOS guidance is commonly used for marine vehicles to follow straight-line paths. It calculates the desired heading based on cross-track error and a lookahead distance.
Key Concepts:
- Cross-Track Error: Perpendicular distance from the desired path
- Lookahead Distance: Fixed distance ahead on the path
- Convergence: Gradually steers back to the path
Implementation:
LOSGuidance guidance(10.0); // 10 meter lookahead
double desired_heading = guidance.calculateDesiredHeading(
current_pos,
path_start,
path_end
);
double error = guidance.calculateCrossTrackError(
current_pos,
path_start,
path_end
);Real vehicles have physical limitations. The constrained solver accounts for:
- Max Speed: Cannot exceed vehicle's maximum velocity
- Turning Radius: Minimum radius for non-holonomic vehicles (boats, fixed-wing)
- Acceleration: Maximum rate of speed change
- Angular Velocity: Maximum rate of heading change
Implementation:
VehicleConstraints constraints(
15.0, // max_speed (m/s)
10.0, // min_turning_radius (m)
3.0, // max_acceleration (m/s²)
0.5 // max_angular_velocity (rad/s)
);
ConstrainedInterceptSolver solver(constraints);
InterceptResult result = solver.solveWithTurningRadius(
pursuer_pos,
pursuer_heading,
pursuer_speed,
target_pos,
target_velocity
);Turning Time Penalty:
The solver estimates additional time required for turning maneuvers:
t_turn = Δθ / ω_max
where:
Δθ = heading change (radians)
ω_max = maximum angular velocity
For vehicles with minimum turning radius, the solver also considers arc length during the turn.
Static class for basic intercept calculations.
static InterceptResult solveQuadraticIntercept2D(
const Vec2& pursuer_pos,
double pursuer_max_speed,
const Vec2& target_pos,
const Vec2& target_velocity
);
static InterceptResult solveQuadraticIntercept3D(
const Vec3& pursuer_pos,
double pursuer_max_speed,
const Vec3& target_pos,
const Vec3& target_velocity
);Result structure containing intercept solution:
struct InterceptResult {
bool feasible; // Can intercept be achieved?
double time_to_intercept; // Time to reach intercept (seconds)
Vec3 intercept_point; // Location where intercept occurs
Vec3 required_velocity; // Velocity vector needed
double required_heading; // Heading angle (radians, 2D only)
double required_speed; // Speed required
};Pure pursuit path following.
explicit PurePursuitGuidance(double lookahead_gain = 1.0);
double calculateDesiredHeading(
const Vec2& position,
double heading,
const Vec2& target_position,
double speed
) const;
double calculateSteeringAngle(
const Vec2& position,
double heading,
const Vec2& target_position,
double speed,
double wheelbase = 1.0
) const;Line-of-sight guidance for marine vehicles.
explicit LOSGuidance(double lookahead_distance = 10.0);
double calculateDesiredHeading(
const Vec2& position,
const Vec2& path_start,
const Vec2& path_end
) const;
double calculateCrossTrackError(
const Vec2& position,
const Vec2& path_start,
const Vec2& path_end
) const;Physical limitations of a vehicle.
struct VehicleConstraints {
double max_speed; // Maximum speed (m/s)
double min_turning_radius; // Minimum turning radius (m)
double max_acceleration; // Maximum acceleration (m/s²)
double max_angular_velocity; // Maximum angular velocity (rad/s)
bool isHolonomic() const; // Can move in any direction?
double getLookaheadFactor() const; // Lookahead scaling factor
};Intercept solver with vehicle constraints.
explicit ConstrainedInterceptSolver(const VehicleConstraints& constraints);
InterceptResult solveWithTurningRadius(
const Vec2& pursuer_pos,
double pursuer_heading,
double pursuer_speed,
const Vec2& target_pos,
const Vec2& target_velocity
) const;
bool isFeasible(
const InterceptResult& result,
double current_heading,
double current_speed = 0.0
) const;
double calculateTurningTimePenalty(
double current_heading,
double required_heading,
double current_speed
) const;Time-stepped simulation engine.
explicit Simulator(double dt = 0.1); // Time step in seconds
size_t addEntity(const Entity& entity);
void setInterceptPair(size_t pursuer_id, size_t target_id);
void step(); // Advance by one time step
bool run(double duration); // Run for specified time
void reset(); // Reset to initial state
bool interceptAchieved(double threshold = 1.0) const;
double getInterceptDistance() const;
const std::vector<Vec3>& getTrajectory(size_t entity_id) const;Visualization and data export.
static void renderFrame2D(
const std::vector<Entity>& entities,
double bounds_min,
double bounds_max,
int resolution = 80
);
static void renderFrame2DAuto(
const std::vector<Entity>& entities,
int resolution = 80
);
static bool exportTrajectoryCSV(
const std::string& filename,
const std::vector<Entity>& entities,
const std::vector<std::vector<Vec3>>& trajectories
);
static void printSummary(
const Simulator& sim,
bool intercept_achieved,
double final_distance
);#include "intercept/intercept_solver.h"
Vec2 pursuer_pos(0.0, 0.0);
double pursuer_speed = 15.0;
Vec2 target_pos(100.0, 50.0);
Vec2 target_velocity(8.0, 4.0);
InterceptResult result = InterceptSolver::solveQuadraticIntercept2D(
pursuer_pos, pursuer_speed, target_pos, target_velocity
);
if (result.feasible) {
std::cout << "Intercept in " << result.time_to_intercept << " seconds\n";
}Vec3 drone_pos(0.0, 0.0, 0.0);
double drone_speed = 25.0;
Vec3 target_pos(150.0, 100.0, 200.0); // 200m altitude
Vec3 target_velocity(10.0, 5.0, 2.0);
InterceptResult result = InterceptSolver::solveQuadraticIntercept3D(
drone_pos, drone_speed, target_pos, target_velocity
);VehicleConstraints boat_constraints(18.0, 25.0, 2.0, 0.3);
ConstrainedInterceptSolver solver(boat_constraints);
InterceptResult result = solver.solveWithTurningRadius(
boat_pos, boat_heading, boat_speed,
target_pos, target_velocity
);
double turn_penalty = solver.calculateTurningTimePenalty(
boat_heading, result.required_heading, boat_speed
);#include "simulator/simulator.h"
#include "simulator/visualizer.h"
Simulator sim(0.1); // 0.1 second time steps
// Add pursuer
VehicleConstraints pursuer_constraints(15.0, 10.0, 3.0, 0.5);
Entity pursuer("Pursuer", Vec3(0,0,0), Vec3(0,0,0), 15.0, pursuer_constraints, true);
size_t pursuer_id = sim.addEntity(pursuer);
// Add target
Entity target("Target", Vec3(100,50,0), Vec3(5,3,0), 10.0);
size_t target_id = sim.addEntity(target);
sim.setInterceptPair(pursuer_id, target_id);
// Run simulation
bool intercepted = sim.run(30.0); // Run for 30 seconds
// Visualize
Visualizer::renderFrame2DAuto(sim.getEntities());
Visualizer::exportTrajectoryCSV("trajectory.csv", sim.getEntities(),
{sim.getTrajectory(pursuer_id), sim.getTrajectory(target_id)});After exporting to CSV, you can plot with Python:
import pandas as pd
import matplotlib.pyplot as plt
df = pd.read_csv('trajectory.csv')
for entity in df['entity'].unique():
data = df[df['entity'] == entity]
plt.plot(data['x'], data['y'], label=entity, marker='o')
plt.xlabel('X (meters)')
plt.ylabel('Y (meters)')
plt.legend()
plt.axis('equal')
plt.grid(True)
plt.title('Intercept Trajectory')
plt.show()-
Required:
- C++17 compatible compiler (GCC 7+, Clang 5+, MSVC 2017+)
- CMake 3.15+
- Google Test (automatically downloaded via CMake FetchContent)
-
Optional:
- Python 3 + matplotlib (for plotting exported trajectories)
The library includes 75+ unit tests covering:
- Vector math operations
- Intercept solver edge cases
- Guidance law correctness
- Constraint handling
- Simulation accuracy
Run tests with:
cd build
./run_all_testsIndividual test suites:
./test_vector_math
./test_intercept_solver
./test_guidance
./test_constraints- Intercept Calculation: O(1) - constant time, uses closed-form solution
- Pure Pursuit: O(1) - simple geometric calculation
- Simulation Step: O(n) where n is number of entities
- Typical Use: Real-time capable for dozens of entities at 10-100 Hz update rates
- Maritime: Patrol boat intercept scenarios, collision avoidance
- Aerial: Drone pursuit, air defense simulations
- Gaming: AI behavior for chase mechanics, missile guidance
- Robotics: Multi-agent coordination, pursuit-evasion games
- Research: Path planning algorithms, guidance law testing
Current Limitations:
- 2D turning constraints use approximate method (pure pursuit with lookahead)
- No obstacle avoidance
- Assumes constant target velocity (no evasive maneuvers prediction)
Potential Enhancements:
- Full Dubins path planning for optimal turns
- Predictive intercept for accelerating targets
- Multi-pursuer coordination
- Obstacle avoidance integration
- 3D turning constraints (for aircraft)
- Energy-optimal trajectories
MIT License - feel free to use in personal, academic, or commercial projects.
-
Pure Pursuit Algorithm:
- Coulter, R. C. (1992). "Implementation of the Pure Pursuit Path Tracking Algorithm"
-
Line-of-Sight Guidance:
- Fossen, T. I. (2011). "Handbook of Marine Craft Hydrodynamics and Motion Control"
-
Intercept Geometry:
- Zarchan, P. (2012). "Tactical and Strategic Missile Guidance"
Contributions welcome! Areas of interest:
- Additional guidance laws (proportional navigation, etc.)
- 3D flight dynamics
- Multi-agent scenarios
- Performance optimizations
For questions, bugs, or feature requests, please open an issue on GitHub.
Built with C++17 • Tested with Google Test • Documented for clarity