From a629bb7b584600896e170b120c752efba22c31d5 Mon Sep 17 00:00:00 2001 From: ShisatoYano Date: Sun, 25 Aug 2024 15:27:37 +0000 Subject: [PATCH] plot xy 2d interpolation --- .../course/cubic_spline/cubic_spline_2d.png | Bin 0 -> 28218 bytes .../cubic_spline/cubic_spline_2d_plot.py | 462 ++---------------- 2 files changed, 34 insertions(+), 428 deletions(-) create mode 100644 src/simulations/course/cubic_spline/cubic_spline_2d.png diff --git a/src/simulations/course/cubic_spline/cubic_spline_2d.png b/src/simulations/course/cubic_spline/cubic_spline_2d.png new file mode 100644 index 0000000000000000000000000000000000000000..58be878dea6963c04f473bcf1a3a32d819df5dd1 GIT binary patch literal 28218 zcmeFZrXg3?GzZ3Lx3Is_!7q`ONxq`M@gTT1Gz{rs-? z{c!$(^XYi$1-SRU?{%*=*PLUFITm3WYVz0^CNA4#DOd^4= z0R~(6$8eu*Q=!3D<@cK-Q^S~mwY1r{%ZD|OR7YHoIo~OzeKf*-(Xiy> zA0TGR#X`H0>2uV&eeU1tmC;4DeRJjv;~t$-h+_y01%Gt96n7!;Z~B{3AozHActuzQ zQgq-?;%fv285x=H&r~2`@b`U2rv}~85-i6cL^1B<@2)l>hd!>F+MYe8B|-|e=h0GNdwcuD zYg{}$P2!~{cg#lP= zXFieb|BQ5h-P^=tX_6wWWm4#Uog^aK_Fwo!{=dQ}8k&&Gps1jt65MI7AwmeTIQU#$ z&6g<|;K;|z`pF%-<0fKTYnazXOGn3UiNKN@9E=3QjaEQFcUL1$Ar8sR%nUq{@^p|H zbOxCgMT&&Wn6#{{Oey50jErtKk_MApE}o#{5=^&Pw(}$M+WNW$f#T44#M z9iyY8znK2+Pu=HJV-J7l|Kn3DJ7=S(`Up)PDc9vvhBpp%q@3T^3 zG>i`ySEJxPE?g9fo1`g_1s0Yy0~VUy`fT!FX=Yw{zY1 zXPiAf<-_n9Pi~LfnZ?^Zk40UiJ|CnqP))RLc^*JZu?`^Di8-O}*G#d>D(M^};x);5!7kL)+U z(oRlJq*^~*;1Uy~NY$vRrSL#)M$#3-opvYkzA&IaefpF|>pPmUiHT-~`3o>)ejg$= zhLCQV)pc3(Nhb3nWAg+3)p&K-N^2mF1mxc(G6G`Y7u?7z1K5_Iho%1*l}aiH6S`{Z zdT_|Nia< z{kz!D^*;YIuuc{8b~1mh`M9m?@!?9+ZGQ%Pdn7~p=4{K%=l;XR-rAy5yR3$WhSo$P zpUnurS-|1K^Agim@~;98FTpke9~06Ugt{?n7Jv-#y;}B6d2UBc%w-(C7AfrzJTAvEjY>&`+g`^Y!R7%+buqmqwEkj=qF z%z(R7)HkDrsL$QdU+BBk98N*&>W_(?O^mnc^L00Z`N@5E3rP=_#4NPdN#V zC)2;RtbFPtB~PB)Y7bI@wlDTi31Z!Op*61u;yVW71A~#Q9}vrp8=rj)3W7rNl#_dx z8sAsi&6W|On}dfw9hawpLYP$3g2%=*hEw@aCGSqU?8=YV*FPFJy1vLA@0gvvsx1$A zxD!s|Hj94wshcKO`2C;N!}{O7B?i^2Akb3zY}O;V+ob9pmmv@kO?{c-J}2OHi#HC!^syJMh50`4>I9YNJD z5qQM#(fhkQ=i`-D!kFn&qpQTxtu$++?+UTsH%E1nKswRaClwPDbKV}InkqMw?D5%~ zEq`2?Y1}j``1Xwkvbxgh1L9fXNBQwmBVXJ7rBMQlCPJxU?Mo0VV|5Nj(VPtXVNpbi zzt?*@eg_ozmT56&y_Q}rt%s#O547l!sY-sK9kgEIFu}|3wh%JY>rhxMjP+}+s{{NE z#gl6HU)QfGv~FjjtHOwe*0{torN*}h2iGuv?pG|B%)2%Q3v}(e|58LH;x)6Gy?*(u z5t9(6(t~>%v4Yz1{@<%BdGG-K*K4R?$}LY;J8j3Z0#*GTHq9MZ2nY1CUv`Ahgm0?n zLdgYD{I6e#1zd9Q*gP4_!_>%;$&YJSjE7amFO4%aNW3d6jm+o_$j)-3$mX;`*cw6{ z&4m6Az)9dh$0CKu$n;oZoO`}rSL5R1&R25M?*EJk%4EiEk?ASqA^In(y|JfReVVGyxdUGIAv z7@jsZH|IJm)?Gi`9#?|oMJeKj27-6URaj=X$rX8LTA9JE+6GZQ6Velbs9k_i?+9)7 zL<~ZKK0Fw3(cK(pxNLMwN^SI#@p|RZ7#@&@DK2wCva_Wo^I@HkyK8!l9|>NTOuc0I zu~l<=*11|=PhuCkag8e9{34$!F(7biyJ7ObztE!;|3DZ)%!vR=V0(q}@bG|!j^3HT zs_ncrNHm=GoW18Zf{6WdN($-0LM`U2A3A{{A@XTHEX|FL*`CKMDZ=lWa&mKd-F7tG zb|)C$`mPje=HgOPM#eEJ=YY(iUuA`^_Dv##Uy*@59DELipyTS_-ANYX1|BhS@l%kJ zI?Z`#X=x$U)YR8o$!4oZN6|S4`1HyVp%5k&i2s8Dg9%KbLoOQ0ux?mqe`TK(>(6h| z0g7tXb|)+p?6H#6>+4%5^C^~bPD(t zbdbV-ft6>Kk%gKCa2npOuXkG>x6l(Klxj*R(M=jz{!I)J`Rz*}h)*g(+2Css zDN6ZV=?>w&dm^J8T@@s(ii3?!34vBtR<`xb*w{GT?Q~t1m5WO)uz%tW59#;57#d}i zpiYZfB-&D=`Vos+Wp!ug3Y|P9q6lN<=%xz6{9Aqz5ktdv|JH;rU$Wd3!j%;iG}^c# zG-XF|z-y{!T-T7zWw z{$3X|8er}!#>YFv8r!GDZ~d<|RGReLO-r_IhD?gR&qC+>?Pd|Y&k+jL!gL2qjS{0H zL$x{qF7GGaxyWg**J4~9m4pfknlJFL4wJ1ed-iac`HW=;DLfz`bTcG+uHc!>4d_B? z>9&eQpe=s40xJQJew)9(k{lf!73vh>N7G1_x#@ez$1&76{a#_S{uCVCU)M$(Yf)wl z-J628UB8UHu$w><@>KJEn0Th*(fONZuAS!k+A{V!iu7q+W_4}nkPKV?0@BIpMqi;> zyM&tvRh`4)=LiyR3sCy{lRIV z7t{X0%f)HiZms9C;Sg~j1iP8ga-U_9@g7x~a7_oy)!rxy&FVL*ZbwUG013(0zMDRo zRF`C>BE~D~EYi-IK2A34CijnL3e%O8S9*xsUm}~UK|eWx)H#tf$2<2?(#aOpA)kPX zpCTC`As0y+SZUd}K38c;D&och;%2Pb^H}FM0S!&ewH5oayu3Uj?qXVN%YCwGc?g3d z4|!DTf`F>yL`2|R{@hfg`_sCncn0*aurP@I`v?g;zUe??_iTe|l!X|Hk4#)o;@Ty# zhLWX!n9=Z%_k>_q+M4jcMfmj@8x#`EU-_y$mj~!wXIn7culIIFflF7P#9e}$Dz@i1 zW$6|7($ls;%}#r6x4zmLq!@z@z+hiARTtQ9T7K#Z3TR+cpA2rW>y~&~&tllv*j)en z_w)B}8ZIubB<#%om-zVj&igZ*GIDZEjDZXUxp-T;Wis>A#ZZW6v0h);T37bfUmYQy z{lS1cA%uL@AZ-h}2Zsg10z^d4rg(Hsx$fTH9QhdP(D3k&$;rtRYb-Bo(apzmWXD^) z-{0Mxk1EFe`X)&OwskK+JlcNh?>J!~fqH=y!!9^>zCG#!ZcfOqi{&W5KO-B*Am;av zFf%iAY;LFu|JiGk&|7|hPyPM<=h_1#(*&KKWQdQ5)Y7(^#sJhZG}BXUGj@51S#Yrr zg?wJ?dVROY{?+L|E{_XhJ5%)4Iq>?_=Y;Ga}%xzz0-Xk}CtV8l=<+@#+}S zdJrP&0qF^6rAl-l6UG+`pw_Qb8OD-uMPf11-7O5&c~Q#=gPe?r9qs<#aqZnGplDx<&K*J zWXM|Px!M8MDKjQ>baX7#E_ecf`?tSK0N$JJQm@p!B>=?`Sx``Lb0`^yO2kcpZw9P+ z7Qp>*0-u~r@@}dywkAGecVU0FTprM%wLgD8Cvh6B&y*UyH9g+at(A)+f4wu7eGT@> z+TkHKV7|e@!QZ_`LA;y*eBKGTsHv%`?ZkIiVepI4%ArP4$h|~1;?m;%Q1vMAIT8%uVM1JPy zv5}Vt009yYm|g4viotuM3}TVPn8i>rHwWf%2e$Xa6GP3DsD>Dn+`8 zJ4Um|;mgB?+Q}A=(M)m82Itq5q8?bxYUv$2Uk6i$m{FHItL+_b)+A9<&uT}Tz{T6Av6Maeu% zk!+vPoafThtK@NurOO#=k;nOJ^dzMJsbYe)rlux7gq4kLFL8T)ef>MAi%KpoE`Lxh zEf^HPTh82vhACnIbf*wQBbkJS(#6h8BkU@JKwE0kEU;XD3u+iXWEVVxVvM-|ojwBX z;e0iy)rt$%HZe$2hK;V)6L!=RzAOMFD}#Z9SK^X54CuDtne3*?eP2o3=TfEqU=zT| zJg}Yf|1E)v$^e-FIU=6A*=;utNlL_ZGZb7#NJz-=3J4QLTpzBEI9qRznib2P<|?xP z{`u;Z-{1(g^mlH62EZNu?U%PR09|MoR8>|}?J6t9w`)H?74=&GXa>}pQ9OTkKkw%! zOl2@(>gRUYSXc=n?t2M;Z-y$F+Ajg&P5(OwbOkU7Fd(hgvR#R&=;)-Gax-p528Qos zL54t#v1;Y=e>91!F+6=g+t$j!_;^zM#UOhrHt)Z%?k2iG%bfb`_W zn3T*|P6CQ4APIc3#sHQ|N0C2Y-Pj097krsDSRwVDpf| zu6~m&X3me{YY3Dp5v|@H_uR}?T8Po&4an8TOA!Lat-G5=+Yu#!%Y>Xf>Q60!xXMr; zgE}c^<&O_#fy*Sf?l1DMUWz7IAEtkQIE@_wBW)Y(^K)~)ugo;%{;)pUE!W%BOa z?r#3O|2=F>!~Ei1tjaeLWUCFEu+WKKVkx@`TNtHq$zyCMvTkc@>v)y5^6*zd9I%&t zw!9}l_(@1eKwv;K{=OZjj;1)m?}7xOmh_Kg*DK5Ny*Vptt8?8NOf`L%4>l61gs+gt z(NY*l7Zw9?jEbX-OiWCw7*JS61rJ=F`gO)v3!&(_H}gXqFm22jpxoky!RWnu!wOOO>FySO?7z32Sv|_8{UjOue%P z+3oH+H-(3Q`ibwI%jZ5G0?j`>(C>tQetks^>Vz%S+HJ&0K6FeM>$w1?=KFYmrMR2~ z$I2l0gT(R~MAT~_lwF??ECA@c=ZavV()+$C*-^e0S})<*EqTB2CvD{l)@iDz06YoCIN1z)$ba8hr3kiQ#`oS*w37~3)%Z8jzsUeYS8hF zW7jz@hofMVXF2_DgO_uV@;(PV-jg6>5Gq`?!uey(+euLBi0G7uK(QGu@E(u2Vi zkc5v5QAuMMk&nb}F2aqKE%M+tR}PJimIJ^^YgzdO_gj0_uL5{S%I$8!KIcl$7VFJkXP40I8+`-iv^<2(ZW%_&8 zw;q-0VL<~LCjE@Onk6`12c!Vlbo54rACZ*Un#_^P7e@Sj@Gt(f@doHHt_=Y+w)0vH z0k3|9x<69#+-Rp)^Hq59W=)1E3bfznyUEBr`2>`DO}Up>6B#EenGQGT6!l=C z!i$wQ2hFFyB11_~zBi%gdCEN;uc|WNO~bOKdv3U&!{F^)Sg&Sl6*FgbNSH#eu^MMnXk$pc~nsZ`LbfU6N;tq^I68S6gj66cPGf-Ztsz{67ZEOZ#H zcJ|Y_Z8wM?&^B0`LpS#os|b*+cpEQd+Qvbx9K;7BhJ1%2%HZ3cZN$JZjtRa_6q8gq zXWJ%HJy+srQG@SbA+fs&lAfNf@m&G0@9rPK zsDFm>Fwy$Dzx$3XQ?ckhVbRC!PsFqtvCB^UTd2Q-{{6&RL9hJh?pABk{e2*lDhx*w zX`xoSpOC7?@SKftKsAkloLu^))$QICASNlIB!vdrC743CxF<74O7gN+$tjRQ1t2Vr zj^cnwu1{?>y!aq~dHg#D?6J8+jH9HVO%T9+S2s7U_^xc~$f7#-raKPirOq6XFbQ#+ z6>A(hlL%RLRfgI8UT-kavY@5Rk&(7)#=Os!%l_y28wzpE=4okx8IM0gjq1a3%*^;X zCvn8D0#oJ*yIOc> z>Fd|8@8YQmW~ovX@vLqto;F5cj^(2Am_sz0J(`y5^O3k%(E16;J_kmSAV8Sb zl+D)}A|JI7xJ(I6q9cf*v9Vt4t{9HC?Y$qf9#ZcADq}q^aLb-08xGML0NQ*q#N81~ z-_hhQ)fcTKFJt8?l=Ug}z(^)IC3P_?P~oAaMdqXPK5>N=x<-N8=iZ|z8XT4TGcxk~ zG3=4iZrfMS>r61pruw}bUYWk27+3#H*Pms1>`@Q-#1qJDN$jUp*& z-i_uW@g{OkFeUUEe3Q9-YIT$jU$x8ktwiVt|zT6t28ehKMh{>@7Ysb4$ zuasC~c7Ey5eL;o>xo++x_5IhUjf6^pOBn1H-vK1%EOYEVHT$6OQ|8Lzd7bV~Y&2|S zHeEtY;_Bxb5{M{3(~Woy;buC-)$E1$rf*&TM9zIa>_#8ICn3l1y??&Cmv&)C9@Vgt zg|oClz2T89Cjg2RKQ>iV&$rATM}Cy`b=X>0V4XuCDozzO zP2R8R>APR4&w<{gGw&68fCk#*RXC!a2p9W^kN3@qy})j2H+WSNNajugMuuDmn~!=GBMNT8)20sehDYO)kM3bLM`P1RzY0z$9BSG6^7&%UCVe zIU+CAlPuP|@6bw&W{BR-s;VGKTuT|(t5TS+b{v?UTfh1N>5GX7dn!lI<54o#CBQXu zW?f`(btF}!l~e53iIA83vh`y- zMO8W*HS=Aq%8=6P>bajKn)JLbfgD})WTb*>>7yx6--Hop!aWDE4kF(8J5!1b0U07t zcD;PkZ^@kbwFuByw0}1Sf0}l0t}S`r_ADCNYwHy!dC%exwq=Ub0}WwHG8 z&o9}Jw5%aAR6gO2G#0A==2Rse&Hne3 zhUN&QB8{g*rU9D@v?An%CKRO4x~pbKI7moVXUzgTq+$!P$mn_*pS>N(gdzgK4Xd{4 zNl~U#B7(;!wox5~fGPBxCSZ7F#<2(xLN`ni;jl8Z(wVB+QH)aJ6u!PYclC6Z8AdDo z6nF142#p`Zh|P$O05Kg!FOZHCNT|7bouD17oqygdkM9SnOWp@d}!+kcXyzfAbb zC+{m^iE=V;qk>RvImZ*b6Y@Fy5HW0r2t0y<0Se++G0Dkh*xa_~rTtJcugw$Z)0{a; zq1q@vhfd~i5Xn2*k@g$81NvbG9=mWNL=jKo?cct+wk$Gr;b1CR?Rv3RkK21b4z%CD znI~(9={wKYBrn#y&50P(#U^KHDuAvwAH&oot?^S{(aVKy3?_Em-wyV^v2sxOO{EETPn0YiPNjPw& zu|rwHGVTNz=tB6stMrdYXZ13XQyCR&GNBe)ilv50t!+yZcN%Vph;-2O=gRY52b!~G z7W=IMk3`kOVJztIf zFvrVb+<*(1h%T^G^a(ho&qdUKa39ZL4LNQ{1(-@H5&!n52nI8AYzO~4hPb7f(m)D9 zpe;(&yrpNNSZ?R;{frNMDS+1gJ(wquy#GsZHg|?lDRX($jc$Hw;qq{Qb#~B0L1ex1 z_?)5pEJBI>*EErSJ9VM4K}~+0{eWyV-_QB^mSNmkBnAYC5IYrY0%)P9my1qtc9n9M zmiqDZ^fWXkM*W|dhzL0(Yj!_-cHcT`QcJ~ULAk{h_+RdPS9vr1wtWUQ`taY zyaprH_Va@a`QNK(_d$bjb#3kG`x=?c;<1PO!kb@$3U2N_W!~o}#-FXt+|VBw)Y!=>q)TOHG=1{XHQzp!5orY+=x;WA;U#~#>?WEt z?fkA(Sy>4uTEOnHy0w)D+(H@-2wp&-hxbCWv$GW~A5D`X`7;PRz{&!c=-e;t@3Z@O zr%Oq)jw^U`n3?l$aVkH0la9X{-w@Xka3pkd zu>mT5I8`*3#EtWJ&-&EjAcyN&j*+1GwXK0ox3B23JR?WT&^22RD= zdt}wG0zE)Pas~~o?=gVr!c}h}z@^v+Xz;8%Cky^$aJ+m8Qu-$A_M%fDsB7fUfzGp&of46nqhMG6g?q z?w*8Fc9j_9xRz)zw8-Oa91Dj|45F#oe>N6|^c8PZS341b*&~oy9WL}g zcf`8BfyU4vF4n<;j718BsT#Dl`I%w3noPgatdEisE!)i?p;5?hMv1pAdkcMt4_ojnmzSXgA9SMhs`CvDS=E_n1$T$}gOz*;fCkW=TPL z36h3a{Pb#C5G+QwEc*zt`~KMS{~4ey)@7x@jD7jvMSkZ|w)e%RR9@Ru-h*El&ga^n zN<#z%y$368Hu@rDcqB}RvD$p}z(9dKwBBGx0tj(cB`zxr%&~!*PC`l|CgjD7v)YII zdqL;3?+SM)SYHaTd~DLljLRsUZ|hPBe_E>#D`-oo^ug??u5tGbixoQM3h5(w-0YzOb?e zkH~UcXAdniMLyev{<}&pPdD?&Q#7Bi%0d3}Y-9jkziwEJ^}9SyY(sGdD-;Wx=!!U2 z2@rvtgWCrmm%D3AS{pU^ikLt6x=1)@`SL|lr4}%s^wC%pd@*SrwnP45aC2rM=ll|( z;(Y;t0S4HhHV$TDlE^$;+eE^m<}ecm{-W&>=+TkX@yc1^#txef1b~Qi^-L`LRG% z*jK>_cW9UnJOZqjbLA~6UqeVmblP00#UQ%n!~kv$=tZXury)<3aVAObE*Tt<=ICDR zL04AR{Uy9KKp8Lj_}`^DvQozBbDC?V^_%QAyIQ<*#UI?W-l*OTY7*$_@t!|-GF`oP zaUcER;eoQ^Z6nr7Y25Qrt@YusD))>bpJmw z@n2n}bxwK!gR;Y8G?j!H7RWT(rk^PO!$?dM7irhYC<%hc|$yj?j{O`nwk6I0iWh z>fJ>$2$KOgP_-WUjBR1k_7PbYGt2nl26e7py+*&7or1*hI*^|rA>b+%AKZZ0!nU=~U3b#%0dd0o|(#QB=^ zIR+$)iU-o*BE1GH>0UHC5JC(cpp|*@-*ZNq;&7O9jBI7zy?%FZH>*}MDi_Z4Ie7JT zqwAQAY?y4r^fT%TMFN+lR-<{LP^)ghAfCM$BBp(Ye{#y}^pz1c2&&(xm0o?TXCOkQ z7DN-TdF)fJ zzdM=reWQ%!W7a5$7P4Wd6=ng8{r5w$ZWu3Y`UjMVeNZpj6sN?9d8FARv7BPE6mc z3?Uj(Vj+Lyd9O{KW&t3@9vz)H!#TDPJSy8rFcETJv4b*nGssH{nf%xVQn{IMg82PE z(6)evg-OLwOQ}Ro;0<(jAr@*%r$64;!$S<7R`Pz?g5vAy`S@@_;{4dAA6l&@L z0x}j&0O8`fN$a~z%Jon%qm-$TI7E1getWym4XR$p5T@;-?n;%)a{J+8M44cf$;t|l z|3j^u$LY*}k>}CJp#lNe9KLD}`2#d2O^~CbvqVe&G@D0{Jq9yjD`9Mka2L>IKkZwm zL6+Ap&lECkVPRs%6UK!3gl2(yuMn%Wc@en8ZW~_=b}KLk#H9DgPwJ zk{ZpFa;upD1^f)@3+Bss#Q!2>p++$h%JE9c{*kfZ*Vi6iT1fvlvz00ZYVs&sWDZ@d z1{Xv$EFBTLp&}i0XP3Y~rJ47i4bVTePIO5z#xeDDThu=|Ln-Dk@5H;vAH<4 zq-zSwn1ns;a6Bk2n7JxHAJ`1W~g&Of(6mehumJVQ>c3+a$>UDs0sD;AtrOdA*pO( z6Ecr5kmg@^ea`m3|5unIr>EoVE4IF|5ib$rUpW;BoLMVsO~U_|)}`)sT=&rcA83)` za|U8|Ca0_AoAa^dM#O<-s<4Z_c@@2<$C%aj)3PB?#x9Sr3#^A|TOTFxV`xy%HY=Tj zXEt<7$!h}ckvC%L$Mk-D2W-hT_#2*2^s-h8qzyjXz}>WQ>?UxZDdNudKNlA8D+Gsx zgru(-7#9{LhUk+F$Z#44-$A%um4p@>)ejo#h5_3edPl44mb2HDqruQ1#tO+BgHlX~ zVqIH8^6>O}$2B0_3Q4?6SoXMRk7dSDf?hlA>!x$-YwSAPe-wX=1b!?ws*mgZaovfW zk~f`%n6;c@@O2=3hG&10!=zlX4yR%~pH_8A@gva^S;)KpJ0*fO4XDqUh z;FQ8^;I`Ri3V^Ks=f1jRo!)ixxRN%H;T`(q{`8t!90MR4jUs$EBDQ`b{g*E%8hF5r zj4hbopSST0>6%Ox(f30APm$As6N#K3GMH3r>pPPbP)VFM`3ZAOM0C+XIiGR>1ekm0=qUHD zW#U^hEV^2+e}p&c8Ti{7A_HcbZx1x8-@xL31Yj2h#S>Kc3a=h=Uiv+pDd*7-iU|sK zr}RE#h!)s$$Pj1k{cee4$on_mr`A`9Yn}LJQNZ@%gF>E?L4oWD|K}gx?{9|a7BvcS zD?sW0#@pqKGZ(v)Bil1DUp50$Ebz4b+hy=Q{$O|*#v z@kr=#6>;^!Pqu5IIC> z)`dylWx2_aUGjS)*vf~oYrX^NzG#^y00N=go2nd{AojTowVccc8K zt-=aQjt%eRWI>$QKI*fNJyEb83?+z0WDxCxUnk&*1W_n8lAd_C#eLj!L}NQ9^-$G+ zxi!i?gNoFx-+@h$5XFp`0*T9oPlj$>!e7)`pxex_P9{LAZj)6EYk2A({H~t@vR8H5 z68&~)?8I5Ni*}F;{zOE?D8|hw4GmK7#$P_1<>bKlY`TRBq?8;739~-9ua47a>2Y^} zgH$>Eiap_B@f3oDQ9&%t@EM$r7@)CTB0c$=(B6WGDpPE^I|2Ll?QSdf-q{6Dxofi@ z2=QOvpe<&7NBY7Ev)caOHKzf#vm+u~mAAZu6D>p|NiFmNbU-F7lpr+X2syb# z*q>NL;qM*~LbX8QfwY`QyAN(6;eN=F@Ph%l?@VU78BzTk8`XHRn0R1|caN|UbRZgF zSgl-u);#QA6(h)o_nVp8b2#5M2-#rL=E>#)FC(Gdp-KA_BosC(&AmBnl_vnl#F85{ z-(L0ONtKiMJf^+T#Lr{$bOb6mINz}v@JO&-mv8>UB{Eb);7N`>d~O9!PdlQi)>|Bm z_7l9XD}-i>Z88r3^tVly;~=3%oChKXvg;*8ZCmFns;EHzH>9tvK7lEuK}2qoIo^X)Dhu+P{%tqXaWCwGk#cyI`g+5uME0RSV1#c)S~M#VT`XouOOc^H@PkbGpJz@fe5 zL4n;~C4!Zv`0v7*()GC-*4006GM=dtNC`c}RJ~i50SZ)46U?M3G)zTlFkYULMpF0x z%oEdMT^Qg>GwksMQY_8s89ll#z(vlij_}Pd-(D;qiAQ81|B91I2?wC;TCY1&fRaFM zamaXf@nN+a-DL#nOI5D(>vOM;#`l4e4;oy=+6ZXCkH-yn^?ka1Mxi{*EKbzX3GXkaZ=cmh2&&ewIwtKP6eOEl9NVtTpq43rJ=7@TCVpW znN<;Hs70dwy(GR>#-aM5LYc3qqol6zbPx#5-`So%?f9-(Bq{hd&THjqh6Lx#`cSO& zaDGivJabR%{$ng75i4DsR20hfF0=VK-G1Nd9$PdB5jA0UO@iyn;b?rvZ-vZs(VrEEw45GknUDAO4} z%d<5ln!@vfAs~O7DjI*|k;@o@PtX%NKqp(G$5qq^ycFZP7Cv2S8QqcRzBeV>0t8Z3 z@531Y_y&d+8=>r5@Wnd%^OV({SJq{xwQZxn&}Df5CP9|_!RGNe{XX|2LP~Q$@hbuo zcbTL=n@GTiKlK9{SU=p3xq6%4)t)6E-mQr-vSUe zdJPJUwbu(K4GP5QFu_y+1oAXJgKy>f8kh-xP8)6{DG)JnvVXKJL}ym8&HJ73ryDmU zOXiB(Tn$qEs@k#fy95zCLIg>~`R-((SwLdDN~I-t#gQF7M!QbACm_j2&HvfEjBbvMcD*QC$T zB!iyiXmT^iR||QQ!yD9}#Nxl5Ddd6xKDHOQ5@{9jA;17E6~_Rq-=qzrbOst{^KICo zEtoq0@bey)qjoJbS0l2PAe0>FV(u6Xo;dxflTAbIz>(738}2B41bI1gHmxV6Pg>zG zKGxQnLQ6MWCWwx{-cKx5r5(-;qw&9f3J%8w$wm-oO9di~Iade`^qcdHUQX{1(JlO@ zF$E~^9OEmVLKB&jtqC*@&p1 z*E5uCy@0^w={RKewI72}9a3)A9u2JDh`{QrB|@kbm`w5^y3mLOd^*%_*5YpAiYQD` zSJX4yvRHS9GkN1tffcZ|uL5C*sG}K(AO7NUeoVTPmx+Ug;PiN}v?Mii#S*c$5hws# zl}j(bZfO7D5rAE4bxx?6;@SeF%0B<5TJDr1h-Wx&4-8yNw0fGh@S(7F$%@xOb7cK7OfcN`QGBncR<}`oPd{0g z`5k8pH8n@Ll*xD~oFbi!WlO{Dc6F(}Y>Zusv<#wAD7}}5*%{(SC5(R*(6fqeXBSEX zK%3t4DD+%#rNVJuX2AWB?WhNjzI!F+<{Z)Q_RV9{Y*|=t?PDcf6>U#5ECk@ztKR`M z;NUzs?7=FLau@qt0r2T~)vv>Rx0Ekz3xx!n9G6{HZy$+S5xl$&>g{uqv1uF~Z}xwi zMWPOuvFmkL+Rq=DMJPN4A%R09(G6Vtnm`N4lYW{Qhkzh+rQyb3VFylB==ZiihHvrI z7-qRx?k73yJ(TK?g9p#E-qRUBv*Ce&0uXRnMbDu^+1QOY&)Y))Dx|0dFl8!*ym$vT zz1t4$$ZXjw0b*s|+6QDL{eV;qCYY5SKly4OO^=sUD>dlQt2`O_5ro(s=YV%00{2voX^5<1vDKaxTUjMB&{P5>oy%Wvl;bQW-aFiU7P)c%N zAa*QLc%4ANDK&6J5&&V5AShf2b38<+1>z+lPyp?ts;j-ftxgxqdY;t(Mo7ID&7H4R zUfL#NgS54sbt_sShkHVQH@MLMiUD5xA=3w7*M*<^ZfI^93)Vb6ZflYt`>q+*}^VzJ_yVo;rlAFqmeEV|K=onApwBT9*Erx?ij*x)A&!=>C|*#yF!uUnOhE2^88nQBnwKm@GF6I<=iuWzoLJ z-?=$%cV1|vkc%StaQA%dNIv{a6KfnK8XQs>eO9^uGlkdkWM`Z{qVM#V;P0$}$A_K8 zdTutILKvuC9Y7`m4)Z!M11r=tL6Sv<`zspfgOrJJtBkkzl8k+iF87jE?&R1!Z?M2| z@SNJa)fZXsUhxuN_i)jGPDX22&_~!?0s>9zP6SAYVG*K4nm*x1A3Ld}rU2_RloR-= zX>faB#E-^m^#^BscYe1^f-~ow#L_QcBB-Sc1p;d^gZQ7#&0uh(ckRpD+mht&zCL>3 z3f?^j42RK7XAu_!G^}8LnSxn$ zsWU$#Kb)*W;RgJD9AXv?NHVvX+J!i9;r0Zh6PSAYf+MFV71f8Zm#WLb2{~LC#GK)V zkahoiqUOC>l2Rm(?;WQbg}JjE27w6Id>q3qrUoe1S8au!JGj1(bnges`Z~v7Gqnl5 z;H=K`sZVc)NeLGl39JKdvCERVySzIf5ZL$tx^$!*M2cf+e0f*Ucj;QJ&QZMFszIPL z^EhH|PCrjtBb^R?75Mj0eg`~eJFopD^N-==AQT5522QZh{Wrt#^E{xaoK(eA;r#s| z{!&^RoWeTSOIp(Ws0J*-{fx;E`qOI^Bd`7H8(Gy$=;H=2m1vhrHsnpbXgIAXk01W9i(QoDf7@+T5! zjbZ6tA7tU&?{&G0QJh6JtUx98BNP!O3!GuCQaSOW0*-NTdIhmL&f(q~G{}Ibd#coE zcJK5HwxR^ZT;@^}vT++nlKI<-?tfcfFXcHkI{r1s;0~BiaNuBlGeFy5YGSOtj-s$p zNh3Ww!{?Q`l&M5=1}EHvd@eb_`HWC-oY`F4tfjr&;WuMH3JVv{@?iqT@+XHZ>XNLQ$0t!k{3i6$+^0&7h z6`ffs#xhA+J8HB&5hQ(}5zgy*WbAe@hqpD99CEI_pOhiytyOmhT-xD4|Dk7LK?8@s zz@am?I;Y=`1RL0jFIHB-$>2YWXvK;Dh+wBoMde)Kzke7VV2AFj@;}3vK@-h)Z9tC?V6okM32sdwq z9)#wguL2KvFgS?=)blqD&T9z1zP<@qA^jzOKD0czLcXNH(gye1hE8}sw9NGh$v@e5 zdVKQ}e!|s}MF6GZRk7|h_#Fd0;L*Sd;iLhYd16V)4t_G*`wz0x1Gu0E7lZk+o0awV-6$-egq0ZHY(MHXc|-ti|&IhwpR&Zoi)a4195{1>M}B^;7M4-FDqDT zif-#&EGyA!@By8}nmM9JM(a=zpw8fW2Q_=<|Ji^ zs5DT9bSM&$WC)2w6f#sKG&vHPL!v@aafFCW4MHkINeW3+q=XC^^1Sz{-}iSv_wf92 zKV4VXRgUu+_g-u5wchKs8J`gNPM?CkoswHj6a_^{A2s^92sU0Rq0dE8lmz*!yLqV8 zI0ro4=JcjFWA0t2=I9-^zNfmqwD7Q39vdE8E|zEhm$72-`f73KRhY6t#dNrSx)Bee z8Tktv%iLY3)9Ca!VW)9_QuB1KwHkEJb>CVLlD2`# zIEDYSp)zG|Vk-rqz5qkS7$7Wx${Y6LU%}pFdjWgVo+26||tHYOx&k+#VCxTO)Z#)pfEBUx2=NWrZJnN57+q{&_ zs32I^&PThnf(TN?D+CZYV553`Kb?SY_@bY>-bETTNIH5GV0UnypFb zxL>jV7#Ik(f7ikyg8${0jNfg;Em=Py@s?nYAI;nZ|*Tl0i=Rmx9cr7V=+ zsBy03D37c=s-S@QJ=5K8A*i#1Tl$}Oz21nk3l-h;&BO8D!?OI6b9-LQHP2DBS5IOz z!yh6~Gtw+G_-VWzcgK^6;rQ6&(`gpV$mc79al_#_y`OKtEOZjf?L~{${N~$39-}1E zIB~W_KLOFNscAxGg&p7YO!2z?s47u~kqnZ*uV~J{&|ed3{$6?z`Y#4Vqt>cy&yMM( zr_FCj4OekrJ`ewdP)iYhcLf%D>^ZM1Zqslaq>!Sw{qkP`k3pTbUKUIoNR^$t_v1YUws5Z_TYGM@%F}KJP*)TIQ)B zxtwcFmVRqIcc*+|W_Nd+UPx)LR6)7~r-@OVSA`zlpI=+~;CIldyWTKCDGaS>Xxx*R z6l+`ma$OaAKhTR&sVU2vsMc34H;0dzlT+uaE=0zOY6crT<{2pmYiU6PLZw<=d&>G_ zxVIi_D9d}EiwHcYtzf?+xK1ML{cbm3jV_1qM&80mHt1dP{>g9F%KSnU-ufrEeBr)LN{(!*^&v)W^%Kd-T|_PSF)15@RvoTF(UfAEr7M3q_zAvw5RDnq=KT z31P?8hoO7<#;?+-D1M_v)erGCyKY-Dp(#pzlWc&DflFtl_6Dg9RnaRP`9=Ao`nL8| z_|Lzq&-OuTIK6O0dtzq%`AXL0)lx?Wi7Zg{i_e=f|GNj3O*T>EnEs2{&ohhF6Re&@ ztw8@$dWlb;Aesj+{``ej-=;i=O=<^TacIv@dzTJw%VBE-K>%H3d6&eu&|67K5&e## z#y56oNa)pBEig{k-#FEAMAdJQ_fh`7q!Y7o3dR!$PBLZs%y!3AsGU4DS7hxAmnM9r<~*Iyer754!?F<8#eR1~ z|9Z5~37~n@4z-Jr4nTD6O}zbjzqIsWOUr0{jjzV#jv4+pj?WH9XD%=GVXBzoHNLd= zQJo8`S&2vUN>@Qu2WC~TYr-FEE`imX<;8fuVdeAnr*&V4JByLV|abdokf(fzL#@IQE8*o3h0e0Do|Ug z%C$Az?5NI|Mgcc5$8YQ2TCKu*FswY}AREI1lNyNe_41Hb`jHg2*fusJs?#_OQe9t8 zG4;v#SwA$~m!!fao{a7k3=kOmJ`D1lNag37k@ziy>HgjZ`DvEFQTOy0D zk+6{s#~4l~ABvgib}0FC_VG!P;;!Vju?;7bBE{xLExX9@!3pHBmkL$^Y_iV2K)=m0 zzdnxg>FJWv;;=u?H=n4=-ril4e|fWT(v#U^0e_+l5-Yg5G*izf-9N$yyN{p{QjW;UwKC=p%99wSfuFb8?y2DetzIbAMvFl^bMA3cXW>x3r%Lb{85lJfy3 zCtRFUE4etEnMqH75C zaPx@&b=JoEt&G6#9r>PK`if%ax0}|glCc#E|GmWk!Q{I&-Y56n!si0&Jmj=D%{k?F z84qyeJDh56%D4cY~N{;nBj`IDDL+63#!Z@$YVG%scIE#w1FI>h( ztgmx+Xe1Z@^Kv^!?XKiX#2JhOww;VwQvJJ1e{r7wU7R-Z zU#E3ihJ0v^ebCM|k%f4Ze~+Qpnj0KD&-16e=eOkMrZOU@VqB!tGA6pnx$a7gKxa<+Be*7`cfa5IrZ)jr75 zU}`TqC{B92!G}|nLrG8Iv1DQ<{=;B5~J+mrYeGjNmIdo6D2UxTE_payRWJ@ zE`9P(g2x*`&CExpakj6jBg7k_i!n&w*nQUxI!UGKvfSl`ngI`8kzK3!#s0)ZD|!jL zjl+WRGx2#02e80K1QFJm z;NW1j_n-e@avlJa^H-=*eI1WN6dE9xvBA(oy@tR!T7_>dSZ+7KcKdnk9BZ4LW!wdQ zQxBj1VUPZ0)<`HaJdtnzafD(>aYia_D~FMA*0(9hmkH22<-2LIk<>)DhP z%6gk0DKwfH9Am8=l<26t$u4DHW?@Nh@9C&CRNA-+*Fru_O|)jpQ5R4AIE&lkg(V1R z&Jl-ax?#eyzrp&(vn+Y9o(b(grm{A*=Q%g3@$2kMUuo>iC!Xa4@hoc>o{*2Mzi6|6 zn+|3@l^0$6(dDi!TxbwJI>G-YZ{zP_h7}@xP-9QpM8CY-l`=bBFx|^AkLzF%224R5 z6cOWUzqiQ4Duq>-+T%%VOL}*82)ZGDbyp83R5N@YW_hjM=%7j@3Ut+1dl{)3#x?93 ztXP(Cro?;Bn+n^C!~Z_r)0X)YT`XS9KRfXg*x6ym+1fTQ4Ey-$xeoR8hJ723%SU#0 z4)0E*(Qd=yMr+>M&Xl!_PgSJnooG+fVlRe$BIdqr{f=eeow4Yr@1rXld}3Agu&cMb zM576jYwGmvlX>zP`htOW*ETopzyqlc3^zwSOl@axEz{4w#V&WYrq*haDyxG0Bg%{sK(2PN*lI(Giz;)zcRqi+`n*2l z=WtTdi~UEop(sMTuwd26g6ZVs8>JY4% z{>8LlL4CD8m3j`B(vt-u0@}8?H>vDS{55`QOlZ03!)p#ew2?uku9zb%qG_FClE%9s zAfaR&QA5lUW$^_90t)2iE}rUQX*zjiw&Dxd3m!&@Lgbi{MmJbao_zeP)qejWB~*Ec z==;ymKI{XwTjtjAz`)9xHI9SPvRx=H+TFS4eii3iNzSLSe?M@rURno}j~%T1N!34h z8o6{dT_5|nSVra+t~Z0-_CB+3KP^o9$Y zQL*JpSr`5LE1#rbO@GV2xp)_GBtB6F3J?DO@$BSCyI3aaPaCZ%x=-eZ+tcNWxltsUYM1YwW^~-5=n6a29DQmyODEqYrX(-x!0n+DBvhPo|yk!!rY@h)fA!l3C8yK?`lfLyHe{=`H0 zm>@98LkK!u>dT_xd-8Q6%|hF+syO}7rL1DV`Nir?}XV#W?IuyeJ{2fPPmE z;#MQd!WlKo%(JfSF{fHwYodfNU|FDYtub@!(E#Vg&n~<_e{EaHT+I_T0c-iBo7(2j zogI&EjusS%HZo75q=Euc?D76=Z0tN#zRuRxZXpzpRnE_Js(pmZBQ9Ln48PI*8LKu5 z1uNx1f0q7(GzH^;uwBMCbGeTk+4G2fU9n3@^M}^$M~!z%B#8%4HZxIzonLW$8To7y zbd7e(Y&394@V{9RTfW0$w5dGch>(IA>-*y)a|tOEIb{cwX6_yCS`dV)mwiak}q=I<|Ab6=^f z)Rx+CbkEwgQFiprE-vy6Qr%zNHph;oZh79_rsm!n5d}{SUBt{6e5~+EwC7HGGWD0K zjSz%DL{K3CX-mU=!4FVYnr@h+`PlCOdWBl{hHKfcL~Q`u2xE4wN2k$9IXc@seSHIa zy)GN?KcdY#CQ`8b)o%3cGtFDd{NMpomc=)%09s?afqtjW=l=WCg0vn~j>ObcC1OCHJQbK<}oH}1XE)s!Cb zrgT15?;JT6;a8-G#O}(4P^pnm5j>p-ar_}{nSg`@Pf$>hkg~N91_qkizqea`_(Hq= zXb*qg`-vfi&;E%qTBkN9-2+?x1lF5OdjNt15LlmXR}S<=XmBuPptGD5k4!bxWNvYA z8ceAdO$!L9uR+|XahVs0wa4HU!^49#RD09>{yYvsueTiCMlRC|1Rp&B zVd8i<35Xdy@AFtA8levw-5kO-bWt-5ja`HZLim;-jQ;Jj{4Z4YGqMJtU@CA4D{;ul z`9_{~@L_Xc10nL2%xQ0L zzb$m1UQF385b69I-4;y34R*ns@p6JoaaKw z5DHcI74U8?&RIyIaC{ap7}yH04-eVUg(S``tW*Waoc&ZM$>U{J(kp+zcMVv(FRk>2#+NnD;2h}?+e0N&7QeXoHvYK?yeIJN^kHhS3;^68t)!`$ zQ(7u2bfIx9*?zla(t1aqh&{v`4k}0n|gF? zK=h`0_DLwfGbg7(Hjf}dKe=jD?Fe+Q(7jEtAx+z4B$+%8uISpU^Odd5lQjeU&f3p{ zcnKV}f!pib2L1zh3&7PuY_pP@m4f@UR8N3z~67ZcOuVl;YebGaO4%h%UATPNEw*MQY&hSQ6Y z3K;GQ>*?=T#7R6{?c@q7J+_FtGnt2=)qRmiXIYj-j+pba#E{32AFJ61bU)?6I&2R*!4qv0&b88JS9rf4??^ zXMZM(^f5w#1;s2BBs^ZJ4K%D_rDbH8;WxM4q+bA2Ej*kXb^goUL(FVkI7~A}WDRnO zs{3C^JWEku)zLZ%;02@vWQPT>u>mWB;NsxseuT7ij*#n^Ce%8puoBMJ&{5>HJh3JM z4Odh&iA*smWb+9KjPSbfc&`5{MY($~8W0FS8tY7w@&sLx5BIGDc3_UkoqcSqta|{+ zBrtFoE>(wztYxFGZHO)m#5((9(9g|0gQ<FX+0BJ3!4Og2Q$I zbwZa{Vk|_g-1ju&m#_MyCKF1vQ!2l4`$ok-*7s{VOm>2Jy8t*u1BP$|6_#zInpVF8 zH$CkV(OKUE>LDgc<`j0~i0Kc2^P4LtC)Wd$(el~tRTj1#3B?8xOEiUrh0k7E>v4VJ zLaT+-_2q}&S40rR0sayIdP>U5ps6&iKrxEh^vXhC(20>-*FQH1%l#O1@#g7NpzhXI ze`S(ZQ+u^^FytAWb=vV^P6EK|xPK)5=umf3#n}gYAXxr9&@WX>12BTTK-Uwtk^UyK z#e^x|khTy=bHD4IpR>jZmKK{2TQSLYc6a>LrZ-B z>zrGUFYZ|ZhF}^bbPCz9G|hn1$>~w4vVpQe+lEW;`F%@P^XjJdO$>c(23#*=uK1cQ zNCHKPskvNvbuoG$22vb0%pmMe!v!jKaazA892Kzlii{+yt?ZIRSq9^uJQz4E_m`+7^TaF`#tV8^p-EzAbb$``%uXN$wOjF1|Bx z+%_BU&J0+w=GDnavOiB+CD%97I#aReT@`C%$mMljx?J!H7-36FN=ghc-!4{6_JeOa z^4Wl1;`Q+?s}69eCe4oED>efJnF)I27Niu-fSomyH5_vRT~aXET-(SlHyp#ya4y<9 zu~=AwXgb?G+pHPGECZ#!GZp6skgtq12Wzdax?1n1?foL3i60*_K8vk!mmmG!vA+82 zF1np^yuuFs!(Lv|gdFTzpP+0J`DVDebPkF@F+YsW9S=n008fv;J^PT87a}+S3RU z<)?t7=OHUqI_}(yC zNEB#<>v>Ht9JOClCCfaWzD6>Z$o0I2+!Jbe1p4e0l3OL!XdrwgaE(vdH&ZjD$&sQ+ z2#n~_0TT_wWsCoDINB`Cm9zIxq8Juk(9FayP}0jh0}zQ}}HjWG=m1v%_tL z6SDzz>>SE$8U9#R>q8w_;2VK?N?5yuVXh=JAFFgV!PzrQWa7_-hd)9!nu&sy#%XxM z1bTd8V&W^qFGxmX$+`9P7OE^au8A=TmhT~Jp#R<0UC(BjPCFKhmq?#<#6E5m13hDE JuI`?b{|6^*yITMN literal 0 HcmV?d00001 diff --git a/src/simulations/course/cubic_spline/cubic_spline_2d_plot.py b/src/simulations/course/cubic_spline/cubic_spline_2d_plot.py index f69f632..37def8e 100644 --- a/src/simulations/course/cubic_spline/cubic_spline_2d_plot.py +++ b/src/simulations/course/cubic_spline/cubic_spline_2d_plot.py @@ -1,452 +1,58 @@ -# """ -# cubic_spline_2d_plot.py - -# Author: Shisato Yano -# """ - -# # import path setting -# import numpy as np -# import sys -# import matplotlib.pyplot as plt -# from pathlib import Path - -# abs_dir_path = str(Path(__file__).absolute().parent) -# relative_path = "/../../../components/" - -# sys.path.append(abs_dir_path + relative_path + "course/cubic_spline_course") - -# # import component module -# from cubic_spline_2d import CubicSpline2D - - -# # flag to show plot figure -# # when executed as unit test, this flag is set as false -# show_plot = True - - -# def main(): -# """ -# Main process function -# """ - -# x_points = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] -# y_points = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] - -# ds = 0.1 # distance between 2 interpolated points - -# cs = CubicSpline2D(x_points, y_points) -# s = np.arange(0, cs.s[-1], ds) - -# xs, ys, yaws, curvs = [], [], [], [] -# for i_s in s: -# i_x, i_y = cs.calc_interpolated_xy(i_s) -# xs.append(i_x) -# ys.append(i_y) - -# plt.subplots(1) -# plt.plot(x_points, y_points, "xb", label="Input points") -# # plt.plot(xs, ys, "-r", label="Cubic spline path") -# plt.grid(True) -# plt.axis("equal") -# plt.xlabel("X[m]") -# plt.ylabel("Y[m]") -# plt.legend() - -# if show_plot: plt.savefig("cubic_spline_2d.png") - - -# if __name__ == "__main__": -# main() - """ -Cubic spline planner - -Author: Atsushi Sakai(@Atsushi_twi) +cubic_spline_2d_plot.py +Author: Shisato Yano """ -import math -import numpy as np -import bisect - - -class CubicSpline1D: - """ - 1D Cubic Spline class - - Parameters - ---------- - x : list - x coordinates for data points. This x coordinates must be - sorted - in ascending order. - y : list - y coordinates for data points - - Examples - -------- - You can interpolate 1D data points. - - >>> import numpy as np - >>> import matplotlib.pyplot as plt - >>> x = np.arange(5) - >>> y = [1.7, -6, 5, 6.5, 0.0] - >>> sp = CubicSpline1D(x, y) - >>> xi = np.linspace(0.0, 5.0) - >>> yi = [sp.calc_position(x) for x in xi] - >>> plt.plot(x, y, "xb", label="Data points") - >>> plt.plot(xi, yi , "r", label="Cubic spline interpolation") - >>> plt.grid(True) - >>> plt.legend() - >>> plt.show() - - .. image:: cubic_spline_1d.png - - """ - - def __init__(self, x, y): - - h = np.diff(x) - if np.any(h < 0): - raise ValueError("x coordinates must be sorted in ascending order") - - self.a, self.b, self.c, self.d = [], [], [], [] - self.x = x - self.y = y - self.nx = len(x) # dimension of x - - # calc coefficient a - self.a = [iy for iy in y] - - # calc coefficient c - A = self.__calc_A(h) - B = self.__calc_B(h, self.a) - self.c = np.linalg.solve(A, B) - - # calc spline coefficient b and d - for i in range(self.nx - 1): - d = (self.c[i + 1] - self.c[i]) / (3.0 * h[i]) - b = 1.0 / h[i] * (self.a[i + 1] - self.a[i]) \ - - h[i] / 3.0 * (2.0 * self.c[i] + self.c[i + 1]) - self.d.append(d) - self.b.append(b) - - def calc_position(self, x): - """ - Calc `y` position for given `x`. - - if `x` is outside the data point's `x` range, return None. - - Returns - ------- - y : float - y position for given x. - """ - if x < self.x[0]: - return None - elif x > self.x[-1]: - return None - - i = self.__search_index(x) - dx = x - self.x[i] - position = self.a[i] + self.b[i] * dx + \ - self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 - - return position - - def calc_first_derivative(self, x): - """ - Calc first derivative at given x. - - if x is outside the input x, return None - - Returns - ------- - dy : float - first derivative for given x. - """ - - if x < self.x[0]: - return None - elif x > self.x[-1]: - return None - - i = self.__search_index(x) - dx = x - self.x[i] - dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 - return dy - - def calc_second_derivative(self, x): - """ - Calc second derivative at given x. - if x is outside the input x, return None - - Returns - ------- - ddy : float - second derivative for given x. - """ - - if x < self.x[0]: - return None - elif x > self.x[-1]: - return None +# import path setting +import numpy as np +import sys +import matplotlib.pyplot as plt +from pathlib import Path - i = self.__search_index(x) - dx = x - self.x[i] - ddy = 2.0 * self.c[i] + 6.0 * self.d[i] * dx - return ddy +abs_dir_path = str(Path(__file__).absolute().parent) +relative_path = "/../../../components/" - def __search_index(self, x): - """ - search data segment index - """ - return bisect.bisect(self.x, x) - 1 +sys.path.append(abs_dir_path + relative_path + "course/cubic_spline_course") - def __calc_A(self, h): - """ - calc matrix A for spline coefficient c - """ - A = np.zeros((self.nx, self.nx)) - A[0, 0] = 1.0 - for i in range(self.nx - 1): - if i != (self.nx - 2): - A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) - A[i + 1, i] = h[i] - A[i, i + 1] = h[i] +# import component module +from cubic_spline_2d import CubicSpline2D - A[0, 1] = 0.0 - A[self.nx - 1, self.nx - 2] = 0.0 - A[self.nx - 1, self.nx - 1] = 1.0 - return A - def __calc_B(self, h, a): - """ - calc matrix B for spline coefficient c - """ - B = np.zeros(self.nx) - for i in range(self.nx - 2): - B[i + 1] = 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1]\ - - 3.0 * (a[i + 1] - a[i]) / h[i] - return B +# flag to show plot figure +# when executed as unit test, this flag is set as false +show_plot = True -class CubicSpline2D: +def main(): """ - Cubic CubicSpline2D class - - Parameters - ---------- - x : list - x coordinates for data points. - y : list - y coordinates for data points. - - Examples - -------- - You can interpolate a 2D data points. - - >>> import matplotlib.pyplot as plt - >>> x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] - >>> y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] - >>> ds = 0.1 # [m] distance of each interpolated points - >>> sp = CubicSpline2D(x, y) - >>> s = np.arange(0, sp.s[-1], ds) - >>> rx, ry, ryaw, rk = [], [], [], [] - >>> for i_s in s: - ... ix, iy = sp.calc_position(i_s) - ... rx.append(ix) - ... ry.append(iy) - ... ryaw.append(sp.calc_yaw(i_s)) - ... rk.append(sp.calc_curvature(i_s)) - >>> plt.subplots(1) - >>> plt.plot(x, y, "xb", label="Data points") - >>> plt.plot(rx, ry, "-r", label="Cubic spline path") - >>> plt.grid(True) - >>> plt.axis("equal") - >>> plt.xlabel("x[m]") - >>> plt.ylabel("y[m]") - >>> plt.legend() - >>> plt.show() - - .. image:: cubic_spline_2d_path.png - - >>> plt.subplots(1) - >>> plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") - >>> plt.grid(True) - >>> plt.legend() - >>> plt.xlabel("line length[m]") - >>> plt.ylabel("yaw angle[deg]") - - .. image:: cubic_spline_2d_yaw.png - - >>> plt.subplots(1) - >>> plt.plot(s, rk, "-r", label="curvature") - >>> plt.grid(True) - >>> plt.legend() - >>> plt.xlabel("line length[m]") - >>> plt.ylabel("curvature [1/m]") - - .. image:: cubic_spline_2d_curvature.png + Main process function """ - def __init__(self, x, y): - self.s = self.__calc_s(x, y) - self.sx = CubicSpline1D(self.s, x) - self.sy = CubicSpline1D(self.s, y) - - def __calc_s(self, x, y): - dx = np.diff(x) - dy = np.diff(y) - self.ds = np.hypot(dx, dy) - s = [0] - s.extend(np.cumsum(self.ds)) - return s - - def calc_position(self, s): - """ - calc position - - Parameters - ---------- - s : float - distance from the start point. if `s` is outside the data point's - range, return None. - - Returns - ------- - x : float - x position for given s. - y : float - y position for given s. - """ - x = self.sx.calc_position(s) - y = self.sy.calc_position(s) - - return x, y - - def calc_curvature(self, s): - """ - calc curvature - - Parameters - ---------- - s : float - distance from the start point. if `s` is outside the data point's - range, return None. - - Returns - ------- - k : float - curvature for given s. - """ - dx = self.sx.calc_first_derivative(s) - ddx = self.sx.calc_second_derivative(s) - dy = self.sy.calc_first_derivative(s) - ddy = self.sy.calc_second_derivative(s) - k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) - return k - - def calc_yaw(self, s): - """ - calc yaw - - Parameters - ---------- - s : float - distance from the start point. if `s` is outside the data point's - range, return None. - - Returns - ------- - yaw : float - yaw angle (tangent vector) for given s. - """ - dx = self.sx.calc_first_derivative(s) - dy = self.sy.calc_first_derivative(s) - yaw = math.atan2(dy, dx) - return yaw - + x_points = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] + y_points = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] -def calc_spline_course(x, y, ds=0.1): - sp = CubicSpline2D(x, y) - s = list(np.arange(0, sp.s[-1], ds)) + ds = 0.1 # distance between 2 interpolated points - rx, ry, ryaw, rk = [], [], [], [] - for i_s in s: - ix, iy = sp.calc_position(i_s) - rx.append(ix) - ry.append(iy) - ryaw.append(sp.calc_yaw(i_s)) - rk.append(sp.calc_curvature(i_s)) - - return rx, ry, ryaw, rk, s - - -def main_1d(): - print("CubicSpline1D test") - import matplotlib.pyplot as plt - x = np.arange(5) - y = [1.7, -6, 5, 6.5, 0.0] - sp = CubicSpline1D(x, y) - xi = np.linspace(0.0, 5.0) - - plt.plot(x, y, "xb", label="Data points") - plt.plot(xi, [sp.calc_position(x) for x in xi], "r", - label="Cubic spline interpolation") - plt.grid(True) - plt.legend() - plt.show() - - -def main_2d(): # pragma: no cover - print("CubicSpline1D 2D test") - import matplotlib.pyplot as plt - x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] - y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] - ds = 0.1 # [m] distance of each interpolated points + cs = CubicSpline2D(x_points, y_points) + s = np.arange(0, cs.s[-1], ds) - sp = CubicSpline2D(x, y) - s = np.arange(0, sp.s[-1], ds) - - rx, ry, ryaw, rk = [], [], [], [] + xs, ys, yaws, curvs = [], [], [], [] for i_s in s: - ix, iy = sp.calc_position(i_s) - rx.append(ix) - ry.append(iy) - ryaw.append(sp.calc_yaw(i_s)) - rk.append(sp.calc_curvature(i_s)) - + i_x, i_y = cs.calc_interpolated_xy(i_s) + xs.append(i_x) + ys.append(i_y) + plt.subplots(1) - plt.plot(x, y, "xb", label="Data points") - plt.plot(rx, ry, "-r", label="Cubic spline path") + plt.plot(x_points, y_points, "xb", label="Input points") + plt.plot(xs, ys, "-r", label="Cubic spline path") plt.grid(True) plt.axis("equal") - plt.xlabel("x[m]") - plt.ylabel("y[m]") + plt.xlabel("X[m]") + plt.ylabel("Y[m]") plt.legend() - - plt.savefig("cubic_spline_2d.png") - - plt.subplots(1) - plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") - plt.grid(True) - plt.legend() - plt.xlabel("line length[m]") - plt.ylabel("yaw angle[deg]") - - plt.savefig("cubic_spline_yaw_angle.png") - - plt.subplots(1) - plt.plot(s, rk, "-r", label="curvature") - plt.grid(True) - plt.legend() - plt.xlabel("line length[m]") - plt.ylabel("curvature [1/m]") - - plt.savefig("cubic_spline_curvature.png") + if show_plot: plt.savefig("cubic_spline_2d.png") -if __name__ == '__main__': - # main_1d() - main_2d() \ No newline at end of file +if __name__ == "__main__": + main()