```python
import sympy as sp
a, x, beta, t = sp.symbols('a, x, beta, t')
sp.init_printing(use_latex = True)
```

```python
u_e = (a*x)/(1 - (beta*t))
u_e
```

```python
u_ex = sp.diff(u_e, x)
u_ex
```

```python
print(u_e * u_ex)
ux = u_e * u_ex
```

    a**2*x/(-beta*t + 1)**2

```python
u_et = sp.diff(u_e, t)
print(u_et)
```

    a*beta*x/(-beta*t + 1)**2

```python
uu = u_et + ux
sp.simplify(uu)
```

```python
cc = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
```

```python
cc[1:-1]
```


# Lecture 10

## Differential Equations II:

### From Words to Maths

```python
import numpy as np
import sympy as sp
import scipy.integrate
sp.init_printing()
##################################################
##### Matplotlib boilerplate for consistency #####
##################################################
from ipywidgets import interact
from ipywidgets import FloatSlider
from matplotlib import pyplot as plt

%matplotlib inline

from IPython.display import set_matplotlib_formats
set_matplotlib_formats('svg')

global_fig_width = 10
global_fig_height = global_fig_width / 1.61803399
font_size = 12

plt.rcParams['axes.axisbelow'] = True
plt.rcParams['axes.edgecolor'] = '0.8'
plt.rcParams['axes.grid'] = True
plt.rcParams['axes.labelpad'] = 8
plt.rcParams['axes.linewidth'] = 2
plt.rcParams['axes.titlepad'] = 16.0
plt.rcParams['axes.titlesize'] = font_size * 1.4
plt.rcParams['figure.figsize'] = (global_fig_width, global_fig_height)
plt.rcParams['font.sans-serif'] = ['Computer Modern Sans Serif', 'DejaVu Sans', 'sans-serif']
plt.rcParams['font.size'] = font_size
plt.rcParams['grid.color'] = '0.8'
plt.rcParams['grid.linestyle'] = 'dashed'
plt.rcParams['grid.linewidth'] = 2
plt.rcParams['lines.dash_capstyle'] = 'round'
plt.rcParams['lines.dashed_pattern'] = [1, 4]
plt.rcParams['xtick.labelsize'] = font_size
plt.rcParams['xtick.major.pad'] = 4
plt.rcParams['xtick.major.size'] = 0
plt.rcParams['ytick.labelsize'] = font_size
plt.rcParams['ytick.major.pad'] = 4
plt.rcParams['ytick.major.size'] = 0
##################################################
```

## Example of forming a differential equation from words:

The length $\;y\;$ cm of a leaf during the period of its growth is proportional to the amount of water it contains.
During this period the leaf retains a similar shape: that is, the ratio of its length to its width remains constant.
The leaf absorbs water from its plant at a rate proportional to $\;y\;$ and it loses water by evaporation at a rate proportional to the area
of the leaf at the time when its length is $\;y\;$ cm.

- Form a differential equation to describe the growth of the leaf.

1. Assume the length of the leaf is $\;y\;$ cm at time $\;t\;$ days after it was first observed.

2. Let the rate the leaf receives water be $\;k_1y\;$ where $\;k_1\;$ is a positive constant.

3. Area of leaf at time $\;t\;$ days is proportional to $\;y^2\;$ since it maintains its shape, so the leaf is losing water at a rate of $\;k_2y^2\;$ where $\;k_2\;$ is another positive constant.

## Example of forming a differential equation from words:

1. Assume the length of the leaf is $\;y\;$ cm at time $\;t\;$ days after it was first observed.
2. Let the rate the leaf receives water be $\;k_1y\;$ where $\;k_1\;$ is a positive constant.
3. Area of leaf at time $\;t\;$ days is proportional to $\;y^2\;$ since it maintains its shape, so the leaf is losing water at a rate of $\;k_2y^2\;$ where $\;k_2\;$ is another positive constant.

4. Rate of growth is given by $\displaystyle{ {\rm d}y\over {\rm d}t}$, which is the rate of change of its length.

$${{\rm d}y\over {\rm d}t}=k_1y-k_2y^2=y(k_1-k_2y)$$

```python
def solve(k1,k2):
    x = np.linspace(0,10,100)
    def dydt(y,t):
        return k1*y - k2*y**2
    y = scipy.integrate.odeint(dydt,0.1,x)
    plt.plot(x,y)
    plt.xlabel('x')
    plt.ylabel('y')
interact(solve, k1 = FloatSlider(value=1,min=0.01,max=2.0,step=0.01, continuous_update=False),
                k2 = FloatSlider(value=1,min=0.01,max=2.0,step=0.01, continuous_update=False))

```

## Example 2: Solid tumour growth.

An avascular tumour tends to grow in a spherical shape, depending on environmental restrictions.

Assuming that the growth rate of the tumour depends only on the availability of oxygen and nutrients, which enter the tumour solely by diffusing through its surface, derive a model for the change in tumour volume over time.

1. Denote the volume of the tumour at time $\;t\;$ by $\;V(t)\;$, and its radius by $\;R(t)$.

2. Since the tumour is a sphere, its volume $\;V\;$ is proportional to $\;R^3\;$ and its surface area to $\;R^2\;$, so the surface area is proportional to $\;V^{2/3}\;$.

3. The rate at which the tumour acquires nutrients, and hence the rate at which the volume increases, is thus proportional to $\;V^{2/3}$.

1. Denote the volume of the tumour at time $\;t\;$ by $\;V(t)\;$, and its radius by $\;R(t)$.
2. Since the tumour is a sphere, its volume $\;V\;$ is proportional to $\;R^3\;$ and its surface area to $\;R^2\;$, so the surface area is proportional to $\;V^{2/3}\;$.
3. The rate at which the tumour acquires nutrients, and hence the rate at which the volume increases, is thus proportional to $\;V^{2/3}$.

This gives us the equation:

$$\frac{{\rm d}V}{{\rm d}t} = kV^{2/3}$$

$$\frac{{\rm d}V}{{\rm d}t} = kV^{2/3}$$

Solve by separation of variables:

$$\int V^{-2/3}~{\rm d}V = \int k~{\rm d}t$$

$$V = \left({kt+c\over 3}\right)^3$$

where $c$ is a constant of integration, the value of which will depend upon the initial conditions.

```python
def solve(k,c):
    t = np.linspace(0,10,100)
    v = ((k*t + c)/3)**3
    plt.plot(t,v)
    plt.xlabel('t')
    plt.ylabel('V')
interact(solve, k = FloatSlider(value=1,min=0.01,max=2.0,step=0.01,continuous_update=False),
                c = FloatSlider(value=10,min=0.01,max=20.0,step=0.01,continuous_update=False))
```

## Second Order Differential Equations

**Example: Solve**  $\quad \displaystyle \frac{{\rm d}^2y}{{\rm d}x^2} = \left(\frac{{\rm d}y}{{\rm d}x}\right)^2$

We will use the substitution $\displaystyle z = \frac{{\rm d}y}{{\rm d}x}$.
This implies that $\displaystyle \frac{{\rm d}z}{{\rm d}x} = \frac{{\rm d}^2y}{{\rm d}x^2}$.

Substituting into the original equation, to eliminate $y$, gives

$$\frac{{\rm d}z}{{\rm d}x} = z^2$$

This is a separable equation:

$$\int z^{-2}~{\rm d}z = \int ~{\rm d}x \qquad\Rightarrow\qquad
  -{1\over z} = x + A \qquad\Rightarrow\qquad
  z = -{1\over x+A}$$

where $A$ is an arbitrary constant.

We can then replace $z$ using the original substitution to obtain a first order differential equation:
$$\frac{{\rm d}y}{{\rm d}x} = -{1\over x+A} ~~~~~~~~~~~~{\rm Equation~(9)}$$

This equation can be solved with a standard integral:

$$y = -\ln(x+A) + B$$

(how do we determine $A$ and $B$?)

Example

1. At $\;x=0,\;$ $\;\displaystyle \frac{{\rm d}y}{{\rm d}x} = -1\;$ and $\;y=0\;$.

We substitute the first condition into $\frac{{\rm d}y}{{\rm d}x} = -{1\over x+A}$ to obtain $\;A=1\;$.

Then substitute $A$ and the second condition into the eventual solution
to find $\;B=0$.

2. $\;y(0)=0\;$ and $\;y(e-1)=-1$.

This time both conditions can be substituted into the solution:

$$y(0)=0 \Rightarrow 0=B-\ln(A) \Rightarrow B=\ln(A)$$

$$y(e-1)=-1 \Rightarrow -1=\ln(A)-\ln{e-1+A} \Rightarrow A=1$$

## More tricks to do integration

Let's return to and solve our leaf example:

$${{\rm d}y\over {\rm d}t}=k_1y-k_2y^2=y(k_1-k_2y)$$

This is a separable differential equation: $$\;\displaystyle\int {{\rm d}y\over
y(k_1-k_2y)} =\int {\rm d}t.\;$$

We can split apart the integral on the RHS using **partial fractions** in SymPy.

We want $\qquad\displaystyle{1\over y(k_1-k_2y)}={A\over y}+{B\over (k_1-k_2y)}$

```python
y, k1, k2 = sp.symbols('y k_1 k_2')
sp.apart(1 / (y*(k1 - k2*y)),y)

```

So $A={1\over k_1}$ and $B={k_2\over k_1}$.

$${\rm~Thus,~~~~}\int {{\rm d}y\over y(k_1-k_2y)}=\int {{\rm d}y\over k_1y}+
\int {k_2\over k_1(k_1-k_2y)}{\rm d}y =\int dt$$

$${\rm~Thus~~~~~~ }{1\over k_1}\ln y-{k_2\over k_1k_2}\ln(k_1-k_2y)=t + c$$

We can get $c$ from initial conditions.
Try doing the algebraic manipulation of this to make $y$ the subject of the equation:

$$y={k_1\exp (k_1t+d)\over 1+ k_2 \exp(k_1t + d)}$$

where $d$ is a constant.


# Simple Car

One of the easiest examples to understand is the simple car, which is shown in Figure 13.1. We all know that a car cannot drive sideways because the back wheels would have to slide instead of roll. This is why parallel parking is challenging. If all four wheels could be turned simultaneously toward the curb, it would be trivial to park a car. The complicated maneuvers for parking a simple car arise because of rolling constraints.

 The car can be imagined as a rigid body that moves in the plane. Therefore, its C-space is $ {\cal C}= {\mathbb{R}}^2 \times {\mathbb{S}}^1$. Figure 13.1 indicates several parameters associated with the car. A configuration is denoted by $ q = (x,y,\theta)$. The body frame of the car places the origin at the center of rear axle, and the $ x$-axis points along the main axis of the car. Let $ s$ denote the (signed) speed13.2 of the car. Let $ \phi $ denote the steering angle (it is negative for the wheel orientations shown in Figure 13.1). The distance between the front and rear axles is represented as $ L$. If the steering angle is fixed at $ \phi $, the car travels in a circular motion, in which the radius of the circle is $ \rho$. Note that $ \rho$ can be determined from the intersection of the two axes shown in Figure 13.1 (the angle between these axes is $ \vert\phi\vert$).

Using the current notation, the task is to represent the motion of the car as a set of equations of the form (13.11)

\begin{equation}\dot x = f_1(x,y,\theta,s,\phi)\end{equation}

\begin{equation}\dot y = f_2(x,y,\theta,s,\phi)\end{equation}

\begin{equation}{\dot \theta} = f_3(x,y,\theta,s,\phi)\end{equation}

# Dubins Curve

 It was shown in Dubin's paper "On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents" that between any two configurations, the shortest path for the Dubins car can always be expressed as a combination of no more than three motion primitives. Each motion primitive applies a constant action over an interval of time. Furthermore, the only actions that are needed to traverse the shortest paths are $ u \in \{-1,0,1\}$. The primitives and their associated symbols are shown in Figure 15.3. The $ S$ primitive drives the car straight ahead. The $ L$ and $ R$ primitives turn as sharply as possible to the left and right, respectively. Using these symbols, each possible kind of shortest path can be designated as a sequence of three symbols that corresponds to the order in which the primitives are applied. Let such a sequence be called a word . There is no need to have two consecutive primitives of the same kind because they can be merged into one. Under this observation, ten possible words of length three are possible. Dubins showed that only these six words are possibly optimal:

$\displaystyle \{ LRL,\; RLR, \; LSL,\; LSR,\; RSL,\; RSR \} .$ 	(15.44)

