# This file is prepared by Dr. Heung Wing Joseph Lee
# as a demonstration
# 
# Apollonius pursuit
# 
# Pursuer and Target are travelling on 2D centered at the origin
# The target is travelling in constant speed 1 and fixed direction 
# start from (0,0)
# The pursuer is travelling in constant speed 0.8 (slower), and
# in constant direction (system parameter chosen by GEKKO), and
# start from (1,0)
# The problem is to find the minimum time (system parmeter) of intercept
# Note that the interception must be on the Apollonius Circle, if exist,
# for various values of target moving direction.
#
# if gekko is not installed, uncomment (delete #) in the following line
#! pip install gekko
# only need to do it once, and put it (#) back as comment once gekko is
# installed, e.g for anaconda python, you only need to do it once to
# install gekko
#
# however, if you are using Google CoLab to run python,
# then, you need to install it every time, if so, keep uncomment if you are
# using Google CoLab

from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt

def apollonius(tTheta,pThetaTry):
  print("tTheta=",tTheta,"pThetaTry=",pThetaTry)
  m = GEKKO()
  nt = 501
  m.time = np.linspace(0,1,nt)
  initialx=4
  initialy=0
  pspeed=0.8
  # (x1,x2) is the position
  x1 = m.Var(value=initialx)
  x2 = m.Var(value=initialy)
  # x3 is time
  x3 = m.Var(value=0)
  # (x4,x5) is the target
  x4 = m.Var(value=0)
  x5 = m.Var(value=0)
  p = np.zeros(nt) # final time = 1 
  p[-1] = 1.0
  final = m.Param(value=p)
  tf = m.FV(value=3,lb=0.1,ub=5.0) 
  tf.STATUS = 1
  ptheta = m.FV(value=pThetaTry,lb=-np.pi,ub=np.pi, fixed_initial=False) 
  ptheta.STATUS = 1
  m.Equation(x1.dt()==pspeed*m.cos(ptheta)*tf)  
  m.Equation(x2.dt()==pspeed*m.sin(ptheta)*tf) 
  m.Equation(x3.dt()==tf)
  m.Equation(x4.dt()==m.cos(tTheta)*tf)  
  m.Equation(x5.dt()==m.sin(tTheta)*tf) 
  m.Equation(((x1-x4)**2+(x2-x5)**2)*final<=0.00001)
  m.Obj(tf)
  m.options.IMODE = 6
  m.solve(COLDSTART=2,disp=False)
  print('Final Time: ' + str(tf.value[0]))
  print('angle (in radiant): ' + str(ptheta.value[0]))
  xx1=x1
  xx2=x2
  xx4=x4
  xx5=x5
  print('xx1[-1]=',xx1[-1],'xx2[-1]=',xx2[-1])
  print('xx4[-1]=',xx4[-1],'xx5[-1]=',xx5[-1])
  return xx1,xx2,xx4,xx5

xx11=np.zeros(501)
xx12=np.zeros(501)
xx14=np.zeros(501)
xx15=np.zeros(501)
[xx11,xx12,xx14,xx15]=apollonius(np.pi/4,np.pi/2)
#tm = np.linspace(0,tf.value[0],nt)

tTheta= 0.7853981633974483 pThetaTry= 1.5707963267948966
Final Time: 3.693100662
angle (in radiant): 2.0595090215
xx1[-1]= 2.6129019206 xx2[-1]= 2.608623031
xx4[-1]= 2.6114165217 xx5[-1]= 2.6114165217

xx21=np.zeros(501)
xx22=np.zeros(501)
xx24=np.zeros(501)
xx25=np.zeros(501)
[xx21,xx22,xx24,xx25]=apollonius(np.pi/6,np.pi/2)
#tm = np.linspace(0,tf.value[0],nt)

tTheta= 0.5235987755982988 pThetaTry= 1.5707963267948966
Final Time: 2.6808999402
angle (in radiant): 2.467639918
xx1[-1]= 2.3241995686 xx2[-1]= 1.3384754713
xx4[-1]= 2.3217274532 xx5[-1]= 1.3404499701

xx31=np.zeros(501)
xx32=np.zeros(501)
xx34=np.zeros(501)
xx35=np.zeros(501)
[xx31,xx32,xx34,xx35]=apollonius(-np.pi/6,-np.pi/4)
#tm = np.linspace(0,tf.value[0],nt)

tTheta= -0.5235987755982988 pThetaTry= -0.7853981633974483
Final Time: 2.6808999402
angle (in radiant): -2.467639918
xx1[-1]= 2.3241995686 xx2[-1]= -1.3384754713
xx4[-1]= 2.3217274532 xx5[-1]= -1.3404499701

