From 6dc278323aa354e82f2a49fc02436d635ec02d44 Mon Sep 17 00:00:00 2001 From: Christopher Latza Date: Tue, 23 Jun 2020 00:51:24 +0200 Subject: [PATCH] add 3 party A* libery --- Roy-T.AStar.dll | Bin 0 -> 22016 bytes src/A/Collections/MinHeap.cs | 128 +++++++++++++++++ src/A/Graphs/Edge.cs | 37 +++++ src/A/Graphs/IEdge.cs | 13 ++ src/A/Graphs/INode.cs | 12 ++ src/A/Graphs/Node.cs | 43 ++++++ src/A/Grids/Grid.cs | 234 +++++++++++++++++++++++++++++++ src/A/Paths/Path.cs | 28 ++++ src/A/Paths/PathFinder.cs | 124 ++++++++++++++++ src/A/Paths/PathFinderNode.cs | 25 ++++ src/A/Paths/PathReconstructor.cs | 37 +++++ src/A/Paths/PathType.cs | 8 ++ src/A/Primitives/Distance.cs | 73 ++++++++++ src/A/Primitives/Duration.cs | 52 +++++++ src/A/Primitives/GridPosition.cs | 32 +++++ src/A/Primitives/GridSize.cs | 30 ++++ src/A/Primitives/Position.cs | 35 +++++ src/A/Primitives/Size.cs | 30 ++++ src/A/Primitives/Velocity.cs | 57 ++++++++ src/BasicPathFinding.cs | 118 +++++++++++++++- src/PathNode.cs | 6 +- 21 files changed, 1114 insertions(+), 8 deletions(-) create mode 100644 Roy-T.AStar.dll create mode 100644 src/A/Collections/MinHeap.cs create mode 100644 src/A/Graphs/Edge.cs create mode 100644 src/A/Graphs/IEdge.cs create mode 100644 src/A/Graphs/INode.cs create mode 100644 src/A/Graphs/Node.cs create mode 100644 src/A/Grids/Grid.cs create mode 100644 src/A/Paths/Path.cs create mode 100644 src/A/Paths/PathFinder.cs create mode 100644 src/A/Paths/PathFinderNode.cs create mode 100644 src/A/Paths/PathReconstructor.cs create mode 100644 src/A/Paths/PathType.cs create mode 100644 src/A/Primitives/Distance.cs create mode 100644 src/A/Primitives/Duration.cs create mode 100644 src/A/Primitives/GridPosition.cs create mode 100644 src/A/Primitives/GridSize.cs create mode 100644 src/A/Primitives/Position.cs create mode 100644 src/A/Primitives/Size.cs create mode 100644 src/A/Primitives/Velocity.cs diff --git a/Roy-T.AStar.dll b/Roy-T.AStar.dll new file mode 100644 index 0000000000000000000000000000000000000000..0a35ba2d8044f7c7522c485d6c05a31c531e306b GIT binary patch literal 22016 zcmeHv3wT@AmF_wZNk_8mSaM=JBu!635`^P8&XWL1uw%!GA#a-`4k?LjAKOY~9V5wk z;xs8~W}s;aw1l=$UIqGq77DaLrw?cu3bzlKX=mDL(_8wH+sAG1OlhaL?Q|gXueG;q z`2k<&ex2`r_q&y&wfDdF+H0@1_WMZ6hMlL$A|e~_^XG}4LdvfOp>Iu&p}6erpD&{) z!rz?vl(GJsGu!)8Id>rI_hys*ZdWps@$+t{=Vk{pZYtw8Z)$V<{cf+Wv@|kXtKPhZ zXuV<4qA$$6qtMzbG|Me9>QMlWqqwW~B6aaThBr~UwQ_YUTPo38{x znfR*h29lRcw5Kkc%XR^YZO4!a;=K{?qE`dDt1j!MeF(CxbPwLFYmC7SM61Ca^_szP z#fxn-DM0sMLbQ1yks;jwJgJ+-b#iDuSToDChM>o;nPDE$sVaa(MZHZ8NWqUyqQpwb zN06>US~AHH97W2w)CqRr-_CHT1OIkb*fD!YI#EUx*_S|%1#7mUBF1nK@|^5Yjf~-T zDDEmPYwa9^uTi*Vw5~VInz0p;c@lcPN^b4%KN+3fQ4)NI*F!J0|3I zC@#mdNLEL@rsv)MR_Fy}bJJF~DJv1IuPV~K%-SRyhKOHMhKqDW#{ z+|c+~a;b_X=iVZgT2RMwR%K-_feJON5JtGbuQX9)HMd%l(6}vfQ zyG5zk%}QV>Cu}z>0^~&Ombo$;<2eL9+6R_JXYoXdz!#PshBd2aL70T(E=P$tiOTGx z!_JMF>`Dk8vkZGKLgIN7SC{_cobr61c+RVvj6zcuG&LaOqxuVK;5WP%zd2>UMWy)7 zD&Qz5>^HBAfSio>8^fsJx2&n~TjsHB^X7_vu##Ut#$+9S>>kKQXD zwE&;O5@4E%wT0}szJx;fgg}W;oU%`%w6f+wGr>yW1ShFH_M33qeYOoKJcOkrH35HDve z1VcF4F(J1@aa%^q>gBAc_8V#p6?2A{vts7Ra`q0kNCpz;IP5#K&q~-?e$;X%vB9E1 z=rqI{qsXkSvL4_xq8vq!b1fMq*dvNt9KWt{DI%!-o^V~e_O5q4q|}TICbZA{xsMdR zjJ*asN(2K8hY_Af=v%uoGH)FQOD7sI6J4M|A6rwzza81c1t3GEENmva$4Y#VC4ww5 z2@=7o9TBa;$JP^B^hqAF+z_X%gHvviQ?5~SpSCAg4f!GLd$yUwb{ZMNOhEPktHxxs zYuJYje84z~1uXw03L*hp(vpDxSAa(okef5VYVOgCka;8c6nHsUADMHEOA^??=N-kG zSuFE3%V0Z)43{J@V(0N5KQC$qLr$U(#aMQ^?P+Kmo{Vc7a%Hv_oL2A9k0A$75$6;t zo0c6&>_ukz{otEN*g^m8XfI+16Ig$ES{-5kg8p?twton;1`{!jRPp?eAnUuRQ!;O3 z$j5k!L>>RbV3r2`ED~p=pDJwg2nQ}45BRenSQ|6#K+p-t>{uYt4_WvXc5p@uBoNDB z$iEOm^FR6DBcWOVEX|R#+QvD^SFXJ3{Q2|BKJy4xXuH@*_+!8w69?IypXMP>^q{Uh z7nPMEKDq)~_Rv3!7R|{+tVd!RpvzDPqakJ`Fx-M1DvZ)#ZU6+8I95R)U8i-aa0GpH z6qYKbr^XVKOw6EnS$>XNP=<0g@ivg!5XQQ+`B+&<;#C$*pvQx?(^M1S!kma%7AUi= zvY;)I;dL!QmumZz1D5R{L_G^)fwH$Y)e8FaptGja^l@kMPns^-VR6cc{kI1)^`e%#7xw(lUEj|@O+>l%wxOe zGV=(hh*%vRHuuUuXP9$?||2PXxz<(F>5095;exaJgCO}iLOz;ak z2Mh>SEts3N3>wISKalML(FJl?l3g{o1lAk|8&-{x&nbB>7$>jyRx}q(V8IC1)@N}c z8_3?wSO%S;svRXEGx2WVMbGFx<^+RBWF4{Ty=Vi=9=CBl+SpfU zBXJ38M=_IFx~gM=s|%`jgrum{30wYsC<{9()vZAE!h(*f9U@v`J26`c#q5G~kzJ>X z?7HG0md(OkdvMsc<+maW^Nto&k2P=pNb|EsEY${#vo%<`uzsY%oI-^TMC(tn_=^27 zve@WOHX3UQ_pHR#;Q7jrG_aT5fDEq=+}*r3PE^KYpk}YK1O16*n!6RMf?|*Et1F>1 zE;_kWRIl#LEk{R15Sv70bY2zKa2_Mrf4TpBtilneYDd)3L*NxX9%Kl-#EaTAtS=vf z-U`+$bth;)67=sui+U%WI%+5N??j2?V?T<_)tlmviZ?}F6?YJ5xqFzoU7E{X;PZz+@kV)-uNHzXEOYe10Tn-s-qB1Y>C0RG1|qdj)bsz zPKxx7QPS2J=^Y}CbH;n1t9GgxQkim`W)e4nWrl(}El4;833Q7{I9gH{Of|y=2`8*1 z!dg-nOf585Zicn?pprxv%D%+s0qkFI-iNw#^$l5FePxiKTRsZ2YyMIDS-I$9ol}#4 z!38=Tm5a)CPK_p904j@i3HrWsBmA5jt z0>ftr=Qi8a`PrPehOohlMlNE_%;r@19A8Ur(l|22uo31?K|gLG_aF)S*qkGlOahfh zhw!tCR`dss7MTAm=C6WORYzprl0vmBz$gjHS=3s5f#QbL_*dFAz}SrhD;K<=`q|(< zmh*?1xRf-o(9mjTVcX}_Z(^`afl9xb(N={le+`gn@qIPIixA5z z5xKTT%*w<%E`nbH^3Z-r{DOy(>IGOvujImluB!lG@S-Ngzu zMwO5wLK9SQB0;O}(&BAg4HO8>%Ns>qee3?FsnMMuc^79+T2;SLy~GdT+Ka+(Ru75HIL-lTr2D& zy6wu2;_xF@qxiqe4J2*d|N)7%Jk+~g7u#^ z!MnPR%ApT^OBb%ax)r0FT;O^P)C*Uq{Z1Cf3!=vMx8D~IW6`01GZqsjw1F(9JMR~S z#h;$W`zv^t;*F4S9b$bjL-zDqq&S2VVkULDe&KEDJkeZ-dGL}x9eg>02^wPhQs6z2 zN%X4-^KTXYzXkS%g7nki@-R+&LI;Ib2(1_TEupUlSmt0L=LBgfG2J5ci2!SUJj$H= zC}srd%R)~HeM)4SB>#}m_c$#1VaY!k;`}Vp`2op4Ch}=1EeZS$TAUm?Q4*v|kvSA* z`D;oB3JfKV&gIBbsinz4KBSS^de}iV#T^>QxBwC`; z>!A)SMy(oEpsa$f)hJNbVNIbvjbcDmG^o+HouoCD-XrLFx;4~hPNVxZ>MGf5O`{hD z{lw@h@vZ4}R-@a(gBDhZS#0O?)N0>^F*Z{WN9l+)gXRlTQJO)^1f6sqjV!g^POFq8 zeIarTs72SiGJMiB z*{Y@grcn&2j=n2Md0S7v(ulpSrztog5wW+oTMH?n(QV<6Sc_=AAm#01>KDY`e$-k_ z`vslEnD`-<-IEGAzoDD3mc37-KL|Rb>jla_ZY`yIG>QRTLZ1?(EW4Cm)`%^;lzuCS zEqm0ujH-A^*O9-RY6Yp*FQ*j>IRhm(;mJy~M(>R>S}*8~6Da$fwUV}J6a%_~x&*1# z8|kk#;?^7KQ-ZklFIlVUWnISo-b6HqZQ)2fXJI84#J0Q$G*gw)+2~8w8d|2x5SOLa zT3V&i9|W~1Nm%whlx@~12GmOL5Tq=-ijHZ-mR&^;2x7}#vDVWIx{NK`K))Bnw*17} zK+asY^Q7~)p?|hEQMpDxjWVhh#Buoz&@7E&KwD^mAk}&swQIz!x6vU%-1;A_cDi4e zaqCypD}vMvxSD=0=%n*6)>3O5y{6Fy%!h4sUeFn`Y{TA0A?*5yCv`;(dpk{02&+cO z-a+#;x)^9DHEML)DYth~n?`p7T}z%uuZPO*J#<*33Y2xy%^C&D%I$9YkVY|}9(qua z^0tqj(TKh6qyHs{y{)!WM7UJ)T)8bg!`@4?1*zGwkJbs|$j`R-(N`66{6M|kPhZ#Q zN6u29Z);Rlav9JMG+J!`v(-Zd3lU+5%#A=CuPzaB;#?E!jp7+quM=~0}G zh>p`efgtwCvl_i>_t*z$79PG5-9jsk0sAn`7j&Fn2_3Yrr;Qq2XCJkP=z5L5XP>rD z(6fTh&@Y|4>>KI(g4n-L*r%wB4^0?Z*VvESH&dlXUkfhJU^fh$HwE z%Gw2~$e*SMm8AH7nx0mO4m)46Ptyw;-5q}3zLjG0rFD7@rNnKJp%M$8W3~~9gTk9{s`St$69!_p0n?vXB49Ikzd;P(QG~tBA!*zU)vw0 zXEgdj$*=AEDO=BFC!I?|zqcQt>owxF=K=blM!fbsK#wa#FWav}@@0)mO6m_d}2U5Zx*0C*e<*P78dR z?$_wK(qG%3rY~#sI!4_i^ff`porcA899YM;!&(d!NaSK8EEICW1 zJ*TSlCZH(_(GNoX#%Jk5jTV;P0kmABhob$)=crwy`p_Lff2$FX)+g!j1s$g?ICnou z<;%E*pHIC?!T9^4c`qhEyJy>Kwpr&qW{$vj&J?Fu*#&j z;ja7U@hPr#eF@8-kqEz;wZ-zq`X6$*?w3LTw6s-sEe}V>!nxL~VRu!N17^2w} zZ3Z=qZ5zb??~8^1Sq?s^fX~Iis(rhQBM_diX*HnsS!QJRB2GY zC>a%{jEu};dQ^NFiH|`y3a6O` z$?9CZYw*mWjwVq9B$_EqbrA!fk?|@&6*Pk9M0JsJS}62Vp^ZXYg>DhrAv7s8CGHg?`eBa@=@Vi0zJj|T;(`%ByMN^hf(=z8vlqcR*t``j_gq{L@4NoRM zTKYSjYE=I7(F+a3XenK7Tu;2W+(~Zaka0g*rTxYUni0Cu2pcO)Z#81ZX`KA7*E)Hp zIU%Q`>xuWL6I5&53k|#X$c$YjU zyW=_Ho$nm+-gu69-|I#n^_vd$(F34q`lN|xJfL+LBaZ~?@VVJjpwsBPpcm3Bp!4W= zpo^(4SVxyrGiVcS0lkU>p*m`#IOsL>Hqb86M$xuMXj15Lq4x@XR_Kp~8V0vJQ)r{m zq|oC+?-ly2&>t(l$?`V{y;tb7LVqljEH1rQ=#PaOxQRf&&_ZE7T3JOoO69 z&hHU=Pl)r+DC%(j6-C3GccV-jgzgb~OsHEXe4%@U9#i>p(GX)wai*@&2BCX|zEUBj zlciK>3M2YBy${be@230dth$UYa`eNuX^1p~O4cW}^6+I^d??m1W zp8%a!awF&^wn*ByBL8*a&ku0vJA_^yx*hrAwq7W`6PzlMRCVW+a^0U=Oy9b+H+(N- zIztbF-YB#%DT1~o+=7DBuK4^{> zfab9xn{)$OFtMI31${SN0{UKj7sH0dLF25(}twSnG)CmJT68*c}FkUHSs zbXZ;ko0g-67Sw9Ttnb7r{yF+O{XJHx3yiGsF5@%C*NpENrRIg^dULyZt@$C-KE``| z@#}Zkz04xUo#sV24e)0cI8xy?7yHH~RAF3Bmm4c-qhTP{@>;R#(%rk4?yjd5EvZcR z=48HaRi|dCl7(YS7E)`I-#?JdCOgyK9-uXu!G3Q9T{oD_7mAwvblU66r~C{HZuOGg zn=uiMcoDS>X1ew)qUMw+Pi7ARz!h4t!S5bS zd#k85@AWsf=Gyv`uprysm&`C{HA39$<#+E8+Clr1=|OMzZrYzpBJe9#by3@)9OUXo zOw84-^)g;I)kTfn-PG;%BnQ*^wgJ!UrnP=D4WGLDkZ;YkW^$=+Z*4Z!O}$K3>{zvL z_wLonu6_70swL&6!Ri>#T9fG>&)neUy=-o?mu>U9{D}mb5u9YE%Nt*HBYtHxp1bDY z0Q$k}-s<%yQyCc4p6Y)Kv35V7OiwJ*erRBVitKh<>hJ_5t(h*r9}SMTY13f7*PpOt zd#W4ba9s472eU~YOXEx0v&sDkSTgVn+(f;HW}-UMt6 zox%Rh1O=*7$LrbZADBRTotNtE%a50C%O|tr+3S+I^QA^hEAIs|>J^m=;<$1hyC$maNVfazE9*4Bn%cI%8CX7C6?QO7SpyRmCY%T5&O93NHr^2kMYPdMy=2an|T$ZHZ+bzqBzkcB5y)& zvHb9C5-mJ|Xd5p)l4E})lV`qan2qD{%xTWcqs#Li@uHhXzepy9MNQTvlyo=d@qu^e zAc~s3&cWVZwtIxvn9F(no#{jEseCcJJ(-1#E%>#fcfil?8!z92HnzbCOt|rdt(hJ_ z+b^>*nSN6#^kq+~SI^qMw5N=vI-y>#-R zhB1y?s`u?uwN#}~-Pl{qw0uyrt#$4V&te5x~*PUVYA+p~u-pro(VR-p%t^kZSk zlhvNrKI-z)Y00!=cplu;BVvX1uduV|7VC7-FuFvnuozOiufD!O5(rPG2{ont9I}lA z_<dTTz2YBJ66#1QdX|w0;BQ@Q4N@x}BKJQx8%fuhpy*BG%1j+um zDZ2(+w{X{cxg2kq1=d#o(ERqgMqYaAy3?qQWrVj=!r)YyHb0ve&Q@hU|GquScJ6*i@`Evc*WVh5d$joD2h5 z3E8s7Y%fktnLPS^Q_t39rWb2smnU;UCU`Q7k&j#^m(OA-%PQfE;+4PL>lEtpb9t3(N_)vH6?G_$EU4t?`@HP%fez;|Ue@)x$f&MMQAxPoRz996 z3sKDRyegIUhj)*4{$N)2m+{dtKe~DU z81~w1a-eUFBqEd}nN>MCkShE20fD;IL7`xC(UEhMxuu%sp>XUO7 zpOH3Wj%8F?(_mKeT{`9dZ}PD>r~$`EF2dpisty*^b{+B?aBSJgzh&Q$Jjlam_yi{F zKXXQoVZ$uds)C|YWmCLm5Z{DSJ%@NP*??y zTm*}c5WG8Ux&GwARR3WAFy3D%Kx;BcSsC)J8ILytM)*S85vs>syao5gA$)>78t3qZ z;sm}YRgbb-a*qhPk@VOr@#NRzY6@E3F5x zxpm_n-#12MA9*P01wXG#wALxb_PelV0otW`E5G;6vqxa>SlfrRC$HfPg-66^Bi4&& z2;6EL=wY-&8ydms72iB~mml`cg+y*R4lb^ggHoDcO)Wwi@#Z-<$LB*G;tj z&t*eBo}kww2D(pXs=jdXyk-@+YAm^U!d|e}z;OK77h6}o`1J)>e&FFxI2&GHv4m{b zFdWMzBY;dS#;`0nCBD*#IY=5+0u)t9C*=xFR!Ox=rXzu*s}LtEjd-mj6%+~uqWrSS zC@sa4tg^D{b_m+7kP$_rP82pc%!vlcLZOtFGh;GDq@#Pyrwp;I|OlaP0$J>L^elddcnPhD7!Z$o)rC1!kGYs%gUU|@pUFY&0^>v zbf)8kTnpLwx=1L%3I7FMGrrDoA|XpEH#;0iXlX7B+3|J5mN$f^#E;z~$_-c;3E8o7 z^~YAOW3nQECZq#Wkb?s}f|AO&_3VR&@W_F~Y*huG3B#ZeegzKi7?@5dWIGO*xBy@~ z-#Ain{+siExJ91C**tvBAb$DkSV4@NU~CEAQP#~vfm81OFjCe3RxniIF6t0>DNtx~Y_g!qEKtpT ztrS&rvmy_>M53Ap5pyvBb4@lu%t{Fs2~gTMkcXR z!7)PX@n*IxUdw*5G$v1cHp`(xe0gkgY0@iL=!r#sfuo5>vLFQM?&XlZ}Wgs|gzq&G4$hb`WnXK~c^KDF<>G zRoe+3P9RH|M4bfI0}dxpyqQ^(nYc`kDeI(+QhUN(97!3IB3 zNhc3 fnreOeA}ZhJO`8!n#PruJs=yK{p=c%0xR@w|nOJ!a6cha24<+*#d4&F%os z@NN%IXSg%C=~QPH&zjv%h<3Yv#>IWfZLD#V>0UpJi*A3NyB*I`a_#{Dw-eVXx0kOi zF04e3F9>yR+aR8YW%D_=*Z~)Ii&P&T+yy2>ju<7X}3c;KvRIl@tX}0t#~Y$m?GrW5Z^S@DaS%rSQJ0P|Bay z|s-0?lpKwHi4$o zB2!O=za_Y}AO@5*K83n`W9i4Ay8U21#%H6h+!k_j-@ylSzys2DpOmD~emCsk)?8Sf zfL!WBzhXyF^33%xaBlYlI7RVId++I24d1MxQI%4(2?bN}4zV8yg7|-7uSdVskB}P2r z2H+)osBZZLk9q!aJDW70#}`MmFf$nOC@sWjtY0JTEtmLgh8`a_3?hE{31U39KF1}9 z_D9zn8=aBZ5I*VWI5r}7Jl^{;TGP^ZZ(fIE`j^*#Nd)%b=mf + where T : IComparable + { + private readonly List Items; + + public MinHeap() + { + this.Items = new List(); + } + + public int Count => this.Items.Count; + + public T Peek() => this.Items[0]; + + public void Insert(T item) + { + this.Items.Add(item); + this.SortItem(item); + } + + public T Extract() + { + var node = this.Items[0]; + + this.ReplaceFirstItemWithLastItem(); + this.Heapify(0); + + return node; + } + + public void Remove(T item) + { + if (this.Count < 2) + { + this.Clear(); + } + else + { + var index = this.Items.IndexOf(item); + if (index >= 0) + { + this.Items[index] = this.Items[this.Items.Count - 1]; + this.Items.RemoveAt(this.Items.Count - 1); + + this.Heapify(0); + } + } + } + + public void Clear() => this.Items.Clear(); + + private void ReplaceFirstItemWithLastItem() + { + this.Items[0] = this.Items[this.Items.Count - 1]; + this.Items.RemoveAt(this.Items.Count - 1); + } + + private void SortItem(T item) + { + var index = this.Items.Count - 1; + + while (HasParent(index)) + { + var parentIndex = GetParentIndex(index); + if (ItemAIsSmallerThanItemB(item, this.Items[parentIndex])) + { + this.Items[index] = this.Items[parentIndex]; + index = parentIndex; + } + else + { + break; + } + } + + this.Items[index] = item; + } + + private void Heapify(int startIndex) + { + var bestIndex = startIndex; + + if (this.HasLeftChild(startIndex)) + { + var leftChildIndex = GetLeftChildIndex(startIndex); + if (ItemAIsSmallerThanItemB(this.Items[leftChildIndex], this.Items[bestIndex])) + { + bestIndex = leftChildIndex; + } + } + + if (this.HasRightChild(startIndex)) + { + var rightChildIndex = GetRightChildIndex(startIndex); + if (ItemAIsSmallerThanItemB(this.Items[rightChildIndex], this.Items[bestIndex])) + { + bestIndex = rightChildIndex; + } + } + + if (bestIndex != startIndex) + { + var temp = this.Items[bestIndex]; + this.Items[bestIndex] = this.Items[startIndex]; + this.Items[startIndex] = temp; + this.Heapify(bestIndex); + } + } + + private static bool ItemAIsSmallerThanItemB(T a, T b) => a.CompareTo(b) < 0; + + private static bool HasParent(int index) => index > 0; + private bool HasLeftChild(int index) => GetLeftChildIndex(index) < this.Items.Count; + private bool HasRightChild(int index) => GetRightChildIndex(index) < this.Items.Count; + + private static int GetParentIndex(int i) => (i - 1) / 2; + private static int GetLeftChildIndex(int i) => (2 * i) + 1; + private static int GetRightChildIndex(int i) => (2 * i) + 2; + } +} diff --git a/src/A/Graphs/Edge.cs b/src/A/Graphs/Edge.cs new file mode 100644 index 0000000..5ad6db7 --- /dev/null +++ b/src/A/Graphs/Edge.cs @@ -0,0 +1,37 @@ +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Graphs +{ + public sealed class Edge : IEdge + { + private Velocity traversalVelocity; + + public Edge(INode start, INode end, Velocity traversalVelocity) + { + this.Start = start; + this.End = end; + + this.Distance = Distance.BeweenPositions(start.Position, end.Position); + this.TraversalVelocity = traversalVelocity; + } + + public Velocity TraversalVelocity + { + get => this.traversalVelocity; + set + { + this.traversalVelocity = value; + this.TraversalDuration = this.Distance / value; + } + } + + public Duration TraversalDuration { get; private set; } + + public Distance Distance { get; } + + public INode Start { get; } + public INode End { get; } + + public override string ToString() => $"{this.Start} -> {this.End} @ {this.TraversalVelocity}"; + } +} diff --git a/src/A/Graphs/IEdge.cs b/src/A/Graphs/IEdge.cs new file mode 100644 index 0000000..f1f3837 --- /dev/null +++ b/src/A/Graphs/IEdge.cs @@ -0,0 +1,13 @@ +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Graphs +{ + public interface IEdge + { + Velocity TraversalVelocity { get; set; } + Duration TraversalDuration { get; } + Distance Distance { get; } + INode Start { get; } + INode End { get; } + } +} \ No newline at end of file diff --git a/src/A/Graphs/INode.cs b/src/A/Graphs/INode.cs new file mode 100644 index 0000000..4d33621 --- /dev/null +++ b/src/A/Graphs/INode.cs @@ -0,0 +1,12 @@ +using System.Collections.Generic; +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Graphs +{ + public interface INode + { + Position Position { get; } + IList Incoming { get; } + IList Outgoing { get; } + } +} diff --git a/src/A/Graphs/Node.cs b/src/A/Graphs/Node.cs new file mode 100644 index 0000000..11ba14e --- /dev/null +++ b/src/A/Graphs/Node.cs @@ -0,0 +1,43 @@ +using System.Collections.Generic; +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Graphs +{ + public sealed class Node : INode + { + public Node(Position position) + { + this.Incoming = new List(0); + this.Outgoing = new List(0); + + this.Position = position; + } + + public IList Incoming { get; } + public IList Outgoing { get; } + + public Position Position { get; } + + internal void Connect(INode node, Velocity traversalVelocity) + { + var edge = new Edge(this, node, traversalVelocity); + this.Outgoing.Add(edge); + node.Incoming.Add(edge); + } + + internal void Disconnect(INode node) + { + for (var i = this.Outgoing.Count - 1; i >= 0; i--) + { + var edge = this.Outgoing[i]; + if (edge.End == node) + { + this.Outgoing.Remove(edge); + node.Incoming.Remove(edge); + } + } + } + + public override string ToString() => this.Position.ToString(); + } +} diff --git a/src/A/Grids/Grid.cs b/src/A/Grids/Grid.cs new file mode 100644 index 0000000..c6600ff --- /dev/null +++ b/src/A/Grids/Grid.cs @@ -0,0 +1,234 @@ +using System; +using System.Collections.Generic; +using Roy_T.AStar.Graphs; +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Grids +{ + public sealed class Grid + { + private readonly Node[,] Nodes; + + public static Grid CreateGridWithLateralConnections(GridSize gridSize, Size cellSize, Velocity traversalVelocity) + { + CheckArguments(gridSize, cellSize, traversalVelocity); + + var grid = new Grid(gridSize, cellSize); + + grid.CreateLateralConnections(traversalVelocity); + + return grid; + } + + public static Grid CreateGridWithDiagonalConnections(GridSize gridSize, Size cellSize, Velocity traversalVelocity) + { + CheckArguments(gridSize, cellSize, traversalVelocity); + + var grid = new Grid(gridSize, cellSize); + + grid.CreateDiagonalConnections(traversalVelocity); + + return grid; + } + + public static Grid CreateGridWithLateralAndDiagonalConnections(GridSize gridSize, Size cellSize, Velocity traversalVelocity) + { + CheckArguments(gridSize, cellSize, traversalVelocity); + + var grid = new Grid(gridSize, cellSize); + + grid.CreateDiagonalConnections(traversalVelocity); + grid.CreateLateralConnections(traversalVelocity); + + return grid; + } + + private static void CheckArguments(GridSize gridSize, Size cellSize, Velocity defaultSpeed) + { + if (gridSize.Columns < 1) + { + throw new ArgumentOutOfRangeException( + nameof(gridSize), $"Argument {nameof(gridSize.Columns)} is {gridSize.Columns} but should be >= 1"); + } + + if (gridSize.Rows < 1) + { + throw new ArgumentOutOfRangeException( + nameof(gridSize), $"Argument {nameof(gridSize.Rows)} is {gridSize.Rows} but should be >= 1"); + } + + if (cellSize.Width <= Distance.Zero) + { + throw new ArgumentOutOfRangeException( + nameof(cellSize), $"Argument {nameof(cellSize.Width)} is {cellSize.Width} but should be > {Distance.Zero}"); + } + + if (cellSize.Height <= Distance.Zero) + { + throw new ArgumentOutOfRangeException( + nameof(cellSize), $"Argument {nameof(cellSize.Height)} is {cellSize.Height} but should be > {Distance.Zero}"); + } + + if (defaultSpeed.MetersPerSecond <= 0.0f) + { + throw new ArgumentOutOfRangeException( + nameof(defaultSpeed), $"Argument {nameof(defaultSpeed)} is {defaultSpeed} but should be > 0.0 m/s"); + } + } + + private Grid(GridSize gridSize, Size cellSize) + { + this.GridSize = gridSize; + this.Nodes = new Node[gridSize.Columns, gridSize.Rows]; + + this.CreateNodes(cellSize); + } + + private void CreateNodes(Size cellSize) + { + for (var x = 0; x < this.Columns; x++) + { + for (var y = 0; y < this.Rows; y++) + { + this.Nodes[x, y] = new Node(Position.FromOffset(cellSize.Width * x, cellSize.Height * y)); + } + } + } + + private void CreateLateralConnections(Velocity defaultSpeed) + { + for (var x = 0; x < this.Columns; x++) + { + for (var y = 0; y < this.Rows; y++) + { + var node = this.Nodes[x, y]; + + if (x < this.Columns - 1) + { + var eastNode = this.Nodes[x + 1, y]; + node.Connect(eastNode, defaultSpeed); + eastNode.Connect(node, defaultSpeed); + } + + if (y < this.Rows - 1) + { + var southNode = this.Nodes[x, y + 1]; + node.Connect(southNode, defaultSpeed); + southNode.Connect(node, defaultSpeed); + } + } + } + } + + private void CreateDiagonalConnections(Velocity defaultSpeed) + { + for (var x = 0; x < this.Columns; x++) + { + for (var y = 0; y < this.Rows; y++) + { + var node = this.Nodes[x, y]; + + if (x < this.Columns - 1 && y < this.Rows - 1) + { + var southEastNode = this.Nodes[x + 1, y + 1]; + node.Connect(southEastNode, defaultSpeed); + southEastNode.Connect(node, defaultSpeed); + } + + if (x > 0 && y < this.Rows - 1) + { + var southWestNode = this.Nodes[x - 1, y + 1]; + node.Connect(southWestNode, defaultSpeed); + southWestNode.Connect(node, defaultSpeed); + } + } + } + } + + public GridSize GridSize { get; } + + public int Columns => this.GridSize.Columns; + + public int Rows => this.GridSize.Rows; + + public INode GetNode(GridPosition position) => this.Nodes[position.X, position.Y]; + + public IReadOnlyList GetAllNodes() + { + var list = new List(this.Columns * this.Rows); + + for (var x = 0; x < this.Columns; x++) + { + for (var y = 0; y < this.Rows; y++) + { + list.Add(this.Nodes[x, y]); + } + } + + return list; + } + + public void DisconnectNode(GridPosition position) + { + var node = this.Nodes[position.X, position.Y]; + + foreach (var outgoingEdge in node.Outgoing) + { + var opposite = outgoingEdge.End; + opposite.Incoming.Remove(outgoingEdge); + } + + node.Outgoing.Clear(); + + foreach (var incomingEdge in node.Incoming) + { + var opposite = incomingEdge.Start; + opposite.Outgoing.Remove(incomingEdge); + } + + node.Incoming.Clear(); + } + + public void RemoveDiagonalConnectionsIntersectingWithNode(GridPosition position) + { + var left = new GridPosition(position.X - 1, position.Y); + var top = new GridPosition(position.X, position.Y - 1); + var right = new GridPosition(position.X + 1, position.Y); + var bottom = new GridPosition(position.X, position.Y + 1); + + if (this.IsInsideGrid(left) && this.IsInsideGrid(top)) + { + this.RemoveEdge(left, top); + this.RemoveEdge(top, left); + } + + if (this.IsInsideGrid(top) && this.IsInsideGrid(right)) + { + this.RemoveEdge(top, right); + this.RemoveEdge(right, top); + } + + if (this.IsInsideGrid(right) && this.IsInsideGrid(bottom)) + { + this.RemoveEdge(right, bottom); + this.RemoveEdge(bottom, right); + } + + if (this.IsInsideGrid(bottom) && this.IsInsideGrid(left)) + { + this.RemoveEdge(bottom, left); + this.RemoveEdge(left, bottom); + } + } + + public void RemoveEdge(GridPosition from, GridPosition to) + { + var fromNode = this.Nodes[from.X, from.Y]; + var toNode = this.Nodes[to.X, to.Y]; + + fromNode.Disconnect(toNode); + } + + private bool IsInsideGrid(GridPosition position) => position.X >= 0 && position.X < this.Columns && position.Y >= 0 && position.Y < this.Rows; + } +} diff --git a/src/A/Paths/Path.cs b/src/A/Paths/Path.cs new file mode 100644 index 0000000..c5917cf --- /dev/null +++ b/src/A/Paths/Path.cs @@ -0,0 +1,28 @@ +using System.Collections.Generic; +using Roy_T.AStar.Graphs; +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Paths +{ + public sealed class Path + { + public Path(PathType type, IReadOnlyList edges) + { + this.Type = type; + this.Edges = edges; + + for (var i = 0; i < this.Edges.Count; i++) + { + this.Duration += this.Edges[i].TraversalDuration; + this.Distance += this.Edges[i].Distance; + } + } + + public PathType Type { get; } + + public Duration Duration { get; } + + public IReadOnlyList Edges { get; } + public Distance Distance { get; } + } +} diff --git a/src/A/Paths/PathFinder.cs b/src/A/Paths/PathFinder.cs new file mode 100644 index 0000000..27d5e53 --- /dev/null +++ b/src/A/Paths/PathFinder.cs @@ -0,0 +1,124 @@ +using System.Collections.Generic; +using System.Linq; +using Roy_T.AStar.Collections; +using Roy_T.AStar.Graphs; +using Roy_T.AStar.Grids; +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Paths +{ + public sealed class PathFinder + { + private readonly MinHeap Interesting; + private readonly Dictionary Nodes; + private readonly PathReconstructor PathReconstructor; + + private PathFinderNode NodeClosestToGoal; + + public PathFinder() + { + this.Interesting = new MinHeap(); + this.Nodes = new Dictionary(); + this.PathReconstructor = new PathReconstructor(); + } + + public Path FindPath(GridPosition start, GridPosition end, Grid grid) + { + var startNode = grid.GetNode(start); + var endNode = grid.GetNode(end); + + var maximumVelocity = grid.GetAllNodes().SelectMany(n => n.Outgoing).Select(e => e.TraversalVelocity).Max(); + + return this.FindPath(startNode, endNode, maximumVelocity); + } + + public Path FindPath(GridPosition start, GridPosition end, Grid grid, Velocity maximumVelocity) + { + var startNode = grid.GetNode(start); + var endNode = grid.GetNode(end); + + return this.FindPath(startNode, endNode, maximumVelocity); + } + + public Path FindPath(INode start, INode goal, Velocity maximumVelocity) + { + this.ResetState(); + this.AddFirstNode(start, goal, maximumVelocity); + + while (this.Interesting.Count > 0) + { + var current = this.Interesting.Extract(); + if (GoalReached(goal, current)) + { + return this.PathReconstructor.ConstructPathTo(current.Node, goal); + } + + this.UpdateNodeClosestToGoal(current); + + foreach (var edge in current.Node.Outgoing) + { + var oppositeNode = edge.End; + var costSoFar = current.DurationSoFar + edge.TraversalDuration; + + if (this.Nodes.TryGetValue(oppositeNode, out var node)) + { + this.UpdateExistingNode(goal, maximumVelocity, current, edge, oppositeNode, costSoFar, node); + } + else + { + this.InsertNode(oppositeNode, edge, goal, costSoFar, maximumVelocity); + } + } + } + + return this.PathReconstructor.ConstructPathTo(this.NodeClosestToGoal.Node, goal); + } + + private void ResetState() + { + this.Interesting.Clear(); + this.Nodes.Clear(); + this.PathReconstructor.Clear(); + this.NodeClosestToGoal = null; + } + + private void AddFirstNode(INode start, INode goal, Velocity maximumVelocity) + { + var head = new PathFinderNode(start, Duration.Zero, ExpectedDuration(start, goal, maximumVelocity)); + this.Interesting.Insert(head); + this.Nodes.Add(head.Node, head); + this.NodeClosestToGoal = head; + } + + private static bool GoalReached(INode goal, PathFinderNode current) => current.Node == goal; + + private void UpdateNodeClosestToGoal(PathFinderNode current) + { + if (current.ExpectedRemainingTime < this.NodeClosestToGoal.ExpectedRemainingTime) + { + this.NodeClosestToGoal = current; + } + } + + private void UpdateExistingNode(INode goal, Velocity maximumVelocity, PathFinderNode current, IEdge edge, INode oppositeNode, Duration costSoFar, PathFinderNode node) + { + if (node.DurationSoFar > costSoFar) + { + this.Interesting.Remove(node); + this.InsertNode(oppositeNode, edge, goal, costSoFar, maximumVelocity); + } + } + + private void InsertNode(INode current, IEdge via, INode goal, Duration costSoFar, Velocity maximumVelocity) + { + this.PathReconstructor.SetCameFrom(current, via); + + var node = new PathFinderNode(current, costSoFar, ExpectedDuration(current, goal, maximumVelocity)); + this.Interesting.Insert(node); + this.Nodes[current] = node; + } + + public static Duration ExpectedDuration(INode a, INode b, Velocity maximumVelocity) + => Distance.BeweenPositions(a.Position, b.Position) / maximumVelocity; + } +} diff --git a/src/A/Paths/PathFinderNode.cs b/src/A/Paths/PathFinderNode.cs new file mode 100644 index 0000000..370dfdc --- /dev/null +++ b/src/A/Paths/PathFinderNode.cs @@ -0,0 +1,25 @@ +using System; +using Roy_T.AStar.Graphs; +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Paths +{ + internal sealed class PathFinderNode : IComparable + { + public PathFinderNode(INode node, Duration durationSoFar, Duration expectedRemainingTime) + { + this.Node = node; + this.DurationSoFar = durationSoFar; + this.ExpectedRemainingTime = expectedRemainingTime; + this.ExpectedTotalTime = this.DurationSoFar + this.ExpectedRemainingTime; + } + + public INode Node { get; } + public Duration DurationSoFar { get; } + public Duration ExpectedRemainingTime { get; } + public Duration ExpectedTotalTime { get; } + + public int CompareTo(PathFinderNode other) => this.ExpectedTotalTime.CompareTo(other.ExpectedTotalTime); + public override string ToString() => $"📍{{{this.Node.Position.X}, {this.Node.Position.Y}}}, ⏱~{this.ExpectedTotalTime}"; + } +} diff --git a/src/A/Paths/PathReconstructor.cs b/src/A/Paths/PathReconstructor.cs new file mode 100644 index 0000000..400e2cf --- /dev/null +++ b/src/A/Paths/PathReconstructor.cs @@ -0,0 +1,37 @@ +using System.Collections.Generic; +using Roy_T.AStar.Graphs; + +namespace Roy_T.AStar.Paths +{ + internal sealed class PathReconstructor + { + private readonly Dictionary CameFrom; + + public PathReconstructor() + { + this.CameFrom = new Dictionary(); + } + + public void SetCameFrom(INode node, IEdge via) + => this.CameFrom[node] = via; + + public Path ConstructPathTo(INode node, INode goal) + { + var current = node; + var edges = new List(); + + while (this.CameFrom.TryGetValue(current, out var via)) + { + edges.Add(via); + current = via.Start; + } + + edges.Reverse(); + + var type = node == goal ? PathType.Complete : PathType.ClosestApproach; + return new Path(type, edges); + } + + public void Clear() => this.CameFrom.Clear(); + } +} diff --git a/src/A/Paths/PathType.cs b/src/A/Paths/PathType.cs new file mode 100644 index 0000000..8f1e891 --- /dev/null +++ b/src/A/Paths/PathType.cs @@ -0,0 +1,8 @@ +namespace Roy_T.AStar.Paths +{ + public enum PathType + { + Complete, + ClosestApproach + } +} diff --git a/src/A/Primitives/Distance.cs b/src/A/Primitives/Distance.cs new file mode 100644 index 0000000..25fbb4d --- /dev/null +++ b/src/A/Primitives/Distance.cs @@ -0,0 +1,73 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct Distance : IComparable, IEquatable + { + public static Distance Zero => new Distance(0); + + private Distance(float meters) + { + this.Meters = meters; + } + + public float Meters { get; } + + public static Distance FromMeters(float meters) => new Distance(meters); + + public static Distance BeweenPositions(Position a, Position b) + { + var sX = a.X; + var sY = a.Y; + var eX = b.X; + var eY = b.Y; + + var d0 = (eX - sX) * (eX - sX); + var d1 = (eY - sY) * (eY - sY); + + return FromMeters((float)Math.Sqrt(d0 + d1)); + } + + public static Distance operator +(Distance a, Distance b) + => new Distance(a.Meters + b.Meters); + + public static Distance operator -(Distance a, Distance b) + => new Distance(a.Meters - b.Meters); + + public static Distance operator *(Distance a, float b) + => new Distance(a.Meters * b); + + public static Distance operator /(Distance a, float b) + => new Distance(a.Meters / b); + + public static bool operator >(Distance a, Distance b) + => a.Meters > b.Meters; + + public static bool operator <(Distance a, Distance b) + => a.Meters < b.Meters; + + public static bool operator >=(Distance a, Distance b) + => a.Meters >= b.Meters; + + public static bool operator <=(Distance a, Distance b) + => a.Meters <= b.Meters; + + public static bool operator ==(Distance a, Distance b) + => a.Equals(b); + + public static bool operator !=(Distance a, Distance b) + => !a.Equals(b); + + public static Duration operator /(Distance distance, Velocity velocity) + => Duration.FromSeconds(distance.Meters / velocity.MetersPerSecond); + + public override string ToString() => $"{this.Meters:F2}m"; + + public override bool Equals(object obj) => obj is Distance distance && this.Equals(distance); + public bool Equals(Distance other) => this.Meters == other.Meters; + + public int CompareTo(Distance other) => this.Meters.CompareTo(other.Meters); + + public override int GetHashCode() => -1609761766 + this.Meters.GetHashCode(); + } +} diff --git a/src/A/Primitives/Duration.cs b/src/A/Primitives/Duration.cs new file mode 100644 index 0000000..fa381e3 --- /dev/null +++ b/src/A/Primitives/Duration.cs @@ -0,0 +1,52 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct Duration : IComparable, IEquatable + { + public static Duration Zero => new Duration(0); + + private Duration(float seconds) + { + this.Seconds = seconds; + } + + public float Seconds { get; } + + public static Duration FromSeconds(float seconds) => new Duration(seconds); + + public static Duration operator +(Duration a, Duration b) + => new Duration(a.Seconds + b.Seconds); + + public static Duration operator -(Duration a, Duration b) + => new Duration(a.Seconds - b.Seconds); + + public static bool operator >(Duration a, Duration b) + => a.Seconds > b.Seconds; + + public static bool operator <(Duration a, Duration b) + => a.Seconds < b.Seconds; + + public static bool operator >=(Duration a, Duration b) + => a.Seconds >= b.Seconds; + + public static bool operator <=(Duration a, Duration b) + => a.Seconds <= b.Seconds; + + public static bool operator ==(Duration a, Duration b) + => a.Equals(b); + + public static bool operator !=(Duration a, Duration b) + => !a.Equals(b); + + public override string ToString() => $"{this.Seconds:F2}s"; + + public override bool Equals(object obj) => obj is Duration duration && this.Equals(duration); + + public bool Equals(Duration other) => this.Seconds == other.Seconds; + + public int CompareTo(Duration other) => this.Seconds.CompareTo(other.Seconds); + + public override int GetHashCode() => -1609761766 + this.Seconds.GetHashCode(); + } +} diff --git a/src/A/Primitives/GridPosition.cs b/src/A/Primitives/GridPosition.cs new file mode 100644 index 0000000..956964a --- /dev/null +++ b/src/A/Primitives/GridPosition.cs @@ -0,0 +1,32 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct GridPosition : IEquatable + { + public static GridPosition Zero => new GridPosition(0, 0); + + public GridPosition(int x, int y) + { + this.X = x; + this.Y = y; + } + + public int X { get; } + public int Y { get; } + + public static bool operator ==(GridPosition a, GridPosition b) + => a.Equals(b); + + public static bool operator !=(GridPosition a, GridPosition b) + => !a.Equals(b); + + public override string ToString() => $"({this.X}, {this.Y})"; + + public override bool Equals(object obj) => obj is GridPosition GridPosition && this.Equals(GridPosition); + + public bool Equals(GridPosition other) => this.X == other.X && this.Y == other.Y; + + public override int GetHashCode() => -1609761766 + this.X + this.Y; + } +} diff --git a/src/A/Primitives/GridSize.cs b/src/A/Primitives/GridSize.cs new file mode 100644 index 0000000..6d0c056 --- /dev/null +++ b/src/A/Primitives/GridSize.cs @@ -0,0 +1,30 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct GridSize : IEquatable + { + public GridSize(int columns, int rows) + { + this.Columns = columns; + this.Rows = rows; + } + + public int Columns { get; } + public int Rows { get; } + + public static bool operator ==(GridSize a, GridSize b) + => a.Equals(b); + + public static bool operator !=(GridSize a, GridSize b) + => !a.Equals(b); + + public override string ToString() => $"(columns: {this.Columns}, rows: {this.Rows})"; + + public override bool Equals(object obj) => obj is GridSize GridSize && this.Equals(GridSize); + + public bool Equals(GridSize other) => this.Columns == other.Columns && this.Rows == other.Rows; + + public override int GetHashCode() => -1609761766 + this.Columns.GetHashCode() + this.Rows.GetHashCode(); + } +} diff --git a/src/A/Primitives/Position.cs b/src/A/Primitives/Position.cs new file mode 100644 index 0000000..51387c8 --- /dev/null +++ b/src/A/Primitives/Position.cs @@ -0,0 +1,35 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct Position : IEquatable + { + public static Position Zero => new Position(0, 0); + + public Position(float x, float y) + { + this.X = x; + this.Y = y; + } + + public static Position FromOffset(Distance xDistanceFromOrigin, Distance yDistanceFromOrigin) + => new Position(xDistanceFromOrigin.Meters, yDistanceFromOrigin.Meters); + + public float X { get; } + public float Y { get; } + + public static bool operator ==(Position a, Position b) + => a.Equals(b); + + public static bool operator !=(Position a, Position b) + => !a.Equals(b); + + public override string ToString() => $"({this.X:F2}, {this.Y:F2})"; + + public override bool Equals(object obj) => obj is Position position && this.Equals(position); + + public bool Equals(Position other) => this.X == other.X && this.Y == other.Y; + + public override int GetHashCode() => -1609761766 + this.X.GetHashCode() + this.Y.GetHashCode(); + } +} diff --git a/src/A/Primitives/Size.cs b/src/A/Primitives/Size.cs new file mode 100644 index 0000000..2304f36 --- /dev/null +++ b/src/A/Primitives/Size.cs @@ -0,0 +1,30 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct Size : IEquatable + { + public Size(Distance width, Distance height) + { + this.Width = width; + this.Height = height; + } + + public Distance Width { get; } + public Distance Height { get; } + + public static bool operator ==(Size a, Size b) + => a.Equals(b); + + public static bool operator !=(Size a, Size b) + => !a.Equals(b); + + public override string ToString() => $"(width: {this.Width}, height: {this.Height})"; + + public override bool Equals(object obj) => obj is Size Size && this.Equals(Size); + + public bool Equals(Size other) => this.Width == other.Width && this.Height == other.Height; + + public override int GetHashCode() => -1609761766 + this.Width.GetHashCode() + this.Height.GetHashCode(); + } +} diff --git a/src/A/Primitives/Velocity.cs b/src/A/Primitives/Velocity.cs new file mode 100644 index 0000000..2ee7122 --- /dev/null +++ b/src/A/Primitives/Velocity.cs @@ -0,0 +1,57 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct Velocity : IComparable, IEquatable + { + private Velocity(float metersPerSecond) + { + this.MetersPerSecond = metersPerSecond; + } + + public float MetersPerSecond { get; } + + public float KilometersPerHour => this.MetersPerSecond * 3.6f; + + + public static Velocity FromMetersPerSecond(float metersPerSecond) + => new Velocity(metersPerSecond); + + public static Velocity FromKilometersPerHour(float kilometersPerHour) + => new Velocity(kilometersPerHour / 3.6f); + + public static Velocity operator +(Velocity a, Velocity b) + => new Velocity(a.MetersPerSecond + b.MetersPerSecond); + + public static Velocity operator -(Velocity a, Velocity b) + => new Velocity(a.MetersPerSecond - b.MetersPerSecond); + + public static bool operator >(Velocity a, Velocity b) + => a.MetersPerSecond > b.MetersPerSecond; + + public static bool operator <(Velocity a, Velocity b) + => a.MetersPerSecond < b.MetersPerSecond; + + public static bool operator >=(Velocity a, Velocity b) + => a.MetersPerSecond >= b.MetersPerSecond; + + public static bool operator <=(Velocity a, Velocity b) + => a.MetersPerSecond <= b.MetersPerSecond; + + public static bool operator ==(Velocity a, Velocity b) + => a.Equals(b); + + public static bool operator !=(Velocity a, Velocity b) + => !a.Equals(b); + + public override string ToString() => $"{this.MetersPerSecond:F2} m/s"; + + public override bool Equals(object obj) => obj is Velocity velocity && this.MetersPerSecond == velocity.MetersPerSecond; + + public bool Equals(Velocity other) => this.MetersPerSecond == other.MetersPerSecond; + + public int CompareTo(Velocity other) => this.MetersPerSecond.CompareTo(other.MetersPerSecond); + + public override int GetHashCode() => -1419927970 + this.MetersPerSecond.GetHashCode(); + } +} diff --git a/src/BasicPathFinding.cs b/src/BasicPathFinding.cs index ecdc40f..54e2c2a 100644 --- a/src/BasicPathFinding.cs +++ b/src/BasicPathFinding.cs @@ -7,6 +7,9 @@ using OpenSim.Region.Framework.Interfaces; using OpenSim.Region.Framework.Scenes; using OpenSim.Region.ScriptEngine.Interfaces; using OpenSim.Region.ScriptEngine.Shared.Api; +using Roy_T.AStar.Grids; +using Roy_T.AStar.Paths; +using Roy_T.AStar.Primitives; using System; using System.Collections.Generic; using System.Drawing; @@ -122,6 +125,8 @@ namespace OpenSim.Modules.PathFinding m_scriptModule.RegisterConstant("PATH_ENV_ERR_NOT_FOUND", 19851); m_scriptModule.RegisterConstant("PATH_ENV_ERR_OUT_OF_RANGE", 19852); m_scriptModule.RegisterConstant("PATH_ENV_ERR_NOT_IN_LINE", 19853); + m_scriptModule.RegisterConstant("PATH_ENV_ERR_START_OR_END_UNKNOWN", 19854); + m_scriptModule.RegisterConstant("PATH_ENV_ERR_TARGET_NOT_CONNECTED", 19855); m_scriptModule.RegisterConstant("PATH_ENV_ERR_UNKNOWN", 19860); } catch (Exception e) @@ -212,7 +217,32 @@ namespace OpenSim.Modules.PathFinding } } - private void setPositionData(ScriptRequestData requestData, Vector3 position, int walkable, int isTarget) + private void removeAllStarts(ScriptRequestData requestData) + { + lock (m_environments) + { + try + { + Environment _env = m_environments.Find(X => X.ID == requestData.EnvironmentID); + + if (_env != null) + { + List _nodes = _env.Nodes.FindAll(X => X.Start == true); + + foreach (PathNode thisNode in _nodes) + { + thisNode.Start = false; + } + } + } + catch (Exception _error) + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19860, _error.Message, requestData.RequestID.ToString()); + } + } + } + + private void setPositionData(ScriptRequestData requestData, Vector3 position, int walkable, int isTarget, int isStart) { lock(m_environments) { @@ -225,11 +255,14 @@ namespace OpenSim.Modules.PathFinding if (isTarget == 1) removeAllTargets(requestData); + if (isStart == 1) + removeAllStarts(requestData); + PathNode _node = _env.Nodes.Find(X => X.PositionX == (int)position.X && X.PositionY == (int)position.Y); if (_node == null) { - _node = new PathNode((int)position.X, (int)position.Y, false, false); + _node = new PathNode((int)position.X, (int)position.Y, false, false, false); _env.Nodes.Add(_node); } @@ -245,6 +278,12 @@ namespace OpenSim.Modules.PathFinding if (isTarget == 0) _node.Target = false; + if (isStart == 1) + _node.Start = true; + + if (isStart == 0) + _node.Start = false; + return; } @@ -294,7 +333,7 @@ namespace OpenSim.Modules.PathFinding while ((int)_PointA.X <= (int)_PointB.X) { - setPositionData(requestData, _PointA, walkable, 0); + setPositionData(requestData, _PointA, walkable, 0, 0); _PointA.X = (int)_PointA.X + 1; } } @@ -315,7 +354,7 @@ namespace OpenSim.Modules.PathFinding while ((int)_PointA.Y <= (int)_PointB.Y) { - setPositionData(requestData, _PointA, walkable, 0); + setPositionData(requestData, _PointA, walkable, 0, 0); _PointA.Y = (int)_PointA.Y + 1; } } @@ -340,9 +379,76 @@ namespace OpenSim.Modules.PathFinding } } + private PathNode getStartNode(ScriptRequestData requestData) + { + lock (m_environments) + { + Environment _env = m_environments.Find(X => X.ID == requestData.EnvironmentID); + + if (_env != null) + { + PathNode _startNode = _env.Nodes.Find(X => X.Start == true); + return _startNode; + } + } + + return null; + } + + private PathNode getTargetNode(ScriptRequestData requestData) + { + lock (m_environments) + { + Environment _env = m_environments.Find(X => X.ID == requestData.EnvironmentID); + + if (_env != null) + { + PathNode _startNode = _env.Nodes.Find(X => X.Target == true); + return _startNode; + } + } + + return null; + } + private void generatePath(ScriptRequestData requestData) { + lock (m_environments) + { + Environment _env = m_environments.Find(X => X.ID == requestData.EnvironmentID); + PathNode _startNode = getStartNode(requestData); + PathNode _targetNode = getTargetNode(requestData); + + if(_startNode != null && _targetNode != null) + { + GridSize _pathFindingGridSize = new GridSize(_env.Size, _env.Size); + Roy_T.AStar.Primitives.Size _pathFindingCellSize = new Roy_T.AStar.Primitives.Size(Distance.FromMeters(1), Distance.FromMeters(1)); + Velocity _pathFindingVelocity = Velocity.FromKilometersPerHour(11.5f); + + Grid _pathFindingGrid = Grid.CreateGridWithLateralAndDiagonalConnections(_pathFindingGridSize, _pathFindingCellSize, _pathFindingVelocity); + PathFinder pathFinder = new PathFinder(); + Path _pathFindingPath = pathFinder.FindPath(new GridPosition(_startNode.PositionX, _startNode.PositionY), new GridPosition(_targetNode.PositionX, _targetNode.PositionY), _pathFindingGrid); + + if(_pathFindingPath.Type == PathType.Complete) + { + String _pathString = ""; + + foreach(var _thisEdge in _pathFindingPath.Edges) + _pathString += "<" + _thisEdge.End.Position.X + ", " + _thisEdge.End.Position.Y + ", 0>;"; + + m_scriptModule.DispatchReply(requestData.ScriptID, 19850, _pathString, requestData.RequestID.ToString()); + } + else + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19855, "", requestData.RequestID.ToString()); + } + } + else + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19854, "", requestData.RequestID.ToString()); + } + } } private void generateDebugImage(ScriptRequestData requestData) @@ -410,10 +516,10 @@ namespace OpenSim.Modules.PathFinding } [ScriptInvocation] - public void osSetPathPositionData(UUID hostID, UUID scriptID, String environmentID, Vector3 position, int walkable, int isTarget) + public void osSetPathPositionData(UUID hostID, UUID scriptID, String environmentID, Vector3 position, int walkable, int isTarget, int isStart) { SceneObjectGroup _host = m_scene.GetSceneObjectGroup(hostID); - (new Thread(delegate () { setPositionData(new ScriptRequestData(hostID, scriptID, environmentID), position, walkable, isTarget); })).Start(); + (new Thread(delegate () { setPositionData(new ScriptRequestData(hostID, scriptID, environmentID), position, walkable, isTarget, isStart); })).Start(); } [ScriptInvocation] diff --git a/src/PathNode.cs b/src/PathNode.cs index 6edbad4..292e03f 100644 --- a/src/PathNode.cs +++ b/src/PathNode.cs @@ -13,8 +13,9 @@ namespace OpenSim.Modules.PathFinding public bool Walkable = false; public bool Target = false; + public bool Start = false; - public int Coast = 0; + public int f_cost = 99999; public PathNode Parent = null; @@ -32,13 +33,14 @@ namespace OpenSim.Modules.PathFinding Walkable = isWalkable; } - public PathNode(int positionX, int positionY, bool isWalkable, bool isTarget) + public PathNode(int positionX, int positionY, bool isWalkable, bool isTarget, bool isStart) { PositionX = positionX; PositionY = positionY; Walkable = isWalkable; Target = isTarget; + Start = isStart; } } }