The shortest path between any two configurations can always be characterized by one of these words. These are called the Dubins curves.

 Figure 15.4: The trajectories for two words are shown in $ {\cal W}= {\mathbb{R}}^2$.

To be more precise, the duration of each primitive should also be specified. For $ L$ or $ R$, let a subscript denote the total amount of rotation that accumulates during the application of the primitive. For $ S$, let a subscript denote the total distance traveled. Using such subscripts, the Dubins curves can be more precisely characterized as

$$\displaystyle \{ L_\alpha R_\beta L_\gamma, \; R_\alpha L_\beta R_\gamma, \; L_\alpha S_d L_\gamma, \; L_\alpha S_d R_\gamma, \; R_\alpha S_d L_\gamma, \; R_\alpha S_d R_\gamma \} ,$$ 	(15.45)

in which $ \alpha, \gamma \in [0,2 \pi)$, $ \beta \in (\pi,2 \pi)$, and $ d \geq 0$. Figure 15.4 illustrates two cases. Note that $ \beta$ must be greater than $ \pi $ (if it is less, then some other word becomes optimal).

It will be convenient to invent a compressed form of the words to group together paths that are qualitatively similar. This will be particularly valuable when Reeds-Shepp curves are introduced in Section 15.3.2 because there are 46 of them, as opposed to $ 6$ Dubins curves. Let $ C$ denote a symbol that means ``curve,'' and represents either $ R$ or $ L$. Using $ C$, the six words in (15.44) can be compressed to only two base words:

$$\displaystyle \{ CCC, \; CSC \} .$$ 	(15.46)

In this compressed form, remember that two consecutive $ C$s must be filled in by distinct turns ($ RR$ and $ LL$ are not allowed as subsequences). In compressed form, the base words can be specified more precisely as

$$\displaystyle \{ C_\alpha C_\beta C_\gamma, \; C_\alpha S_d C_\gamma \} ,$$ 	(15.47)

in which $ \alpha, \gamma \in [0,2 \pi)$, $ \beta \in (\pi,2 \pi)$, and $ d \geq 0$.

## Diagram for all six types of dubins curves are given below :-

From the above figure, we can see that $$ 90 + \phi  + \theta + 90 + \theta_i = 360 $$

$$ \therefore \theta_i = 180 - (\theta + \phi) $$

Our robot car is moving along a left arc from source position at point $ t_1(x_s,y_s) $ to goal position value at point $ t_2(x_g,y_g) $

$$ x_g = x_s - \rho cos(90-\theta) + \rho cos(90-\theta_i) $$

$$ x_g = x_s - \rho (cos90cos\theta + sin90sin\theta) + \rho (cos90cos\theta_i + sin90sin\theta_i) $$

we know that cos90 = 0, sin90 = 1 $$ x_g = x_s - \rho sin\theta + \rho sin\theta_i $$

$$ x_g = x_s - \rho sin\theta + \rho sin[180 - (\theta + \phi)] $$

$$ x_g = x_s - \rho sin\theta + \rho sin90cos[90-(\theta + \phi)]+cos90sin[90-(\theta + \phi)] $$

$$ x_g = x_s - \rho sin\theta + \rho cos[90-(\theta + \phi)] $$

$$ x_g = x_s - \rho sin\theta + \rho cos90cos(\theta + \phi)+ sin90sin(\theta + \phi) $$

$$ \therefore x_g = x_s - \rho sin\theta + \rho sin(\theta + \phi) $$

$$ y_g = y_s + \rho cos\theta + \rho cos\theta_i $$

$$ y_g = y_s + \rho cos\theta + \rho cos[180 - (\theta + \phi)] $$

$$ y_g = y_s + \rho cos\theta + \rho [cos90cos(90-(\theta + \phi)) - sin90sin(90-(\theta + \phi))] $$

$$ y_g = y_s + \rho cos\theta - \rho sin[90-(\theta + \phi)] $$

$$ y_g = y_s + \rho cos\theta - \rho [sin90cos(\theta + \phi) - cos90sin(\theta + \phi)] $$

$$ \therefore y_g = y_s + \rho cos\theta - \rho cos(\theta + \phi) $$

Let $(x_0,y_0;\alpha)$ be the initial configuration and $(x_1,y_1;\beta)$ e the terminal configuration.
Three corresponding operators, $L_v$(for left turn), $R_v$(for right turn) and $S_v$(for straight), are needed which transform an arbitrary configuration $(x,y,\theta)$ into its corresponding image configuration,

$$L_v(x,y;\theta) = (x_s - \rho sin\theta + \rho sin(\theta + \phi),(y_s + \rho cos\theta - \rho cos(\theta + \phi);(\theta + \phi))$$

Now imagine a right turn from $t_2$ to $t_1$ where, assume that the intial yaw angle is $\theta_h = \theta + \phi$ and final yaw angle is $(\theta_h - \phi)$ with respect to x axis

$$R_v(x,y;\theta_h) = (x_s - \rho sin(\theta_h - \phi) + \rho sin\theta_h),(y_s + \rho cos(\theta_h - \phi) - \rho cos\theta_h);(\theta - \phi))$$

$$S_v(x,y;\theta) = (x_s + vsin\theta, y_s + vcos\theta; \theta)$$

where, v is the length along the line segment

From above figure, $$\angle t_2Ct_1 = (90 - \theta_i) + (90 - \theta) = 180 - \theta_i - \theta = \theta + \phi - \theta = \phi$$

For left or right arc, the length of the line segment is denoted by $$v = r\phi$$
Euclidean distance represented as $$ d = \sqrt {(x_1 - x_0)^2 + (y_1 - y_0)^2}$$

Dubin path lengths of three segments of a whole path are t, p, q respectively where, l = t + p + q

```python
"""
Dubins Path
"""

import math
import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as Rot
import draw
import random
%matplotlib inline
```

```python
# class for PATH element
class PATH:
    def __init__(self, L, mode, x, y, yaw):
        self.L = L  # total path length [float]
        self.mode = mode  # type of each part of the path [string]
        self.x = x  # final x positions [m]
        self.y = y  # final y positions [m]
        self.yaw = yaw  # final yaw angles [rad]
```

```python
# utils
def pi_2_pi(theta):
    while theta > math.pi:
        theta -= 2.0 * math.pi

    while theta < -math.pi:
        theta += 2.0 * math.pi

    return theta
```

```python
def mod2pi(theta):
    return theta - 2.0 * math.pi * math.floor(theta / math.pi / 2.0)
```

Now apply all three operators upon source waypoint to get the goal position values for any LSL dubin path

$$L_q(S_p(L_t(x_0,y_0;\alpha)))=(x_1,y_1;\beta)$$

We know that $t=\rho\phi_1$ and $q=\rho\phi_2$

By applying the corresponding operators, we will get the following equation for $x_1$

$$x_0 - \rho sin\alpha + \rho sin(\alpha+\phi_1)+ pcos(\alpha+\phi_1)-\rho sin(\alpha+\phi_1)+\rho sin(\alpha+\phi_1+\phi_2)=x_1$$

$$x_0 - \rho sin\alpha + \rho sin(\alpha+\frac{t}{\rho}) + pcos(\alpha+\frac{t}{\rho})-\rho sin(\alpha+\frac{t}{\rho})+\rho sin(\alpha+\frac{t}{\rho}+\frac{q}{\rho})=x_1$$

$$\frac{x_0}{\rho}-sin\alpha + sin(\alpha+\frac{t}{\rho})+ \frac{p}{\rho}cos(\alpha+\frac{t}{\rho})-sin(\alpha+\frac{t}{\rho})+sin(\alpha+\frac{t}{\rho}+\frac{q}{\rho})=\frac{x_1}{\rho}$$

Let us assume that $$\hat{x_0}=\frac{x_0}{\rho},\hat{x_1}=\frac{x_1}{\rho},\hat{y_0}=\frac{y_0}{\rho},\hat{y_1}=\frac{y_1}{\rho},\hat{t}=\frac{t}{\rho},\hat{p}=\frac{p}{\rho},\hat{q}=\frac{q}{\rho},\hat{d}=\frac{d}{\rho}$$

Now above equation can be overwritten as equation(1) $$\hat{x_0}-sin\alpha + sin(\alpha+\hat{t})+ \hat{p}cos(\alpha+\hat{t})-sin(\alpha+\hat{t})+sin(\alpha+\hat{t}+\hat{q})=\hat{x_1}$$

By applying the corresponding operators, we will get the following equation (2) for $y_1$

$$\hat{y_0}+cos\alpha-cos(\alpha+\hat{t}) + \hat{p}sin(\alpha+\hat{t})+cos(\alpha+\hat{t})-cos(\alpha+\hat{t}+\hat{q})=\hat{y_1}$$

We will get another equation $$\alpha+\hat{t}+\hat{q}=\beta(mod2\pi) $$

Now equation (1) and (2) can be rewritten as follows

$$\hat{p}cos(\alpha+\hat{t})=\hat{x_1}-\hat{x_0}+sin\alpha-sin(\alpha+\hat{t}+\hat{q})$$

$$\hat{p}cos(\alpha+\hat{t})=(\hat{x_1}-\hat{x_0})+(sin\alpha-sin\beta)$$

$$\hat{p}sin(\alpha+\hat{t})=(\hat{y_1}-\hat{y_0})-(cos\alpha-cos\beta)$$

After $(1)^2 + (2)^2$, we get