xx41=np.zeros(501)
xx42=np.zeros(501)
xx44=np.zeros(501)
xx45=np.zeros(501)
[xx41,xx42,xx44,xx45]=apollonius(-np.pi/4,-np.pi/4)
#tm = np.linspace(0,tf.value[0],nt)

tTheta= -0.7853981633974483 pThetaTry= -0.7853981633974483
Final Time: 3.693100662
angle (in radiant): -2.0595090215
xx1[-1]= 2.6129019206 xx2[-1]= -2.608623031
xx4[-1]= 2.6114165217 xx5[-1]= -2.6114165217

import matplotlib.pyplot as plt
fig, ax = plt.subplots()
ax.set_aspect('equal')
ax.set(xlim=(-1, 6), ylim = (-4, 4))
plt.plot(xx11, xx12, 'b--')
plt.plot(xx14, xx15, 'r--')
plt.plot(xx21, xx22, 'b--')
plt.plot(xx24, xx25, 'r--')
plt.plot(xx31, xx32, 'b--')
plt.plot(xx34, xx35, 'r--')
plt.plot(xx41, xx42, 'b--')
plt.plot(xx44, xx45, 'r--')
pspeed=0.8
ep1=xx14[0]*(pspeed/(1+pspeed))+xx11[0]*(1/(1+pspeed))
ep2=xx14[0]*(-pspeed/(1-pspeed))+xx11[0]*(1/(1-pspeed))
rrr=(ep2-ep1)/2
circle1=plt.Circle(((ep1+ep2)/2,0),rrr,edgecolor='c',linestyle='--',fill=False)
ax.add_patch(circle1)
ratio=1
ax.scatter((ep1),(0),color='r',s=100)
ax.scatter((ep2),(0),color='k',s=100)
ax.scatter((xx11[0],xx11[-1]),(xx12[0],xx12[-1]),color='b',s=100)
ax.scatter((xx14[-1],xx14[0]),(xx15[-1],xx15[0]),color='b',s=100)
ax.scatter((xx21[0],xx21[-1]),(xx22[0],xx22[-1]),color='g',s=100)
ax.scatter((xx24[-1],xx24[0]),(xx25[-1],xx25[0]),color='g',s=100)
ax.scatter((xx31[0],xx31[-1]),(xx32[0],xx32[-1]),color='m',s=100)
ax.scatter((xx34[-1],xx34[0]),(xx35[-1],xx35[0]),color='m',s=100)
ax.scatter((xx41[0],xx41[-1]),(xx42[0],xx42[-1]),color='c',s=100)
ax.scatter((xx44[-1],xx44[0]),(xx45[-1],xx45[0]),color='c',s=100)
fig.set_dpi(150)
plt.show()

import matplotlib.pyplot as plt
fig, ax = plt.subplots()
ax.set_aspect('equal')
ax.set(xlim=(-1, 21), ylim = (-5, 5))
plt.plot(xx11, xx12, 'b--')
plt.plot(xx14, xx15, 'r--')
plt.plot(xx21, xx22, 'b--')
plt.plot(xx24, xx25, 'r--')
plt.plot(xx31, xx32, 'b--')
plt.plot(xx34, xx35, 'r--')
plt.plot(xx41, xx42, 'b--')
plt.plot(xx44, xx45, 'r--')
pspeed=0.8
ep1=xx14[0]*(pspeed/(1+pspeed))+xx11[0]*(1/(1+pspeed))
ep2=xx14[0]*(-pspeed/(1-pspeed))+xx11[0]*(1/(1-pspeed))
rrr=(ep2-ep1)/2
circle1=plt.Circle(((ep1+ep2)/2,0),rrr,edgecolor='c',linestyle='--',fill=False)
ax.add_patch(circle1)
ratio=1
ax.scatter((ep1),(0),color='r',s=100)
ax.scatter((ep2),(0),color='k',s=100)
ax.scatter((xx11[0],xx11[-1]),(xx12[0],xx12[-1]),color='b',s=100)
ax.scatter((xx14[-1],xx14[0]),(xx15[-1],xx15[0]),color='b',s=100)
ax.scatter((xx21[0],xx21[-1]),(xx22[0],xx22[-1]),color='g',s=100)
ax.scatter((xx24[-1],xx24[0]),(xx25[-1],xx25[0]),color='g',s=100)
ax.scatter((xx31[0],xx31[-1]),(xx32[0],xx32[-1]),color='m',s=100)
ax.scatter((xx34[-1],xx34[0]),(xx35[-1],xx35[0]),color='m',s=100)
ax.scatter((xx41[0],xx41[-1]),(xx42[0],xx42[-1]),color='c',s=100)
ax.scatter((xx44[-1],xx44[0]),(xx45[-1],xx45[0]),color='c',s=100)
fig.set_dpi(150)
plt.show()