From 7ca29bfaa7a23d06374c2456e0360c911bd9aa3e Mon Sep 17 00:00:00 2001 From: Juan Linietsky Date: Sat, 22 Feb 2014 20:28:19 -0300 Subject: [PATCH] -added kinematic body -added kinematic body demos --- core/script_language.h | 2 + demos/2d/kinematic_char/colworld.gd | 17 + demos/2d/kinematic_char/colworld.scn | Bin 0 -> 5065 bytes demos/2d/kinematic_char/engine.cfg | 13 + demos/2d/kinematic_char/icon.png | Bin 0 -> 1513 bytes demos/2d/kinematic_char/obstacle.png | Bin 0 -> 490 bytes demos/2d/kinematic_char/player.gd | 116 ++++ demos/2d/kinematic_char/player.png | Bin 0 -> 502 bytes demos/2d/kinematic_char/player.scn | Bin 0 -> 1511 bytes demos/2d/kinematic_char/princess.png | Bin 0 -> 504 bytes demos/2d/kinematic_col/colworld.scn | Bin 0 -> 2941 bytes demos/2d/kinematic_col/engine.cfg | 12 + demos/2d/kinematic_col/icon.png | Bin 0 -> 1426 bytes demos/2d/kinematic_col/obstacle.png | Bin 0 -> 453 bytes demos/2d/kinematic_col/player.gd | 36 + demos/2d/kinematic_col/player.png | Bin 0 -> 502 bytes demos/2d/kinematic_col/player.scn | Bin 0 -> 1495 bytes scene/2d/physics_body_2d.cpp | 300 ++++----- scene/2d/physics_body_2d.h | 7 +- scene/main/node.cpp | 115 +++- scene/main/node.h | 3 + scene/register_scene_types.cpp | 3 +- script/gdscript/gd_editor.cpp | 8 + script/gdscript/gd_script.h | 2 + script/multiscript/multi_script.h | 1 + servers/physics_2d/body_2d_sw.h | 1 + .../physics_2d/broad_phase_2d_hash_grid.cpp | 3 +- servers/physics_2d/collision_object_2d_sw.cpp | 1 + servers/physics_2d/collision_object_2d_sw.h | 3 + .../physics_2d/collision_solver_2d_sat.cpp | 622 ++++++++++++------ servers/physics_2d/collision_solver_2d_sat.h | 2 +- servers/physics_2d/collision_solver_2d_sw.cpp | 18 +- servers/physics_2d/collision_solver_2d_sw.h | 4 +- servers/physics_2d/physics_2d_server_sw.cpp | 12 +- servers/physics_2d/physics_2d_server_sw.h | 13 +- servers/physics_2d/shape_2d_sw.h | 45 ++ servers/physics_2d/space_2d_sw.cpp | 309 +++++---- servers/physics_2d/space_2d_sw.h | 8 +- servers/physics_2d_server.cpp | 11 +- servers/physics_2d_server.h | 31 +- tools/doc/doc_data.cpp | 11 + tools/editor/editor_node.cpp | 1 + 42 files changed, 1171 insertions(+), 559 deletions(-) create mode 100644 demos/2d/kinematic_char/colworld.gd create mode 100644 demos/2d/kinematic_char/colworld.scn create mode 100644 demos/2d/kinematic_char/engine.cfg create mode 100644 demos/2d/kinematic_char/icon.png create mode 100644 demos/2d/kinematic_char/obstacle.png create mode 100644 demos/2d/kinematic_char/player.gd create mode 100644 demos/2d/kinematic_char/player.png create mode 100644 demos/2d/kinematic_char/player.scn create mode 100644 demos/2d/kinematic_char/princess.png create mode 100644 demos/2d/kinematic_col/colworld.scn create mode 100644 demos/2d/kinematic_col/engine.cfg create mode 100644 demos/2d/kinematic_col/icon.png create mode 100644 demos/2d/kinematic_col/obstacle.png create mode 100644 demos/2d/kinematic_col/player.gd create mode 100644 demos/2d/kinematic_col/player.png create mode 100644 demos/2d/kinematic_col/player.scn diff --git a/core/script_language.h b/core/script_language.h index 102d7c8436f..97312736100 100644 --- a/core/script_language.h +++ b/core/script_language.h @@ -31,6 +31,7 @@ #include "resource.h" #include "map.h" +#include "pair.h" /** @author Juan Linietsky */ @@ -157,6 +158,7 @@ public: virtual void get_recognized_extensions(List *p_extensions) const=0; virtual void get_public_functions(List *p_functions) const=0; + virtual void get_public_constants(List > *p_constants) const=0; virtual void frame(); diff --git a/demos/2d/kinematic_char/colworld.gd b/demos/2d/kinematic_char/colworld.gd new file mode 100644 index 00000000000..efd1dab8052 --- /dev/null +++ b/demos/2d/kinematic_char/colworld.gd @@ -0,0 +1,17 @@ + +extends Node2D + +# member variables here, example: +# var a=2 +# var b="textvar" + +func _ready(): + # Initalization here + pass + + + + +func _on_princess_body_enter( body ): + #the name of this editor-generated callback is unfortunate + get_node("youwin").show() diff --git a/demos/2d/kinematic_char/colworld.scn b/demos/2d/kinematic_char/colworld.scn new file mode 100644 index 0000000000000000000000000000000000000000..b1285b48118c799fde6ad921ad8bf1a25e034b1f GIT binary patch literal 5065 zcmWFvc6Md}0RaYvHyR8Kd)OHm7&sUh-ZC>VFfuSSFfuSUuq7}s@F=iy1tcbCr=|oa zr{<+9@IEkQU=UNVV_;TbX4KPbNMm4f;AUjtQ()yODN4*MPRlRK)i1~|P7vfPN-fSW zElN(+&r8frWe`?S7EUfLF3Hc0FV4>?OD&2|%1kU~;7|}_O|2+N%`0gzVJl89$}A|+ zZ(w6&U{{c3Flex6;7Z_PC{fU6DJ@E6;61?05ucxyR-9VGz@H$%Qk;=kka|Fffippf zwJ0?`Ge2(uPl7x{aY7h-PHJ9yNd^O}f(mm^ettm$2Xk>rYC*y-g)_{_*~R(_`An4s zsSj8M84D5>Oc?|eOdl{Pvt;I#q!vA3GiT0Dtt?I`=8R7&%}dEiO<`bBFmiaq5VGJP zgP;Pxq9Tu#zJ7jEaYA>I+?qH|Dz~Jb>?C^pyBvV0ONzOsqf!{$tS>8#8K|(>*fyY7O zp~y6cPzUA%3=G@~91HjpG961DgdeaoI4xz6QxI`TamZPi{y_JfLjkK}US@7$iPL$9 z%?>{tE;vYXgF^x90tW_$0}c+J4|&ZUnpjHJgB;twrN9Q_{s&3VL~;~eosV}-NXbLJ(^evG0F&G}A?4?TBxXDVh;Qjlbb zRbX~GcuXomjma-R#Z}CCD_2=&ab{9xPG(7^egYRmPJ4XE&U%;QGQ8E_5(&ihGMl0hSUV}_G=0P3`Ocs z5)QJ*$LFS&B*w?5TyR~>oLSXi!CY9Hn4%KFP|_gi@Qa~D^(I5S z`%k9{2|Uaxi6x1N9Br=>T9~~v^HN<6S(2UdQ!3Sr9p|&<=9gvWrN=9%GL$%MU@%PC zmLbU;pPXN+a>>;+f!~8Mfg>>_fzv_g0mID%zXwcWOquEF3gxa^iZeKKGE-9H%QH(d z^wsYvG%_-XD{gtfBF>O~U`|5Z0~RR-FUHK22b_jX$r*_nGd1!ODiaJDiq&}8^m7x7 z(lhfCxgIcxBq%XBJG46}F$6C+an(=Qm{@RNlT#`~v4$ICNooQYLvRC^Tk^>Q$0999 zg(`;Brred~4wkHGIhh6V84nn_7|IeXnaa|NG{1K~WXM>+?NFP*mEg~uo1apu*wRqE za=9~i0~cd_k@|WhKfFM&@YI0)Q0{#SU#*|d|Du&98Zr0rVyv&k(g)oNn zgka{%{L*qIMaKpfpTwlpoCNPCvxF}V#z}4r`U-m!mM-8^>Ru7HK!7PJzeM|3$|AwePfm^wqAwG46YM56zL%g!?AxFmeB$by8@d}E{@(ybsF!8y$J73nVaS>u~IV?z|bJv;mW|EAnm~HaL_^D#fu@hP11E)&o-5FXa0EAIZSt`a6HctpP0ayU!;1H zp;YBBL%eGmV`{OQ*8^sa2TUOjVh*z%>>b&dlS_+=6x9=g9qJE=_ib%Be?Z@{vSEVr z4PAG}^ppgBccljL2Mj9~NII-kKH^Z~z~vC*n9N*|S&^!Cogq*0wnOxRatAras(j@T zM+K(F4+pZ99JrbC(~6TOvKdO69sW_?l=a!cpTW2xikpLhfq@l@*{aeKix^lK92l5> zIkPITIdC(j<(DO}Gcz!lI5`M2a4B$dm1U-u7vvX}6f?*v@H(h6NGY&0uqiM*C^B#= zu(PLR7MCREC8s9HfJ$2j2MGrSmWueI{E`Kt4SEiu43!T!L>c%MWSL8gQ{xp1<92gwD7z7kJ9OSrC^U5-d^7C?2^GY7D$U97CDabEo;89S2z|8RA@A3yP zmpd6V@F{T|+Q}B5o1c=(z^ow1z^K4{K$aVp2XivhGn)Jl$U1%HD9*?)k54bkOi9W- zAnBBtAjz6vl$oBH_kcy(=@wH}Uh3jda4qAI$snOnC>9S&WhMDV@kOP1@x`enC7F5Y z#qse;^PQeMs5?Y*L7O3oMd`%}!Vc{mx;Y9e#h}(rf}nG*gUbVk3l0npb_|jV91j@M zm140f~ z4h+s|4h#-{4h#+-92gkb9Y9Jr6qp?t9F{mRIP7y^VBiLcIXrS;VBiI@92Yq-IRABE zaG37Ez#s?`VGshTXAlOnMZj!PFk1}F76-8%E;}$dayu|M|8rnqkOqk`$be;KL2Q?E z4h#(PAeJM014APt1ET_?0|Nuo0(J)G1?&tg3)mT07qBz1EnsJ0U%<}5v4EX{a{)U8 z*8+A1?gi`&JPX(vco(oU@GW3x;9tPbAh3X)L2v;(gU|wY2H^$l3?d8I8AKPbGl(r< zXAob&&LFXXok4N|JA>2$b_VGM>2-0iBT~SNe#%Q zjEb3{bOAOU6#6g|L7~K`pa_x!#S2&-D(4IeQ$}S@2L^b&z+}MTsSL6k7GqEuC$M=+ zAoHLi&LDdkl?&1QiJl5zs&|4yMHv(}Fq2_v0hR`!DjmQvr~vXUOau`x=xGFByui{G z)D#!6S`|jWnThU?A_ z+{~6joz4ml49*@09Go1yJl;4kx3Mra*fTs}V0ggD?I7>K;BkYQ>$)VSS-}JfcjxB&JFAgDhUou3=C@)@H4eJi7D_pFgW-yFe`94)HG-_GB`La zXVhR&NZ`vz%*$4Y%r8}NE=|hJ=W^iW3QjCbO;HHRNL2_}ki($3z?`vRzk|8M0j|9K zl+uRgA&%ky0tN-H1O^8_22KS|he8Jihgt_|hgt`A2UiCM2TuoOM?(i?hffa5 z4%QCJj%p6d512unR~`puhhztK2YUww2YZKT2YWbM`2l+}1CN4?gD|9SQ($+fXW$2s z511Jo>lg$S1RVMuq8)-Asvj^jI4Cm+DF`@BchGhyc3^lQJpDkkgR(=l!)^v)1rvwK z3?d5b4wD%~L8RkZhj0dQ1%3wxCpAz70;+bwJz53^X$2OCZg3Zzfk9S*1zbhXcDVgO zdbWeIqr8Ljf!Pdl3eFCb8RQk%!3yOa7#I{4Se*DBk{uzQkask8xb49HP<%Rrl7fkY zJ%h3Wy8{D*iUNy+H-oAIyF)R9+5&b4bp>`ub*G~a><;=2 zItuKL&JGM93mEhiSQvB_m>pI-+-A^M;C8TPFi>E3P-ZYxV0X-CFandtU>i&nSR4*B zm@2S4)-#wXupg{Hz~ETTV6MR95baR!up8_v>xbOzj=LYSXFK#eTz5F_SnP24kv_Xa zv;)JVVD zFx%n(Bk9!+$`9CQJFIt@?7-~M?7(m!*@+*N2%N8j6T$7r{QMvl$qt8IgdLb27@jzb zA7Ef`RN!EAU}#wFDDBYhaQ*@RYW3?0?F`Ne>@M>eT)?C&m~;b^?hDu%JQlDscrIXP z@B)X9H#nAk6qsF(Gx#d7J4!S7DX=?KGx#g8yIf}oP+)fuX9!eaci?9TQebzGX9!kc zcUaC4vVff-RDs>)KSP)TyNfeJxB|PAJwt>7yMsDI2B9SBO!V1 fHywBn9%YD8P-2KyVCDv8bkMk6P;ihlXb=wo3jhiO literal 0 HcmV?d00001 diff --git a/demos/2d/kinematic_char/engine.cfg b/demos/2d/kinematic_char/engine.cfg new file mode 100644 index 00000000000..0132442c188 --- /dev/null +++ b/demos/2d/kinematic_char/engine.cfg @@ -0,0 +1,13 @@ +[application] + +name="Kinematic Collision" +main_scene="res://colworld.scn" +icon="res://icon.png" + +[input] + +move_up=[key(Up)] +move_left=[key(Left)] +move_right=[key(Right)] +move_bottom=[key(Down)] +jump=[key(Space)] diff --git a/demos/2d/kinematic_char/icon.png b/demos/2d/kinematic_char/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..bdca104c1f4b9c26b7b96d3a41696cc2cf0d403c GIT binary patch literal 1513 zcmeAS@N?(olHy`uVBq!ia0y~yU~m9o4mJh`hElop|Qr*c0Dc1D&8$E zDod8M_*Jkl8O1T!vndrObWC*E!=)+Kc~U?~Xy*H6ySo-trG5V=xma}jnJV||m)_mE z^6J&=_4`(>&wqMyviia38qNHlrA!Z44sbRIGpIB8G0b7GU@Slg+&gyq-~0Lp-|zo( z*z)4^N6imUYaf_$@c+5QsUE*wA?v}_pPy#yU*kP6TW$Xx{*F5PM;sCc>i<7q-!G&q zFmX5cbw#BKN;Mx|{{HdgID^BxJz|YYFBZJea5?Y$MJwWM=)Xhi>QQUt8)klc>gXu) zrZMo?87mussdLlkX60(KuRqS<;J>V=OC-@k-$QW8iO^*hD>wxeCZ~UrJHCi9@_Jd= z1->I^npjszwVW)wZu{Y-1j7fPbv<2tUA7t>99B7!0*p$I5f}7IH0O3KW>A=X!SSNv z%Jt1khqm!3eNc+jl5Rcmq~Do&**#XarE{j9V{!C)x21$J_{zcGsljd6TO>p?)$~Y75k=mSlllA>F4>;hE2a=`NoaK zi@)BTA6`B6siA*uT)M%py~!V#;x@B9yIEJ*lsmg-_Sz3ypZw+B)e!T)?!e~s^A$fn zD8Adb=2_qNZF{fJdG_{{@jJilbC;LL-_>@QU;k~+HSYym7x*<+PxFg*5Yn^ukZTNI zVLE@~Y}4r{V`5?oDl0p83kz=Hb8u1G&AMFhVTVT39c$-*zMs}EOS^W;NarrYg0DyF zDnB;ekJ}d$GWCz=!?!Ela$28#e0dQSxx2M;d=I{H>ygswv^?jY0uW-$ule@3H z+HP<_`koA9m!#{|prU_j44i2iX=yFi0w&*^4Hoz{YQ5kv<1o8Z826yy;(6|@(mOkO z8`e~YrW*6i;JuO1@hb1#>CIdHu0%Pq*fTH~zco+q(&$-ibmlqhMgIxEOusfy)p@AX z8O>Nw?jkD9cC7HonSiS@#-2(n6MY25LiiXD#C$&N79e=;oT#qT&bl}OM?(dc?d#b8 zNim2VJR{Nekc(lr{EJsCl|MAvnHF%*>oWLqH#{}*;Grq6rYLRUx~$W+y^OK^&gGB@ z!!FSr58WG2>s@qY56y1oID7kV_k%Uh_I`UFbR*TlMPb#>$IcA9O8RfRUa+-4!WG7& z&3d(HGUHs`G`1s+5))iksxoR!JLXZo%hBZQzxz#v%AB)`tHm>F`2+&btXa<(>Z@MH z&2UzlCEuj-uehp#uR&f*;bvCHU1tT)R$jNwm~7OuQhI&j0iK2cugGb7suNh}FZXr2 z=yY)tkJtBF-_I)xCyLlJZ(d!slxwZ#gA=Ly{2x48CVhuhVe-7txNZ8+ttUALU1XAR zV@|tv_K`V%-YOmLAmNs^4);zrTuc9zQ@QC&)kT|j<%k&qf)y3^Q!FO1eYqJoW$E_a z+f_fl)-BfS4txKC>EiB&RY}ddTGG1F_xLzJoDj1xKFWWvAus3HM((-IZ5M15SX8zp zz0jO+nC;)M&+@w(RvGa)msjTQwBK#F{~_D$gKUaQ7S`d*RzCb|e`M$3+jaKmxj&z4 zUtV{I!C}jrD|T~VRjvJg=lDX9MAQ49xXr>xKRSe-e3Moqla_H^*`1xBO>3&am>pihH+N>i>S4E0YlSbA3QqRdfG*I|Dbd=8LV? zKiG0jZ)It4m*_8RxxfEaVvmaOttETyWr}Z@rQhXJFG9?p@lb-**2_K*dk_q}yxXoK3oF%5KlOeEW<= zs={F-I qXIJW7y7Tt+tGM+czodJP$X|PsBf9q1abpGs1_n=8KbLh*2~7a;>(2N9 literal 0 HcmV?d00001 diff --git a/demos/2d/kinematic_char/player.gd b/demos/2d/kinematic_char/player.gd new file mode 100644 index 00000000000..b35bbfa6938 --- /dev/null +++ b/demos/2d/kinematic_char/player.gd @@ -0,0 +1,116 @@ + +extends KinematicBody2D + +# This is a simple collision demo showing how +# the kinematic cotroller works. +# move() will allow to move the node, and will +# always move it to a non-colliding spot, +# as long as it starts from a non-colliding spot too. + + +#pixels / second +const GRAVITY = 500.0 + +# Angle in degrees towards either side that the player can +# consider "floor". +const FLOOR_ANGLE_TOLERANCE = 40 +const WALK_FORCE = 600 +const WALK_MAX_SPEED = 200 +const STOP_FORCE = 1300 +const JUMP_SPEED = 200 +const JUMP_MAX_AIRBORNE_TIME=0.2 + +var velocity = Vector2() +var on_air_time=100 +var jumping=false + +var prev_jump_pressed=false + +func _fixed_process(delta): + + #create forces + var force = Vector2(0,GRAVITY) + + var stop = velocity.x!=0.0 + + var walk_left = Input.is_action_pressed("move_left") + var walk_right = Input.is_action_pressed("move_right") + var jump = Input.is_action_pressed("jump") + + var stop=true + + if (walk_left): + if (velocity.x<=0 and velocity.x > -WALK_MAX_SPEED): + force.x-=WALK_FORCE + stop=false + + elif (walk_right): + if (velocity.x>=0 and velocity.x < WALK_MAX_SPEED): + force.x+=WALK_FORCE + stop=false + + if (stop): + var vsign = sign(velocity.x) + var vlen = abs(velocity.x) + + vlen -= STOP_FORCE * delta + if (vlen<0): + vlen=0 + + velocity.x=vlen*vsign + + + + #integrate forces to velocity + velocity += force * delta + + #integrate velocity into motion and move + var motion = velocity * delta + + #move and consume motion +# + motion = move(motion) + + + var floor_velocity=Vector2() + + if (is_colliding()): + #ran against something, is it the floor? get normal + var n = get_collision_normal() + + if ( rad2deg(acos(n.dot( Vector2(0,-1)))) < FLOOR_ANGLE_TOLERANCE ): + #if angle to the "up" vectors is < angle tolerance + #char is on floor + on_air_time=0 + floor_velocity=get_collider_velocity() + #velocity.y=0 + + # But we were moving and our motion was interrupted, + # so try to complete the motion by "sliding" + # by the normal + motion = n.slide(motion) + velocity = n.slide(velocity) + + #then move again + move(motion) + + if (floor_velocity!=Vector2()): + #if floor moves, move with floor + move(floor_velocity*delta) + + if (jumping and velocity.y>0): + jumping=false + + if (on_air_timelTRgyWxzDy`)NV=e)m{=D*L^5fpS`ImW`czuZhhsd@c*{gjL4uV;Fu1YHeL zHDft;ux;~-nCrKGFWV4p)33JJ;{4<{Q38jLF&P;D`SU0Ci`mg>MkInU=SM z^Ki-}uVvfkRB7;hdbMEJicMmiW5xOij#{n3rh4G*ccf&6=XT%lS=baa#B+mm=t&%^cX@E^jS)aQW^La_!W|Q ztn~E@auO?3i}bV@*cF5rf)gAcaLYP4FmNd-vgG6^C*~-aF&QxEE@XeeEXriakhDOQ zK~O=PKPWZ1Brz{NCp9=Du^`pRCBel(o`FR{#)08L2r~o20|y7@hwQ!ww3(zB7!u|= z$}+Gia65`H6*DL)a5AVUFgxsMILgQ%puorDotc-Kn^=;W?3ABUsZio*!&R19oSBrF zlUY)!pTNbC(-@q<-5|!CUyzurAjilcq2Ty{NsOg9H770pfHZ^916FC){Ji**{DQ`6UU{A213s6e~J0q$XK06gx_=#3$#M<~5ci@G}?d z8|Wu+Bt* zv8jqJJ~y=_F+M(V9&2$yQD#YM0vAJY1D8_|L&>5ZrX95NlYF-kJ1gmT5Frev1n7sVIklqxYW#5-j&rWPx{dcdslfGNm9 z%%Rf3-qD3QxwNQA@m)f&1H%FFmQRhm4pSY(oi9FNS;3f|k|6HTuejDB%7M#4%dwra zAhRMhC%!l@u^^$)fdS+X=BoVs+=L&@%PyR9a1d~0VaiV{PC3Bd>)miD=eXt}2XO~~ z2IGb(hqVqu4$RC|X^BM%#>@;1znmQe9PF9W^2-v~nHd;NoE(H5H?Wmurj{4v7pYEh zyyl?lu+%BsLDBI$V@jr?5mR1rYJvR4_Ks~ZZTEmr7jL-K6~bjtwX!Zq$dmsj76m?v8<^jC7F5YN8%k~ z9n>8nR|>1OF%%~VJG8Ut<|wEXtNAjdCI~v`I=DPwxZuFxVE2UQI7@O-YHFU!aR&t# zR+R*YK8G_doG*oJ9CRHR+*=tKQVU#58kk+CKVWBYSn0svI>jN^k=sp!t(ZZ9fq@}k zftlg|0tSZ~2L=Z|*IN$%9T*%69i*L4JFq*rIxskRIw-r=J19Hsa8P#SbWnDvbWnDf z>Y(iC?x4)T>tLh6;P}#k!6nIo!NuEw!EK&{vqQ6kvuBut^8;oE22lkbN5%){s~N-; z+#RNS>~>&qY;m~kxW!?$%W?;H25AKuhsz8y3)mTC71$k)yOui~XOLImcTjf>K5+g3 z1B0RhhXb?2YKQ9%@eT}*#*Y4u#t!UIHn*d(gR(=ogR)nvgEWJ(f{KH%1A~LHgRqBa Q<3)B*sU8#@l?=3#Df;Ffg`yx;TbdobH|MyH3QA zr)7W9CFA@y<_(X`=6smj!(71h&Es&%qoju==b1Mu1TFcq;*HksHS_J|zL{*De8+cQ z`Q*bEzIMXurSpy*e&X@#f7AK$=Igu93M%~k_D|MdnX{>WJsuyRF)x`(k^WN>a7q z21lcJY5%%ZkKb5kt(}zPnB!`@&Eb*iVYS&_uS^fHu9JGRnmhW!y<162SF|RF@$?=M z{v`FZQFZV0xb_!Hi&h+8I`=@sEtAEnWlTM zlzLLM;P|IMuTHgdO?!7Z=F%req2mc(ss*keh;yx0lM9tU&ouw-zbge&Jag9^eDVEU z%*}pVdDZs?v3<%ns%}r7w^&8yo}GH(z5|~np6)+jc{blmV)3z#H)}v~%;4$j=d#Wz Gp$Pz@f!@9V literal 0 HcmV?d00001 diff --git a/demos/2d/kinematic_col/colworld.scn b/demos/2d/kinematic_col/colworld.scn new file mode 100644 index 0000000000000000000000000000000000000000..064ff12075eeb348d460d2c64ebcd43e81beb544 GIT binary patch literal 2941 zcmWFvc6Md}0RaYva4`mkZ|n>V*~|cMmiW5xOij#{n3rh4G*cchu6=WF<8tfUk61W&j6tr1Ni&7bQ5Abrt=clC= zroz{44zRGOEPlbXW7q@d=|$`G<3 zia}6;Utt%ImA-y{QgKOQa!#tA)%Z5Nt!i3FTNzdAgO`7q$n}3I4!>@SHB>?Y0?8mL6)NY5(WVU z%Lj~t48@AC8B!D0v&YBhrj{he$2TS`_AvP-79_mlNKVbki7(EqN==AkE-XznDTi+6vc#fHg*gnx ziKiXh9FIGMIR!E^Ff`aZEIz={Al!JKfk8pqf!Xo;fdEEH21ile)RfGU{G#}RoYM5n zym;jRrjpcRMb!t)8crMx!O7nf#2ktpCOF(=;~gMh;; zru?+x#6|4ko|cCmT={U&LEIsV!MGvHVXcD@NO4+XQGziu1H&(82LT6rrnLOB1a@Wy z1`{U-VMi~vvdq--g8ZU|1rDkXOPvKA6dflrrerD_G36zvCdhz%>fj*ZpukuW@6_vH zXa~?ADIV3o9 zIG=dH%<$mv@&_-MJ7u`vIkb~8K3Byznf-8^gDiJSYFc7xPDy-DW_m`G{{dO2uk6Jc z`Q`EHMVal@2PB;m6C|1Pi!xQTl3EW)JAGiP%1d1w%6#_B8C!>Tw~QwY35-RhDzU7o zB_)}8=||$7jytG3M6Re}PAp0)`T$;erE$gTxb_<1EQV zsi}D?#~l=&Fzt5{a1L^qI6#h%I1e5MRK~AhCd* zL2>~*gVX|c2I&Rt3^EJY8Dtl*GsrDqXOLgO&Y-Y>-QlML0|TQX$Q4i#M?tWNB*>)@ zHIABK5kn*qN3e*e0|Nsq*aw7M3^m&!5Ts8b5@C`@m899_0P-WFLLx{PID|lP28$o4$qwLfRRD!6*gar% z=xR=aeR>gO4m9S`R5*g-j!}^llqR9^4>cbt-q7`c<3SPRMsyjFKN%H4aeyY{pa^ye z$jz|y2ue>-yO3fQO_p1MK@?Q~GcYiSF|dJHj0|E9Y>XJG8p$*!1_n@Wa&N#)`%wFs z7#JiJm>uk~r~rjDdhS5zl2l-JNGDmB6q+uCdD05Z4*E#uL1i34>6cM)q5}impHLYl z1_oKE9=hgxaOy(N`Jf!mr~rypy1D~Y{(#*A4G#x!d7}Vw4c*-VDhI)CQCPt4lFP`* z;IN*DfdRQGx}2YZp(wT3N?#w+xYN^8c*MoPU{UkTDM2aDnN@+CflYy#B`-fEwOGN2 ziGjgZftlg|0tSZw2L=Z|1`Y)d1}1R%&#b`WAPp+v9b6q496TMA8Q2wg9F!f59h4b3 z75EP*A7F53bzpXgX5dy3aWHpaa4>(M9PME3Q10-l}%WQgQ2Tly~24H-XyFSYI!x$~iaBaq=7% z?=Ehtcpan2_KfM?4;?#)J;mSR_}yNuJGIDuiBUlUukWn2I~KBWcDXK`ANN;o_P&}Q z3Cp-v97<}D?LS;OFQ8SHPvU<2pS$m_YH!KWS3jo6^L$nF!H+Y}eg0ZxV!S0!|LB9$ zzrS~U|E{34z{hw_PmYVrjXf2N<;Bg{-Y!*GuBgKN+(D_?@bd?T&0eb3a@xI7+onG< zzVqGSxkD}gJ1YSOW9ANrqjqi+JQhhA>#%Iei;r#C#izn_EAOC^!ZVk}T=N(eRk~#R zIUfooNNt-gWS|k;zDsXz_53`&cf!giED!L^dYQVo;qt1zGa{F=88-jkXP46>XUu#q z^a`hk-Hjc471;TU@*MnF64se4niJ&pPvz>X;I+5Cb;b6uSK7;T3boFdynjWLQI*tV zO;+h`;@pV~0%qP*|8h#f>tNdorC9E+V{eZ-dAuz0nQi3GzaS<;qS#Bd{cu37X^#F< zE2q8*V#!{@kJ+arOsZXeY1NX2ZEu&duzlNiiHX5PT!8J$r%g5uyZCnOx98MZdrO^d zMboz1EisNIR{~~4x?WFOqpIrl`QnDXSJr7x?+u&9cJ(2@7}%JaR2?nRi(#d!e4D&(IMskE->`S zii*$AzDr6NES0w}&hY83tf)Bfu%O}D^W!`XGo2Rq9_Ta(7BFm^^~JipIr!JhV)rxL zzh7Tg|MT}}F`psJ691yDd#?*6S?w&n6exE(V?&V+l3Vjf{Tv*j7iY6bttY=cZ=F1+)rTs78RyidnU21WKE}ryTE8-! zj%=8z7@L%>LZ}Fav=otPVkKf)F)pFu~#rQh7MeG`L82f=sRR^>rFTDEK zaDD&Yf3M2l|Et;VXPeOZac%d1#i|2QVsUKKTMO^$6sE{Tlr7s-7l$kH;p~! zy?e!#x1BlPZ1o-a5Ak{*Lm7@$G31*)*yC{T?SpUCJ63CdxGI{lojG^=g0pN_zOSw_ zH2U}aJZt0eDu(jdpyoa7xn`UlHY;BXdH>qC?NoM^<*yKdA1?eK_UUb6dT6V_^v#ZO zN`LlEd4sqg!440EZ5Na@#j=Z(u4uSe+VI_h?b*tz19u$X#|Z2Yv%jZ){~Uuz;t~d@ zo3<4^2fV(s&tYbY=GY)0_mc4{YnRuQ*oChcBUNHw|9Ix~V$yf^8g35z@9a8j+1Kzj i%&$7|KZgGY`&H{UGlPGN-5D4d7(8A5T-G@yGywpW(RU~S literal 0 HcmV?d00001 diff --git a/demos/2d/kinematic_col/obstacle.png b/demos/2d/kinematic_col/obstacle.png new file mode 100644 index 0000000000000000000000000000000000000000..693f115a98ed363fafbd3aa4ec1d3ac0c631c6d8 GIT binary patch literal 453 zcmeAS@N?(olHy`uVBq!ia0y~yU=RRd4mJh`2Kmqb6B!s7*pj^6T^Rm@;DWu&Co?cG za29w(7Bet#3xhBt!>lb ze|wkw?B@vKx?L~oZgX$a4Yz~$zn6z_rLn#H^Xp2NijL~PJ$o0!t!Mb|tudl2IH}jGoUuS*yqx9kNi+rBFf%Q|2r(L=mb*`MdY+i)dkJqhBTb6!F z(C6I7J2~NEwscaY*N?(2x9_TVpR`-l=oV04x9rC3C2SX}<=3v^QF-`oZN~k=8M!$w z3?{dJ-^*C`g<)grma=)5`Ar|qYnq!LJ@=$YzRyAt9VVqb<6cc3Ubb%b0}O@kulL2j z(ONq9etTah)6Ld=PCzp~YwiBsg{I->oB9)el-}igzopr E04ZI<;Q#;t literal 0 HcmV?d00001 diff --git a/demos/2d/kinematic_col/player.gd b/demos/2d/kinematic_col/player.gd new file mode 100644 index 00000000000..36784a9d9ff --- /dev/null +++ b/demos/2d/kinematic_col/player.gd @@ -0,0 +1,36 @@ + +extends KinematicBody2D + +# This is a simple collision demo showing how +# the kinematic cotroller works. +# move() will allow to move the node, and will +# always move it to a non-colliding spot, +# as long as it starts from a non-colliding spot too. + + +#pixels / second +const MOTION_SPEED=160 + +func _fixed_process(delta): + + var motion = Vector2() + + if (Input.is_action_pressed("move_up")): + motion+=Vector2(0,-1) + if (Input.is_action_pressed("move_bottom")): + motion+=Vector2(0,1) + if (Input.is_action_pressed("move_left")): + motion+=Vector2(-1,0) + if (Input.is_action_pressed("move_right")): + motion+=Vector2(1,0) + + motion = motion.normalized() * MOTION_SPEED * delta + move(motion) + + +func _ready(): + # Initalization here + set_fixed_process(true) + pass + + diff --git a/demos/2d/kinematic_col/player.png b/demos/2d/kinematic_col/player.png new file mode 100644 index 0000000000000000000000000000000000000000..0e7d843899f3f6c0ebca826205fbfb9539f1ca64 GIT binary patch literal 502 zcmeAS@N?(olHy`uVBq!ia0y~yU=RRd4mJh`2Kmqb6B!s7*pj^6T^Rm@;DWu&Co?cG za29w(7Bet#3xhBt!>lTRgyWxzDy`)NV=e)m{=D*L^5fpS`ImW`czuZhhsd@c*{gjL4uV;Fu1YHeL zHDft;ux;~-nCrKGFWV4p)33JJ;{4<{Q38jLF&P;D`SU0Ci`mg>MkInU=SM z^Ki-}uVvfkRB7;hdbMEJicMmiW5xOij#{n3rh4G*ccf&6=XT%lS=baa#B+mm=t&%^cX@E^jS)aQW^La_!W|Q ztn~E@auO?3i}bV@*cF5rf)gAcaLYP4FmNd-vgG6^C*~-aF&QxEE@XeeEXriakhDOQ zK~O=PKPWZ1Brz{NCp9=Du^`pRCBel(o`FR{#)08LhynwHqXY9pcHaZq3{na*4)Yvk z8Q2uK9YvUm8I%+_8B`RQ9V8o$GBOA#@bP$O=B4H)mSiS7<)>6ClsMXOm1P!ZCS~Si zmQ?B|a53aG1}AVgh%x6EBql4!F)~OfI6hz!V<}F}NsB)q%^>uERhl(FFTNzdAhD9W zq$n}3I4!>@SHB>?vHJm|AX8C(Ny798jDifsicSovNmdNSjuI^K$@!&ujU@^E%*FZ! z`UxBfyAn7ZgdQ;1Cq_SD5@X6tPgnTGAfhmbFF8LaCo?5AzC5!eL%+Dhk%f^#`~kZ- zL-s-Wgt!MRMhaexnJEuA4cU@25{nX(OB7NZuO=9B6=&w>>E|XErDx_PGBgz{+;nPe zs$z@JO)W``k58P(T3k?+S(2K-#Sq-U<r(hwmMilFgPt`U`UWZz~G?Wz;Hm>!JC=uy0VRfxr4p4(__Kk z&UFr%4%-+c6+F1(<5N>IOY)223vx=87#QN6G8t2g6<0s~Z!kk=M zRHXPWA=rW8fOyNNMqY=h4&siYPgqtkrl%x`JM=5Ab%=7{a?o;Y=PbyqNX>~a&Pyyv zD0E-|IiI;IKR-9&2Xjcfk%NPPBMVc0T5-w&_FnIXLpjGa4>^cC_%j$cL^-T=5OQE< zu1ZTRN-$<-VEE3uu68anz7%SpkbRCQw7##E*L>Vd{aEQ9@V=gUDjaNuz%g9g9 z&r8gC!1B(~#6gZdHLonQC_gXPF~_mNfx#ifA;D#ZYyJafh6jI_KX|#^N!X+G&`ySU zRa=JCWcI^t4zk=SscDI&IVJHqnduo#{s&~8zOof(z>r$tTGGJmGW`KNgF~Xje>WMXVg>~TIfi@%W`_R@7#s>57##Rq&pCW{ zU~ouvkaphh!0zDcz~JENpzK=gpzN^5LD}K2gR-NzgR(=XgR-NwgE9lJgN*`%<6Q>^ zmoNtg7kdW=_jeA?4$ThEo?Z^l511JkL=|`(zCJKt%^;@W?l9eBy#s?|g~MgX6%MOi zW;?JmNGr%VTxO72z|J76!0vF|HQV7hgS-O2gSunzf%69#7!(ya9GD$eJ6v~&cVKWd zcJy~Nc3_9F9h4o79h4oy9hAK)9i$nQ6;vFA9T*&p9fUnN8!xhhO7WoJAZJjK4ggA> ByHWrE literal 0 HcmV?d00001 diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index 166ee4daa82..afdca4a3bec 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -744,192 +744,149 @@ bool KinematicBody2D::_ignores_mode(Physics2DServer::BodyMode p_mode) const { return true; } -bool KinematicBody2D::is_trapped() const { - - ERR_FAIL_COND_V(!is_inside_scene(),false); - - Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space()); - ERR_FAIL_COND_V(!dss,false); - - const int max_shapes=32; - Physics2DDirectSpaceState::ShapeResult sr[max_shapes]; - - Set exclude; - exclude.insert(get_rid()); - - - for(int i=0;iintersect_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),sr,max_shapes,exclude); - - for(int j=0;jbody_get_mode(sr[j].rid); - if (!_ignores_mode(bm)) { - return true; //it's indeed trapped - } - - } - - } - - return false; - -} -void KinematicBody2D::untrap() { - - //this is reaaaaaaaaally wild, will probably only work for simple cases - ERR_FAIL_COND(!is_inside_scene()); - - Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space()); - ERR_FAIL_COND(!dss); - - const int max_shapes=32; - Physics2DDirectSpaceState::ShapeResult sr[max_shapes]; - const int max_contacts=8; - Vector2 pairs[max_contacts*2]; - - Set exclude; - exclude.insert(get_rid()); - - Vector2 untrap_vec; - - for(int i=0;iintersect_shape(get_shape(i)->get_rid(), shape_xform,Vector2(),sr,max_shapes,exclude); - - for(int j=0;jbody_get_mode(sr[j].rid); - if (_ignores_mode(bm)) { - exclude.insert(sr[j].rid); - } else { - - int rc; - bool c = Physics2DServer::get_singleton()->body_collide_shape(sr[j].rid,sr[j].shape,get_shape(i)->get_rid(),shape_xform,Vector2(),pairs,max_contacts,rc); - - if (c) { - - for(int k=0;kspace_get_direct_state(get_world_2d()->get_space()); ERR_FAIL_COND_V(!dss,Vector2()); const int max_shapes=32; - Physics2DDirectSpaceState::ShapeResult sr[max_shapes]; + Vector2 sr[max_shapes*2]; + int res_shapes; - float best_travel = 1e20; - Physics2DDirectSpaceState::MotionCastCollision mcc_final; Set exclude; exclude.insert(get_rid()); - print_line("pos: "+get_global_pos()); - print_line("mlen: "+p_motion); - if (!collide_static || ! collide_rigid || !collide_character || !collide_kinematic) { + //recover first + int recover_attempts=4; + + bool collided=false; + uint32_t mask=0; + if (collide_static) + mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY; + if (collide_kinematic) + mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY; + if (collide_rigid) + mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY; + if (collide_character) + mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY; + +// print_line("motion: "+p_motion+" margin: "+rtos(margin)); + + //print_line("margin: "+rtos(margin)); + do { + //fill exclude list.. for(int i=0;iintersect_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),p_motion,sr,max_shapes,exclude); + if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),margin,sr,max_shapes,res_shapes,exclude,0,mask)) + collided=true; - for(int j=0;jbody_get_mode(sr[j].rid); - if (_ignores_mode(bm)) { - exclude.insert(sr[j].rid); - } else { - // print_line("DANGER???"); - } - } } - } + + if (!collided) + break; + + Vector2 recover_motion; + + for(int i=0;icast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, mcc,exclude,0); - if (res==false) - continue; - if (mcc.travel<=0) { - //uh it's trapped - colliding=false; - return p_motion; + float lsafe,lunsafe; + bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, 0,lsafe,lunsafe,exclude,0,mask); + //print_line("shape: "+itos(i)+" travel:"+rtos(ltravel)); + if (!valid) { + safe=0; + unsafe=0; + best_shape=i; //sadly it's the best + break; } - if (mcc.travel < best_travel) { + if (lsafe==1.0) { + continue; + } + if (lsafe < safe) { - mcc_final=mcc; - best_travel=mcc.travel; + safe=lsafe; + unsafe=lunsafe; + best_shape=i; } } - float motion; - Vector2 motion_ret; - Vector2 push; - if (best_travel>1) { + + //print_line("best shape: "+itos(best_shape)+" motion "+p_motion); + + if (safe>=1) { //not collided colliding=false; - motion=p_motion.length(); //no stopped } else { - colliding=true; - collision=mcc_final.point; - normal=mcc_final.normal; - collider=mcc_final.collider_id; - Vector2 mnormal=p_motion.normalized(); - - float sine = Math::abs(mnormal.dot(normal)); - float retreat=0; - motion = p_motion.length()*mcc_final.travel; - - if (sine==0) { - //something odd going on, do not allow motion? - - retreat=motion; - + //it collided, let's get the rest info in unsafe advance + Matrix32 ugt = get_global_transform(); + ugt.elements[2]+=p_motion*unsafe; + Physics2DDirectSpaceState::ShapeRestInfo rest_info; + bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt*get_shape_transform(best_shape), Vector2(), margin,&rest_info,exclude,0,mask); + if (!c2) { + //should not happen, but floating point precision is so weird.. + colliding=false; } else { - retreat = margin/sine; - if (retreat>motion) - retreat=motion; + //print_line("Travel: "+rtos(travel)); + colliding=true; + collision=rest_info.point; + normal=rest_info.normal; + collider=rest_info.collider_id; + collider_vel=rest_info.linear_velocity; } - motion_ret=p_motion.normalized() * ( p_motion.length() - motion); - motion-=retreat; - - } + Vector2 motion=p_motion*safe; Matrix32 gt = get_global_transform(); - gt.elements[2]+=p_motion.normalized()*motion; + gt.elements[2]+=motion; set_global_transform(gt); - return motion_ret; + return p_motion-motion; } @@ -938,18 +895,31 @@ Vector2 KinematicBody2D::move_to(const Vector2& p_position) { return move(p_position-get_global_pos()); } -bool KinematicBody2D::can_move_to(const Vector2& p_position) { +bool KinematicBody2D::can_move_to(const Vector2& p_position, bool p_discrete) { ERR_FAIL_COND_V(!is_inside_scene(),false); Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space()); ERR_FAIL_COND_V(!dss,false); - const int max_shapes=32; - Physics2DDirectSpaceState::ShapeResult sr[max_shapes]; + uint32_t mask=0; + if (collide_static) + mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY; + if (collide_kinematic) + mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY; + if (collide_rigid) + mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY; + if (collide_character) + mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY; Vector2 motion = p_position-get_global_pos(); + Matrix32 xform=get_global_transform(); + + if (p_discrete) { + + xform.elements[2]+=motion; + motion=Vector2(); + } - Physics2DDirectSpaceState::MotionCastCollision mcc_final; Set exclude; exclude.insert(get_rid()); @@ -957,19 +927,9 @@ bool KinematicBody2D::can_move_to(const Vector2& p_position) { for(int i=0;iintersect_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),motion,sr,max_shapes,exclude); - - for(int j=0;jbody_get_mode(sr[j].rid); - if (_ignores_mode(bm)) { - exclude.insert(sr[j].rid); - continue; - } - - return false; //omg collided - - } + bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i),motion,0,NULL,0,exclude,0,mask); + if (col) + return false; } return true; @@ -993,6 +953,12 @@ Vector2 KinematicBody2D::get_collision_normal() const { return normal; } + +Vector2 KinematicBody2D::get_collider_velocity() const { + + return collider_vel; +} + ObjectID KinematicBody2D::get_collider() const { ERR_FAIL_COND_V(!colliding,0); @@ -1051,9 +1017,6 @@ float KinematicBody2D::get_collision_margin() const{ void KinematicBody2D::_bind_methods() { - ObjectTypeDB::bind_method(_MD("is_trapped"),&KinematicBody2D::is_trapped); - ObjectTypeDB::bind_method(_MD("untrap"),&KinematicBody2D::untrap); - ObjectTypeDB::bind_method(_MD("move","rel_vec"),&KinematicBody2D::move); ObjectTypeDB::bind_method(_MD("move_to","position"),&KinematicBody2D::move_to); @@ -1063,6 +1026,7 @@ void KinematicBody2D::_bind_methods() { ObjectTypeDB::bind_method(_MD("get_collision_pos"),&KinematicBody2D::get_collision_pos); ObjectTypeDB::bind_method(_MD("get_collision_normal"),&KinematicBody2D::get_collision_normal); + ObjectTypeDB::bind_method(_MD("get_collider_velocity"),&KinematicBody2D::get_collider_velocity); ObjectTypeDB::bind_method(_MD("get_collider:Object"),&KinematicBody2D::get_collider); @@ -1085,7 +1049,7 @@ void KinematicBody2D::_bind_methods() { ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/kinematic"),_SCS("set_collide_with_kinematic_bodies"),_SCS("can_collide_with_kinematic_bodies")); ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/rigid"),_SCS("set_collide_with_rigid_bodies"),_SCS("can_collide_with_rigid_bodies")); ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/character"),_SCS("set_collide_with_character_bodies"),_SCS("can_collide_with_character_bodies")); - ADD_PROPERTY( PropertyInfo(Variant::REAL,"collision/margin",PROPERTY_HINT_RANGE,"0.01,256,0.01"),_SCS("set_collision_margin"),_SCS("get_collision_margin")); + ADD_PROPERTY( PropertyInfo(Variant::REAL,"collision/margin",PROPERTY_HINT_RANGE,"0.001,256,0.001"),_SCS("set_collision_margin"),_SCS("get_collision_margin")); } @@ -1100,7 +1064,7 @@ KinematicBody2D::KinematicBody2D() : PhysicsBody2D(Physics2DServer::BODY_MODE_KI colliding=false; collider=0; - margin=1; + margin=0.01; } KinematicBody2D::~KinematicBody2D() { diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h index 6596c0ce04a..e7b65b1ef3e 100644 --- a/scene/2d/physics_body_2d.h +++ b/scene/2d/physics_body_2d.h @@ -243,6 +243,7 @@ class KinematicBody2D : public PhysicsBody2D { bool colliding; Vector2 collision; Vector2 normal; + Vector2 collider_vel; ObjectID collider; @@ -254,16 +255,14 @@ protected: static void _bind_methods(); public: - bool is_trapped() const; - void untrap(); - Vector2 move(const Vector2& p_motion); Vector2 move_to(const Vector2& p_position); - bool can_move_to(const Vector2& p_position); + bool can_move_to(const Vector2& p_position,bool p_discrete=false); bool is_colliding() const; Vector2 get_collision_pos() const; Vector2 get_collision_normal() const; + Vector2 get_collider_velocity() const; ObjectID get_collider() const; void set_collide_with_static_bodies(bool p_enable); diff --git a/scene/main/node.cpp b/scene/main/node.cpp index 1f54040de41..5ac09e837ff 100644 --- a/scene/main/node.cpp +++ b/scene/main/node.cpp @@ -462,7 +462,7 @@ void Node::_set_name_nocheck(const StringName& p_name) { void Node::set_name(const String& p_name) { - String name=p_name.replace(":","").replace("/",""); + String name=p_name.replace(":","").replace("/","").replace("@",""); ERR_FAIL_COND(name==""); data.name=name; @@ -479,45 +479,97 @@ void Node::set_name(const String& p_name) { } } +static bool node_hrcr=false; +static SafeRefCount node_hrcr_count; + +void Node::init_node_hrcr() { + node_hrcr_count.init(1); +} + +void Node::set_human_readable_collision_renaming(bool p_enabled) { + + node_hrcr=p_enabled; +} + + void Node::_validate_child_name(Node *p_child) { /* Make sure the name is unique */ - String basename = p_child->data.name; - if (basename=="") { - - basename = p_child->get_type(); - } - - int val=1; - - for(;;) { - - String attempted = val > 1 ? (basename + " " +itos(val) ) : basename; + if (node_hrcr) { + + //this approach to autoset node names is human readable but very slow + //it's turned on while running in the editor + + String basename = p_child->data.name; + + if (basename=="") { + + basename = p_child->get_type(); + } + + int val=1; + + for(;;) { + + String attempted = val > 1 ? (basename + " " +itos(val) ) : basename; + + bool found=false; + + for (int i=0;iget_name() == attempted) { + found=true; + break; + } - bool found=false; - - for (int i=0;iget_name() == attempted) { - found=true; - break; } - + + if (found) { + + val++; + continue; + } + + p_child->data.name=attempted; + break; } - - if (found) { - - val++; - continue; + } else { + + //this approach to autoset node names is fast but not as readable + //it's the default and reserves the '@' character for unique names. + + bool unique=true; + + if (p_child->data.name==StringName() || p_child->data.name.operator String()[0]=='@') { + //new unique name must be assigned + unique=false; + } else { + //check if exists + Node **childs=data.children.ptr(); + int cc = data.children.size(); + + for(int i=0;idata.name==p_child->data.name) { + unique=false; + break; + } + } + } + + if (!unique) { + + node_hrcr_count.ref(); +#ifdef DEBUG_ENABLED + String name = "@"+String(p_child->get_type_name())+itos(node_hrcr_count.get()); +#else + String name = "@"+itos(node_hrcr_count.get()); +#endif + p_child->data.name=name; } - - p_child->data.name=attempted; - break; } - } void Node::_add_child_nocheck(Node* p_child,const StringName& p_name) { @@ -541,6 +593,7 @@ void Node::_add_child_nocheck(Node* p_child,const StringName& p_name) { } + void Node::add_child(Node *p_child) { ERR_FAIL_NULL(p_child); diff --git a/scene/main/node.h b/scene/main/node.h index f38a7821816..ec03fb19e81 100644 --- a/scene/main/node.h +++ b/scene/main/node.h @@ -261,6 +261,9 @@ public: void queue_delete(); + static void set_human_readable_collision_renaming(bool p_enabled); + static void init_node_hrcr(); + /* CANVAS */ Node(); diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp index ae67023d5b5..007ecb88b77 100644 --- a/scene/register_scene_types.cpp +++ b/scene/register_scene_types.cpp @@ -223,6 +223,7 @@ void register_scene_types() { OS::get_singleton()->yield(); //may take time to init + Node::init_node_hrcr(); #ifdef OLD_SCENE_FORMAT_ENABLED ObjectTypeDB::register_type(); @@ -452,7 +453,7 @@ void register_scene_types() { ObjectTypeDB::register_virtual_type(); ObjectTypeDB::register_type(); ObjectTypeDB::register_type(); - //ObjectTypeDB::register_type(); + ObjectTypeDB::register_type(); ObjectTypeDB::register_type(); ObjectTypeDB::register_type(); ObjectTypeDB::register_type(); diff --git a/script/gdscript/gd_editor.cpp b/script/gdscript/gd_editor.cpp index 4bb5d3206cb..c10cadf83f8 100644 --- a/script/gdscript/gd_editor.cpp +++ b/script/gdscript/gd_editor.cpp @@ -282,6 +282,14 @@ void GDScriptLanguage::get_public_functions(List *p_functions) const } } +void GDScriptLanguage::get_public_constants(List > *p_constants) const { + + Pair pi; + pi.first="PI"; + pi.second=Math_PI; + p_constants->push_back(pi); +} + String GDScriptLanguage::make_function(const String& p_class,const String& p_name,const StringArray& p_args) const { String s="func "+p_name+"("; diff --git a/script/gdscript/gd_script.h b/script/gdscript/gd_script.h index bb9beaaf568..70dec4e8eec 100644 --- a/script/gdscript/gd_script.h +++ b/script/gdscript/gd_script.h @@ -440,6 +440,8 @@ public: virtual void frame(); virtual void get_public_functions(List *p_functions) const; + virtual void get_public_constants(List > *p_constants) const; + /* LOADER FUNCTIONS */ virtual void get_recognized_extensions(List *p_extensions) const; diff --git a/script/multiscript/multi_script.h b/script/multiscript/multi_script.h index a67cedc56be..87d4b4e4c88 100644 --- a/script/multiscript/multi_script.h +++ b/script/multiscript/multi_script.h @@ -148,6 +148,7 @@ public: virtual void get_recognized_extensions(List *p_extensions) const {} virtual void get_public_functions(List *p_functions) const {} + virtual void get_public_constants(List > *p_constants) const {} MultiScriptLanguage() { singleton=this; } virtual ~MultiScriptLanguage() {}; diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index 14cae3dbb02..ffe47e0267d 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -62,6 +62,7 @@ class Body2DSW : public CollisionObject2DSW { Vector2 applied_force; real_t applied_torque; + SelfList active_list; SelfList inertia_update_list; SelfList direct_state_query_list; diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp index 129c9ecb9c6..0f08f63937b 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp +++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp @@ -434,8 +434,9 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell,const Rect2& p_aabb,const if (E->key()->pass==pass) continue; - if (use_aabb && !p_aabb.intersects(E->key()->aabb)) + if (use_aabb && !p_aabb.intersects(E->key()->aabb)) { continue; + } if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to)) continue; diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp index 3a5c8c3ade0..e07dca472be 100644 --- a/servers/physics_2d/collision_object_2d_sw.cpp +++ b/servers/physics_2d/collision_object_2d_sw.cpp @@ -218,4 +218,5 @@ CollisionObject2DSW::CollisionObject2DSW(Type p_type) { type=p_type; space=NULL; instance_id=0; + user_mask=0; } diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h index 74457cfa0a1..8138cfcc693 100644 --- a/servers/physics_2d/collision_object_2d_sw.h +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -65,6 +65,7 @@ private: Space2DSW *space; Matrix32 transform; Matrix32 inv_transform; + uint32_t user_mask; bool _static; void _update_shapes(); @@ -117,6 +118,8 @@ public: _FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; } + void set_user_mask(uint32_t p_mask) {user_mask=p_mask;} + _FORCE_INLINE_ uint32_t get_user_mask() const { return user_mask; } void remove_shape(Shape2DSW *p_shape); void remove_shape(int p_index); diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp index fdbbebefcfa..7d85183645a 100644 --- a/servers/physics_2d/collision_solver_2d_sat.cpp +++ b/servers/physics_2d/collision_solver_2d_sat.cpp @@ -321,7 +321,7 @@ static void _generate_contacts_from_supports(const Vector2 * p_points_A,int p_po -template +template class SeparatorAxisTest2D { const ShapeA *shape_A; @@ -334,6 +334,8 @@ class SeparatorAxisTest2D { int best_axis_index; Vector2 motion_A; Vector2 motion_B; + real_t margin_A; + real_t margin_B; _CollectorCallback2D *callback; public: @@ -397,6 +399,13 @@ public: else shape_B->project_range(axis,*transform_B,min_B,max_B); + if (withMargin) { + min_A-=margin_A; + max_A+=margin_A; + min_B-=margin_B; + max_B+=margin_B; + } + min_B -= ( max_A - min_A ) * 0.5; max_B += ( max_A - min_A ) * 0.5; @@ -468,6 +477,14 @@ public: } } + if (withMargin) { + + for(int i=0;ixform(supports_B[i]); } } + + if (withMargin) { + + for(int i=0;i -static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const SegmentShape2DSW *segment_A = static_cast(p_a); const SegmentShape2DSW *segment_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,segment_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(segment_A,p_transform_a,segment_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -570,24 +599,39 @@ static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_tr if (!separator.test_cast()) return; + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) return; if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b))) return; + if (withMargin) { + //points grow to circles + + + if (TEST_POINT( p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(segment_B->get_a())) ) + return; + if (TEST_POINT( p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(segment_B->get_b())) ) + return; + if (TEST_POINT( p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(segment_B->get_a())) ) + return; + if (TEST_POINT( p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(segment_B->get_b())) ) + return; + } + separator.generate_contacts(); } -template -static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const SegmentShape2DSW *segment_A = static_cast(p_a); const CircleShape2DSW *circle_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(segment_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -612,13 +656,13 @@ static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_tra separator.generate_contacts(); } -template -static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const SegmentShape2DSW *segment_A = static_cast(p_a); const RectangleShape2DSW *rectangle_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(segment_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -635,17 +679,55 @@ static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_ if (!separator.test_axis(p_transform_b.elements[1].normalized())) return; + if (withMargin) { + + Matrix32 inv = p_transform_b.affine_inverse(); + + Vector2 a = p_transform_a.xform(segment_A->get_a()); + Vector2 b = p_transform_a.xform(segment_A->get_b()); + + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a))) + return; + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b))) + return; + + if (castA) { + + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a+p_motion_a))) + return; + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b+p_motion_a))) + return; + } + + if (castB) { + + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a-p_motion_b))) + return; + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b-p_motion_b))) + return; + } + + if (castA && castB) { + + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a-p_motion_b+p_motion_a))) + return; + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b-p_motion_b+p_motion_a))) + return; + } + + } + separator.generate_contacts(); } -template -static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const SegmentShape2DSW *segment_A = static_cast(p_a); const CapsuleShape2DSW *capsule_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -671,13 +753,13 @@ static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_tr separator.generate_contacts(); } -template -static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const SegmentShape2DSW *segment_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(segment_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -692,6 +774,16 @@ static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix3 if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) return; + + if (withMargin) { + + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(convex_B->get_point(i) ))) + return; + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(convex_B->get_point(i) ))) + return; + + } + } separator.generate_contacts(); @@ -701,14 +793,14 @@ static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix3 ///////// -template -static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const CircleShape2DSW *circle_A = static_cast(p_a); const CircleShape2DSW *circle_B = static_cast(p_b); - SeparatorAxisTest2D separator(circle_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(circle_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -724,14 +816,14 @@ static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_tran } -template -static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const CircleShape2DSW *circle_A = static_cast(p_a); const RectangleShape2DSW *rectangle_B = static_cast(p_b); - SeparatorAxisTest2D separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -741,7 +833,7 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_t const Vector2 &sphere=p_transform_a.elements[2]; const Vector2 *axis=&p_transform_b.elements[0]; - const Vector2& half_extents = rectangle_B->get_half_extents(); +// const Vector2& half_extents = rectangle_B->get_half_extents(); if (!separator.test_axis(axis[0].normalized())) return; @@ -749,75 +841,45 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_t if (!separator.test_axis(axis[1].normalized())) return; + Matrix32 binv = p_transform_b.affine_inverse(); { - Vector2 local_v = p_transform_b.affine_inverse().xform(p_transform_a.get_origin()); - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - - if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized())) + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv,sphere ) ) ) return; } if (castA) { Vector2 sphereofs = sphere + p_motion_a; - Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - - if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized())) + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) ) return; } if (castB) { Vector2 sphereofs = sphere - p_motion_b; - Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - - if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized())) + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) ) return; } if (castA && castB) { Vector2 sphereofs = sphere - p_motion_b + p_motion_a; - Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - - if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized())) + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) ) return; } separator.generate_contacts(); } -template -static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const CircleShape2DSW *circle_A = static_cast(p_a); const CapsuleShape2DSW *capsule_B = static_cast(p_b); - SeparatorAxisTest2D separator(circle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(circle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -840,14 +902,14 @@ static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_tra } -template -static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const CircleShape2DSW *circle_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -872,14 +934,14 @@ static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32 ///////// -template -static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const RectangleShape2DSW *rectangle_A = static_cast(p_a); const RectangleShape2DSW *rectangle_B = static_cast(p_b); - SeparatorAxisTest2D separator(rectangle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(rectangle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -901,17 +963,56 @@ static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& if (!separator.test_axis(p_transform_b.elements[1].normalized())) return; + if (withMargin) { + + Matrix32 invA=p_transform_a.affine_inverse(); + Matrix32 invB=p_transform_b.affine_inverse(); + + if (!separator.test_axis( rectangle_A->get_box_axis(p_transform_a,invA,rectangle_B,p_transform_b,invB) ) ) + return; + + if (castA || castB) { + + Matrix32 aofs = p_transform_a; + aofs.elements[2]+=p_motion_a; + + Matrix32 bofs = p_transform_b; + bofs.elements[2]+=p_motion_b; + + Matrix32 aofsinv = aofs.affine_inverse(); + Matrix32 bofsinv = bofs.affine_inverse(); + + if (castA) { + + if (!separator.test_axis( rectangle_A->get_box_axis(aofs,aofsinv,rectangle_B,p_transform_b,invB) ) ) + return; + } + + if (castB) { + + if (!separator.test_axis( rectangle_A->get_box_axis(p_transform_a,invA,rectangle_B,bofs,bofsinv) ) ) + return; + } + + if (castA && castB) { + + if (!separator.test_axis( rectangle_A->get_box_axis(aofs,aofsinv,rectangle_B,bofs,bofsinv) ) ) + return; + } + } + } + separator.generate_contacts(); } -template -static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const RectangleShape2DSW *rectangle_A = static_cast(p_a); const CapsuleShape2DSW *capsule_B = static_cast(p_b); - SeparatorAxisTest2D separator(rectangle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(rectangle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -940,15 +1041,7 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_ { Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5); - const Vector2& half_extents = rectangle_A->get_half_extents(); - Vector2 local_v = boxinv.xform(capsule_endpoint); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized())) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) return; } @@ -957,16 +1050,7 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_ Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5); capsule_endpoint-=p_motion_a; - - const Vector2& half_extents = rectangle_A->get_half_extents(); - Vector2 local_v = boxinv.xform(capsule_endpoint); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized())) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) return; } @@ -974,16 +1058,7 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_ Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5); capsule_endpoint+=p_motion_b; - - const Vector2& half_extents = rectangle_A->get_half_extents(); - Vector2 local_v = boxinv.xform(capsule_endpoint); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized())) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) return; } @@ -993,15 +1068,7 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_ capsule_endpoint+=p_motion_b; - const Vector2& half_extents = rectangle_A->get_half_extents(); - Vector2 local_v = boxinv.xform(capsule_endpoint); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized())) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) return; } @@ -1011,13 +1078,13 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_ separator.generate_contacts(); } -template -static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const RectangleShape2DSW *rectangle_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -1033,10 +1100,36 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri return; //convex faces + Matrix32 boxinv; + if (withMargin) { + boxinv=p_transform_a.affine_inverse(); + } for(int i=0;iget_point_count();i++) { if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) return; + + if (withMargin) { + //all points vs all points need to be tested if margin exist + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))))) + return; + if (castA) { + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))-p_motion_a))) + return; + } + if (castB) { + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b))) + return; + } + if (castA && castB) { + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b-p_motion_a))) + return; + } + + } } separator.generate_contacts(); @@ -1046,14 +1139,14 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri ///////// -template -static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const CapsuleShape2DSW *capsule_A = static_cast(p_a); const CapsuleShape2DSW *capsule_B = static_cast(p_b); - SeparatorAxisTest2D separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -1089,14 +1182,14 @@ static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_tr } -template -static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const CapsuleShape2DSW *capsule_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(capsule_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(capsule_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -1135,14 +1228,14 @@ static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix3 ///////// -template -static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const ConvexPolygonShape2DSW *convex_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(convex_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D separator(convex_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B); if (!separator.test_previous_axis()) return; @@ -1161,6 +1254,19 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) return; + + } + + if (withMargin) { + + for(int i=0;iget_point_count();i++) { + for(int j=0;jget_point_count();j++) { + + if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)) , p_transform_b.xform(convex_B->get_point(j)))) + return; + } + } + } separator.generate_contacts(); @@ -1170,7 +1276,7 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const //////// -bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis) { +bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis,float p_margin_A,float p_margin_B) { Physics2DServer::ShapeType type_A=p_shape_A->get_type(); @@ -1186,121 +1292,238 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_ static const CollisionFunc collision_table[5][5]={ - {_collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon}, + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, {0, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon}, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, {0, 0, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon}, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, {0, 0, 0, - _collision_capsule_capsule, - _collision_capsule_convex_polygon}, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, {0, 0, 0, 0, - _collision_convex_polygon_convex_polygon} + _collision_convex_polygon_convex_polygon} }; static const CollisionFunc collision_table_castA[5][5]={ - {_collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon}, + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, {0, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon}, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, {0, 0, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon}, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, {0, 0, 0, - _collision_capsule_capsule, - _collision_capsule_convex_polygon}, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, {0, 0, 0, 0, - _collision_convex_polygon_convex_polygon} + _collision_convex_polygon_convex_polygon} }; static const CollisionFunc collision_table_castB[5][5]={ - {_collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon}, + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, {0, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon}, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, {0, 0, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon}, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, {0, 0, 0, - _collision_capsule_capsule, - _collision_capsule_convex_polygon}, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, {0, 0, 0, 0, - _collision_convex_polygon_convex_polygon} + _collision_convex_polygon_convex_polygon} }; static const CollisionFunc collision_table_castA_castB[5][5]={ - {_collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon}, + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, {0, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon}, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, {0, 0, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon}, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, {0, 0, 0, - _collision_capsule_capsule, - _collision_capsule_convex_polygon}, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, {0, 0, 0, 0, - _collision_convex_polygon_convex_polygon} + _collision_convex_polygon_convex_polygon} }; + static const CollisionFunc collision_table_margin[5][5]={ + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, + {0, + 0, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, + {0, + 0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon} + + }; + + static const CollisionFunc collision_table_castA_margin[5][5]={ + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, + {0, + 0, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, + {0, + 0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon} + + }; + + static const CollisionFunc collision_table_castB_margin[5][5]={ + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, + {0, + 0, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, + {0, + 0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon} + + }; + + static const CollisionFunc collision_table_castA_castB_margin[5][5]={ + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, + {0, + 0, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, + {0, + 0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon} + + }; + + _CollectorCallback2D callback; callback.callback=p_result_callback; callback.swap=p_swap; @@ -1314,32 +1537,49 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_ const Matrix32 *transform_B=&p_transform_B; const Vector2 *motion_A=&p_motion_A; const Vector2 *motion_B=&p_motion_B; + real_t margin_A=p_margin_A,margin_B=p_margin_B; if (type_A > type_B) { SWAP(A,B); SWAP(transform_A,transform_B); SWAP(type_A,type_B); SWAP(motion_A,motion_B); + SWAP(margin_A,margin_B); callback.swap = !callback.swap; } CollisionFunc collision_func; - if (*motion_A==Vector2() && *motion_B==Vector2()) { - collision_func = collision_table[type_A-2][type_B-2]; - } else if (*motion_A!=Vector2() && *motion_B==Vector2()) { - collision_func = collision_table_castA[type_A-2][type_B-2]; - } else if (*motion_A==Vector2() && *motion_B!=Vector2()) { - collision_func = collision_table_castB[type_A-2][type_B-2]; + + if (p_margin_A || p_margin_B) { + if (*motion_A==Vector2() && *motion_B==Vector2()) { + collision_func = collision_table_margin[type_A-2][type_B-2]; + } else if (*motion_A!=Vector2() && *motion_B==Vector2()) { + collision_func = collision_table_castA_margin[type_A-2][type_B-2]; + } else if (*motion_A==Vector2() && *motion_B!=Vector2()) { + collision_func = collision_table_castB_margin[type_A-2][type_B-2]; + } else { + collision_func = collision_table_castA_castB_margin[type_A-2][type_B-2]; + } } else { - collision_func = collision_table_castA_castB[type_A-2][type_B-2]; + + if (*motion_A==Vector2() && *motion_B==Vector2()) { + collision_func = collision_table[type_A-2][type_B-2]; + } else if (*motion_A!=Vector2() && *motion_B==Vector2()) { + collision_func = collision_table_castA[type_A-2][type_B-2]; + } else if (*motion_A==Vector2() && *motion_B!=Vector2()) { + collision_func = collision_table_castB[type_A-2][type_B-2]; + } else { + collision_func = collision_table_castA_castB[type_A-2][type_B-2]; + } + } ERR_FAIL_COND_V(!collision_func,false); - collision_func(A,*transform_A,B,*transform_B,&callback,*motion_A,*motion_B); + collision_func(A,*transform_A,B,*transform_B,&callback,*motion_A,*motion_B,margin_A,margin_B); return callback.collided; diff --git a/servers/physics_2d/collision_solver_2d_sat.h b/servers/physics_2d/collision_solver_2d_sat.h index 95468a18b8a..be5a3dc79f6 100644 --- a/servers/physics_2d/collision_solver_2d_sat.h +++ b/servers/physics_2d/collision_solver_2d_sat.h @@ -32,6 +32,6 @@ #include "collision_solver_2d_sw.h" -bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A,const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL); +bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A,const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0); #endif // COLLISION_SOLVER_2D_SAT_H diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp index 5d43510aeab..4c7e68d643b 100644 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ b/servers/physics_2d/collision_solver_2d_sw.cpp @@ -150,6 +150,8 @@ struct _ConcaveCollisionInfo2D { const Matrix32 *transform_B; Vector2 motion_A; Vector2 motion_B; + real_t margin_A; + real_t margin_B; CollisionSolver2DSW::CallbackResult result_callback; void *userdata; bool swap_result; @@ -169,7 +171,7 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex if (!cinfo.result_callback && cinfo.collided) return; //already collided and no contacts requested, don't test anymore - bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex,*cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis ); + bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex,*cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis,cinfo.margin_A,cinfo.margin_B ); if (!collided) return; @@ -179,7 +181,7 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex } -bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) { +bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis,float p_margin_A,float p_margin_B) { const ConcaveShape2DSW *concave_B=static_cast(p_shape_B); @@ -195,6 +197,8 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix3 cinfo.collided=false; cinfo.collisions=0; cinfo.sep_axis=sep_axis; + cinfo.margin_A=p_margin_A; + cinfo.margin_B=p_margin_B; cinfo.aabb_tests=0; @@ -227,7 +231,7 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix3 } -bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis) { +bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis,float p_margin_A,float p_margin_B) { @@ -236,12 +240,14 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_tra Physics2DServer::ShapeType type_B=p_shape_B->get_type(); bool concave_A=p_shape_A->is_concave(); bool concave_B=p_shape_B->is_concave(); + real_t margin_A=p_margin_A,margin_B=p_margin_B; bool swap = false; if (type_A>type_B) { SWAP(type_A,type_B); SWAP(concave_A,concave_B); + SWAP(margin_A,margin_B); swap=true; } @@ -292,16 +298,16 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_tra return false; if (!swap) - return solve_concave(p_shape_A,p_transform_A,p_motion_A,p_shape_B,p_transform_B,p_motion_B,p_result_callback,p_userdata,false,sep_axis); + return solve_concave(p_shape_A,p_transform_A,p_motion_A,p_shape_B,p_transform_B,p_motion_B,p_result_callback,p_userdata,false,sep_axis,margin_A,margin_B); else - return solve_concave(p_shape_B,p_transform_B,p_motion_B,p_shape_A,p_transform_A,p_motion_A,p_result_callback,p_userdata,true,sep_axis); + return solve_concave(p_shape_B,p_transform_B,p_motion_B,p_shape_A,p_transform_A,p_motion_A,p_result_callback,p_userdata,true,sep_axis,margin_A,margin_B); } else { - return collision_solver(p_shape_A, p_transform_A,p_motion_A, p_shape_B, p_transform_B, p_motion_B,p_result_callback,p_userdata,false,sep_axis); + return collision_solver(p_shape_A, p_transform_A,p_motion_A, p_shape_B, p_transform_B, p_motion_B,p_result_callback,p_userdata,false,sep_axis,margin_A,margin_B); } diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h index 11b5701f46b..07141b0d092 100644 --- a/servers/physics_2d/collision_solver_2d_sw.h +++ b/servers/physics_2d/collision_solver_2d_sw.h @@ -37,14 +37,14 @@ public: private: static bool solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result); static void concave_callback(void *p_userdata, Shape2DSW *p_convex); - static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL); + static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0); static bool solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL); public: - static bool solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL); + static bool solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0); }; diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp index 0fbd461f46f..fd1ea579f0f 100644 --- a/servers/physics_2d/physics_2d_server_sw.cpp +++ b/servers/physics_2d/physics_2d_server_sw.cpp @@ -132,8 +132,12 @@ real_t Physics2DServerSW::shape_get_custom_solver_bias(RID p_shape) const { void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) { + CollCbkData *cbk=(CollCbkData *)p_userdata; + if (cbk->max==0) + return; + if (cbk->amount == cbk->max) { //find least deep float min_depth=1e20; @@ -159,6 +163,7 @@ void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p cbk->ptr[cbk->amount*2+0]=p_point_A; cbk->ptr[cbk->amount*2+1]=p_point_B; + cbk->amount++; } } @@ -648,19 +653,20 @@ uint32_t Physics2DServerSW::body_get_object_instance_ID(RID p_body) const { }; -void Physics2DServerSW::body_set_user_flags(RID p_body, uint32_t p_flags) { +void Physics2DServerSW::body_set_user_mask(RID p_body, uint32_t p_flags) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); + body->set_user_mask(p_flags); }; -uint32_t Physics2DServerSW::body_get_user_flags(RID p_body, uint32_t p_flags) const { +uint32_t Physics2DServerSW::body_get_user_mask(RID p_body, uint32_t p_flags) const { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND_V(!body,0); - return 0; + return body->get_user_mask(); }; void Physics2DServerSW::body_set_param(RID p_body, BodyParameter p_param, float p_value) { diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h index ef00eae7e47..e50bb0ab96c 100644 --- a/servers/physics_2d/physics_2d_server_sw.h +++ b/servers/physics_2d/physics_2d_server_sw.h @@ -58,6 +58,12 @@ friend class Physics2DDirectSpaceStateSW; mutable RID_Owner body_owner; mutable RID_Owner joint_owner; + + + +// void _clear_query(Query2DSW *p_query); +public: + struct CollCbkData { int max; @@ -68,9 +74,6 @@ friend class Physics2DDirectSpaceStateSW; static void _shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata); -// void _clear_query(Query2DSW *p_query); -public: - virtual RID shape_create(ShapeType p_shape); virtual void shape_set_data(RID p_shape, const Variant& p_data); virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias); @@ -158,8 +161,8 @@ public: virtual void body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode); virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const; - virtual void body_set_user_flags(RID p_body, uint32_t p_flags); - virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const; + virtual void body_set_user_mask(RID p_body, uint32_t p_mask); + virtual uint32_t body_get_user_mask(RID p_body, uint32_t p_mask) const; virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value); virtual float body_get_param(RID p_body, BodyParameter p_param) const; diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h index ba5f60cb327..d3fcf1fab25 100644 --- a/servers/physics_2d/shape_2d_sw.h +++ b/servers/physics_2d/shape_2d_sw.h @@ -354,6 +354,51 @@ public: r_max = distance + length; } + + + _FORCE_INLINE_ Vector2 get_circle_axis(const Matrix32& p_xform, const Matrix32& p_xform_inv,const Vector2& p_circle) const { + + Vector2 local_v = p_xform_inv.xform(p_circle); + + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); + + return (p_xform.xform(he)-p_circle).normalized(); + } + + _FORCE_INLINE_ Vector2 get_box_axis(const Matrix32& p_xform, const Matrix32& p_xform_inv,const RectangleShape2DSW *p_B,const Matrix32& p_B_xform, const Matrix32& p_B_xform_inv) const { + + Vector2 a,b; + + { + Vector2 local_v = p_xform_inv.xform(p_B_xform.get_origin()); + + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); + + a=p_xform.xform(he); + + } + { + Vector2 local_v = p_B_xform_inv.xform(p_xform.get_origin()); + + Vector2 he( + (local_v.x<0) ? -p_B->half_extents.x : p_B->half_extents.x, + (local_v.y<0) ? -p_B->half_extents.y : p_B->half_extents.y + ); + + b=p_B_xform.xform(he); + + } + + return (a-b).normalized(); + } + + DEFAULT_PROJECT_RANGE_CAST }; diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 2c714f50650..d1aec92984b 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -31,7 +31,22 @@ #include "physics_2d_server_sw.h" -bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set& p_exclude,uint32_t p_user_mask) { +_FORCE_INLINE_ static bool _match_object_type_query(CollisionObject2DSW *p_object, uint32_t p_user_mask, uint32_t p_type_mask) { + + if (p_user_mask && !(p_object->get_user_mask()&p_user_mask)) + return false; + + if (p_object->get_type()==CollisionObject2DSW::TYPE_AREA && !(p_type_mask&Physics2DDirectSpaceState::TYPE_MASK_AREA)) + return false; + + Body2DSW *body = static_cast(p_object); + + return (1<get_mode())&p_type_mask; + +} + +bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) { + ERR_FAIL_COND_V(space->locked,false); @@ -55,8 +70,8 @@ bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vec for(int i=0;iintersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA) - continue; //ignore area + if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask)) + continue; if (p_exclude.has( space->intersection_query_results[i]->get_self())) continue; @@ -120,7 +135,7 @@ bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vec } -int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set& p_exclude,uint32_t p_user_mask) { +int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) { if (p_result_max<=0) return 0; @@ -129,6 +144,7 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri ERR_FAIL_COND_V(!shape,0); Rect2 aabb = p_xform.xform(shape->get_aabb()); + aabb=aabb.grow(p_margin); int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); @@ -137,11 +153,8 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri for(int i=0;i=p_result_max) - break; - - if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA) - continue; //ignore area + if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask)) + continue; if (p_exclude.has( space->intersection_query_results[i]->get_self())) continue; @@ -150,7 +163,7 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; int shape_idx=space->intersection_query_subindex_results[i]; - if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),NULL,NULL,NULL)) + if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),NULL,NULL,NULL,p_margin)) continue; r_results[cc].collider_id=col_obj->get_instance_id(); @@ -168,79 +181,31 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri } -struct MotionCallbackRayCastData { - Vector2 best_contact; - Vector2 best_normal; - float best_len; - Matrix32 b_xform_inv; - Matrix32 b_xform; - Vector2 motion; - Shape2DSW * shape_B; - -}; - -static void _motion_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) { +bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) { - MotionCallbackRayCastData *rd=(MotionCallbackRayCastData*)p_userdata; - - Vector2 contact_normal = (p_point_B-p_point_A).normalized(); - - Vector2 from=p_point_A-(rd->motion*1.01); - Vector2 p,n; - - if (contact_normal.dot(rd->motion.normalized())motion; //avoid precission issues - - - bool res = rd->shape_B->intersect_segment(rd->b_xform_inv.xform(from),rd->b_xform_inv.xform(to),p,n); - - - if (!res) { - print_line("lolwut failed"); - return; - } - - p = rd->b_xform.xform(p); - - n = rd->b_xform_inv.basis_xform_inv(n).normalized(); - } - - float len = p.distance_to(from); - - if (lenbest_len) { - rd->best_contact=p; - rd->best_normal=n; - rd->best_len=len; - } -} - -bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set& p_exclude,uint32_t p_user_mask) { Shape2DSW *shape = static_cast(Physics2DServer::get_singleton())->shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape,0); + ERR_FAIL_COND_V(!shape,false); Rect2 aabb = p_xform.xform(shape->get_aabb()); aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion + aabb=aabb.grow(p_margin); + + //if (p_motion!=Vector2()) + // print_line(p_motion); int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); - bool collided=false; - r_result.travel=1; + float best_safe=1; + float best_unsafe=1; - MotionCallbackRayCastData best_normal; - best_normal.best_len=1e20; for(int i=0;iintersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA) - continue; //ignore area + if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask)) + continue; if (p_exclude.has( space->intersection_query_results[i]->get_self())) continue; //ignore excluded @@ -252,54 +217,17 @@ bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32 Matrix32 col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? - if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL)) { - + if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) { continue; } //test initial overlap - if (CollisionSolver2DSW::solve(shape,p_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL)) { + if (CollisionSolver2DSW::solve(shape,p_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) { - r_result.collider_id=col_obj->get_instance_id(); - r_result.collider=r_result.collider_id!=0 ? ObjectDB::get_instance(col_obj->get_instance_id()) : NULL; - r_result.shape=shape_idx; - r_result.rid=col_obj->get_self(); - r_result.travel=0; - r_result.point=Vector2(); - r_result.normal=Vector2(); - return true; + return false; } -#if 0 - Vector2 mnormal=p_motion.normalized(); - Matrix32 col_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - ShapeSW *col_shape = col_obj->get_shape(shape_idx); - - real_t min,max; - col_shape->project_rangev(mnormal,col_shape_xform,min,max); - real_t width = max-min; - - int a; - Vector2 s[2]; - col_shape->get_supports(col_shape_xform.basis_xform(mnormal).normalized(),s,a); - Vector2 from = col_shape_xform.xform(s[0]); - Vector2 to = from + p_motion; - - Matrix32 from_inv = col_shape_xform.affine_inverse(); - - Vector2 local_from = from_inv.xform(from-mnormal*width*0.1); //start from a little inside the bounding box - Vector2 local_to = from_inv.xform(to); - - Vector2 rpos,rnorm; - if (!col_shape->intersect_segment(local_from,local_to,rpos,rnorm)) - return false; - - //ray hit something - - - Vector2 hitpos = p_xform_B.xform(rpos); -#endif //just do kinematic solving float low=0; @@ -312,7 +240,7 @@ bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32 float ofs = (low+hi)*0.5; Vector2 sep=mnormal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep); + bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,p_margin); if (collided) { @@ -323,38 +251,165 @@ bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32 } } + if (lowget_shape(shape_idx); - best_normal.motion=p_motion*hi; - best_normal.b_xform=col_obj_xform; - best_normal.b_xform_inv=col_obj_xform.affine_inverse(); + } - bool sc = CollisionSolver2DSW::solve(shape,p_xform,p_motion*hi,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_motion_cbk_result,&best_normal); - print_line("CLD: "+itos(sc)); + p_closest_safe=best_safe; + p_closest_unsafe=best_unsafe; + + return true; - if (collided && low>=r_result.travel) +} + + +bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) { + + + if (p_result_max<=0) + return 0; + + Shape2DSW *shape = static_cast(Physics2DServer::get_singleton())->shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape,0); + + Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); + aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion + aabb=aabb.grow(p_margin); + + int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + + bool collided=false; + int cc=0; + r_result_count=0; + + Physics2DServerSW::CollCbkData cbk; + cbk.max=p_result_max; + cbk.amount=0; + cbk.ptr=r_results; + CollisionSolver2DSW::CallbackResult cbkres=NULL; + + Physics2DServerSW::CollCbkData *cbkptr=NULL; + if (p_result_max>0) { + cbkptr=&cbk; + cbkres=Physics2DServerSW::_shape_col_cbk; + } + + + for(int i=0;iintersection_query_results[i],p_user_mask,p_object_type_mask)) continue; - collided=true; - r_result.travel=low; + const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; + int shape_idx=space->intersection_query_subindex_results[i]; - r_result.collider_id=col_obj->get_instance_id(); - r_result.collider=r_result.collider_id!=0 ? ObjectDB::get_instance(col_obj->get_instance_id()) : NULL; - r_result.shape=shape_idx; - r_result.rid=col_obj->get_self(); + if (p_exclude.has( col_obj->get_self() )) + continue; + + + if (CollisionSolver2DSW::solve(shape,p_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),cbkres,cbkptr,NULL,p_margin)) { + collided=true; + } } - if (collided) { - ERR_FAIL_COND_V(best_normal.best_normal==Vector2(),false); - r_result.normal=best_normal.best_normal; - r_result.point=best_normal.best_contact; - } + r_result_count=cbk.amount; return collided; +} +struct _RestCallbackData { + + const CollisionObject2DSW *object; + const CollisionObject2DSW *best_object; + int shape; + int best_shape; + Vector2 best_contact; + Vector2 best_normal; + float best_len; +}; + +static void _rest_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) { + + + _RestCallbackData *rd=(_RestCallbackData*)p_userdata; + + Vector2 contact_rel = p_point_B - p_point_A; + float len = contact_rel.length(); + if (len <= rd->best_len) + return; + + rd->best_len=len; + rd->best_contact=p_point_B; + rd->best_normal=contact_rel/len; + rd->best_object=rd->object; + rd->best_shape=rd->shape; + +} + + +bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,ShapeRestInfo *r_info, const Set& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) { + + + Shape2DSW *shape = static_cast(Physics2DServer::get_singleton())->shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape,0); + + Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); + aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion + aabb=aabb.grow(p_margin); + + int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); + + _RestCallbackData rcd; + rcd.best_len=0; + rcd.best_object=NULL; + rcd.best_shape=0; + + for(int i=0;iintersection_query_results[i],p_user_mask,p_object_type_mask)) + continue; + + const CollisionObject2DSW *col_obj=space->intersection_query_results[i]; + int shape_idx=space->intersection_query_subindex_results[i]; + + if (p_exclude.has( col_obj->get_self() )) + continue; + + rcd.object=col_obj; + rcd.shape=shape_idx; + bool sc = CollisionSolver2DSW::solve(shape,p_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_rest_cbk_result,&rcd,NULL,p_margin); + if (!sc) + continue; + + + } + + if (rcd.best_len==0) + return false; + + r_info->collider_id=rcd.best_object->get_instance_id(); + r_info->shape=rcd.best_shape; + r_info->normal=rcd.best_normal; + r_info->point=rcd.best_contact; + r_info->rid=rcd.best_object->get_self(); + if (rcd.best_object->get_type()==CollisionObject2DSW::TYPE_BODY) { + + const Body2DSW *body = static_cast(rcd.best_object); + Vector2 rel_vec = r_info->point-body->get_transform().get_origin(); + r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); + + } else { + r_info->linear_velocity=Vector2(); + } + + return true; } diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h index 978e88479de..9d3dfae9b50 100644 --- a/servers/physics_2d/space_2d_sw.h +++ b/servers/physics_2d/space_2d_sw.h @@ -46,9 +46,11 @@ public: Space2DSW *space; - bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set& p_exclude=Set(),uint32_t p_user_mask=0); - int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set& p_exclude=Set(),uint32_t p_user_mask=0); - bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set& p_exclude=Set(),uint32_t p_user_mask=0); + virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set& p_exclude=Set(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); + virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set& p_exclude=Set(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); + virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set& p_exclude=Set(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); + virtual bool collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set& p_exclude=Set(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); + virtual bool rest_info(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,ShapeRestInfo *r_info, const Set& p_exclude=Set(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); Physics2DDirectSpaceStateSW(); }; diff --git a/servers/physics_2d_server.cpp b/servers/physics_2d_server.cpp index cbf5cffce6b..0851ad59efc 100644 --- a/servers/physics_2d_server.cpp +++ b/servers/physics_2d_server.cpp @@ -145,7 +145,7 @@ Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Ma ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult)); - int rc = intersect_shape(p_shape,p_xform,Vector2(),res,p_result_max,exclude,p_user_mask); + int rc = intersect_shape(p_shape,p_xform,Vector2(),0,res,p_result_max,exclude,p_user_mask); if (rc==0) return Variant(); @@ -163,15 +163,13 @@ Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Ma Variant Physics2DDirectSpaceState::_cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector& p_exclude,uint32_t p_user_mask) { +#if 0 Set exclude; for(int i=0;i& p_exclude=Set(),uint32_t p_user_mask=0)=0; + virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set& p_exclude=Set(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0; struct ShapeResult { @@ -119,25 +129,26 @@ public: }; - virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set& p_exclude=Set(),uint32_t p_user_mask=0)=0; + virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set& p_exclude=Set(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0; - struct MotionCastCollision { + virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set& p_exclude=Set(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0; + + virtual bool collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set& p_exclude=Set(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0; + + struct ShapeRestInfo { - float travel; //0 to 1, if 0 then it's blocked Vector2 point; Vector2 normal; RID rid; ObjectID collider_id; - Object *collider; int shape; + Vector2 linear_velocity; //velocity at contact point }; - virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set& p_exclude=Set(),uint32_t p_user_mask=0)=0; - - + virtual bool rest_info(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,ShapeRestInfo *r_info, const Set& p_exclude=Set(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0; Physics2DDirectSpaceState(); @@ -327,8 +338,8 @@ public: virtual void body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode)=0; virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const=0; - virtual void body_set_user_flags(RID p_body, uint32_t p_flags)=0; - virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const=0; + virtual void body_set_user_mask(RID p_body, uint32_t p_mask)=0; + virtual uint32_t body_get_user_mask(RID p_body, uint32_t p_mask) const=0; // common body variables enum BodyParameter { diff --git a/tools/doc/doc_data.cpp b/tools/doc/doc_data.cpp index 5167b9a0b0e..35f11406447 100644 --- a/tools/doc/doc_data.cpp +++ b/tools/doc/doc_data.cpp @@ -530,6 +530,17 @@ void DocData::generate(bool p_basic_types) { } + List > cinfo; + lang->get_public_constants(&cinfo); + + + for(List >::Element *E=cinfo.front();E;E=E->next()) { + + ConstantDoc cd; + cd.name=E->get().first; + cd.value=E->get().second; + c.constants.push_back(cd); + } } } diff --git a/tools/editor/editor_node.cpp b/tools/editor/editor_node.cpp index ed932396db4..33b2e72d62a 100644 --- a/tools/editor/editor_node.cpp +++ b/tools/editor/editor_node.cpp @@ -4148,6 +4148,7 @@ EditorNode::EditorNode() { EditorSettings::get_singleton()->enable_plugins(); + Node::set_human_readable_collision_renaming(true); // Ref it = gui_base->get_icon("logo","Icons"); // OS::get_singleton()->set_icon( it->get_data() );