$$\hat{p}^2 = (\hat{x_1}-\hat{x_0})^2 + (sin\alpha-sin\beta)^2 + 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + (\hat{y_1}-\hat{y_0})^2 + (cos\alpha-cos\beta)^2 - 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{x_1}-\hat{x_0})^2 + (\hat{y_1}-\hat{y_0})^2 + (sin\alpha-sin\beta)^2 + (cos\alpha-cos\beta)^2 + 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + (sin\alpha)^2+(sin\beta)^2-2sin\alpha sin\beta + (cos\alpha)^2+(cos\beta)^2-2cos\alpha cos\beta - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + [(sin\alpha)^2+(cos\alpha)^2]+[(sin\beta)^2+(cos\beta)^2]-2(sin\alpha sin\beta+cos\alpha cos\beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + 2 - 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

Let us assume $$d_n = (\hat{d})^2 + 2 - 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\therefore \hat{p} = \sqrt{d_n}$$

After dividing equation(2) by (1), we get

$$ tan(\alpha+\hat{t}) = \frac{(\hat{y_1}-\hat{y_0})-(cos\alpha-cos\beta)}{(\hat{x_1}-\hat{x_0})+(sin\alpha-sin\beta)} = k$$
where, k is a constant

$$(\alpha+\hat{t})= \arctan{k} = rec $$

$$\hat{t} = (-\alpha + rec)mod2\pi $$

$$\hat{q} = (\beta - rec)mod2\pi $$

```python
def LSL(alpha, beta, dist, dx, dy):
    sin_a = math.sin(alpha)
    sin_b = math.sin(beta)
    cos_a = math.cos(alpha)
    cos_b = math.cos(beta)
    cos_a_b = math.cos(alpha - beta)

    p_lsl = 2 + dist ** 2 - 2 * cos_a_b + 2 * dx * (sin_a - sin_b) + 2 * dy * (cos_a + cos_b)

    if p_lsl < 0:
        return None, None, None, ["L", "S", "L"]
    else:
        p_lsl = math.sqrt(p_lsl)

    k = (dy - cos_a + cos_b)/(dx + sin_a - sin_b)
    t_lsl = mod2pi(-alpha + math.atan(k))
    q_lsl = mod2pi(beta - math.atan(k))

    return t_lsl, p_lsl, q_lsl, ["L", "S", "L"]
```

Now apply all three operators upon source waypoint to get the goal position values for any RSR dubin path

$$R_q(S_p(R_t(x_0,y_0;\alpha)))=(x_1,y_1;\beta)$$

We know that $t=\rho\phi_1$ and $q=\rho\phi_2$

Let us assume that $$\hat{x_0}=\frac{x_0}{\rho},\hat{x_1}=\frac{x_1}{\rho},\hat{y_0}=\frac{y_0}{\rho},\hat{y_1}=\frac{y_1}{\rho},\hat{t}=\frac{t}{\rho},\hat{p}=\frac{p}{\rho},\hat{q}=\frac{q}{\rho},\hat{d}=\frac{d}{\rho}$$

By applying the corresponding operators, we will get the following equation (2) for $x_1$

$$\hat{x_0}-sin(\alpha-\hat{t}) + sin\alpha + \hat{p}cos(\alpha-\hat{t})-sin(\alpha-\hat{t}-\hat{q})+sin(\alpha-\hat{t}) =\hat{x_1}$$

By applying the corresponding operators, we will get the following equation (2) for $y_1$

$$\hat{y_0}+cos(\alpha-\hat{t})-cos\alpha + \hat{p}sin(\alpha-\hat{t})+cos(\alpha-\hat{t}-\hat{q})-cos(\alpha-\hat{t})=\hat{y_1}$$

We will get another equation $$\alpha-\hat{t}-\hat{q}=\beta(mod2\pi) $$

Now equation (1) and (2) can be rewritten as follows

$$\hat{p}cos(\alpha-\hat{t})=(\hat{x_1}-\hat{x_0})-(sin\alpha-sin\beta)$$

$$\hat{p}sin(\alpha-\hat{t})=(\hat{y_1}-\hat{y_0})+(cos\alpha-cos\beta)$$

After $(1)^2 + (2)^2$, we get

$$\hat{p}^2 = (\hat{x_1}-\hat{x_0})^2 + (sin\alpha-sin\beta)^2 - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + (\hat{y_1}-\hat{y_0})^2 + (cos\alpha-cos\beta)^2 + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{x_1}-\hat{x_0})^2 + (\hat{y_1}-\hat{y_0})^2 + (sin\alpha-sin\beta)^2 + (cos\alpha-cos\beta)^2 - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + (sin\alpha)^2+(sin\beta)^2-2sin\alpha sin\beta + (cos\alpha)^2+(cos\beta)^2-2cos\alpha cos\beta - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + [(sin\alpha)^2+(cos\alpha)^2]+[(sin\beta)^2+(cos\beta)^2]-2(sin\alpha sin\beta+cos\alpha cos\beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + 2 - 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

Let us assume $$d_n = (\hat{d})^2 + 2 - 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\therefore \hat{p} = \sqrt{d_n}$$

After dividing equation(2) by (1), we get

$$ tan(\alpha-\hat{t}) = \frac{(\hat{y_1}-\hat{y_0})+(cos\alpha-cos\beta)}{(\hat{x_1}-\hat{x_0})-(sin\alpha-sin\beta)} = k$$
where, k is a constant

$$(\alpha-\hat{t})= \arctan{k} = rec $$

$$\hat{t} = (\alpha - rec)mod2\pi $$

$$\hat{q} = (-\beta + rec)mod2\pi $$

```python
def RSR(alpha, beta, dist, dx, dy):
    sin_a = math.sin(alpha)
    sin_b = math.sin(beta)
    cos_a = math.cos(alpha)
    cos_b = math.cos(beta)
    cos_a_b = math.cos(alpha - beta)

    p_rsr = 2 + dist ** 2 - 2 * cos_a_b - 2 * dx * (sin_a - sin_b) + 2 * dy * (cos_a - cos_b)

    if p_rsr < 0:
        return None, None, None, ["R", "S", "R"]
    else:
        p_rsr = math.sqrt(p_rsr)

    k = (dy + cos_a - cos_b)/(dx - sin_a + sin_b)
    t_rsr = mod2pi(alpha - math.atan(k))
    q_rsr = mod2pi(-beta + math.atan(k))

    return t_rsr, p_rsr, q_rsr, ["R", "S", "R"]

```

Now apply all three operators upon source waypoint to get the goal position values for any LSR dubin path

$$L_q(S_p(R_t(x_0,y_0;\alpha)))=(x_1,y_1;\beta)$$

We know that $t=\rho\phi_1$ and $q=\rho\phi_2$

By applying the corresponding operators, we will get the following equation for $x_1$

$$x_0 - \rho sin(\alpha-\phi_1)+\rho sin\alpha + pcos(\alpha-\phi_1)-\rho sin(\alpha-\phi_1)+\rho sin(\alpha-\phi_1+\phi_2)=x_1$$

$$x_0 - \rho sin(\alpha-\frac{t}{\rho})+\rho sin\alpha + pcos(\alpha-\frac{t}{\rho})-\rho sin(\alpha-\frac{t}{\rho})+\rho sin(\alpha-\frac{t}{\rho}+\frac{q}{\rho})=x_1$$

$$\frac{x_0}{\rho}-sin(\alpha-\frac{t}{\rho})+sin\alpha + \frac{p}{\rho}cos(\alpha-\frac{t}{\rho})-sin(\alpha-\frac{t}{\rho})+sin(\alpha-\frac{t}{\rho}+\frac{q}{\rho})=\frac{x_1}{\rho}$$

Let us assume that $$\hat{x_0}=\frac{x_0}{\rho},\hat{x_1}=\frac{x_1}{\rho},\hat{y_0}=\frac{y_0}{\rho},\hat{y_1}=\frac{y_1}{\rho},\hat{t}=\frac{t}{\rho},\hat{p}=\frac{p}{\rho},\hat{q}=\frac{q}{\rho},\hat{d}=\frac{d}{\rho}$$

Now above equation can be overwritten as equation(1) $$\hat{x_0}-sin(\alpha-\hat{t})+sin\alpha + \hat{p}cos(\alpha-\hat{t})-sin(\alpha-\hat{t})+sin(\alpha-\hat{t}+\hat{q})=\hat{x_1}$$

By applying the corresponding operators, we will get the following equation (2) for $y_1$

$$\hat{y_0}+cos(\alpha-\hat{t})-cos\alpha + \hat{p}sin(\alpha-\hat{t})+cos(\alpha-\hat{t})-cos(\alpha-\hat{t}+\hat{q})=\hat{y_1}$$

We will get another equation $$\alpha-\hat{t}+\hat{q}=\beta(mod2\pi) $$

Now equation (1) and (2) can be rewritten as follows

$$\hat{p}cos(\alpha-\hat{t})-2sin(\alpha-\hat{t})=\hat{x_1}-\hat{x_0}-sin\alpha-sin(\alpha-\hat{t}+\hat{q})$$

$$\hat{p}cos(\alpha-\hat{t})-2sin(\alpha-\hat{t})=(\hat{x_1}-\hat{x_0})-(sin\alpha+sin\beta)$$

$$\hat{p}sin(\alpha-\hat{t})+2sin(\alpha-\hat{t})=(\hat{y_1}-\hat{y_0})+(cos\alpha+cos\beta)$$

After $(1)^2 + (2)^2$, we get

$$\hat{p}^2 + 4 = (\hat{x_1}-\hat{x_0})^2 + (sin\alpha+sin\beta)^2 - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + (\hat{y_1}-\hat{y_0})^2 + (cos\alpha+cos\beta)^2 + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{x_1}-\hat{x_0})^2 + (\hat{y_1}-\hat{y_0})^2 + (sin\alpha+sin\beta)^2 + (cos\alpha+cos\beta)^2 - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{d})^2 + (sin\alpha)^2+(sin\beta)^2+2sin\alpha sin\beta + (cos\alpha)^2+(cos\beta)^2+2cos\alpha cos\beta - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{d})^2 + [(sin\alpha)^2+(cos\alpha)^2]+[(sin\beta)^2+(cos\beta)^2]+2(sin\alpha sin\beta+cos\alpha cos\beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{d})^2 + 2 + 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 - 2 + 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

Let us assume $$d_n = (\hat{d})^2 - 2 + 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\therefore \hat{p} = \sqrt{d_n}$$

After dividing equation(1) by (2), we get

$$\frac{\hat{p}cos(\alpha-\hat{t})-2sin(\alpha-\hat{t})}{\hat{p}sin(\alpha-\hat{t})+2cos(\alpha-\hat{t})} = \frac{(\hat{x_1}-\hat{x_0})-(sin\alpha+sin\beta)}{(\hat{y_1}-\hat{y_0})+(cos\alpha+cos\beta)} = k$$
where, k is a constant

$$\frac{\hat{p}-2tan(\alpha-\hat{t})}{\hat{p}tan(\alpha-\hat{t})+ 2} = k $$

$$\hat{p}-2tan(\alpha-\hat{t}) = k\hat{p}tan(\alpha-\hat{t})+ 2k $$

$$tan(\alpha-\hat{t})= \frac{\hat{p}-2k}{k\hat{p}+2} $$

$$(\alpha-\hat{t})= \arctan{\frac{\hat{p}-2k}{k\hat{p}+2}} = rec $$

$$\hat{t} = (\alpha - rec)mod2\pi $$

$$\hat{q} = (\beta - rec)mod2\pi $$

```python
def LSR(alpha, beta, dist, dx, dy):
    sin_a = math.sin(alpha)
    sin_b = math.sin(beta)
    cos_a = math.cos(alpha)
    cos_b = math.cos(beta)
    cos_a_b = math.cos(alpha - beta)

    p_lsr = -2 + dist ** 2 + 2 * cos_a_b - 2 * dx * (sin_a + sin_b) + 2 * dy * (cos_a + cos_b)

    if p_lsr < 0:
        return None, None, None, ["L", "S", "R"]
    else:
        p_lsr = math.sqrt(p_lsr)

    k = (dx - sin_a - sin_b)/(dy + cos_a + cos_b)
    rec = math.atan2(p_lsr - 2*k , k*p_lsr + 2)
    t_lsr = mod2pi(alpha - rec)
    q_lsr = mod2pi(beta - rec)

    return t_lsr, p_lsr, q_lsr, ["L", "S", "R"]

```

Now apply all three operators upon source waypoint to get the goal position values for any RSL dubin path

$$R_q(S_p(L_t(x_0,y_0;\alpha)))=(x_1,y_1;\beta)$$

We know that $t=\rho\phi_1$ and $q=\rho\phi_2$

Let us assume that $$\hat{x_0}=\frac{x_0}{\rho},\hat{x_1}=\frac{x_1}{\rho},\hat{y_0}=\frac{y_0}{\rho},\hat{y_1}=\frac{y_1}{\rho},\hat{t}=\frac{t}{\rho},\hat{p}=\frac{p}{\rho},\hat{q}=\frac{q}{\rho},\hat{d}=\frac{d}{\rho}$$

By applying the corresponding operators, we will get the following equation for $x_1$

$$\hat{x_0}-sin\alpha+sin(\alpha+\hat{t}) + \hat{p}cos(\alpha+\hat{t})-sin(\alpha+\hat{t}-\hat{q})+sin(\alpha+\hat{t})=\hat{x_1}$$

By applying the corresponding operators, we will get the following equation (2) for $y_1$

$$\hat{y_0}+cos\alpha-cos(\alpha+\hat{t}) + \hat{p}sin(\alpha+\hat{t})+cos(\alpha+\hat{t}-\hat{q})-cos(\alpha+\hat{t})=\hat{y_1}$$

We will get another equation $$\alpha+\hat{t}-\hat{q}=\beta(mod2\pi) $$

Now equation (1) and (2) can be rewritten as follows

$$\hat{p}cos(\alpha+\hat{t})+2sin(\alpha+\hat{t})=(\hat{x_1}-\hat{x_0})+(sin\alpha+sin\beta)$$

$$\hat{p}sin(\alpha+\hat{t})-2cos(\alpha+\hat{t})=(\hat{y_1}-\hat{y_0})-(cos\alpha+cos\beta)$$

After $(1)^2 + (2)^2$, we get

$$\hat{p}^2 + 4 = (\hat{x_1}-\hat{x_0})^2 + (sin\alpha+sin\beta)^2 + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + (\hat{y_1}-\hat{y_0})^2 + (cos\alpha+cos\beta)^2 - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{x_1}-\hat{x_0})^2 + (\hat{y_1}-\hat{y_0})^2 + (sin\alpha+sin\beta)^2 + (cos\alpha+cos\beta)^2 + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{d})^2 + (sin\alpha)^2+(sin\beta)^2+2sin\alpha sin\beta + (cos\alpha)^2+(cos\beta)^2+2cos\alpha cos\beta + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{d})^2 + [(sin\alpha)^2+(cos\alpha)^2]+[(sin\beta)^2+(cos\beta)^2]+2(sin\alpha sin\beta+cos\alpha cos\beta) + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{d})^2 + 2 + 2cos(\alpha - \beta) + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 - 2 + 2cos(\alpha - \beta) + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

Let us assume $$d_n = (\hat{d})^2 - 2 + 2cos(\alpha - \beta) + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\therefore \hat{p} = \sqrt{d_n}$$

