title: Python - OpenDrive Map - Spiral / Clothoid / Euler Spiral / Cornu Spiral Interpolation using Fresnel Integrals
body: <p>The map format <a href="http://www.opendrive.org/" rel="nofollow noreferrer">OpenDrive</a>, provides (among others) the geometry of a road. Each segment of the road can have a different geometry (e.g. line, arc, spiral, polynomial). The provided information for a road geometry "spiral", is the following:</p>
  
  <pre><code> - s      - relative position of the road segment in respect to the beginning
                                                  of the road (not used in here)
   - x      - the "x" position of the starting point of the road segment
   - y      - the "y" position of the starting point of the road segment
   - hdg       - the heading of the starting point of the road segment
   - length      - the length of the road segment
   - curvStart   - the curvature at the start of the road segment
   - curvEnd     - the curvature at the end of the road segment
  </code></pre>
  
  <p>My goal is to interpolate points along the spiral, given a "resolution" parameter (e.g. resolution = 1, interpolates a point along the spiral each meter).
  The spiral geometry is such that it introduces a constant change in curvature (1/radius), so that it creates a smooth and stable transition from a line to an arc, so that lateral acceleration forces on a vehicle are smaller then a transition from a line to an arc directly (line curvature = 0, arc curvature = constant).</p>
  
  <p>The spiral always has one of it's ending points with a curvature of 0 (where it connects to the line segment of the road) and the other as a constant (e.g. 0.05 where it connects to an arc). Depending on the connection sequence, <code>curvStart</code> can be equal to  0 or constant and <code>curvEnd</code> can be also equal to 0 or constant. They cannot be both equal to 0 or constant at the same time.</p>
  
  <p>The code below is a function that takes as arguments the previously discussed parameters (given by the format) and the resolution.</p>
  
  <p>Currently, I am experiencing troubles with the following:</p>
  
  <ul>
  <li>Interpolate equidistant points of 1 meter apart (check plot 1)</li>
  <li>Obtain the correct heading of the points (check plot 2)</li>
  <li>Find a solution for the last 2 cases</li>
  </ul>
  
  <p>From my research on how to fulfill the task, I came upon few helpful resources, but none of them helped me obtain the final solution:</p>
  
  <ul>
  <li><a href="http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf" rel="nofollow noreferrer">OpenDrive Specification</a></li>
  <li><a href="https://gupea.ub.gu.se/bitstream/2077/23047/1/gupea_2077_23047_1.pdf" rel="nofollow noreferrer">Open source road generation and editing software</a> - page 40(31)</li>
  <li><a href="https://en.wikipedia.org/wiki/Euler_spiral" rel="nofollow noreferrer">Euler Spiral Wiki</a></li>
  <li><a href="http://www.netlib.org/cephes/" rel="nofollow noreferrer">Cephes library</a>, from which the scipy.special.fresnel function is derived</li>
  <li><a href="https://de.wikipedia.org/wiki/Klothoide" rel="nofollow noreferrer">Klothoide</a> - the German version of the "Clothoid" Wiki page has more formulas</li>
  <li><a href="https://stackoverflow.com/questions/12595218/parameterized-function-for-clothoid">Parameterized function for clothoid</a></li>
  <li><a href="https://stackoverflow.com/questions/13308573/scipy-what-are-the-arguments-in-scipy-special-fresnelx-out1-out2">SciPy: What are the arguments in <code>scipy.special.fresnel(x\[, out1, out2\])</code>?</a> - where it is pointed out that "scipy implementation of the function scales the argument by pi/2"</li>
  </ul>
  
  <pre><code>import numpy as np
  from math import cos, sin, pi, radians
  from scipy.special import fresnel
  import matplotlib.pyplot as plt
  %matplotlib inline
  
  def spiralInterpolation(resolution, s, x, y, hdg, length, curvStart, curvEnd):
      points = np.zeros((int(length/resolution), 1))
      points = [i*resolution for i in range(len(points))]
      xx = np.zeros_like(points)
      yy = np.zeros_like(points)
      hh = np.zeros_like(points)
      if curvStart == 0 and curvEnd &gt; 0:
          print("Case 1: curvStart == 0 and curvEnd &gt; 0")
          radius = np.abs(1/curvEnd)
          A_sq = radius*length
          ss, cc = fresnel(np.square(points)/(2*A_sq*np.sqrt(np.pi/2)))
          xx = points*cc
          yy = points*ss
          hh = np.square(points)*2*radius*length
          xx, yy, hh = rotate(xx, yy, hh, hdg)
          xx, yy = translate(xx, yy, x, y)
          xx = np.insert(xx, 0, x, axis=0)
          yy = np.insert(yy, 0, y, axis=0)
          hh = np.insert(hh, 0, hdg, axis=0)
  
      elif curvStart == 0 and curvEnd &lt; 0:
          print("Case 2: curvStart == 0 and curvEnd &lt; 0")
          radius = np.abs(1/curvEnd)
          A_sq = radius*length
          ss, cc = fresnel(np.square(points)/(2*A_sq*np.sqrt(np.pi/2)))
          xx = points*cc
          yy = points*ss*-1
          hh = np.square(points)*2*radius*length
          xx, yy, hh = rotate(xx, yy, hh, hdg)
          xx, yy = translate(xx, yy, x, y)
          xx = np.insert(xx, 0, x, axis=0)
          yy = np.insert(yy, 0, y, axis=0)
          hh = np.insert(hh, 0, hdg, axis=0)
  
      elif curvEnd == 0 and curvStart &gt; 0:
          print("Case 3: curvEnd == 0 and curvStart &gt; 0")
  
      elif curvEnd == 0 and curvStart &lt; 0:
          print("Case 4: curvEnd == 0 and curvStart &lt; 0")
  
      else:
          print("The curvature parameters differ from the 4 predefined cases. "
                "Change curvStart and/or curvEnd")
  
      n_stations = int(length/resolution) + 1
      stations = np.zeros((n_stations, 3))
      for i in range(len(xx)):
          stations[i][0] = xx[i]
          stations[i][1] = yy[i]
          stations[i][2] = hh[i]
  
      return stations
  
  def rotate(x, y, h, angle):
      # This function rotates the x and y vectors around zero
      xx = np.zeros_like(x)
      yy = np.zeros_like(y)
      hh = np.zeros_like(h)
      for i in range(len(x)):
          xx[i] = x[i]*cos(angle) - y[i]*sin(angle)
          yy[i] = x[i]*sin(angle) + y[i]*cos(angle)
          hh[i] = h[i] + angle
      return xx, yy, hh
  
  def translate(x, y, x_delta, y_delta):
      # This function translates the x and y vectors with the delta values
      xx = np.zeros_like(x)
      yy = np.zeros_like(y)
      for i in range(len(x)):
          xx[i] = x[i] + x_delta
          yy[i] = y[i] + y_delta 
      return xx, yy
  
  stations = spiralInterpolation(1, 77, 50, 100, radians(56), 40, 0, 1/20)
  
  x = []
  y = []
  h = []
  
  for station in stations:
      x.append(station[0])
      y.append(station[1])
      h.append(station[2])
  
  plt.figure(figsize=(20,13))
  plt.plot(x, y, '.')
  plt.grid(True)
  plt.axis('equal')
  plt.show()
  
  def get_heading_components(x, y, h, length=1):
      xa = np.zeros_like(x)
      ya = np.zeros_like(y)
      for i in range(len(x)):
          xa[i] = length*cos(h[i])
          ya[i] = length*sin(h[i])
      return xa, ya
  
  xa, ya = get_heading_components(x, y, h)
  plt.figure(figsize=(20,13))
  plt.quiver(x, y, xa, ya, width=0.005)
  plt.grid(True)
  plt.axis('equal')
  plt.show()
  </code></pre>