From 1144280e2bb1bfa75577ffcf4f5a208fd684ea0f Mon Sep 17 00:00:00 2001
From: Quentin Bolsee <quentinbolsee@hotmail.com>
Date: Mon, 22 May 2023 03:56:10 +0200
Subject: [PATCH] ND curve solver
---
.gitignore | 1 +
curves.py | 120 ++++++++++++++++
curves/p.npy | Bin 0 -> 4224 bytes
curves/p_prime.npy | Bin 0 -> 4224 bytes
curves/p_prime2.npy | Bin 0 -> 4224 bytes
curves/u.npy | Bin 0 -> 2176 bytes
curves/u_dot.npy | Bin 0 -> 2176 bytes
plot_solution_video.py | 117 +++++++++++++++
main_animate.py => solve_animate.py | 0
solve_curves.py | 211 ++++++++++++++++++++++++++++
10 files changed, 449 insertions(+)
create mode 100644 .gitignore
create mode 100644 curves.py
create mode 100644 curves/p.npy
create mode 100644 curves/p_prime.npy
create mode 100644 curves/p_prime2.npy
create mode 100644 curves/u.npy
create mode 100644 curves/u_dot.npy
create mode 100644 plot_solution_video.py
rename main_animate.py => solve_animate.py (100%)
create mode 100644 solve_curves.py
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..bee8a64
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+__pycache__
diff --git a/curves.py b/curves.py
new file mode 100644
index 0000000..4a3aadc
--- /dev/null
+++ b/curves.py
@@ -0,0 +1,120 @@
+import numpy as np
+import math
+
+
+class Curve:
+ def __call__(self, u):
+ raise NotImplementedError()
+
+ def derivative(self, u):
+ raise NotImplementedError()
+
+ def second_derivative(self, u):
+ raise NotImplementedError()
+
+
+class Line1D(Curve):
+ def __init__(self, a, b):
+ self.a = a
+ self.b = b
+
+ def __call__(self, u):
+ v = 1 - u
+ return (self.a * v + self.b * u)[:, np.newaxis]
+
+ def derivative(self, u):
+ return u[:, np.newaxis]*0.0 + (self.b - self.a)
+
+ def second_derivative(self, u):
+ return u[:, np.newaxis]*0.0
+
+
+class Line2D(Curve):
+ def __init__(self, a, b):
+ self.a = a
+ self.b = b
+
+ def __call__(self, u):
+ v = 1 - u
+ n = len(u)
+ p = np.zeros((n, 2))
+ p[:, 0] = (self.a[0] * v + self.b[0] * u)
+ p[:, 1] = (self.a[1] * v + self.b[1] * u)
+ return p
+
+ def derivative(self, u):
+ n = len(u)
+ p = np.zeros((n, 2))
+ p[:, 0] = (self.b[0] - self.a[0])
+ p[:, 1] = (self.b[1] - self.a[1])
+ return p
+
+ def second_derivative(self, u):
+ n = len(u)
+ p = np.zeros((n, 2))
+ return p
+
+
+class Circle2D(Curve):
+ def __init__(self, r):
+ self.r = r
+
+ def __call__(self, u):
+ n = len(u)
+ p = np.zeros((n, 2))
+ p[:, 0] = np.cos(2*math.pi*u)
+ p[:, 1] = np.sin(2*math.pi*u)
+ return p
+
+ def derivative(self, u):
+ n = len(u)
+ p = np.zeros((n, 2))
+ p[:, 0] = -2*math.pi*np.sin(2*math.pi*u)
+ p[:, 1] = 2*math.pi*np.cos(2*math.pi*u)
+ return p
+
+ def second_derivative(self, u):
+ n = len(u)
+ p = np.zeros((n, 2))
+ p[:, 0] = -4*math.pi*math.pi*np.cos(2*math.pi*u)
+ p[:, 1] = -4*math.pi*math.pi*np.sin(2*math.pi*u)
+ return p
+
+
+class CubicBezier(Curve):
+ def __init__(self, A, B, C, D):
+ self.n_dims = len(A)
+ self.A = A
+ self.B = B
+ self.C = C
+ self.D = D
+
+ def __call__(self, u):
+ v = 1-u
+ n = len(u)
+ p = np.zeros((n, self.n_dims))
+ for i in range(self.n_dims):
+ p[:, i] += (v*v*v)*self.A[i]
+ p[:, i] += 3*v*v*u*self.B[i]
+ p[:, i] += 3*v*u*u*self.C[i]
+ p[:, i] += u*u*u*self.D[i]
+ return p
+
+ def derivative(self, u):
+ v = 1-u
+ n = len(u)
+ p = np.zeros((n, self.n_dims))
+ for i in range(self.n_dims):
+ p[:, i] += 3*v*v*(self.B[i]-self.A[i])
+ p[:, i] += 6*v*u*(self.C[i]-self.B[i])
+ p[:, i] += 3*u*u*(self.D[i]-self.C[i])
+ return p
+
+ def second_derivative(self, u):
+ v = 1 - u
+ n = len(u)
+ p = np.zeros((n, self.n_dims))
+ for i in range(self.n_dims):
+ p[:, i] += 6*v*(self.C[i]-2*self.B[i]+self.A[i])
+ p[:, i] += 6*u*(self.D[i]-2*self.C[i]+self.B[i])
+ return p
diff --git a/curves/p.npy b/curves/p.npy
new file mode 100644
index 0000000000000000000000000000000000000000..8b34498dcf3181666b25fe14d6d3900ba5b1ad8b
GIT binary patch
literal 4224
zcmbR27wQ`j$;eQ~P_3SlTAW;@Zl$1ZlV+i=qoAIaUsO_*m=~X4l#&V(cT3DEPSsIR
zFV09TNL9B|&@eJJ(@`+e)KREaAQNygpn*yU35Fm4+wH^WU;i(zz+u08)lH8d&!^k7
ztuvedoL|FU%kk(|p*suhy-G6F*8R7#f9<m1Pvnl}_Q|Ke+--aBZqIqC&+%2qTKkx$
zmlLa(1=;_8GvV3WWgG3Uxq3d}IvHipss6Qiaq|}Y{DXg$P4trNpY_|kT^P09UW%<S
z`sliJdyfU{%f4#wviFP1@7?kw*Z%Q^X(p?_@3G%{xnzc2dXat3EGzFbd-vO?XJ^Fu
znwHt0+*SDA*#4mXn-^RmN206jyWhU)JTm8y{m%neOdj5>v%jQh%s%h?VS5=9`FjP|
z8|{Bj&Zs=*f7E`x=7UR&xh?kb4I4i>FFj`8Q=J*+q}y&Esch6L^W(TZPfEGlMe#0s
z=be1cVIC*#+hmV5`0Di9m)B+6eV=;DUh8AWM=jF{_QC6OI$0l{wofbTUV7xkM0@QH
zGqZNhv-V4Ndw3t-Fxfud^=(*F{yBU8d-DRn^-Q(*P~Q6Y&!+SCSAJ!y>()-USJ01~
z_pkDTy_|lZwRz)A`=FUDlU*b)+Dp%P{O9qE+4iaL-lYa_zi9s@$m<r<k-7FAK3WQT
zA(!lP>;e`VGA^)xdhx`m>+dhwS4buWz0zE0zi!T*yx^|O_GTwfnhVD)vgbctF1ked
zioJErH;yUO7u%auuQ`5Z_Z54YhheoLcb3?DpYG|q5OmePAT7$rMr)aU-A%3HO)sw6
zpZS%qYuvcpp7F?xy3*Qf_NUgc-B5VA!v4>S7dvGHuiO9a>SccGxypX-G9A6ui?7@N
zTwI)9zk9X4;O(*}%BDB$L)kQT?>Aj*-<ERoY3I=!_Nxv^ZkxJeoxM(Cn)QQ_oA$>)
z+;0D(xWQigz5YAR$2aXy?B22e!s`w8n{_<cvkGq6FFz3#cX{VV`!D7P`xpPYWxsA-
zrTvHQP4?pYYbV@nziqE^^o*T!(q?-v136)Sp*!|*hq*00Y_{0%id0i@pMA%EuEIOE
zY^kmG(@tJL(4u_T-Y`YGz5LHsd!BhN|4yyCYo97wJtgtcHhZ_Pbq{3>@7dSyp28)2
zX1jgC+Zn~R+wR$)yQ_b6?#><d`<E=)``+rl{Q_o|1=m;Yw2!vja-wYCeR~@aHGh}=
z_9t)7btycse_dv(>NaDy{nVYpi_;Sy*#El3*q$_HkA3W_@YKr-AJ}j6e_GByWv{))
zF~?cwA3m_pDVcs*WBNXOPMPRqv5F7v<4>F~-8FZ=z2t74y!wQP_60AMWN!o?u+QIQ
zFJ?CXq5Ztxh<|mr4%o{H$~rgSeP|yuCFRWNq=WWO&wML0q#xM_9#ksb@#diYJBMi7
z=Mj(Wm2Tucv#U5{Z)tE&m~q-8`?5JIe37h&?QIV0H*UZ5$o}I+`CANA4%>5aa&qzT
zKeo59v9T~vIby#@;IR>d_hb8y^FqZQZare3@zSkfYTIM`!+YJ6cs-BWt1jAGcJ|<7
z`;eG-oa^r%wde4<Fl*(X$M$Y^s=xOaAG6o`z+R|r{>0u<{e-d=_i_6Li{I9U6h5(k
zeQQ&p_>$xHjm2DUYO9~vKb1LnDbVhO{gb@szKfnbvA?z9oXO*xC+zPp;gvci`_w)!
zYJ2O^vXk~*Ds1}m!=Bo^-VNLQN#vCM&p(lZ%KcC6+5dK|PT6wG-o<v3Yxwb}_F=V}
zF`}WT?OVQ>2iX39YJXEvWb?mor|r)fUfFuh?3umwI+b}+v(DI?vwZlalK0GB@|hWT
zvfWwx>s2C4G?zZJm+4h|aq;n4`@>64nLfMy%>G5%d8NdzbM_(=XPd+bJ-1gi=UvHV
zbl(1vpMYww$8-DYgS#_M+&XV>_jY4<Ro!#@BPqAly__!CJ6ChQRo(L39!yQYV9&;}
z;OvB#&+WHuXi%O1_JaMqMgKPLk$Yk9X44|e;d0SlIclBU>fjgl|9hXZyqR^;ex<F&
zjM$DB_M6%#U2^|)(cU9Qb@A0bFYHaYD{CdZF4>D6{dZX8;|u$+Uu}P)7G1K}ZuhO1
zQGIFu#%z@v&+kk2b2iS3d>8T3{=}jSL7E|#?OTKMuXp#nv_ByKLVW$&%l2QYLu$Y6
ze`&v<|6b25_AB-*R+rD~e|~B2aAo0&7l~Kw&&(+awp4p%UvOSNY1*DE_DP!$_J~Bj
zvS&+n+ps|Vs{P#_XNJSwuk0@uFn6&OU$qamXf|@&_sV|7EthBe&Rw;?b%b-p{130}
zqndu~KBINbzSA+`=>?_N_PacJn6%rk*;nr6&%Yh=+J3H3-ui2gui5W*UwHCB+iUyF
z`?z<Xaky@u`+ja^(~j5nMh2Iqx#nHBUv#*SS?Sel`!0`7C%S)Mw=cN-=k)^VH})G}
zPTS!ZdBgs#ZsB$Y-#7N9U-KUMZ@*!0^r+;OQ{5Z;wZ0m*J)$@5L;p|U%v}G*UhB$+
z{R|~H?X#x%%`AEF#(rPcw}e%fZrb;3Fq2Ccd~0v}`I&f|;Vt`fNta}toZi}3`WM`u
zFyWTH$F5b9?8R^GT@o7>9e;nzUg(&|%<YTc+Lv*$rl|Yhws)9nst|hRt^F;XH)3no
z-L`*a_RHuY%R76wRmz(}`R>@O?UE0RHGOA4A!VkHT;3gfZasaz)2Z+5TWqasna|y^
z-?(t&eeG%Q?0<`WpT(qi*Iq}(z`ya>JNqM-cGXDq-L;=`#Q5QvZ}0541eHiSy}N7A
zFv(R)K>fY_yb8gL8ozt?t+U_N`Gmi>4^(T2KeFzgz14^9lbhS$+b^+Q_f3rdzP-(S
zt-IT{zqij?(5X_Gf8T!k_Vf3jKYwpuV8HO;{Q3L#CMpsFJUk!lPdxw6FCp>3K50YN
zL=}|}_NVoC=em15uz$p^VrFOa!Cv>_ik6a!2lh6bIu1mJe6YXv#{R<G6%Xtg%<q0G
z&-q}#!TI~?gEt=7Z<9^ypV{)ke%UqG&-XYU+P`|69<hJU2m5)`79IR-@z7pFUtQqU
zmJjw#B`ur&WjwV1mGPoN>g)%5yTl~Ne={E1zg7AW68Q9k{Z8M_FF%}kXy5&$DXstS
z2m3o(@6@jTcxazs`ZWKl=tuj>FFQ+CYCN+4F(=wsMen2i@v6VRIT4TS?<~?Pu5tTl
zA9sGgo=nFh`<dsaeY_X<(ca~T`=ssLAKC9({-nUC{G)wR8_yS;=a1}9ZH?G`wEv_1
z!c#5hSBX8g4{I>5_FnPP{`FM#1^-<i+du0&+I8>1M|-WC*$*vCAKP<ex-Y4}`O)5`
z^#otUlE?NQ(-^z-KYg@U>)x_B;__pAgLxMJ-f?}h-+g<Tv=!46druC-!~2v!**{Mg
z`}E7;iM>*c{_OcSpX{e8R4tkr|HR%!vBG0g=qLM4-#k``^gXe+x$oaQJ@1pf^6j5>
zS$m$?Pm%qty`lY+{gr*pi)OxhVt+k(-?KXlKH0CCbhl@L#8dl8miOxAc73v+;r?$|
zt=m(3f3^8LOD})2uRV8Xxqiu0`&k=Z!ydo+WbYyI;oO#mPwoFDiAiLzeYSrfx_&kH
z`KR`5he8jsD15dz*fC+J(VwUGc3~bOyDUH3D}3NC(a?NmZ?N3xa!2rI``PQJhrJGc
zW}jsfd7v=&vwi22Rd*_zp4peoWtdmm{@MOY)1Jml>z>))3Cz`-y704oT9xy`&$pi0
z>pXmHd~x?@d-Xlr#O|;?w|{?WMS<3}&-RR#y?;B6p4)F+$FO|ihtKwRAKHHW9sAt=
z?{EE?=G<TGmoB%@HRyb9ALj%rNx#^GsV&d#6FAj(9dh_$Ki602&8>US?I+wQ&)pgQ
z#eNb0#izcUFYL?AH$A;r_Ql?8)1J^;;}`bx)=E9)ne@fpRn2F6dh84PDTlu5X0QEX
zzcc1rD|h<~`zL?fo8F%IVqfKEWShR>g}uac(WO(Ke6cUQQWjBp^M(Ch)+gbK3}5X#
zzFP%5Fuk;od1w|JBJ<UL)q<}pc4@t|Z(<BC&$IYyzwdzT)n`F3?f1N`$=DF`)qcH)
zR`B7fm-Z8Me}0lG{A!<~Icc8XqL=o49y~YK^?tQK6@131{p3q~hhx9)<gEH?e{6?;
zclL*u_6B;K2Sbm4wRiKJto%#tmHjLB8uzRxU+wk3@s=CfzOw&wYr4^D#&7m{8_XC5
zl3&@s(_82wDgVv>bw-Qrl#W;SKHYchx7vKOpCFL8e9zif_KV8<p4CTvvv--1wX6No
zEBor|-@hs=zS)2J@Zsa<Z?Ei=<TC7+O#5cv!d-Flztn5{N6#h7{%`qa|L8{9o|Sg5
z?K|Rw5_>Lxv;Xdy;`Sl&wf#Mw$iv>BzS*zw-~Q-g%WM0B^1?Vf!SD7jZaw4iUH;ns
zq}qcoDF)x|XWq$lNI3c0zGTr$j|2YS?U%T2|H1kCwf&#MOEQ7^-|fG!xoz~|d1L=Z
z=Dv|c|9AT$!QQ7*x^L_+YK8cUt^aP%8OAJF@B7ByOu0<Y=lpm3g-7Mr7w5jQU((w-
zW7qrd_DX?~*57;H*suP0$u~*hhkeycHA9XyZ|pB#n<n65_`|-Z{y+E9Q*Z2-9Fbg{
z5cI>|jU~$c%!@bn>zx)H-%<R-enQ6DxEi*%_H{S%9DJw#un(DAaCWouTl-nOr_Rf5
z|6yM_S&F~V;jR6|%x~s;H-Ff_u~wSDDdw&HyJ8FLn%_U{r?mh3QC9KR-g3*8*_?7e
z?W5AwX6%{#)}DD2zsz-qpY~d31b#HGduxCBi*DW1<e&DhmzhjCdg86U2}?<fY5Pz6
zwig^PS|7c&-;%EO@%XBr_Rktw@(=!gYkzN9XWQI!KkdIH&s$O^^3LAI^AX$jPe1MF
z)cdk5(RpVdajnEeRQ#9yfukLq9xm_f4_$4&xWoFFeSrFL{lw^Z_KfEv7c5HnWp6UI
z{?psSclHZBO_MLT{<05d&^-I5{hhtu*_b=ttAE+gjI^?jnfuP(_@T%-o{PWi-%Zro
zVYTU<y$$c8Ut-^X+1t!2*xz>So&A*?9xB<gzwL9Ao6p$YdS~Bu+LxEh<+pvty_->q
zZ{OM9&G}mXCF{4n#GhAB-~4}PpXSn{rakGmea2edm-hwV+poE4cYEE=-}VlVB_3EP
zzqhy8k{QwP@V9+ipR(IG<M;M6qCFE9aQ?B^T5x2Vn9F<nkZ`4~{6>H51vra+X9T~u
z|D|Usc|Pioz0$6wPb-t&+lypf^1I*s$KGej<y$)o-rE~ZFp_Xw`^UawNAZfVy7%@s
zH0GIqz4phx>-nFPe%<fwr|yo-|HJUtzCh{Ot_9QI+kZX3vN2ryuf0&zvTt6C-`h97
zy=1^1_SZhDe#;N<b?@zu&6c$1Y5Z$%(A;jbXvcedzobKX>1+Pl*M&{9^*Qw3e%T4Z
zCdnIr?e$)TmwTLkZyyonW39~c&t6mCId$sg_x7JAZHs9&_-7v>cD~8*_Ivw3tW#Gw
z#Qw8iHlfH{`q6uPMsuBqp<Vy%fAuY~3ViY2zJKk36GylIv$tcqbNb@j_x1)KrzuW+
z`p-W0c7N6EPw(yXJ3p`8A^hLop4<G;ns4vzum6=Yuyy)xf40d(j^)>T`yW>Wv*Zi^
z+kY~$=34OQy?yA+J8}sN{@YLGX=AMa_ujtY{C|Fi3;*pGF)XN`|Np%`w7c&Bp+DFI
E0IW_&8UO$Q
literal 0
HcmV?d00001
diff --git a/curves/p_prime.npy b/curves/p_prime.npy
new file mode 100644
index 0000000000000000000000000000000000000000..a00dc31931dedb2d125dcff6fca6ca5b0728540b
GIT binary patch
literal 4224
zcmbR27wQ`j$;eQ~P_3SlTAW;@Zl$1ZlV+i=qoAIaUsO_*m=~X4l#&V(cT3DEPSsIR
zFV09TNL9B|&@eJJ(@`+e)KREaAQNygfB}aCm||dHI5Z)_fZ+nWL;L*g=b!(dZlALF
zkz(Qkc8Bk3;qBX>FSmbX-gZT)klo=dlV$$W+Z*kJ{JcF}o!A|&nNIr_wtc((tY?O`
zCxqD@rs^Dy{Lr@7{y2kG@tUV>4(*@g&sZ!yXn$=^vew7#Y!35{jzzw0IAVWn*Sba1
zy4V~h1&dsr8g|@%R+RLeIk9XGNnu`gMrx<*18P=YV>V=S;Iqz{dg%Qb`{(kJlTNd+
zIZS2DyXCg|ynVc@7XOzUtPa1gtZj?8xM;uojf`*YT2=??SrXGlr(ClCC2HuC-o)yl
z5l}ww=$p&-(g)LcHife~sC+*;C*SF+z1+LKy|KEi4!r#WlZ2;UvuB=bb|I6Q)!~V_
zciqM(*X>WAy>R*3O%{ih6Dt!v4R6|)H1y0^vVp}R=Fe>F+f}#h8MkK2U2bP__#3{7
zC;i}U`@HOo)TDS8hn^*SuDoHnYrk)CM{bZgiv#DQU6XTM@7aHfJ!Q6-pT(g>V6Eq~
zru+76vvd!JykK^CHr@00nz#q{|G?%jJ8&&9JDdFLf&Go?5t6rNF*__-Fn8AUDG%+Z
zL@GRATf*$Hb!J_%hwdYLgSnh#kNlV&Y`br&mmPUzzadHFSh*&%gH7qW=L%7e?L}5O
zT+d};cGwjDcag`t$MzA$eJhXLXM%?R6Z?jJe{E`ZGdVD`aq{pBJ+<%X?wdGeI+MfQ
zi$`~^UHH_#<d(y7-V!DUgK2+~PU=6icUmn{_dI~f;b+iV_M|<}>_7eG6jRb;a<JsM
zvAo><xqa&;h5WtTOb+k2);M!rd~W|kBeHYnOGXEonAx`^BVX7DFm6r}JHhC1=$CQs
zvimRWJEHX%uPkG9xI5A9_xhBV_Djr9$9-yLbjVk{++z0PrTwC*owbFjj1HA6Z*p2?
zy|QmC&5R9nWpwzZ8P&Dx)hm0K>skLMsxmr!Tol5#BlETWi(S15#%zoZ@ggT{jh?@@
z&sVtAZ2FwR!L@Gt2hHR+_BYrqGG?4$a9I36K7QWaH}=Yf`*~wlF*uZ3Go9=YdutyU
zlTp&p!{D&LFHY~@*|+wICynN?<})}n-ZJTb>HN+<c!yl;>p%vFT_9@5J9~9a9epEH
z28YaOe`j;;_xAUsqi5ZeU~uT0FnQX{Iq&UXDTMbP{`uct^!|z>8@3Pj4|zce`M<rw
zrz6v|Y(Ln~`ImT9qvyYUD&st>>%|}Jr9YouQXBT){uz%+bkoWY_VKSP*rq7{w_hyM
z{xkdD2m6%g8LzV6{<8-sZ=sL&M$cylu<rV2e^w%8{V&&#_J>~yS|+srv$qj_cWi#e
zM|+7+rxw?Q{<DA1YZ}wM>Z85M-(wS^RQ}mR^6y7`As#k{U!VWlzx(|2KP%5C`)zU^
zAES=^wfA~h&bQS1lf9g&Z))w-zxIz$ZJO1T^~v7LKQuZq{jdG@4Xc*SoAJrsI(NZk
zHmkq($0o(={5<-}{$rosxm@1A_JV~M4JUp2WS_a^zt^;ff9$7wGi=S2`)t4buCQ0b
zwm<d}N^iE-dw#ZG#g%<pyXTL6=+`MGSBgK|7h1EPpON&(zVFB{gY>zd?f>OJ3OR21
z$DV0|>}rc+pY45CFvV^c{A0fnoSr}1Cq7Aip7H9p{Ty(H|6*^Y!t>?!;otU$Zhz)h
zvH4<uGyQ9mz`Wn~sx!E1-zR>tw^^?^g{|_p{o#On*+1L9*eCy*puR8UxBVQ}{2Pv|
zzu5bGI@_8X{<hz+ZO@?-=f2qgpKWoqknguWL&|ZjzK>t*OFtF7&wcaDz6V@je6?Ta
z^QK$-^e_A1&B7&DOupK$UIhZb>_fret9>rd8t>O#zwD=c4&EYC`qlnR;fK7p*}v?0
z`}ijcP5EkXf4f{{qwg>KLn<Aw{Wg8I_w#VFF*EpOziIp4!za&wwO8Kh|Exy%m;F7r
z4U)}ozS{57T_Du@{inUlrTDj19N+9EmKh)PyYtijdE^<pjmqEbt(3Sv-8}fy{^;GW
zylQse>=hO%FI8Ld)BZv1ov54P-|Ww*Hrz7l|7mY}t6ueV!8d!Wwepi#%75A)4SJCK
zyW^XEo(0pHsqsJUCm#Q6oVDPaea3P1ju#$3?PprtC>P)U&0aNX!m&R_Kke@>&9`Je
z`^|pY2kZ7rvOn#k`HxiSKKf?=rc3KoCg)Fk(bB7?)4qMP|E3Ysf9UHEd#>w?3v9W*
z+aEo3;o6-?KkThd^);1czuQL?bS&6$;fMY5{_=Qt{qOc5Uxx1gVGr{4Du?g(XOA7&
zwPoE8dt*y`SMR{@_G`->xv$RqVIS17?~7LAcl#h6)_dFff7q|N{7ccZ;Jf{vC)~zS
zjX&(2q`#b5TKC=l&4N&_{l!1*MdDX|b?EtSe?Q?-^3${*_KNdm=Sa=`Za-aRll|GK
zANHBIeGeEd{cbP(=E&SK{~z{mg{_l1H+;7b*)CQ0(B+5y8jq)qa=X9VN40H!z-#@(
zenq8^{HG(|?NuWCkN!6NVSjH~ffdWS@AjMY*XgX*{9*5ZA@Fj@weR*UTmoV;N<ZxX
zetrM)@!jwCGB36?`bqz=e+sT&zT5u*RYRgb>^aSnC+vOo-G0XU<M+Sw|FF*tuu$Om
z@ZJ8;4AV<_+&}DD(ogA5{PNviwe6YPa`qqgw<;xfM*R40FI;}hb|K3T`!^k`hvI&J
zw?CRKxIKpHhrQ*LcOHxXez&(x>E3jc;fMX<*(JVe|G(Q;CU_V~F#NEG)bE&lboIxe
z=2=0_6G5|24b43;_cBA>3-eC~)ITu)azOnH3lB(riwLhj(C~tVryn#tm7w8$7aHD?
z(D+ymjSuwrLXS`M_(o3;=;>uIG(9;((^D`sy{(0&w{>Xg5td#}py?Hso&%oM2!qly
zEWNLQrgvC=uz}_WSbpJ#=9kyd{3Hg=PfMZsEe4w3RG|6s4m3Za=U4RnjGo`o%LDZC
z0#=?JgO(?-^5zA!yn&TRuF&!bR$kqOmRGRyY$LQhgOzs&q2(Q{Jmi6vhp_T;GPJw|
zm8Pp!A<EOWXyt7QT6v6KUZa=i=;b|neE_R3)S&f+HMBlC0<BMA_02qJeFLkHHbd(p
z^!f_DK7-YFVbJ;xRv$(}>qGSV@;zF8Y6-1xk3;KQSbe+=S|6j=*XZ>*dVP=H9zbs|
z9D%kcY@zK5SbO6fw7mgqkIV)&w}n2!+bfHp?G;#i<|(v2gWleOwTI-P?V;Du_L3O1
zy>tiKp6Z9Tr_|8eTd?-nc4&JH)?TZJw%1_oxqZ;~9C~{X)*f60Z4biQi*C^NBCI``
z2yIWo+MA!D?M+yF^bfQ>3Tv-cK-;V6?OF8pE_!<y)?Su_wwGI=?dgxu_Vh<+d)o@y
z-iEcuzeC&Ou=ct>w7rhro^L>F@5A~73&8!cC-D9PBt1Wd_b2S2{Rvoq!yek-m;~*Q
z?11)1VEvUv(EbXnKf?*_&!G2rVEv&|Xn$xww7<jy?JvRlQxl;5DNrjC)Zb!`fb_Ru
z{jo@Be+<@NTMq57!TNKL!S(HRjQ$?1Kd1)n55oG3I?(>&9%z430o>re1n+M$U4`^F
zpF;bii=q8de`tS|1Julae+J&4O@Q`iVg237(Ecv0KfDMW{zu^b<z8ri8P=a(4((3|
kK>OQoq5W-Ge|$HzKMw1!--q_sVg306Xn!8k&qwt40rk55NdN!<
literal 0
HcmV?d00001
diff --git a/curves/p_prime2.npy b/curves/p_prime2.npy
new file mode 100644
index 0000000000000000000000000000000000000000..2ba851498aa33eaaa1b90fcb4cc53d2fcf424adc
GIT binary patch
literal 4224
zcmbR27wQ`j$;eQ~P_3SlTAW;@Zl$1ZlV+i=qoAIaUsO_*m=~X4l#&V(cT3DEPSsIR
zFV09TNL9B|&@eJJ(@`+e)KREaAQNygfPusTFvTF@aN#@%h#!E`4h!akfcOC@?NE>p
z0^$dtw1a~^2#6nm(hmG!AbtQ$@ryg$0|T)GVCtTj!#Xe!I{>EEi8<7Pf!F~sRVU`)
z2L@sXz?7eugB%!$9RO2uVh-=#f`I4&DDAKp3`7rrslB2Oy<i}E08I6YI>ds3=m9Vl
zE9#&N2BHVRl&+}5A21L(0H*$kIGh0kkpp1rjEKV=Fc3Karsjw^<bZ+50Wg&#;$Q;?
zA_u^fjfeve7>FDIQ#>LLcW#4#@Bt|8ux2#~2p@pb4mH&vAbbEyJNS5mfban*?I0r!
z0>THNw8NWMARu%AN;~WV1EB+8YL}2h7Z?Z~08?E;4pCqrbO2062{~wifzSakr6uI>
z>n8{Z9)Qvgr%r-^-~lM@Fl#0V2p)ja4q2HXAb0>uJ6KtQfZzcr?ZCwe0)hviw8ITB
z5I6v)ZU{K600V&oU}}YcLj@QJ8~{@l0uCNvAaDRoc?dX2fPugPFeM=XO`rVG^vMrR
zkNnW|$nPKkNq7gKw8I8)LgPCCr5*gi35)Llly-OzN=|Qi4?t;$esIF!JpiR0^uP&)
z_W+c3I0sG`JO`k(LmoIG@Em~B4t$V^KLDkn@y!j5Z*FM(azo>n8ycTn(D>wX=mAF@
z*8wQ)pbd^Ft^-io0TeqYPjViB(hi{5$jszC0Hqy3q0h<5c>qd7!<z#d-W<^I<$#7S
zhr^5KAi#bAN;`D4g8=&hDD9x34g%~4ptQsBV<5nG07^Tgr-1<50VwUj&ISetptQr1
zr69n107^T!xPk!d0VwV81PoXXfT<@e4lQ87asW)VusEoI0m}g}rNZKH=r9N{AAr&h
zNy#9<d;m&2FfoI{0VwUTa1jVF9e~mfj!q!JbO1^_JOl&A17PYQqeCMYFdhI?jf@V8
zV8D0)Oer!t8~_6bFnQnrgF^xsFdP6=2@DR9a+CpDo-#no$N&2w<>P<*25>?5Z$Ff_
zhZJ0Y_d{uWhQHvFXg`#;hZHb>_Csm=Do}Az{d+%@wuj{VU;Cl7JtQCh+z+MgEkN1R
z^5=diZ4b$3KlVdu`yJas;QM|kZU5sN80?4A_7-L!@NGYowl6CMfv@|awEea%An;{B
zl(zr&84UJAX?p`>5cs?wO4}C{fxxHzP}+XOMiBV8A4=PQfPnoF>Vv%j7<||drVKvV
z-vL)7@AgA!`xtP+{&qi<wuh9nulGY~`v`Es_i8_swuh8MFZM%e`-pH5c)lM>+uyhf
z0#EltY5Rx}5O}g5O50z#1OkutLuvbvKoEGiA4=P=09BXs@9&4w_6xuj$DRF9+I|73
zIGuTOKa{p#FbxE*?T6C#3nqiW<^53Fe!&C~xUe5e+Y5lI1L@QIp|rh#C<q+g52fuD
zz!l}b{ZQJTK?npk?T6C#4B&!v#(pRb<HKl#I*>M@P5T)j=GjB({R|-MM334->HQ24
z_t-;exI3ZlMfd~ct;v@W{seh@+BHNtz`_L*UWjmmgr_~E{c(SP04TTvAKF9d{Wn0-
zap{pgl!nI>G``^R2#rs8JVWCfo(`bt0iI5v>1F>7Q1yNLwLO&H9|1{k_E38N4M=*l
zhtlwL3Qe!@bPP?;@N^DM@9=y8%@6Q=0?jY`KY+8-XL~5Ue*-AHZTVskrT15WvSaC2
zdngUhr_lTg&&SaG4A1A#{0=V%pydI)oPd@W`+tC%2S0z>L+SlHzy-}OdnmoX0#Y8?
zLuq(91ud`O<ruU)gO_vA@(x}OLd!#VISDN<_e0B5c)1EIXQA|ggW!^w!2wFc%V}tN
z4KK%`<vF~ZhnDy7dH`A<!0QQUeQ|&hT%9mGK<NXCpz0=>*#Sx)I0&ka4l_GIX?Q&a
zt*_wq7_>fv*K^SN4qgvJ>qB@w39T;=uz;&mHU}tuAO%$2rm;Cd=>sP~)$uVl2Ph4%
zr=j&VydH<v=kR(STHnLl0g(0<qMZO~pK%=EfHWi=p!9(Za6^OB0ZJb@0d9zJIzZ_I
zI*|5?1C)lhW1#IBcsmEue&vF<gCOl)L^}!6KIVqDqj{k1Y9462n+Mts=Q(g5(jIew
z(g*Y*?KKA|4R6Om+jH=C9<;p&ZwEr#g9p|_+KUcQ8rqNHhxTju4}f|<`T6_~Q2GF<
zw{-qIzXOzpw^PBztOTMR3khFDI~S7Q5ba<{cq7`$kn%_X8vcUN@E1G)$(Mo-P#WG2
zhqlMz?R02+9o~+Iw&&sPd`SC15Z(^}N1GO+p8&2lqY(WFaI>@v(a!)E1G|I{fO;dZ
zUI{rsX?Q;c+FyajuQ0UTi|FS-`#aEfu`slqjOZsp>Th9a{3H5RuznVlhNcHZzYJV`
z%t7?qz{%qbqF)DYUHuV(_xp6f=|$85N*{o<BSamb^no65e?Zg$N*~w*?r4fSK<NW-
z!2JhN2Ph5ir-G|ZIYd7e-0ko~^mD<@z&b=f7@X|ZA^OSSYUQ5T0X}elM%)2PAFu;;
kx9!Cppft4KBM$8cA^Pc%`V!HPhxX^8?NSM7c`9)L0G(N+8vp<R
literal 0
HcmV?d00001
diff --git a/curves/u.npy b/curves/u.npy
new file mode 100644
index 0000000000000000000000000000000000000000..db5f73d34a33111edf97ffafa41855a68631221a
GIT binary patch
literal 2176
zcmbR27wQ`j$;eQ~P_3SlTAW;@Zl$1ZlV+i=qoAIaUsO_*m=~X4l#&V(cT3DEPSsIR
zFV09TNL9B|&@eJJ)6vvXs8t{va4|rE02mb5gDHUqdkHY;fbu8Ui-5rtsQ3(f88Daw
zm0w^l1O|(s>Xz7xfx$AU`W5z4V6Y0RZ;ibi7_5Wp-(W8Y1{<N~Y_b;ygUwL$w%Ch;
z!B(ib+w8@`U^~?O9rlu7uoG&}E_-P(*bTLBkG(7y?1kF9&t4u3_CxJIU_Sv24nW;;
z(0<ZH5I6{R&msFsU~mZPuEX|ICWF9XsQZrCPXU7?P<I}+pEeZ)jzZmg%zhde9D}<1
zxc!XjAaES&{uA~yz~BVbA1Cc+%>;pyQ2(5=p9Kb|p#C~-KW8=woQC@EjQt!iI0N<P
zS^If&LEtRZzvt}dfx$Vbzt7t*m=6Nyq5i*M9{>gypy6=A{^A8NfQH9K`#><b2o0Bu
z_LnY#0W^Fr*$08aC1^NZvcG%@44~n4**+KyE<?lZvi+6IU;qujEA}B^a0MEUSM0A|
z0Rw1wUbPPegR9VRy=s5$Di}b+_nLhe7+iyf^ELbH*T4W8-q-EJ!QeVH+^^f;xDE!;
z@V{Xn0R}gq@o>Zb<_$1_#>Y+jNHDkwjhCDDw{C&~G=6T`M}fgDXguArzkLe~pz(Fv
zJ{k;eL*wnX{hixj0FA#p_Ay{^2O5ue?C;(I1898SwT}gZyU=*OYk%)97(nCqo_!n`
z+=IsRJ^TCjzyKQG_wD1s;660o@7q7P4+hZqe_(F_1`nX=;DLPs7(9TchX?i>H-Nwc
zXu5b{|M3F|Jb<Q;hxUeG@DQ3#9@-az!9!?zd1$|BBM3Z%rkjWMpFV=XLumSWWN!op
zkD%%3k$n*uJc6dDNA{aHfxshZx_V^)`4b2{f~K#>_QqiF7@E!=+ZThuV`zGNY`<kQ
z2t0<SyT|rlK7+txX!?6%ZvqBSpy}|5eF+#mfu_eN_FK1rz!PY?d}9Cg3kW=crq8GL
zreN?CnoghEmx94lXnK8WzilfBJcXv)r}p2zg1}Q~`h8|^1_sZd>G+v_85lf+rsrq&
z+qZ$hGibVgX8-*g2t0$P@8|aBVDKE8&Y#<tgTZrXdVg-eV><{uho<}I_CLOZz;kH&
ze_?L{1}~ub;Dvn!7`%YyhZpuccYwePXuf!1|MLe3ynyD9m-d!m@DiF&UfNfJ!Aod<
zd1=3ECkVWR=9`!HzkY(iOKARiWp4!rub}zpm3<W$yn^PZSN6Mifxs(hzItW<`xgkj
zg66N+_SRtV8k)~u+gF3ZYiNFZZNFzX2)u^oyVv%AeuKbkX#RU+ZvzHzp!x8PeGM4A
zf#%0I_Ivk$z#C}3d}IIj4+y-0=FhkGwqWoUnor-_*Mh-YXnuWbzi%%HyoKi5xAy=3
zg1}p7{(Wa}2L|t;`S_iE9T>cW=I3|z`}cvsJ7~UsXaD~n2)u*l@Avi)@E)4a-`m%N
Z!Fy<ae{X+aKM1^s=KJ^dkZS*fJpj_e$HV{t
literal 0
HcmV?d00001
diff --git a/curves/u_dot.npy b/curves/u_dot.npy
new file mode 100644
index 0000000000000000000000000000000000000000..8ddef6c76b92842f1593aa6ed25d31cbb35924e2
GIT binary patch
literal 2176
zcmbR27wQ`j$;eQ~P_3SlTAW;@Zl$1ZlV+i=qoAIaUsO_*m=~X4l#&V(cT3DEPSsIR
zFV09TNL9B|&@eJJ)6vvXs8t{va4|qZ#YYo0ue;mqdk=5B`F8gq`&DN>p4?|TVc)dy
zs%w1K8GA#~*$)ELFW4_{{VI|E_M-i~T}^YQPQGma=x*`RD#fezpXwSIZ020Ex4ZPY
zvzh&deZlF2H#pL6+TW-;ee3?tTlQzBrJY;Ja>u?^Ao2PZ_q+CS0`DyZTkhG1y+1Cv
zVAp;7@ZXEx^OZfYf44JVbSlq7`yKE47pg6MXs@a`-SeOEBm1dy_W$)e`pEu@&n9!%
z(8u;F;$_SnPafO1JuAJuv+#-iKF=50mH(dDKbX8+FR|;X{fp;zd|jf?>_fMD<#H}~
zW`AU}lG$DL=k_tp2iYA~J+~L-s=3{&{=$CETmFmR=D)D-a{tJ8QTV03<xDLnujZHb
z$(fE<CcJxTzoL5a^E08Z>{|-osVVM!WuN&czF@V)Yx}s~u9Vi2*Y-|Xr|%v)^V+_I
z>10Ta;v4%0=`hpXMQ`luCMT=)9e87()UDwDpX;stnLi(XCI`K>*PWBd&pZ9Cecagx
zryXy;wO@YQ*waz+oqfj{i#g8W@9YCi8c$VCerNy0<NOTabMNfy;-*=evA?(H=GS`2
zX8+z^!As3$S?PQGjNqNjMyuc3r`$Tub@cvw`?NjU?{{;4u-9^(S7@O8!G2C}xUZMZ
z2m6P~6JHC3ez1?9?D%z3?gx9e;^g#8tsm^4m0VhKW9|q0eQDfnYqox{|ME~KJ>={M
z`y1sfCfA;Pu)n~$w%Gj72YVB_-GQ~jAMJVP`7U3h_0hhN^$OD|$B*{XEn@3$Mtrnq
z+t{)5e*Q;$4&6WB@3(!lH#mBf_ukx(_N7j#1$Vc6w7<o^FZ<!?kM^(bzEOSq=%c;i
zv7?Kae|@y)O=3Q-F8Ik_FU%l1M&pycr9=?l68lf~raPl67{fo=Z)wPrDbM?4@74A;
zi@oiWeZ<p@!&~Nlvfr^hrL1x5C;JD&|BvOL{bc`ZMdh2ur=RTS>{%|n>+dJ~J&CRf
zQevO&8UI?mTcY>bUTXcpOVRG1?aex;$T}u`woi8sKlY&Fv;DM->5EJ!e70Y+|K*f`
zWuNV>Rem};?*455I$2Ch?EGi@z0VpC-Ff)gzFXjJ*TT=A?f1>Nrcl80#lC0f?SNpB
zFZOQ~Y}G=Qzu4d3boNJ`;TQWu+bSz>+I_Jv+1a-@$NP(Yh2V1!hwv}<aq;5f(MezI
zH&*Rfd^qQeefPa5AM4A$*f$^gJ85eD7ki(slWRCTzSviA{Qmc#|BL-=E&Uhr)4$jk
z{maQZHTR4CpGG77CyT$>7gvgR7p(kZ-|frSShVho{T61gqi;5Sv42x^e*eR5U+fog
zZ|@7<^~GNOoHB>Y-Y@pI6F=|Xvj2;H%o44y%MO0A|5>59Tl(-9`~A`dmxPaev5(m|
ze{Vk$4HN$bRR_~|2WpN6)ZC>|d)`Cs-2rvS1gN{pq3&d8g1EZ^>W`05f7L<#83grr
z7c?9uLBnM`G@MeP;Z_F?$4Y3p9*2hW6KJ@9fX2fFXuKSP##0|O-u6S|@hvo7?V$0z
z9vbg6q3IwCnl3D$=>(Q;n4swhmabsw>^C&sJ%XmgUC?wH1x=^0bPG$zuykz!P3OAM
zbk7aV2fEOF@eG<zu0!)pF*G0PL-Q3ZpTY8-R0|{@!ty0QG@qJ5^DQhN!}2vOpTqJ!
ztQ>%q3$StmR&Kz`5m>nbD`#Nk4y+uKgO*Di&~ge^ZasvSV}8(btqoevErphQuyRlj
zS}vZ0mXpcQauZgL!pc=xIqL{5cVXpl7PMT3mD8)B<#qzJ9Jhd$>#%YjR_=>I>w!3E
zy>JX#PeelNjW^JG<UX`s*$b^_VD*kNv>t-hOVgnBlp3_&+6t}5UP0@%5NJIY1g-aC
zp!FcEUOW%2C+|V)%?fBeY7MPdKS1kQSiL(RS`V*+*2`wldU`6f-kt}m$D#Fl7POv+
HR{IP9l;lV2
literal 0
HcmV?d00001
diff --git a/plot_solution_video.py b/plot_solution_video.py
new file mode 100644
index 0000000..cfa785d
--- /dev/null
+++ b/plot_solution_video.py
@@ -0,0 +1,117 @@
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.animation import FuncAnimation
+import time
+
+ani = None
+interval_ms = 16
+t = 0
+
+
+def update(frame, lines, u, u_dot, p, p_prime, p_prime2):
+ global ani, interval_ms, t
+
+ n = len(u)
+ u_dot_mid = (u_dot[:-1] + u_dot[1:])*0.5
+ t_delta = (u[1:] - u[:-1])/u_dot_mid
+ t_delta_ext = np.zeros((n,))
+ t_delta_ext[1:] = t_delta
+ t_axis = np.cumsum(t_delta_ext)
+ a = np.zeros((n-1, 2))
+ p_prime_mid = 0.5 * (p_prime[:-1, :] + p_prime[1:, :])
+ p_prime2_mid = 0.5 * (p_prime2[:-1, :] + p_prime2[1:, :])
+ u_dot_prime = (u_dot[1:] - u_dot[:-1]) / (u[1:] - u[:-1])
+ for i in range(2):
+ a[:, i] += p_prime2_mid[:, i] * u_dot_mid * u_dot_mid
+ a[:, i] += p_prime_mid[:, i] * u_dot_mid * u_dot_prime
+
+ if t > t_axis[-1]:
+ # cycle
+ t = 0
+ # time.sleep(0.5)
+
+ p_now = np.zeros((2,))
+ a_now = np.zeros((2,))
+ p_now[0] = np.interp(t, t_axis, p[:, 0])
+ p_now[1] = np.interp(t, t_axis, p[:, 1])
+ a_now[0] = np.interp(t, t_axis[:-1], a[:, 0])
+ a_now[1] = np.interp(t, t_axis[:-1], a[:, 1])
+
+ a_scale = 0.03
+
+ lines[0].set_xdata(p[:, 0])
+ lines[0].set_ydata(p[:, 1])
+ lines[1].set_xdata(p_now[0])
+ lines[1].set_ydata(p_now[1])
+ lines[2].set_xdata([p_now[0], p_now[0]+a_scale*a_now[0]])
+ lines[2].set_ydata([p_now[1], p_now[1]+a_scale*a_now[1]])
+
+ lines[4].set_xdata(t_axis)
+ lines[4].set_ydata(u_dot)
+ lines[5].set_xdata([t, t])
+ lines[5].set_ydata([0, np.max(u_dot)*1.5])
+
+ t += interval_ms/1000.0
+
+ return lines
+
+
+def main_func():
+ global ani, interval_ms
+
+ u = np.load("curves/u.npy")
+ u_dot = np.load("curves/u_dot.npy")
+ p = np.load("curves/p.npy")
+ p_prime = np.load("curves/p_prime.npy")
+ p_prime2 = np.load("curves/p_prime2.npy")
+ n = len(u)
+
+ u_mid = (u[:-1] + u[1:]) * 0.5
+ u_delta = u[1:] - u[:-1]
+ u_dot_mid = (u_dot[:-1] + u_dot[1:]) * 0.5
+
+ t_delta = u_delta / u_mid
+ a = (u_dot[1:] - u_dot[:-1]) / t_delta
+
+ t_delta_ext = np.zeros((n,))
+ t_delta_ext[1:] = t_delta
+ t = np.cumsum(t_delta_ext)
+
+ fig, axes = plt.subplots(1, 2)
+ ax = axes[0]
+ ax2 = axes[1]
+ ln0, = ax.plot([], [], 'b')
+ ln1, = ax.plot([], [], 'k.', ms=20)
+ ln2, = ax.plot([], [], 'r-')
+ ln3, = ax.plot([], [], 'g-')
+ ax.set_xlim([np.min(p[:, 0])-0.5, np.max(p[:, 0])+0.5])
+ ax.set_ylim([np.min(p[:, 1])-0.5, np.max(p[:, 1])+0.5])
+ ax.set_aspect("equal")
+ ax.set_xlabel("x")
+ ax.set_ylabel("y")
+ ax.set_title("Motion")
+ # ax.legend(["speed", "min", "max"])
+
+ ln4, = ax2.plot([], [], 'b')
+ ln5, = ax2.plot([], [], 'k')
+ ln6, = ax2.plot([], [], 'r')
+ ax2.set_xlim([0, 2])
+ ax2.set_ylim([0, 1])
+ ax2.set_xlabel("t")
+ ax2.set_ylabel("u_dot")
+ ax2.set_title("Curve reading speed")
+ # ax2.legend(["acc.", "min", "max"])
+
+ lines = [ln0, ln1, ln2, ln3, ln4, ln5, ln6]
+ plt.tight_layout()
+
+ def init():
+ return lines
+
+ ani = FuncAnimation(fig, update, init_func=init, fargs=(lines, u, u_dot, p, p_prime, p_prime2), blit=True, interval=interval_ms)
+ plt.show()
+
+
+if __name__ == "__main__":
+ main_func()
diff --git a/main_animate.py b/solve_animate.py
similarity index 100%
rename from main_animate.py
rename to solve_animate.py
diff --git a/solve_curves.py b/solve_curves.py
new file mode 100644
index 0000000..021fcbd
--- /dev/null
+++ b/solve_curves.py
@@ -0,0 +1,211 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import curves
+
+
+# def a_mp_func(v, v1=0.2, a0=7.0, ap=5.0, am=7.0):
+# n = len(v)
+# a_minus = np.zeros((n,))
+# a_plus = np.zeros((n,))
+# alpha1 = np.minimum(v[v > 0]/v1, 1)
+# alpha2 = np.minimum(-v[v <= 0]/v1, 1)
+# a_minus[v > 0] = -(alpha1 * am + (1-alpha1) * a0)
+# a_plus[v > 0] = alpha1 * ap + (1-alpha1) * a0
+# a_minus[v <= 0] = -(alpha2 * ap + (1-alpha2) * a0)
+# a_plus[v <= 0] = alpha2 * am + (1-alpha2) * a0
+# return a_minus, a_plus
+
+
+def a_mp_func(v, ap=5.0):
+ # basic, constant acceleration constraint
+ n = len(v)
+ a_minus = np.zeros((n,))
+ a_plus = np.zeros((n,))
+ a_plus[:] = ap
+ a_minus[:] = -ap
+ return a_minus, a_plus
+
+
+def constraints_a(p_prime, p_prime2, u, u_dot, jerk_max=50.0, show=False):
+ n, n_dims = p_prime.shape
+
+ a = np.zeros((n-1, n_dims))
+
+ a_minus = np.zeros((n-1, n_dims))
+ a_plus = np.zeros((n-1, n_dims))
+
+ u_mid = 0.5 * (u[:-1] + u[1:])
+ u_dot_mid = 0.5 * (u_dot[:-1] + u_dot[1:])
+ p_prime_mid = 0.5 * (p_prime[:-1, :] + p_prime[1:, :])
+ p_prime2_mid = 0.5 * (p_prime2[:-1, :] + p_prime2[1:, :])
+ u_dot_prime = (u_dot[1:] - u_dot[:-1]) / (u[1:] - u[:-1])
+ dt = (u[1:] - u[:-1]) / u_dot_mid
+ dt_a = (dt[:-1] + dt[1:]) * 0.5
+
+ u_dot2_min_all = -np.inf * np.ones((n - 1, n_dims))
+ u_dot2_max_all = np.inf * np.ones((n - 1, n_dims))
+
+ v = np.zeros((n-1, n_dims))
+ for i in range(n_dims):
+ v[:, i] = p_prime_mid[:, i] * u_dot_mid
+ a[:, i] += p_prime2_mid[:, i] * u_dot_mid * u_dot_mid
+ a[:, i] += p_prime_mid[:, i] * u_dot_mid * u_dot_prime
+
+ a_plus_next = a[:-1, i] + jerk_max * dt_a
+ a_minus_next = a[:-1, i] - jerk_max * dt_a
+ a_plus_prev = a[1:, i] + jerk_max * dt_a
+ a_minus_prev = a[1:, i] - jerk_max * dt_a
+
+ # boundary condition
+ # a_minus[0, i] = a_minus_prev[0]
+ # a_minus[-1, i] = a_minus_next[-1]
+ # a_plus[0, i] = a_plus_prev[0]
+ # a_plus[-1, i] = a_plus_next[-1]
+
+ # zero acceleration beyond curve
+ a_minus[0] = -jerk_max * dt_a[0]
+ a_plus[0] = jerk_max * dt_a[0]
+ a_minus[-1] = -jerk_max * dt_a[-1]
+ a_plus[-1] = jerk_max * dt_a[-1]
+
+ a_minus[1:-1, i] = np.maximum(a_minus_next[:-1], a_minus_prev[1:])
+ a_plus[1:-1, i] = np.minimum(a_plus_next[:-1], a_plus_prev[1:])
+
+ # min and max prescribed by speed
+ a_neg_bound, a_pos_bound = a_mp_func(v[:, i])
+ a_minus[:, i] = np.minimum(a_minus[:, i], a_pos_bound)
+ a_minus[:, i] = np.maximum(a_minus[:, i], a_neg_bound)
+ a_plus[:, i] = np.minimum(a_plus[:, i], a_pos_bound)
+ a_plus[:, i] = np.maximum(a_plus[:, i], a_neg_bound)
+
+ # acceleration along the curve only
+ a_plus_along = a_plus[:, i] - p_prime2_mid[:, i] * u_dot_mid*u_dot_mid
+ a_minus_along = a_minus[:, i] - p_prime2_mid[:, i] * u_dot_mid*u_dot_mid
+
+ mask = np.abs(p_prime_mid[:, i]) > 1e-7
+
+ u_dot2_1 = a_plus_along[mask]/p_prime_mid[mask, i]
+ u_dot2_2 = a_minus_along[mask]/p_prime_mid[mask, i]
+ u_dot2_min_all[mask, i] = np.minimum(u_dot2_1, u_dot2_2)
+ u_dot2_max_all[mask, i] = np.maximum(u_dot2_1, u_dot2_2)
+
+ u_dot2_min = np.max(u_dot2_min_all, axis=-1)
+ u_dot2_max = np.min(u_dot2_max_all, axis=-1)
+
+ if show:
+ plt.figure()
+ plt.plot(u_mid, u_dot_prime * u_dot_mid, 'k')
+ plt.plot(u_mid, u_dot2_min, 'b')
+ plt.plot(u_mid, u_dot2_max, 'r')
+ plt.title("u_dot2")
+
+ plt.figure()
+ for i in range(n_dims):
+ plt.subplot(n_dims, 1, i+1)
+ plt.plot(u_mid, u_dot2_min_all[:, i], 'b')
+ plt.plot(u_mid, u_dot2_max_all[:, i], 'r')
+ plt.title(f"u_dot2_{i}")
+
+ plt.figure()
+ for i in range(n_dims):
+ plt.subplot(n_dims, 1, i+1)
+ plt.plot(u_mid, a[:, i], 'k')
+ plt.plot(u_mid, a_minus[:, i], 'b')
+ plt.plot(u_mid, a_plus[:, i], 'r')
+ plt.title(f"a_{i}")
+
+ return u_dot2_min, u_dot2_max
+
+
+def constraints_v(p_prime, u, u_dot, u_dot2_min, u_dot2_max, v_abs_max=4.0, show=False):
+ n, n_dims = p_prime.shape
+ u_dot_min = np.zeros((n,))
+ u_dot_max = np.zeros((n,))
+
+ # v = np.zeros((n - 1, n_dims))
+ du = u[1:] - u[:-1]
+ u_dot_max_next = np.sqrt(np.maximum(u_dot[:-1]**2 + 2 * u_dot2_max * du, 0))
+ u_dot_min_next = np.sqrt(np.maximum(u_dot[:-1]**2 + 2 * u_dot2_min * du, 0))
+ u_dot_min_prev = np.sqrt(np.maximum(u_dot[1:]**2 - 2 * u_dot2_max * du, 0))
+ u_dot_max_prev = np.sqrt(np.maximum(u_dot[1:]**2 - 2 * u_dot2_min * du, 0))
+ u_dot_min[0] = u_dot_min_prev[0]
+ u_dot_min[-1] = u_dot_min_next[-1]
+ u_dot_max[0] = u_dot_max_prev[0]
+ u_dot_max[-1] = u_dot_max_next[-1]
+ u_dot_min[1:-1] = np.maximum(u_dot_min_next[:-1], u_dot_min_prev[1:])
+ u_dot_max[1:-1] = np.minimum(u_dot_max_next[:-1], u_dot_max_prev[1:])
+ u_dot_min = np.minimum(u_dot_min, u_dot_max)
+
+ p_prime_norm = np.linalg.norm(p_prime, axis=-1)
+ u_dot_min[:] = np.maximum(u_dot_min, 0)
+ u_dot_max[:] = np.minimum(u_dot_max, v_abs_max / p_prime_norm)
+
+ if show:
+ plt.figure()
+ plt.plot(u, u_dot, 'k')
+ plt.plot(u, u_dot_min, 'b')
+ plt.plot(u, u_dot_max, 'r')
+ plt.title("u_dot")
+
+ return u_dot_min, u_dot_max
+
+
+def main():
+ n = 256
+ # c = curves.Line1D(0, 4)
+ # c = curves.Line2D([0, 0], [4, 0])
+ # c = curves.CubicBezier([0, 0], [1, 0], [1, 2], [0, 0.7])
+ c = curves.CubicBezier([0, 0], [1, 0], [1, 1], [2, 1])
+ # c = curves.Circle2D(1.5)
+ u = np.linspace(0, 1, n)
+ u_dot = np.zeros((n,))
+ p = c(u)
+ p_prime = c.derivative(u)
+ p_prime2 = c.second_derivative(u)
+
+ u_dot[1:-1] = 1e-4
+ # u_dot[:] = 0.1*(2*u - 2*u*u)
+
+ # lambd = 0.1
+ lambd = 0.3
+ optimizing = True
+ max_itr = int(5e4)
+ k = 0
+ while optimizing:
+ show = k == -1
+ u_dot2_min, u_dot2_max = constraints_a(p_prime, p_prime2, u, u_dot, show=show)
+ u_dot_min, u_dot_max = constraints_v(p_prime, u, u_dot, u_dot2_min, u_dot2_max, show=show)
+
+ if show:
+ plt.show()
+ return
+
+ mse_diff = np.mean(np.square(u_dot[1:-1] - u_dot_max[1:-1]))
+ u_dot[1:-1] = lambd * u_dot_max[1:-1] + (1 - lambd) * u_dot[1:-1]
+ k += 1
+ optimizing = (mse_diff > 1e-10) and (k < max_itr)
+
+ np.save("curves/u.npy", u)
+ np.save("curves/u_dot.npy", u_dot)
+ np.save("curves/p.npy", p)
+ np.save("curves/p_prime.npy", p_prime)
+ np.save("curves/p_prime2.npy", p_prime2)
+
+ print(f"{k} steps")
+
+ u_mid = (u[:-1] + u[1:]) * 0.5
+
+ plt.figure()
+ plt.plot(u, u_dot, 'k')
+ plt.plot(u, u_dot_min, 'b')
+ plt.plot(u, u_dot_max, 'r')
+
+ plt.figure()
+ plt.plot(u_mid, u_dot2_min, 'b')
+ plt.plot(u_mid, u_dot2_max, 'r')
+
+ plt.show()
+
+
+if __name__ == "__main__":
+ main()
--
GitLab