{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "59981fce",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from road_gen.integrator.integrator import integrate_road\n",
    "from road_gen.plotting.plot_road import plot_road"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a8d303ad",
   "metadata": {},
   "source": [
    "# Describing a road as a planar curve"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "08dda386",
   "metadata": {},
   "source": [
    "Let $r(s)$ be a [planar curve](https://en.wikipedia.org/wiki/Plane_curve) describing a road in the xy-plane as a function of *arc length* $s$ (distance traveled along the road), where $s \\in [0, L]$.\n",
    "Suppose a road of total length $L$ in the xy-plane. Let the distance traveled along the road be denoted $s$ where $s \\in [0, L]$.\n",
    "\n",
    "Let $\\kappa(s)$ be the curvature of $r(s)$\n",
    "\n",
    "$$\n",
    "\\kappa(s) = \\frac{d\\theta}{ds}\n",
    "$$\n",
    "\n",
    "where $\\theta$ is the heading (direction) of the road. Basically, $\\kappa(s)$ tells us at arc length $s$ whether the path is about to turn left or right, with larger magnitude of $\\kappa(s)$ indicating a sharper change, and $\\kappa(s) = 0$ indicating no change. A path in the xy-plane can now be fully defined by $\\kappa(s)$ and $L$, as the heading angle is simply\n",
    "\n",
    "$$\n",
    "\\theta(s) = \\int_0^s \\kappa(u)~du\n",
    "$$\n",
    "\n",
    "and\n",
    "\n",
    "$$\n",
    "x(s) = \\int_0^s \\cos(\\theta(u))~du \\; , \\; y(s) = \\int_0^s \\sin(\\theta(u))~du\n",
    "$$\n",
    "\n",
    "#### Discrete form\n",
    "\n",
    "In practice, if we are going to generate a road in computer code we need to discretize this above formulation. Suppose a step size $\\Delta s$, then at the $i$-th waypoint we have traveled a distance $s_i = i \\Delta s$ and we have $N = L/\\Delta s$ waypoints in total. At waypoint $i$, with known curvature $\\kappa_i$, the next heading is\n",
    "\n",
    "$$\n",
    "\\theta_{i+1} = \\theta_i + \\kappa_i \\Delta s\n",
    "$$\n",
    "\n",
    "which by recursion means that\n",
    "\n",
    "$$\n",
    "\\theta_{i+1} = \\sum_{j=0}^i \\theta_j + \\kappa_j \\Delta s\n",
    "$$\n",
    "\n",
    "and likewise\n",
    "\n",
    "$$\n",
    "x_{i+1} = \\sum_{j=0}^i x_j \\cos(\\theta_j) \\Delta s \\; , \\; y_{i+1} = \\sum_{j=0}^i y_j \\sin(\\theta_j) \\Delta s.\n",
    "$$\n",
    "\n",
    "This shows that with starting conditions $(x_0, y_0, \\theta_0)$ and discrete curvature field $\\{\\kappa_0, \\kappa_1 ... \\kappa_{N-1}\\}$ we can construct a path of any arbitrary length $L$ with its shape entirely determined by the curvature field."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7a22a5ef",
   "metadata": {},
   "source": [
    "# Visualizing curvature effect on final road composition"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "26135d05",
   "metadata": {},
   "source": [
    "#### Straight road\n",
    "A straight road can be defined by using curvature $\\kappa_i = 0$ at every step $i$; this would produce a straight road of length $L$ in the direction of the intitial heading $\\theta_0$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "20f6ccb4",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 16344 characters]",
      "text/plain": [
       "<Figure size 640x480 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "\n",
    "curvature = np.zeros(10)\n",
    "x, y = integrate_road(curvature)\n",
    "plot_road(x, y)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ad3897f0",
   "metadata": {},
   "source": [
    "#### Instant turn\n",
    "\n",
    "We could add a turn by setting $\\kappa \\neq 0$ somewhere along the curvature."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "7a1aceb1",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 23608 characters]",
      "text/plain": [
       "<Figure size 640x480 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "\n",
    "curvature[4] = 0.1\n",
    "x, y = integrate_road(curvature)\n",
    "plot_road(x, y)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3c04dad9",
   "metadata": {},
   "source": [
    "#### Smooth turn\n",
    "\n",
    "A smooth turn would look like curvature being constant for a few steps."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "80696363",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 25568 characters]",
      "text/plain": [
       "<Figure size 640x480 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "curvature = np.zeros(10)\n",
    "curvature[4:] = 0.02\n",
    "x, y = integrate_road(curvature)\n",
    "plot_road(x, y)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": ".venv",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