After dividing equation(1) by (2), we get

$$\frac{\hat{p}cos(\alpha+\hat{t})+2sin(\alpha+\hat{t})}{\hat{p}sin(\alpha+\hat{t})-2cos(\alpha+\hat{t})} = \frac{(\hat{x_1}-\hat{x_0})+(sin\alpha+sin\beta)}{(\hat{y_1}-\hat{y_0})-(cos\alpha+cos\beta)} = k$$
where, k is a constant

$$\frac{\hat{p}+2tan(\alpha+\hat{t})}{\hat{p}tan(\alpha+\hat{t})- 2} = k $$

$$\hat{p}+2tan(\alpha+\hat{t}) = k\hat{p}tan(\alpha+\hat{t})- 2k $$

$$tan(\alpha+\hat{t})= \frac{\hat{p}+2k}{k\hat{p}-2} $$

$$(\alpha+\hat{t})= \arctan{\frac{\hat{p}+2k}{k\hat{p}-2}} = rec $$

$$\hat{t} = (-\alpha + rec)mod2\pi $$

$$\hat{q} = (-\beta + rec)mod2\pi $$

```python
def RSL(alpha, beta, dist, dx, dy):
    sin_a = math.sin(alpha)
    sin_b = math.sin(beta)
    cos_a = math.cos(alpha)
    cos_b = math.cos(beta)
    cos_a_b = math.cos(alpha - beta)

    p_rsl = -2 + dist ** 2 + 2 * cos_a_b + 2 * dx * (sin_a + sin_b) - 2* dy * (cos_a + cos_b)

    if p_rsl < 0:
        return None, None, None, ["R", "S", "L"]
    else:
        p_rsl = math.sqrt(p_rsl)

    k = (dx + sin_a + sin_b)/(dy - cos_a - cos_b)
    rec = math.atan2(p_rsl + 2 * k , k * p_rsl - 2)
    t_rsl = mod2pi(-alpha + rec)
    q_rsl = mod2pi(-beta + rec)

    return t_rsl, p_rsl, q_rsl, ["R", "S", "L"]

```

Now apply all three operators upon source waypoint to get the goal position values for any RLR dubin path

$$R_q(L_p(R_t(x_0,y_0;\alpha)))=(x_1,y_1;\beta)$$

We know that $t=\rho\phi_1$ , $p=\rho\phi_2$ and $q=\rho\phi_3$

Let us assume that $$\hat{x_0}=\frac{x_0}{\rho},\hat{x_1}=\frac{x_1}{\rho},\hat{y_0}=\frac{y_0}{\rho},\hat{y_1}=\frac{y_1}{\rho},\hat{t}=\frac{t}{\rho},\hat{p}=\frac{p}{\rho},\hat{q}=\frac{q}{\rho},\hat{d}=\frac{d}{\rho}$$

By applying the corresponding operators, we will get the following equation (1) for $x_1$

$$\hat{x_0}-sin(\alpha-\hat{t}) + sin\alpha - sin(\alpha-\hat{t})+ sin(\alpha-\hat{t}+\hat{p}) - sin(\alpha-\hat{t}+\hat{p}-\hat{q}) + sin(\alpha-\hat{t}+\hat{p}) =\hat{x_1}$$

By applying the corresponding operators, we will get the following equation (2) for $y_1$

$$\hat{y_0}+cos(\alpha-\hat{t})-cos\alpha + cos(\alpha-\hat{t}) - cos(\alpha-\hat{t}+\hat{p})+cos(\alpha-\hat{t}+\hat{p}-\hat{q}) - cos(\alpha-\hat{t}+\hat{p}) =\hat{y_1}$$

We will get another equation $$\alpha-\hat{t}+\hat{p}-\hat{q}=\beta(mod2\pi) $$

Now equation (1) and (2) can be rewritten as follows

$$ 2sin(\alpha-\hat{t}+\hat{p}) - 2sin(\alpha-\hat{t}) = (\hat{x_1}-\hat{x_0})-(sin\alpha-sin\beta)$$

$$ - 2cos(\alpha-\hat{t}+\hat{p}) + 2cos(\alpha-\hat{t}) = (\hat{y_1}-\hat{y_0})+(cos\alpha-cos\beta)$$

After $(1)^2 + (2)^2$, we get

