From f1bed93ca70b1cd4e44324aebcf0cd620972b23d Mon Sep 17 00:00:00 2001
From: Pim Nelissen <pim.nelissen@protonmail.com>
Date: Fri, 20 Feb 2026 12:01:33 +0100
Subject: [PATCH] add integrator for road generation

---
 src/road_gen/integrator/integrator.py | 21 +++++++++++++++++++++
 1 file changed, 21 insertions(+)
 create mode 100644 src/road_gen/integrator/integrator.py

diff --git a/src/road_gen/integrator/integrator.py b/src/road_gen/integrator/integrator.py
new file mode 100644
index 0000000..e1f8817
--- /dev/null
+++ b/src/road_gen/integrator/integrator.py
@@ -0,0 +1,21 @@
+import numpy as np
+
+
+def integrate_road(curvature: np.ndarray, ds: float = 1.0):
+    """Integrate along the curvature field to obtain X and Y coordinates.
+
+    Args:
+        curvature (np.ndarray): The curvature field.
+        ds (float, optional): _description_. Defaults to 1.0.
+
+    Returns:
+        x, y (np.ndarray): X and Y waypoints.
+    """
+
+    theta = np.zeros(len(curvature))
+    theta[1:] = np.cumsum(curvature[:-1] * ds)
+
+    x = np.cumsum(np.cos(theta) * ds)
+    y = np.cumsum(np.sin(theta) * ds)
+
+    return x, y