Track Transition Curve Experiment (Clothoid/Arc/Clothoid transition)

clothoid.py

#!/usr/bin/env python3

import numpy as np
import scipy.special as sc
import matplotlib.pyplot as plt
import matplotlib.widgets as wi

# Resolution
COUNT = 100

class Affine2D:
	def __init__(self):
		self.M = np.identity(3)

	def rotate(self, theta):
		c, s = np.cos(theta), np.sin(theta)
		R = np.array(((c, -s, 0), (s, c, 0), (0, 0, 1)))
		self.M = np.matmul(R, self.M)
		return self

	def translate(self, dx, dy):
		R = np.array(((1, 0, dx), (0, 1, dy), (0, 0, 1)))
		self.M = np.matmul(R, self.M)
		return self

	def scale(self, cx, cy):
		R = np.array(((cx, 0, 0), (0, cy, 0), (0, 0, 1)))
		self.M = np.matmul(R, self.M)
		return self

	def reflect_origin(self):
		R = np.array(((-1, 0, 0), (0, -1, 0), (0, 0, 1)))
		self.M = np.matmul(R, self.M)
		return self

	def reflect_x(self):
		R = np.array(((1, 0, 0), (0, -1, 0), (0, 0, 1)))
		self.M = np.matmul(R, self.M)
		return self

	def reflect_y(self):
		R = np.array(((-1, 0, 0), (0, 1, 0), (0, 0, 1)))
		self.M = np.matmul(R, self.M)
		return self

def make_homogeneous(xs, ys):
	return np.vstack((xs, ys, np.ones_like(xs)))

def reduce_homogeneous(hs):
	return hs[:-1]/hs[-1]

def run_affine(affine: Affine2D, xs, ys):
	hs = make_homogeneous(xs, ys)
	return reduce_homogeneous(np.matmul(affine.M, hs))

def unit_arc(base, arclen):
	ts = np.linspace(base, base + arclen, COUNT)
	return (np.cos(ts), np.sin(ts))

class Fresnel:
	def __init__(self):
		self.lim = 0.8
		self.arclen = 0.15

		self.fig = plt.figure()
		gs = self.fig.add_gridspec(ncols=1, nrows=3, height_ratios=[25.0, 1.0, 1.0])
		self.ax = self.fig.add_subplot(gs[0,0])
		self.ax.set_title('Transition Curve: Clothoid – Circular Arc – Clothoid')
		self.ax.set_xlim(0.0, 2.0)
		self.ax.set_ylim(0.0, 2.0)
		self.ax.set_aspect(aspect='equal')
		self.ax.grid(b=True)

		pseudo_xs, pseudo_ys = np.zeros(shape=(2, COUNT))
		self.p_k1,   = self.ax.plot(pseudo_xs, pseudo_ys, label='Clothoid 1')
		self.p_circ, = self.ax.plot(pseudo_xs, pseudo_ys, label='Arc')
		self.p_k2,   = self.ax.plot(pseudo_xs, pseudo_ys, label='Clothoid 2')

		self.Q1 = None
		self.Q2 = None

		self.data()
		self.ax.legend(loc='upper left')

		self.lim_slider_ax = self.fig.add_subplot(gs[1,0])
		self.lim_slider = wi.Slider(self.lim_slider_ax, 'Clothoid Length (L)', 0.05, 2.0, valinit=0.8)
		self.lim_slider.on_changed(self.changed)

		self.arclen_slider_ax = self.fig.add_subplot(gs[2,0], label='pseudo')
		self.arclen_slider = wi.Slider(self.arclen_slider_ax, 'Arc Length', 0.25, 2.0, valinit=0.15)
		self.arclen_slider.on_changed(self.changed)

		plt.show()

	def changed(self, val):
		self.lim = self.lim_slider.val
		self.arclen = self.arclen_slider.val
		self.data()
		self.fig.canvas.draw_idle()

	def data(self):
		radius = 1.0/self.lim
		# lamda = np.pi/2.0
		# curvature = 2.0 * lamda * lim
		T = np.pi/2.0 * self.lim*self.lim

		self.arclen = self.arclen*self.lim

		ts = np.linspace(0, self.lim, COUNT)
		ys, xs = sc.fresnel(ts)
		self.p_k1.set_xdata(xs)
		self.p_k1.set_ydata(ys)

		base  = -np.pi/2.0 + T
		mid_x = -radius * np.sin(T)
		mid_y =  radius * np.cos(T)

		RC = Affine2D()
		RC.scale(radius, radius)
		RC.translate(xs[-1], ys[-1])
		RC.translate(mid_x, mid_y)

		arc_xs, arc_ys = unit_arc(base, self.arclen)
		circ_xs, circ_ys = run_affine(RC, arc_xs, arc_ys)
		self.p_circ.set_xdata(circ_xs)
		self.p_circ.set_ydata(circ_ys)

		# Tangent vector angle at the end of our arc
		end = base + self.arclen + np.pi/2.0

		left = True

		RR = Affine2D()
		RR.reflect_origin()
		RR.translate(xs[-1], ys[-1])
		if left:
			RR.reflect_x()
			RR.rotate(T)
		else:
			RR.rotate(-T)
		RR.rotate(end)
		RR.translate(circ_xs[-1], circ_ys[-1])

		antixs, antiys = run_affine(RR, xs, ys)
		self.p_k2.set_xdata(antixs)
		self.p_k2.set_ydata(antiys)

		if self.Q1 is not None:
			self.Q1.remove()
		self.Q1 = self.ax.quiver(circ_xs[-1], circ_ys[-1], np.cos(end), np.sin(end))
		#self.ax.plot([circ_xs[-1], circ_xs[-1] + np.cos(end)], [circ_ys[-1], circ_ys[-1] + np.sin(end)], color='k', linestyle='-', label='tangent')

		if self.Q2 is not None:
			self.Q2.remove()
		self.Q2 = self.ax.quiver(xs[-1], ys[-1], np.cos(T), np.sin(T))

fr = Fresnel()