$$ 4 + 4 = (\hat{x_1}-\hat{x_0})^2 + (sin\alpha-sin\beta)^2 - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + (\hat{y_1}-\hat{y_0})^2 + (cos\alpha-cos\beta)^2 + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$ 8 = (\hat{x_1}-\hat{x_0})^2 + (\hat{y_1}-\hat{y_0})^2 + (sin\alpha-sin\beta)^2 + (cos\alpha-cos\beta)^2 - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$ 8 = (\hat{d})^2 + (sin\alpha)^2+(sin\beta)^2-2sin\alpha sin\beta + (cos\alpha)^2+(cos\beta)^2-2cos\alpha cos\beta - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$ 8 = (\hat{d})^2 + [(sin\alpha)^2+(cos\alpha)^2]+[(sin\beta)^2+(cos\beta)^2]-2(sin\alpha sin\beta+cos\alpha cos\beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + 2 - 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

Let us assume $$d_n = (\hat{d})^2 + 2 - 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\therefore \hat{p} = \sqrt{d_n}$$

After dividing equation(2) by (1), we get

$$ tan(\alpha-\hat{t}) = \frac{(\hat{y_1}-\hat{y_0})+(cos\alpha-cos\beta)}{(\hat{x_1}-\hat{x_0})-(sin\alpha-sin\beta)} = k$$
where, k is a constant

$$(\alpha-\hat{t})= \arctan{k} = rec $$

$$\hat{t} = (\alpha - rec)mod2\pi $$

$$\hat{q} = (-\beta + rec)mod2\pi $$

```python
def RLR(alpha, beta, dist, dx, dy):
    sin_a = math.sin(alpha)
    sin_b = math.sin(beta)
    cos_a = math.cos(alpha)
    cos_b = math.cos(beta)
    cos_a_b = math.cos(alpha - beta)

    rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_a - sin_b)) / 8.0

    if abs(rec) > 1.0:
        return None, None, None, ["R", "L", "R"]

    p_rlr = mod2pi(2 * math.pi - math.acos(rec))
    t_rlr = mod2pi(alpha - math.atan2(cos_a - cos_b, dist - sin_a + sin_b) + mod2pi(p_rlr / 2.0))
    q_rlr = mod2pi(alpha - beta - t_rlr + mod2pi(p_rlr))

    return t_rlr, p_rlr, q_rlr, ["R", "L", "R"]
```

```python
def LRL(alpha, beta, dist, dx, dy):
    sin_a = math.sin(alpha)
    sin_b = math.sin(beta)
    cos_a = math.cos(alpha)
    cos_b = math.cos(beta)
    cos_a_b = math.cos(alpha - beta)

    rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_b - sin_a)) / 8.0

    if abs(rec) > 1.0:
        return None, None, None, ["L", "R", "L"]

    p_lrl = mod2pi(2 * math.pi - math.acos(rec))
    t_lrl = mod2pi(-alpha - math.atan2(cos_a - cos_b, dist + sin_a - sin_b) + p_lrl / 2.0)
    q_lrl = mod2pi(mod2pi(beta) - alpha - t_lrl + mod2pi(p_lrl))

    return t_lrl, p_lrl, q_lrl, ["L", "R", "L"]
```

```python
def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):
    # for straight path computing the next wayponts' state values
    # l is length of a single part out of three
    # maxc is maximum curvature of car in radian
    # note that straight
    if m == "S":
        px[ind] = ox + l / maxc * math.cos(oyaw)
        py[ind] = oy + l / maxc * math.sin(oyaw)
        pyaw[ind] = oyaw
    # for curve path computing the next waypoints' state values
    # why sin(l) ?
    else:
        ldx = math.sin(l) / maxc
        if m == "L":
            ldy = (1.0 - math.cos(l)) / maxc
        elif m == "R":
            ldy = (1.0 - math.cos(l)) / (-maxc)

        gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy
        gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy
        px[ind] = ox + gdx
        py[ind] = oy + gdy

    # for left curve tangent's slope or car's yaw angle value is increasing via 1 that means rotating in anti clockwise direction
    # for right curve tangent's slope or car's yaw angle value is decreasing via 1 that means rotating in clockwise direction
    if m == "L":
        pyaw[ind] = oyaw + l
    elif m == "R":
        pyaw[ind] = oyaw - l

    if l > 0.0:
        directions[ind] = 1
    else:
        directions[ind] = -1

    return px, py, pyaw, directions
```

```python
def generate_local_course(total_length, lengths, mode, max_curv, step_size):
    # point_num specifies the number of intermediate points between start and goal waypoints
    point_num = int(total_length / step_size) + len(lengths)

    # for index variable '_' in range of point_num
    # all values in the arrays like px, py, pyaw and directions are initialized to 0 or 0.0
    # px, py denotes the coordinates of intermediate waypoints
    # pyaw specifies the yaw angle of the car at an intermdiate waypoint
    # direction represents the direction taken by car at an intermediate waypoint
    px = [0.0 for _ in range(point_num)]
    py = [0.0 for _ in range(point_num)]
    pyaw = [0.0 for _ in range(point_num)]
    directions = [0 for _ in range(point_num)]
    ind = 1

    # direction[i] = 0 means car is moving along straight line
    # direction[i] = 1 means left steering by car
    # direction[i] = -1 means right steering by car
    if lengths[0] > 0.0:
        directions[0] = 1
    else:
        directions[0] = -1

    # length[i] = +ve means distance is going to be increased in positive side
    # length[i] = -ve means distance is going to be increased in negaitive side
    if lengths[0] > 0.0:
        d = step_size
    else:
        d = -step_size

    ll = 0.0

    for m, l, i in zip(mode, lengths, range(len(mode))):
        # this lengths array consists of the lengths of three parts of the dubin path between two consecutive waypoints
        # l[i] denotes the length of any one part
        # while car is moving in straight direction or taking a left steering
        if l > 0.0:
            d = step_size
        # while car is taking a right steering
        else:
            d = -step_size

        # storing x, y and yaw values of the starting point of any one specific part
        # any part may be a right or left curve or may be a staright line
        # index value bocomes 0 from 1
        ox, oy, oyaw = px[ind], py[ind], pyaw[ind]
        ind -= 1

        # while two consecutive mode combination is straight-left or left-straight
        # we will get a positive value for multiplication of their lengths
        if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
            pd = -d - ll
        else:
            pd = d - ll

        # call interpolate() to generate all the equally spaced intermediate watpoints for any single part
        # interpolation is a process of determining the unknown values that lie in between the known data points.
        # It is mostly used to predict the unknown values for any geographical related data points such as noise level, rainfall, elevation, and so on.
        while abs(pd) <= abs(l):
            ind += 1
            px, py, pyaw, directions = interpolate(ind, pd, m, max_curv, ox, oy, oyaw, px, py, pyaw, directions)
            pd += d

        # caluculation of remaining length
        ll = l - pd - d
        # Now we use inerpolate() to generate the accute path for one part via using those generated waypoints
        ind += 1
        px, py, pyaw, directions = interpolate(ind, l, m, max_curv, ox, oy, oyaw, px, py, pyaw, directions)

    if len(px) <= 1:
        return [], [], [], []

    # remove unused data
    while len(px) >= 1 and px[-1] == 0.0:
        px.pop()
        py.pop()
        pyaw.pop()
        directions.pop()

    return px, py, pyaw, directions

```

```python
def planning_from_origin(dx, dy, dyaw, curv, radius, step_size):
    # origin is first waypoint
    # Prior Python 3.8, hypot() is used previously to find the hypotenuse of a right-angled triangle: sqrt(x*x + y*y)
    # From Python 3.8, this method is used to calculate the Euclidean norm as well.
    # For n-dimensional cases, the coordinates passed are assumed to be like (x1, x2, x3, ..., xn)
    # So Euclidean length from the origin is calculated by sqrt(x1*x1 + x2*x2 +x3*x3 .... xn*xn).) & stored in dist variable
    # radius of the circle or curvature path along which robot car is moving
    dist = math.hypot(dx, dy)
    dist_hat = dist/radius
    dx_hat = dx/radius
    dy_hat = dy/radius

    # The math.atan2() method returns the arc tangent of y/x, in radians where, x and y are the coordinates of a point (x,y).
    # The arctan function is the inverse of the tangent function.
    # It returns the angle whose tangent is a given number.
    theta = mod2pi(math.atan2(dy, dx))
    alpha = mod2pi(-theta)
    beta = mod2pi(dyaw - theta)

    # store function or methods names in planners list
    planners = [LSL, RSR, LSR, RSL, RLR, LRL]

    # best_cost value is set to positive infinity
    # None is null value
    best_cost = float("inf")
    bt, bp, bq, best_mode = None, None, None, None

    # call each planner function in the list
    # any dubin path between two waypoints is divided into three parts
    # distances of those parts are stored in t,p,q variable respectively
    # mode is an array of direction elements
    for planner in planners:
        t, p, q, mode = planner(alpha, beta, dist_hat, dx_hat, dy_hat)

        if t is None:
            continue

        # total cost or distance of the dubin path
        # we want the best directions or mode with the minimum distance
        # store the length of three parts of the path with minimum cost into bt, bp, bq
        cost = (abs(t) + abs(p) + abs(q))
        if best_cost > cost:
            bt, bp, bq, best_mode = t, p, q, mode
            best_cost = cost
    lengths = [bt, bp, bq]

    # call the generate_local_course()
    # returning the x values, y values and yaw values with directions that are needed to compute the dubin path
    x_list, y_list, yaw_list, directions = generate_local_course(sum(lengths), lengths, best_mode, curv, step_size)

    return x_list, y_list, yaw_list, best_mode, best_cost
```

```python
def calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, curv, radius, step_size=0.1):
    # step_size = 0.1 means
    # calculate the differences between source and goal positions
    dx = gx - sx
    dy = gy - sy

    # robot car is moving in xy plane and steering along z axis or direction
    # from_euler() method rotates our car along z axix using syaw angle and as_dcm() converts a car's 3D rotation into a direction cosine matrix
    # l_rot stores the (3X3) direction cosine matrix as a (2X2) matrix
    # stack() method makes a matrix of dimension (1X2)
    # T denotes the traonspose matrix with dimension(1X2) of the previous matrix  and @ denotes the matrix multiplication
    # multiplication result is stored in 1e_xy (1x2) matrix and now car's rotation is complete
    l_rot = Rot.from_euler('z', syaw).as_dcm()[0:2, 0:2]
    le_xy = np.stack([dx, dy]).T @ l_rot
    le_yaw = gyaw - syaw

    # call planning_from_origin() method with below arguments
    lp_x, lp_y, lp_yaw, mode, lengths = planning_from_origin(le_xy[0], le_xy[1], le_yaw, curv, radius, step_size)

    #
    rot = Rot.from_euler('z', -syaw).as_dcm()[0:2, 0:2]
    converted_xy = np.stack([lp_x, lp_y]).T @ rot
    x_list = converted_xy[:, 0] + sx
    y_list = converted_xy[:, 1] + sy
    yaw_list = [pi_2_pi(i_yaw + syaw) for i_yaw in lp_yaw]

    return PATH(lengths, mode, x_list, y_list, yaw_list)
```

```python
def main():
    # choose robot vehicle's states pairs: (x coordinate, y coordinate, yaw in degree
    # Yaw happens when the weight of your vehicle shifts from its center of gravity to the left or the right.
    # This is a shift you will definitely feel when you’re inside your vehicle.
    # The times this will happen is when you suddenly steer, brake, or accelerate.
    # This might cause your vehicle to spin around its center of gravity.
    # states is a list that stores multiple other lists with triplets like x, y and yaw angle values
    # simulation-1
    states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120),(35, -5, 30), (25, -10, -120), (15, -15, 100), (15, -25, 90)]

    # simulation-2
    #states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),(35, 10, 180), (32, -10, 180), (5, -12, 90)]

    # max curvature of robot vehicle in radian
    max_curv = 0.25

    # radius of the curve along which the car is moving
    radius = 4.15

    # Initialization of arrays for x, y and yaw values
    path_x, path_y, yaw = [], [], []

    # start, goal denote the source and goal states or two consecutive waypoints respectively
    # deg2rad() coverts given yaw degree value into radian
    for i in range(len(states) - 1):
        start_x = states[i][0]
        start_y = states[i][1]
        start_yaw = np.deg2rad(states[i][2])
        goal_x = states[i + 1][0]
        goal_y = states[i + 1][1]
        goal_yaw = np.deg2rad(states[i + 1][2])

        # call calc_dubins_path() method with arguments like x, y and yaw values of source and goal waypoints and maximum curvature
        # compute the dubin path between two consecutive states or waypoints
        path_i = calc_dubins_path(start_x, start_y, start_yaw, goal_x, goal_y, goal_yaw, max_curv, radius)

        # append all the x, y and yaw values between two consecutive waypoints to the path_x, path_y and yaw arrays respectively
        for x, y, iyaw in zip(path_i.x, path_i.y, path_i.yaw):
            path_x.append(x)
            path_y.append(y)
            yaw.append(iyaw)

    # show the animation of computed dubin path
    # Interactive mode is on
    plt.ion()

    # plot a new figure
    plt.figure()

    # for each waypoint
    # clf() function clears the current figure
    # plot() function show the dubin curve
    for i in range(len(path_x)):
        plt.clf()
        plt.plot(path_x, path_y, linewidth=1, color='gray')

        # for each waypoint or state draw an arrow with given arguments like x, y, yaw, length and color values
        for x, y, theta in states:
            draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet')

        # for each state draw a continuously moving car with arguments like x, y, yaw , width and length values
        draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3)

        # plot the figure with equal length axis
        # setting the x axis to -10 to 42 and y axis to -20 to 20 via axis() function
        # draw() function updates a figure that has been altered
        # pause() function show the figure for given interval seconds
        plt.axis("equal")
        plt.title("Simulation of Dubins Path")
        plt.axis([-10, 42, -35, 20])
        plt.draw()
        plt.pause(0.001)

    plt.pause(1)

if __name__ == '__main__':
    main()
```


#  Arbitrary Periodic Forcing (Fourier Series)

## Sympy

SymPy is a Python package for symbolic computing. It can do many symbolic operations, for instance, integration, differentiation, linear algebra, etc. See http://sympy.org for more details of the features and the documentation. Today we will cover how to do integrals using SymPy and use it to find the Fourier series that represents a sawtooth function.

```python
import sympy as sm
```

The function `init_printing()` enables LaTeX based rendering in the Jupyter notebook of all SymPy objects.

```python
sm.init_printing()
```

Symbols can be created by using the `symbols()` function.

```python
x, y, z = sm.symbols('x, y, z')
```

```python
x, y, z
```

The `integrate()` function allows you to do symbolic indefinite or definite integrals. Note that the constants of integration are not included in indefinite integrals.

```python
sm.integrate(x * y, x)
```

The `Integral` class creates and unevaluated integral, where as the `integrate()` function automatically evaluates the integral.

```python
expr = sm.Integral(x * y, x)
expr
```

To evaluate the unevaluated form you call the `.doit()` method. Note that all unevaluated SymPy objects have this method.

```python
expr.doit()
```

This shows how to create an unevaluated definite integral, store it in a variable, and then evaluate it.

```python
expr = sm.Integral(x * y, (x, 0, 5))
expr
```

```python
expr.doit()
```

# Fourier Coefficients for the Sawtooth function

Now let's compute the Fourier coefficients for a saw tooth function. The function that describes the saw tooth is:

$$
F(t) =
\begin{cases}
      A \left( \frac{4t}{T} - 1 \right) & 0 \leq t \leq T/2 \\
      A \left( 3 - \frac{4t}{t} \right) & T/2 \leq t \leq T
\end{cases}
$$

where:

- $A$ is the amplitude of the saw tooth
- $T$ is the period of the saw tooth
- $\omega_T$ is the frequency of the saw tooth, i.e. $\omega_T = \frac{2\pi}{T}$
- $t$ is time

This is a piecewise function with two parts from $t=0$ to $t=T$.

```python
A, T, wT, t = sm.symbols('A, T, omega_T, t', real=True, positive=True)
A, T, wT, t
```

The first Fourier coefficient $a_0$ describes the average value of the periodic function. and is:

$$a_0 = \frac{2}{T} \int_0^T F(t) dt$$

This integral will have to be done in two parts:

$$a_0 = a_{01} + a_{02} = \frac{2}{T} \int_0^{T/2} F(t) dt + \frac{2}{T} \int_{T/2}^T F(t) dt$$

These two integrals are evaluated below. Note that $a_0$ evaluates to zero. This is because the average of our function is 0.

```python
ao_1 = 2 / T * sm.Integral(A * (4 * t / T - 1), (t, 0, T / 2))
ao_1
```

```python
ao_1.doit()
```

```python
ao_2 = 2 / T * sm.Integral(A * (3 - 4 * t / T), (t, T / 2, T))
ao_2
```

```python
ao_2.doit()
```

But SymPy can also handle piecewise directly. The following shows how to define a piecewise function.

```python
F_1 = A * (4 * t / T - 1)
F_2 = A * (3 - 4 * t / T)
```

```python
F = sm.Piecewise((F_1, t<=T/2),
                 (F_2, T/2<=t))
F
```

The integral can be taken of the entire piecewise function in one call.

```python
sm.integrate(F, (t, 0, T))
```

Now the Fourier coefficients $a_n$ and $b_n$ can be computed.

$$
a_n = \frac{2}{T}\int_0^T F(t) \cos n\omega_Tt dt \\
b_n = \frac{2}{T}\int_0^T F(t) \sin n\omega_Tt dt
$$

```python
n = sm.symbols('n', real=True, positive=True)
```

For $a_n$:

```python
an = 2 / T * sm.Integral(F * sm.cos(n * wT * t), (t, 0, T))
an
```

```python
an.doit()
```

This can be simplified:

```python
an = an.doit().simplify()
an
```

Now substitute the $2\pi/T$ for $\omega_T$.

```python
an = an.subs({wT: 2 * sm.pi / T})
an
```

Let's see how this function varies with increasing $n$. We will use a loop but the SymPy expressions will not automatically display because they are inside a loop. So we need to use SymPy's `latex()` function and the IPython display tools. SymPy's `latex()` function  transforms the SymPy expression into a string of matching LaTeX commands.

```python
sm.latex(an, mode='inline')
```

    '$\\frac{A}{2 \\pi^{2} n^{2}} \\left(- 2 \\pi n \\sin{\\left (2 \\pi n \\right )} + 8 \\cos{\\left (\\pi n \\right )} - 4 \\cos{\\left (2 \\pi n \\right )} - 4\\right)$'

The `display()` and `LaTeX()` functions then turn the LaTeX string in to a displayed version.

```python
from IPython.display import display, Latex
```

Now we can see how $a_n$ varies with $n=1,2,\ldots$.

```python
for n_i in range(1, 6):
    ans = an.subs({n: n_i})
    display(Latex('$a_{} = $'.format(n_i) + sm.latex(ans, mode='inline')))
```

$a_1 = $$- \frac{8 A}{\pi^{2}}$

$a_2 = $$0$

$a_3 = $$- \frac{8 A}{9 \pi^{2}}$

$a_4 = $$0$

$a_5 = $$- \frac{8 A}{25 \pi^{2}}$

For even $n$ values the coefficient is zero and for even values it variers with the inverse of $n^2$. More precisely:

$$
a_n =
\begin{cases}
0 & \textrm{if }n\textrm{ is even} \\
-\frac{8A}{n^2\pi^2} & \textrm{if }n\textrm{ is odd}
\end{cases}
$$

SymPy can actually reduce this further if your set the assumption that $n$ is an integer.

```python
n = sm.symbols('n', real=True, positive=True, integer=True)
an = 2 / T * sm.Integral(F * sm.cos(n * wT * t), (t, 0, T))
an = an.doit().simplify()
an.subs({wT: 2 * sm.pi / T})
```

The odd and even versions can be computed by setting the respective assumptions.

```python
n = sm.symbols('n', real=True, positive=True, integer=True, odd=True)
an = 2 / T * sm.Integral(F * sm.cos(n * wT * t), (t, 0, T))
an = an.doit().simplify()
an.subs({wT: 2 * sm.pi / T})
```

Note that $b_n$ is always zero:

```python
bn = 2 / T * sm.Integral(F * sm.sin(n * wT * t), (t, 0, T))
bn
```

```python
bn.doit().simplify().subs({wT: 2 * sm.pi / T})
```

# Random questions questions from class

The source code for any function is shown with the double question mark.

```python
sm.Integral??
```

The location of the file the function comes from can be found by accessing the `__file__` attribute.

```python
sm.__file__
```

    '/usr/lib/python3.5/site-packages/sympy/__init__.py'

SymPy can handle unit for you too.

```python
from sympy.physics.units import meter, kilogram, second
```

```python
a = 10 * second
a
```

```python
b = 5 * meter
```

```python
c = 6 * kilogram
```

```python
a, b, c
```

```python
expr = c * b / a**2
expr
```

```python
from sympy.physics.units import newton
```

```python
5 * newton
```

# Numerical evalution of the Fourier Series

Now the Fourier coefficients can be used to plot the approximation of the saw tooth forcing function.

```python
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline
```

The following function plots the actual sawtooth function. It does it all in one line by cleverly using the absolute value and the modulo functions.

```python
def sawtooth(A, T, t):
    return (4 * A / T) * (T / 2 - np.abs(t % T - T / 2) ) - A
```

```python
A = 1
T = 2
t = np.linspace(0, 5, num=500)
```

```python
plt.figure()

plt.plot(t, sawtooth(A, T, t));
```

# Exercise

Write a function that computes the Fourier approximation of the sawtooth function for a given value of $n$, i.e. using a finite number of terms. Then plot it for $n=2, 4, 6, 8, 10$ on top of the actual sawtooth function. How many terms of the infinite series are needed to get a good sawtooth?

```python
def sawtooth_approximation(n, A, T, t):
    # odd values of indexing variable up to n
    n_i = np.arange(1, n+1, 2)[:, np.newaxis]
    # cos coefficients
    an = -8 * A / (n_i**2 * np.pi**2)
    # sawtooth frequency
    wT = 2 * np.pi / T
    # sum of n cos functions
    f = np.sum(an * np.cos(n_i * wT * t), axis=0)
    return f

# plot sawtooth
f = sawtooth(A, T, t)
plt.plot(t, f, color='k', label='true sawtooth')

for n in np.arange(2, 12, 2):
    f_approx = sawtooth_approximation(n, A, T, t)
    plt.plot(t, f_approx, label='n = {}'.format(n))

plt.legend()
# zoom in a bit on the interesting bit
plt.xlim(0, T)
```

# Exercise

Now, using the Fourier series you've developed compute the response of a linear system to the sawtooth function and plot the steady state trajectory of $x$ give:

$$m\ddot{x} + c\dot{x} + kx = F(t)$$

Use:

- $m$: 100 kg
- $\omega_n$: 10 rad/s
- $\zeta$: 0.01
- $T=\frac{2\pi}{5 rad/s}$
- $A=1000N$

Investigate the difference in steady state response for different approximations of $F(t)$, i.e. various $n$, and report your findings in a markdown cell.

The steady state response to a forcing function with Fourier coefficients $a_0$, $a_n$, and $b_n$ is:

$$
x_\text{ss}(t) = \frac{a_0}{2k} + \sum_{n=1}^\infty \left[ x_\text{cn}(t) + x_\text{sn}(t) \right]
$$

where

$$
\begin{aligned}
x_\text{cn}(t) &= \frac{a_n/m}{\sqrt{\left[ \omega_n^2 - \left( n\omega_T \right)^2 \right]^2 + \left(2\zeta\omega_n n\omega_T\right)^2}} \cos\left(n\omega_T t - \theta_n\right) \\
x_\text{sn}(t) &= \frac{b_n/m}{\sqrt{\left[ \omega_n^2 - \left( n\omega_T \right)^2 \right]^2 + \left(2\zeta\omega_n n\omega_T\right)^2}} \sin\left(n\omega_T t - \theta_n\right)
\end{aligned}
$$

and

$$
\theta_n = \tan^{-1} \frac{2\zeta\omega_n n \omega_T}{\omega_n^2 - \left(n \omega_T\right)^2}
$$

We have already found that $a_0 = 0$ and $b_n = 0$ for all $n$, so we only need to consider the $x_\text{cn}$ terms.

```python
from ipywidgets import interact

# system parameters
m = 100
wn = 10
z = 0.01
wT = 5
T = 2 * np.pi / wT
A = 1000

t = np.arange(0, 10*T, 0.01)

def sawtooth_response(N=10):
    """
    Computes the steady state m-c-k response to a sawtooth and
    plots the result.
    """
    # column array of n/an values
    n = np.arange(1, N+1, 2)[:, np.newaxis]
    an = -8 * A / (n**2 * np.pi**2)

    # phase shift of each term in the series
    theta_n = np.arctan2(2*z*wn*n*wT, wn**2-(n*wT)**2)

    # an is a col and t is 1D, so each row of xcn is a term
    # in the series at all times in t
    xcn = an / m / np.sqrt((wn**2 - (n*wT)**2)**2 +
                            (2*z*wn*n*wT)**2) * \
                   np.sin(n*wT*t - theta_n)

    # x is the sum of each xcn term (the rows)
    x = np.sum(xcn, axis=0)

    plt.plot(t, x)

interact(sawtooth_response, N=(2, 100, 2))
```

We can see from interacting with the slider to show the approximated response for different numbers of terms in the series approximation that there is not much of a difference in the response when $n > 4$. We already saw that the sawtooth approximation is decent around $n = 6$, so this isn't too surprising. Also, since the forcing function has a period exactly twice as long as the system's natural period, so there isn't much of an interesting interaction between the two frequencies involved. Try again with $\omega_n = 35$ rad/s, for example, to see the solution change dramatically with $n < 10$.


# SymPy: A Python Solution for Symbolic Computation

### SymPy is an open source Computer Algebra System (CAS) that allows the use of symbolic computation.

To explain what SymPy does, when it becomes useful, and why it was developed, take the following example comparing SymPy and Python's math.py library (SymPy Development Team 2018).

Python comes with the builtin math.py library, which can be imported into any shell to perform mathematical calculations beyond simple arithmetic (Python Software Foundation 2019). A common example is the square root function, or sqrt():

```python
import math
math.sqrt(25)
# this results in a perfect square
```

    5.0

The square root of 25 is a rational number, 5.0. However, in the case of irrational numbers such as `pi`, `e`, and the square root of 8, `math.sqrt()` returns an irrational number which can only be displayed as an approximation:

```python
math.sqrt(8)
# this results in an approximation of an irrational number
```

    2.8284271247461903

If we are doing math that only requires an approximate decimal of the square root of 8, we can leave this equation only approximately evaluated. However, for instances which require an exact value, SymPy leaves imperfect square roots unevaluated and simplified:

```python
import sympy
sympy.sqrt(8)
# this results in an algebraic, or symbolic computation
```

    2*sqrt(2)

which translates into `√8 = √4⋅2 = 2√2`

This final value, `2√2`, is an exact equivalent of the approximate value `2.8284271247461903`, but is left unevaluated, thereby increasing the speed of the program and reducing the chance of errors in a code.

Python's builtin math.py library is not equipped to handle the advanced calculations and printing needs required in many science, engineering, and mathematics professions. That's where SymPy comes in. SymPy allows the use of symbolic computation, as well as the use of unicode and pretty pringting to print out equations in a more legible format, rather than the default Python equation formatting (SymPy Development Team 2018).

### Makes sense, so where did SymPy come from?

SymPy is an open source library released under the BSD Licence (https://opensource.org/licenses/BSD-2-Clause), and is developed and maintained by Aaron Meurer. Its first version, v0.5.13-hg, was released on April 1, 2008, with its latest release of v1.13 on September 14, 2018. The library is programmed purely with Python, and can be used by all Python versions 2.7 and above. It can also be used across all operating systems (PyPi 2018), which distinguishes it from other CAS libraries such as Axiom (Dhingra, G. 2017).

Since its first release, SymPy has become highly reknowned and popular in scientific and mathematics computing communities. Many resources have been developed by the team behind SymPy as well as external users and delevopers. A Github file called "SymPy Papers", located at https://github.com/sympy/sympy/wiki/SymPy-Papers, tracks Zotero publications which cite the library. Another Github file in the SymPy Github wiki, https://github.com/sympy/sympy/wiki/Presentations, lists SymPy presentations from conferences across the globe. The wiki also provides a list of cookbooks for SymPy: https://github.com/sympy/sympy/wiki/Cookbook, and countless tutorials have been written both by the maintenance team and by external developers and users: https://github.com/sympy/sympy/wiki/External-SymPy-Media%2C-Tutorials%2C-and-Presentations.

### Who uses SymPy and What can you do with it?

SymPy is designed for use in mathematics, engineering, and physics (PyPi 2018). It can be used in functions, in various areas of calculus, and in linear algebra: for example, simplifying expressions; computing differentials, integrals, and limits; working with matrices; solving equations. In a nutshell, SymPy can help you pass MATH1310 and MATH1410. For those who aren't York University math and science students, but who happen to be professionals in a STEM field, SymPy can greatly reduce the time and error margin involved in solving complex problems.

Direct from the main SymPy website, SymPy can be used for:
"plotting, printing (like 2D pretty printed output of math formulas, or LATEX), code generation, physics, statistics, combinatorics, number theory, geometry, logic, and more" (SymPy Development Team 2018).

If that doesn't get you excited to do some Python math, I don't know what will.

### There are other CAS libraries, so why choose SymPy?

Excellent question. That all depends on your mathematical needs. Axiom (http://axiom-developer.org/) is an excellent open source alternative, as a general computer algebra system. It is mostly used by mathematical researchers, whereas SymPy is more often used for scientific research as well as mathematics. Additionally, SymPy is available on more operating systems than Axiom, since it is a pure Python program and requires only Python 2.7 and above. Overall, both are good options, but SymPy is more accessible due to its only dependability being Python 2.7 or higher, and it's ease of use: if you're already using Python, you can use SymPy, as its language and syntax are the same (Dhingra, G 2017).

Magma (http://magma.maths.usyd.edu.au/magma/) is another CAS very similar to SymPy and can handle many of the same types of expressions and mathematics. However, Magma does not have the ability to perform plotting, while SymPy, with the addition of other libraries such as Matplotlib, can perform sophisticated plots. Magma is also proprietary software, and while a free trial is available, any prolonged use requires purchase of a licence of its latest version, v2.18-2 (Guarín-Zapata, N. 2016).

Additional comparisons between SymPy and similar CAS libraries can be found at https://github.com/sympy/sympy/wiki#documentation. Many of these are proprietary and require purchase, often upwards of $1000 USD (SymPy Development Team 2018). SymPy, on the other hand, is not only free, but its source code can be altered and sold for profit, according to its BSD licence (Open Source Initiative ND).

SymPy is a highly user-friendly, easy to download and install CAS library with a lot of online support. Depending on your particular mathematical needs, it serves as an ideal choice for symbolic computing (SymPy Development Team 2018).

### SymPy in Action:

In the introduction to this notebook there are a few minor examples of how SymPy works differently than other mathematics libraries. Below are some more advanced functions and expressions that SymPy can handle and how they make scientific and mathematical computing easier.

The basic code for first two examples come from the SymPy docs tutorials:
https://docs.sympy.org/latest/tutorial/intro.html#the-power-of-symbolic-computation (SymPy Development Team 2018). The basic code for last example comes from a user, Pedro Jorge De Los Santos, from Stack Overflow: https://stackoverflow.com/questions/40747474/sympy-and-plotting

The most important feature to remember about SymPy is that variables must be defined. Take the expression `x+2y`:

```python
from sympy import symbols
x, y = symbols('x y')
expr = x + 2*y
expr
```

    x + 2*y

Normally, Python will evaluate the expression `x + 2*y`, but SymPy returns the variable `expr` as the expression `x + 2*y`. Now we can work with this expression as a variable:

```python
expr + 1
```

    x + 2*y + 1

```python
expr - x
```

    2*y

In the case above, SymPy simplifies the second expression, which automatically cancels `x` out. This is similar to how the expression `sympy.sqrt(8)` returns `2*sqrt(2)`.

However, SymPy does not always simplify an expression, and this is most often the case when the form of the expression is not obvious, as in when there is a factored and expanded form of an equation. By default, SymPy will give the factored result when not explicitly told which form to give:

```python
x*expr
```

    x*(x + 2*y)

Based on the result from `expr - x = 2*y`, we might expect SymPy to return `x^2+2xy` from `x*expr`.

In order to simplify the result, we can tell SymPy to either factor or expand the result:

```python
# to return an expanded result:
from sympy import expand, factor
expandedExpr = expand(x*expr)
expandedExpr
```

    x**2 + 2*x*y

```python
# to return a factored result:
factor(expandedExpr)
```

    x*(x + 2*y)

#### Let's look at some calculus with SymPy

First, to print mathematical expressions in unicode, we use this script:

```python
from sympy import *
init_printing(use_unicode=True)
```

This makes our expressions easier for us to read. Other print types can be used with the same function call: init_printing(); init_printing(pretty_print), init_printing(use_unicode=False), etc. However, some shells cannot print in certain types and the result will print in whichever type it can (SymPy Development Team 2018).

Now we will take the derivitive of `sin(x)*e**x`. First, we define any variables we will be using, followed by the expression:

```python
x, t, z, nu = symbols('x t z nu')
diff(sin(x)*exp(x), x)

```

Another example:

Compute ∫(e^xsin(x)+e^xcos(x))d^x:

```python
integrate(exp(x)*sin(x) + exp(x)*cos(x), x)
```

Whether you understand how to do calculus and algebra or not, the above examples illustrate the ability of SymPy not only to handle complex mathemetics, but to do so easily and symbolically, making it much faster and clearer to work with. The language and syntax is easy to learn as it uses the Python language (SymPy Development Team 2018).

This also means that SymPy calculations can integrate easily with other Python code and modules. When developing software that requires a plotted graph or other data, you can simply import SymPy into your existing code and add the script for your data, or create a separate .py file and import it with SymPy.

#### Plotting with SymPy and other modules:

Let's take a look at a final example program in which we create two plots on one interactive graph, integrated with the standard Python library as well as a few other libraries: Numpy and Matplotlib.pyplot.

```python
# this program creates a graph with two plots: a red linear equation and a blue curve.

import matplotlib.pyplot as plt
import numpy
%matplotlib notebook
from sympy import *

# define SymPy variables:
x, y = symbols("x y")

# use any integers you need to plot:
hp = plot_implicit(Eq(x**2 + y**4, 8), (x, -3, 5), (y, -5, 3))
fig = hp._backend.fig
ax = hp._backend.ax

# create list of points:
xx = yy = numpy.linspace(-3,3)

# plot functions from matplotlib:
ax.plot(xx,yy) # y = x
ax.plot([0],[0],'o') # Point (0,0)
ax.set_aspect('equal','datalim')
# show plot:
plt.show()
```

    <IPython.core.display.Javascript object>

In addition to displaying two plots, this program also has a mouse-over function that displays the coordinates of the cursor location in the bottom-right corner. It also features an "off-switch" next to the plot title to end this mouse-over function (De Los Santos 2016).

This plot graph can be saved as a .py file and imported into other Python programs. It can be altered by changing the variable integer values.

### Conclusion

SymPy is a highly versitile library in its ability to function alongside other modules and languages, as well as on various operating systems. Because it is written entirely in Python and depends only on having an installation of Python 2.7 or newer, it can be used by anyone with even a basic understanding of the Python language. It works well beside other mathematics and science modules, such as Numpy and Scipy, and can be used with Matplotlib's Pyplot module to plot graphs. It features the ability to print results in pretty print, LaTeX, unicode, or the standard Python mathematics syntax, which provides options for improving human readability or increasing processing speed.

Whether you're struggling through a calculus class, or you're a geomatics researcher and need to plot different wavelengths of ocean topography, SymPy will simplify your experience.

#### Citations

De Los Santos, P. J. (2016 November 22). Sympy and Plotting. Stack Overflow. Retrieved from https://stackoverflow.com/questions/40747474/sympy-and-plotting

Dhingra, G. (2017 May 17). SymPy vs. Axiom. Github. Retrieved from https://github.com/sympy/sympy/wiki/SymPy-vs.-Axiom

Guarín-Zapata, N. (2016 July 15). SymPy vs. Magma. Github. Retrieved from https://github.com/sympy/sympy/wiki/SymPy-vs.-Magma

Meurer, A. (2018 August 17). SymPy Wiki. Github. Retrieved from https://github.com/sympy/sympy/wiki

Open Source Initiative. (No Date). The 2-Clause BSD License. The Open Source Initiative. Retrieved from https://opensource.org/licenses/BSD-2-Clause

PyPi. (2018 September 14). Sympy. PyPi. Retrieved from https://pypi.org/project/sympy/

Python Software Foundation. (2019 January 28). math - Mathematical Functions. Python 3.7.2 Documentation. Retrieved from https://docs.python.org/3/library/math.html

SymPy Development Team. (2018 September 14). Intro. SymPy. Retrieved from https://docs.sympy.org/latest/tutorial/intro.html#the-power-of-symbolic-computation

SymPy Development Team. (2018 September 14). Preliminaries. SymPy. Retrieved from https://docs.sympy.org/latest/tutorial/preliminaries.html

SymPy Development Team. (2018 September 14). Preliminaries. SymPy. Retrieved from https://docs.sympy.org/latest/tutorial/printing.html

SymPy Development Team. (2018 August 24). SympPy Gamma. Retrieved from https://www.sympygamma.com/

Filipowich_SymPy-A Python Solution for Symbolic Computation.ipynb


# SMU Honors Physics: Adventures in Spacetime

**Authors**: Stephen Sekula

In this python notebook, we will explore numbers, especially vectors - numbers that define both a length and a direction. Vectors are a crucial class of numbers in physics. Many quantities in the natural world must be specified by more than just a size or a length; they indicate where to go next, and also need to specify directional information. In this unit, we explore simple numbers without direction ("scalars"), then move on to numbers with direction ("vectors"). This will all be done with a programming language, called PYTHON, which you will use over and over this semester to explore different questions about the natural world.

In this unit, you will learn the following:

* How to define scalars in PYTHON.
* How to operate on scalars to make them do things, like make other scalars.
* How to define vectors in general
* How to define vectors in PYTHON
* How to operate on vectors
* How to use vectors to represent physical quantities, like those that describe motion in space and time
* Apply vectors to model a physical process

### Difficulty level of a section

Sections below are marked by one of three graphics: green circle, blue square, and black diamond. If you have ever gone skiing before, you might recognize these as ski trail difficulty markers. "Green circle" indicates a beginner-level trail; "Blue square" is intermediate-level; "Black diamond" is advanced level.

Everyone should be able to make progress on green circle material. Once you have a good comfort level with green circle, blue square will feel much more approachable. Black diamond may challenge even experienced people, but nonetheless it is important to see what is possible and aspire!

* Green Circle: BEGINNER
* Blue Square: INTERMEDIATE
* Black Diamond: ADVANCED

## Scalars (in general, and in PYTHON)

A "scalar" is a number with magnitude, but no directions. Here are some physical quantities that are represented by scalars:

* Your age: "I am 19 years old" is a description of the *magnitude* of your age. Of course, we all know that time only ever seems to move in one direction, so we never bother to specify the direction of age.
* The temperature of a room: "It's 70F in this office right now" is a statement about the magnitude of the temperature (heat energy content) of the room. It has no directional information. For instance, knowing that it is 70F in this room doesn't tell us about the temperature in the hall, or the next room. It has no information about what temperature is doing elsewhere.
* Speed: "I am driving at 70 miles per hour" is a statement about how far you go over a certain amount of time. Nobody can figure out, from just this information, whether you are driving north, south, east, west, or some combination of those. Speed is a scalar that gives no information about what your speed will be later, what it was earlier, or in what direction your motion is aimed.

Now that you have familiarized yourself with scalars in the physical world, let us represent them using the PYTHON programming language and then learn to operate on scalars to do things - like make other scalars.

### Scalars in PYTHON - integers and floating point numbers

PYTHON has two built-in simple scalar types: integers (0,1,2,3,... or -1,-2,-3,...) and "floating point" numbers, more commonly known as "decimal numbers" (1.1, 3.14159, 75.008). PYTHON allows you to have a symbol represent a number (like in algebra) and then use the symbol to conduct operations on the numbers. Consider the following first steps into PYTHON to store a decimal number using a "variable" (a symbol that stands for a number and whose value can be altered), and then act on the variable with mathematical operations (addition, +, subtraction, -, multiplication, \*, and division, /).

To make the next block of "code" (programming commands) do something, click your mouse cursor into the block and press the SHIFT key and the ENTER key at the same time.

```python
# This is a comment. PYTHON ignores these, but they are helpful for people!
# Any line that begins with a # symbol is a comment.

# Let's define a variable, x:
x = 5.0

# By just placing the variable on its own line, we can print its value:
x

# That's not very pretty. Just a number on a line. Let's print this with some helpful text:
print("The value of x is %f" %(x))

# How about printing a multiplication operation?
print("%f" %(x*5.0))

# We don't need all those decimal places. Let's print just a few (or maybe lots more! Play around!)
print("%.2f" % (x*5.0))

# Let's define a new variable, y, based on x:
y = x*2.0
print("The value of y is:")
print(y)

# Let's print out some arithmetic operations between x and y:
print("Addition")
print(x+y)

print("Subtraction")
print(x-y)

print("Multiplication")
print(x*y)

print("Division")
print(x/y)

# Congratulations - you just learned how to make a computer do whatever you want. Imagine the endless applications!

```

    The value of x is 5.000000
    25.000000
    25.00
    The value of y is:
    10.0
    Addition
    15.0
    Subtraction
    -5.0
    Multiplication
    50.0
    Division
    0.5

## Vectors

A "vector" is a collection of scalars that, together, represent both magnitude and direction. Many physical quantities can only be accurately described by vectors. For example:

* Velocity: "I am driving northeast at 70 miles per hour" is a statement both about direction ("northwest") and magnitude ("70 miles per hour"). Not only do you know how much space is being covered, and in what time it is covered - you know where to find the person at a later time because you know their direction! If you know the starting point, you can keep track of a journey.
* Acceleration: "I am falling down to the earth and my speed increases by 9.8 meters per second, every second" is a statement about the rate of change of speed, and the direction in which that change occurs. The earth's gravitational force causes such a change in speed near the surface of the earth, and always seems to cause the change in a direction that points toward the center of the earth ("down").
* Electric force: "The lightning jumped from the storm cloud to the ground" is a statement about how much charge was drained from a storm cloud (in the sky) to the ground (down below the storm). This is a vector, containing both direction (downward) and magnitude (lightning is a number of electric charges that all make the jump). Even without actual motion, electric charges can exert forces on each other and those forces have directionality and strength.

There are many notations for a vector. For instance, let us try to represent the first example above - velocity - using a notation.

**"I am driving northeast at 70 miles per hour."**

Let us assume that the driver is moving an equal amount of distance north for every amount west that they move. We can represent one step in their motion as follows:

```python
# We'll need this more later, but this code snippet will draw arrows. We need them to represent the above statement
import matplotlib.pyplot as plt
import math
%matplotlib inline
plt.axis('on')
plt.xlim(-1.0,1.0)
plt.ylim(-1.0,1.0)

# Draw an east-pointing arrow
plt.arrow(0, 0, 1/math.sqrt(2)-0.1, 0, head_width=0.05, head_length=0.1, fc='k', ec='k');

# Draw a north-pointing arrow
plt.arrow(1/math.sqrt(2), 0, 0, 1/math.sqrt(2)-0.1, head_width=0.05, head_length=0.1, fc='k', ec='k');

# Draw the actual arrow representing the total motion of the car
plt.arrow(0, 0, 0.9/math.sqrt(2), 0.9/math.sqrt(2), head_width=0.05, head_length=0.1, fc='c', ec='c');

```

The above code snippet will help us later in doing more vector visualization, but for now focus on notating the above picture. For every bit of distance, x, that we go eastward, we go a distance, y, northward. In the above picture, x=y . . . but that doesn't always have to be the case. In fact, most of the time it's not.

Let us look at two popular notations for the above:

$\vec{v} = (x,y)$

or

$\vec{v} = x\hat{i} + y\hat{j}$

The first notation uses parentheses and commas to separate the horizontal component (x) from the vertical component (y). The second notation uses an additional kind of vector, a *unit vector*, to represent "direction along horizontal" ($\hat{i}$) and "direction along vertical" ($\hat{j}$).

For this exercise, we prefer the first notation since it's very similar to what we will do in PYTHON to represent vectors. Let's look at that.

### Mathematical Operations on Vectors

* Adding two vectors is as simple as adding the like-components: the first component in each vector is added to the first component in the next; the second component in each vector is added to the second component of the next; etc. Mathematically, we can represent this as follows: ${\vec v}_1 + {\vec v}_2 = (1,5) + (4,5) = (1+4, 5+5) = (5,10)$. Note that the sum of two vectors yields a new vector.
* Subtracting two vectors is just like addition, but with minus signs. For example, ${\vec v}_1 - {\vec v}_2 = (1,5)-(4,5) = (1-4, 5-5) = (-3, 0)$. Again, subtraction yields a new vector.
* Scalar multiplication involves multiplying a vector by a scalar. In this case, you change the overall length of the vector, but not its direction (we will see this in PYTHON below). For example, let $a=5$ and ${\vec v} = (1,5)$, and then multiply them: $a{\vec v} = 5(1,5) = (5 \cdot 1, 5 \cdot 5) = (5,25)$. Note that scalar multiplication yields a vector.
* Dot-product vector multiplication: one way to multiply two vectors is to take the "dot product," in which you multiply each component by the same component from another vector and sum the resulting products. For example: ${\vec v}_1 \cdot {\vec v}_2 = (1,5) \cdot (4,5) = (1 \cdot 4) + (5 \cdot 5) = 4 + 25 = 29$. Note that the dot-product of two vectors is a scalar - a pure number.

### The Length of a Vector

A vector is an arrow. It has direction, but also length ("magnitude"). We now have enough mathematical information to calculate the length of any vector, knowing its components.

We can determine the length of a vector using the Pythagorean Theorem. Consider the picture above of driving east and then driving north. The eastward movement creates a horizontal vector. The northward movement creates a vertical vector. The resulting total motion - driving northeast - is the sum of two independent motions, one eastward and one northward. We can write the eastward vector (of length $e$) as:

\begin{equation} \vec{e} = (e,0) \end{equation}

and the northward vector (of length $n$) as:

\begin{equation} \vec{n} = (0,n) \end{equation}

If we add them, using the rules above, we arrive at the total motion vector:

\begin{equation} \vec{v} = (e,0) + (0,n) = (e,n). \end{equation}

But what is the *length* of this total vector? The two components form the base and height if a right-triangle. You can imagine, then, that if we know the length of the base and the length of the height, we can find the total length of the hypotenuse - which is just the length ($L$) of the total vector! This would look like this:

\begin{equation} L^2 = e^2 + n^2 \longrightarrow L = \sqrt{e^2 + n^2} \end{equation}

Consider the mathematical operations we introduced above. Which operation does the length-squared, $L^2$, resemble?

If you said "dot-product," you're correct. In fact, the dot-product of a vector with its own self gives you the square of the length of that vector:

\begin{equation} {\vec v} \cdot {\vec v} = (e,n) \cdot (e,n) = (e \cdot e) + (n \cdot n) = e^2 + n^2 = L^2 \end{equation}

Let us now play with all of this in PYTHON.

## Vectors in PYTHON

Below, we show you how to write vectors using PYTHON and then act on vectors with various algebraic operations. Play around with these and see if you can get a feel for what it means to define and then manipulate a vector. The visualization below will update when you make changes.

```python
# import a library of numerical tools for defining and manipulating vectors
import numpy as np

# Let's define a vector using two scalar variables to set the components
x = 1.0
y = 5.0

v_1 = np.array([x,y])

# Let us print the components of the vector
print("The components of the vector, v_1, are as follows:")
print("x = %f" % (v_1[0]))
print("y = %f" % (v_1[1]))

# We see that adding the operator [] after the vector, with a number inside the square brackets, gains us
# access to the components of the vector.

# Let us define a second vector
v_2 = np.array([4,5])

# Let us do vector addition:
print("Vector addition of v_1 + v_2 yields: (%f, %f)" % ((v_1+v_2)[0], (v_1+v_2)[1]))

# We see that this, indeed, yields what we expect from the definition discussed above!
# Let us subtract them:
print("Vector subtraction of v_1 - v_2 yields: (%f, %f)" % ((v_1-v_2)[0], (v_1-v_2)[1]))

# Let us now multiply the vector by a scalar
a = 5
print("Scalar multiplication of a * v_1 yields: (%f, %f)" % ((a*v_1)[0],(a*v_1)[1]))

# Just as we expected from the definitions above!

# Let us now do the dot-product multiplication of two vectors.
# Numpy provides a special "function" to execute the dot product:
print("Dot-product multiplication of v_1 * v_2 yields: %f" % (np.dot(v_1, v_2)))

# Again, this gives us what we expected from the definition of the dot product.

# Let us now calculate the length of the vector, v_1, in two ways.

# The first way is "brute-force" - do the dot-product multiplication and then square-root it:
length = math.sqrt( np.dot(v_1, v_1))
print("Length Method A: the length of v_1 is %f" % (length))

# The second way is to use numpy's built-in function for computing the length:
length = np.linalg.norm(v_1)
print("Length Method B: the length of v_1 is %f" % (length))

# Oh, look. They are the same!
# Method B utilizes the "Linear Algebra" tools, and specifically the "norm()" function, which is
# designed to return the length of a vector of any size (we used a 2-dimensional vector, but you
# could give it a 10-dimensional vector and it still works!). "Linear Algebra" is a mathematics class
# you might take later, and it's the algebra of structured numbers like vectors and matrices, generically
# known as "tensors".

# Here is a 5-dimensional vector:
v_3 = np.array([1,3,5,7,9])
print("The length of our 5-D vector is: %f" % (np.linalg.norm(v_3)))

# Neat, huh? Try computing the length yourself and see if the above answer is right.

```

    The components of the vector, v_1, are as follows:
    x = 1.000000
    y = 5.000000
    Vector addition of v_1 + v_2 yields: (5.000000, 10.000000)
    Vector subtraction of v_1 - v_2 yields: (-3.000000, 0.000000)
    Scalar multiplication of a * v_1 yields: (5.000000, 25.000000)
    Dot-product multiplication of v_1 * v_2 yields: 29.000000
    Length Method A: the length of v_1 is 5.099020
    Length Method B: the length of v_1 is 5.099020
    The length of our 5-D vector is: 12.845233

## Play with vectors

The interactive PYTHON game below lets you play with a vector, interacting by using sliders to change things like the total length of the vector or the lengths of its components. Consider the following critical questions:

1. What happens to the *direction* of the total vector when you change its *total length*?
2. What happens to the *direction* of the total vector when you change the length of either of its components?

```python
from ipywidgets import widgets
from ipywidgets import interact
import matplotlib.pyplot as plt
import math
%matplotlib inline

def f(x):
    print(x)

global x_last
x_last = 0.0

global y_last
y_last = 0.0

def draw_vector(x,y):
    global x_last
    global y_last

    plt.axis('on')
    plt.xlim(-5.0,5.0)
    plt.ylim(-5.0,5.0)

    xlength = x
    ylength = y
    length = math.sqrt(x**2 + y**2)

    # Draw the previous vector in a fainter color/shade
    if (x_last != 0 and y_last != 0):
        length_last = math.sqrt(x_last**2 + y_last**2)
        plt.arrow(0, 0, x_last, y_last, head_width=0.2, head_length=0.2, fc='c', ec='c');

    # Draw the vector
    plt.arrow(0, 0, xlength, ylength, head_width=0.2, head_length=0.2, fc='k', ec='k');
    plt.show()

    x_last = x
    y_last = y

interact(draw_vector,x=widgets.IntSlider(min=-4,max=4,step=1,value=1),y=widgets.IntSlider(min=-4,max=4,step=1,value=1))

print("Note: the current vector is in black, while the previous vector is in cyan (light blue).")

```

# Fun With Vectors - Moving About

Let us now program a simple "animation" by adding TIME as an additional dimension to the problem. The above graphic of a northeast-pointing vector is static - it represents a moment in frozen time. It never changes. Let's add time as a step, and have some fun with this by making something that moves.

Let us do this. Let us:

* Define a "game board" of a fixed size.
* Draw a vector on the game board that fits inside the board.
* Wait a second, then draw a new vector on the game board that starts at the tip of the previous vector and points in the same direction as the original one. If the length of this vector exceeds the game board size, "bend" it to point back into the board. In essence, "bounce" it off the wall.
* Keep this going. Draw a new vector. If it fits in the board, draw the next one in the same direction. If it doesn't, "bounce" it off the wall.

How do we do the "bounce"? Let's choose the following:

* If the horizontal component of the vector exceeds the horizontal boundary, flip the horizontal component around and start the next vector on the "wall" of the board.
* if the vertical component of the vector exceeds the vertical boundary, flip the vertical component around and again start the next vector on the wall.
* If BOTH exceed the boundary, flip both and start on the wall.

You can make up your own rules. Here is what the program looks like in PYTHON if we implement these.

Note:

Try playing around with the total number of steps in the game ("total_steps") and the direction and length of the original vector (look for the comment, "initial vector," preceding those lines of code).

```python
# Library setups
import numpy as np
import math
import matplotlib.pyplot as plt

# Define variables accessible (and modifiable) by functions
global board_length
board_length = 1

global board_height
board_height = 1

# colors that we can assign to the arrow and change in the game
global colors
colors=['k','b','g','r','c','m'] # black, blue, red, green, cyan, magenta

global vector_color
vector_color = 0

# Define some functions for determining if our vector is on the board
# A "function" takes input and does something to it. It allows you to
# encapsulate repetative tasks.

# Is the vector's front end inside the bounds of the board's horizontal size?
def vector_in_board_horizontally(x_start = 0, v = np.array([])):
    if (x_start + v[0]) < 0:
        return -1
    if (x_start + v[0]) > board_length:
        return 1
    return 0

# Is the vector's front end inside the bounds of the board's vertical size?
def vector_in_board_vertically(y_start = 0, v = np.array([])):
    if (y_start + v[1]) < 0:
        return -1
    if (y_start + v[1]) > board_length:
        return 1
    return 0

# Define some functions for flipping vector components
def flip_horizontal(v = np.array([])):
    return np.array([-v[0],v[1]])

def flip_vertical(v = np.array([])):
    return np.array([v[0],-v[1]])

# function to draw vector
def draw_vector(x_initial = 0.5, y_initial = 0.5, v = np.array([])):
    plt.arrow(x_initial, y_initial, v[0], v[1], head_width=0.05, head_length=0.1, fc=colors[vector_color], ec=colors[vector_color]);
    return v

def next_color():
    global vector_color
    vector_color += 1
    if vector_color >= len(colors):
        vector_color = 0
    return vector_color

# Draw the board and then play the game
# We'll need this more later, but this code snippet will draw arrows. We need them to represent the above statement
import matplotlib.pyplot as plt
import math
%matplotlib inline
plt.axis('on')
plt.xlim(0,board_length)
plt.ylim(0,board_height)

# initial vector
x_start = 0.32
y_start = 0.5
v = draw_vector(x_initial = x_start, y_initial = y_start, v=np.array([0.1,0.2]))

# Place a dot at the starting point - this helps us find the start point!
plt.plot(x_start,y_start, marker='o', color=colors[vector_color], ls='')

# Step the arrow around the board, applying our rules when a vector reaches the boundaries
# When we "bounce" off a wall, change the color!
total_steps = 50

# a "for loop" repeats a task over and over again, up to a limit that you define
for step in range(total_steps):
    v_next = v

    # update the start point
    x_start = x_start + v[0]
    y_start = y_start + v[1]

    horizontal_choice = vector_in_board_horizontally(x_start, v)
    if horizontal_choice == 0:
        v_next = v_next
    elif horizontal_choice == -1:
        v_next = flip_horizontal(v_next)
        x_start = 0
        next_color()
    elif horizontal_choice == 1:
        v_next = flip_horizontal(v_next)
        x_start = board_length
        next_color()

    vertical_choice = vector_in_board_vertically(y_start, v)
    if vertical_choice == 0:
        v_next = v_next
    elif vertical_choice == -1:
        v_next = flip_vertical(v_next)
        y_start = 0
        next_color()
    elif vertical_choice == 1:
        v_next = flip_vertical(v_next)
        y_start = board_length
        next_color()

    v = v_next
    draw_vector(x_start, y_start, v)

```

# Acknowledgements

The graphics used for difficult ratings are from openclipart.org:

* https://openclipart.org/detail/30529/led-triangular-black
* https://openclipart.org/detail/212963/blue-square-button
* https://openclipart.org/detail/26193/button-green

```python

```