From 794c8ec0648cbf93b88e04f318eeb75dc62d38c1 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 16 Apr 2015 10:17:35 -0700 Subject: [PATCH] add BspDemo.bsp data file add sphere2.urdf move btSpatialAlgebra into LinearMath remove some warnings, introduce BT_ZERO, BT_ONE, BT_HALF as defines for 0.f/0., 1.f/1., 0.5f/0.5 respectively --- build3/bin2cpp.bat | 2 +- build3/bin2cpp.lua | 2 +- build3/findOpenGLGlewGlut.lua | 30 +- build3/premake4.lua | 98 ++---- data/BspDemo.bsp | Bin 0 -> 105100 bytes data/sphere2.urdf | 47 +++ examples/HelloWorld/HelloWorld.cpp | 2 +- .../btCollisionWorldImporter.h | 2 +- .../btGeneric6DofSpring2Constraint.cpp | 10 +- .../ConstraintSolver/btHingeConstraint.cpp | 2 +- .../Featherstone/btMultiBodyLink.h | 306 +--------------- src/LinearMath/btMatrixX.h | 2 +- src/LinearMath/btScalar.h | 8 + src/LinearMath/btSpatialAlgebra.h | 331 ++++++++++++++++++ .../testExecuteBullet3NarrowphaseKernels.cpp | 2 +- test/TestBullet3OpenCL/premake4.lua | 5 +- test/collision/premake4.lua | 6 + test/gtest-1.7.0/premake4.lua | 8 +- test/hello_gtest/premake4.lua | 5 + 19 files changed, 448 insertions(+), 420 deletions(-) create mode 100644 data/BspDemo.bsp create mode 100644 data/sphere2.urdf create mode 100644 src/LinearMath/btSpatialAlgebra.h diff --git a/build3/bin2cpp.bat b/build3/bin2cpp.bat index 28c5bedc0..df84f9259 100644 --- a/build3/bin2cpp.bat +++ b/build3/bin2cpp.bat @@ -2,6 +2,6 @@ rem @echo off -premake4 --file=bin2cpp.lua --binfile="../btgui/FontFiles/OpenSans.ttf" --cppfile="../btgui/OpenGLWindow/OpenSans.cpp" --stringname="OpenSansData" bin2cpp +premake4 --file=bin2cpp.lua --binfile="../btgui/OpenGLWindow/OpenSans.ttf" --cppfile="../btgui/OpenGLWindow/OpenSans.cpp" --stringname="OpenSansData" bin2cpp pause diff --git a/build3/bin2cpp.lua b/build3/bin2cpp.lua index b3d82a4b8..8ae42290f 100644 --- a/build3/bin2cpp.lua +++ b/build3/bin2cpp.lua @@ -9,7 +9,7 @@ function convertFile(filenameIn, filenameOut, stringname) local bytes = f:read(block) if not bytes then break end for b in string.gfind(bytes, ".") do - fw:write(string.format("char(%u),", string.byte(b))) + fw:write(string.format("%u,", string.byte(b))) end --io.write(string.rep(" ", block - string.len(bytes) + 1)) --io.write(string.gsub(bytes, "%c", "."), "\n") diff --git a/build3/findOpenGLGlewGlut.lua b/build3/findOpenGLGlewGlut.lua index e3e2581cd..886b6a561 100644 --- a/build3/findOpenGLGlewGlut.lua +++ b/build3/findOpenGLGlewGlut.lua @@ -43,26 +43,6 @@ configuration{} end - function initGlut() - configuration {} - if os.is("Windows") then - configuration {"Windows"} - includedirs { - projectRootDir .. "btgui/OpenGLWindow/Glut" - } - libdirs { projectRootDir .. "btgui/OpenGLWindow/Glut"} - configuration {"Windows", "x32"} - links {"glut32"} - configuration {"Windows", "x64"} - links {"glut64"} - end - - configuration {"MacOSX"} - links { "Glut.framework" } - configuration {"Linux"} - links {"glut"} - configuration{} - end function initGlew() configuration {} @@ -70,9 +50,9 @@ configuration {"Windows"} defines { "GLEW_STATIC"} includedirs { - projectRootDir .. "btgui/OpenGLWindow/GlewWindows" + projectRootDir .. "examples/ThirdPartyLibs/Glew" } - files { projectRootDir .. "btgui/OpenGLWindow/GlewWindows/glew.c"} + files { projectRootDir .. "examples/ThirdPartyLibs/Glew/glew.c"} end if os.is("Linux") then configuration{"Linux"} @@ -83,9 +63,9 @@ print("Using static glew and dynamic loading of glx functions") defines { "GLEW_STATIC","GLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1"} includedirs { - projectRootDir .. "btgui/OpenGLWindow/GlewWindows" + projectRootDir .. "examples/ThirdPartyLibs/Glew" } - files { projectRootDir .. "btgui/OpenGLWindow/GlewWindows/glew.c"} + files { projectRootDir .. "examples/ThirdPartyLibs/Glew/glew.c"} links {"dl"} end @@ -100,7 +80,7 @@ else print("No X11/X.h found, using dynamic loading of X11") includedirs { - projectRootDir .. "btgui/OpenGLWindow/optionalX11" + projectRootDir .. "examples/ThirdPartyLibs/optionalX11" } defines {"DYNAMIC_LOAD_X11_FUNCTIONS"} links {"dl","pthread"} diff --git a/build3/premake4.lua b/build3/premake4.lua index f6f9bd39b..81150eaaf 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -114,83 +114,35 @@ language "C++" + include "../examples/ExampleBrowser" + include "../examples/OpenGLWindow" + + include "../examples/ThirdPartyLibs/Gwen" + + include "../examples/HelloWorld" + include "../examples/BasicDemo" + + + if not _OPTIONS["without-gtest"] then include "../test/gtest-1.7.0" -- include "../test/hello_gtest" include "../test/collision" include "../test/TestBullet3OpenCL" end - -if findOpenGL3() then - include "../Demos3/AllBullet2Demos" - include "../Demos3/GpuDemos" - include"../Demos3/BasicDemoConsole" - include"../Demos3/BasicDemoCustomOpenGL2" --- include "../Demos3/CpuDemos" --- include "../Demos3/Wavefront" --- include "../btgui/MultiThreading" - - include "../btgui/OpenGLWindow" - include "../btgui/Bullet3AppSupport" - --- include "../Demos3/ImplicitCloth" - include "../Demos3/SimpleOpenGL3" - include "../btgui/urdf" - - include "../btgui/lua-5.2.3" - include "../test/lua" - include "../btgui/Gwen" - include "../btgui/GwenOpenGLTest" -end - - --- include "../demo/gpudemo" -if _OPTIONS["midi"] then - include "../btgui/MidiTest" -end - --- include "../opencl/vector_add_simplified" --- include "../opencl/vector_add" --- include "../test/clew" --- include "../Demos3/GpuGuiInitialize" - --- include "../test/OpenCL/BasicInitialize" - include "../test/OpenCL/KernelLaunch"-- --- include "../test/OpenCL/BroadphaseCollision" --- include "../test/OpenCL/NarrowphaseCollision" - include "../test/OpenCL/ParallelPrimitives" - include "../test/OpenCL/RadixSortBenchmark" - - include "../src/BulletSoftBody" - include "../src/BulletDynamics" - include "../src/BulletCollision" - include "../src/LinearMath" - - include "../src/Bullet3Dynamics" - include "../src/Bullet3Common" - include "../src/Bullet3Geometry" - include "../src/Bullet3Collision" - include "../src/Bullet3Serialize/Bullet2FileLoader" - include "../src/Bullet3OpenCL" - --- include "../demo/gpu_initialize" --- include "../opencl/lds_bank_conflict" --- include "../opencl/reduce" --- include "../btgui/OpenGLWindow" --- include "../demo/ObjLoader" --- include "../test/b3DynamicBvhBroadphase" - - if _OPTIONS["enet"] then - include "../btgui/enet" - include "../test/enet/server" - include "../test/enet/client" - end - - if _OPTIONS["bullet2demos"] then - include "../Extras" - if findOpenGL() then - include "../Demos" - end - end - + + include "../src/BulletSoftBody" + include "../src/BulletDynamics" + include "../src/BulletCollision" + include "../src/LinearMath" + + include "../src/Bullet3Common" + include "../src/Bullet3Geometry" + include "../src/Bullet3Collision" + include "../src/Bullet3Dynamics" + include "../src/Bullet3OpenCL" + include "../src/Bullet3Serialize/Bullet2FileLoader" + + + \ No newline at end of file diff --git a/data/BspDemo.bsp b/data/BspDemo.bsp new file mode 100644 index 0000000000000000000000000000000000000000..4ed414d75b92d4d94b05589c46bc8f66bee34686 GIT binary patch literal 105100 zcmeIb3%njxb^rb30)(3!ZgN!*S3v(Vty*f)Qfuw0rIb=?Q)?~yr{w*8_g-tBJ@f3D2|4y{-}mqS zurhmQzO&YEt-WW@JkJR^r!0H%iX(cx-dnDp*Sk>L)v|kK7tQPSipObtsP=d1^&Z$s z`+N0zmz*JH>t64XvvquauXoRRz1|_>`zQ2zKi;a>J9=5KcdWQabbRsWd%cI`cY5!* z71ld!pLxC0b&hzn`u586ufFoNm!E&d)2=-KiYo*0*#E%urI%fC(b?ynfB9p-dUyqX zeqMTpUW{D=C9NsJ?VMR z*Uy*rFpY!S^@G~=y*tdG$6sFcKVCaOUOP|zG>;#~(_)7citp#bzVpWHTl2rK{Fk|h z3u@OFwe#{<@vnH^@H1`kI0iReJKrmuuK9sSW9|B0?fS9W`7t~1Ih^kMicLE`<>h}? ztB5!B&90Aoe*J9i`q|p`8*A5ZwCnMLk3}gj|BKw8&e6OcUc*Yg#N1u;e|JsaSZ(~q zG=AQQU(~KIYWk*Y=cjeva}R#lm+rEqy!@|D_ryAGT)%dG@TcS6i06&#$J2dr2jR zv)XQu&0w}adBjieTX)1y&kN^^6JNeT`%APR&TF6Z<^H$d^2rLX{d#=r6|Tj<=?T8$ zKJzy{&-|r%4&)Q|&Bs?JZQp!cT|0mEB(8r#0~o;O1d?0@S`f3}`n z$=UXorTe?S^P&o`{aSv#T7Esv7uSx*jkWwX*7EyAP5&oq`4NxiHM&USecc3O&e8XM zV=wk+m=m_|yw&F>-w*lsdN(oum~SEf=e+nZ<{$Gh~Xi{PQoGVSX{+0O>tJd63md zuitSE`^=x2@@@CUv1jG4nf zw_xI)EAQ9++Use5Li;n?zx`iqtlxRix+Tw3{@M?7LHp~pzv1Jy|A6Xq?Vfk6*M9J= z{dL-3w)p*fP3-%P=Z9lAUwGqs?aydG_|SgP|3$?+?z0Of&VOT!ul*VA&uIVlC7+J| zet5vTCGS*x?T2f%zfSwASFiW+x12GvN&4@7O}t+F6WX8AKKT{5YX4E4-}l;hUi&lJ zpV9v93vb?Q;@YnVJ;mQ&aN~OI&uBmB)&4Tex4239@3hw)>%AV@U#Is~)}S&Frs(vHc4F^)K%-nflZIhLnHs zdzasy`qRF~)5ji<;dvZlv8Vk|zl72JqPc#5A1HlzzV2iFkF-`{T=k=cpIfT_1ZMlU z+kSc;zJA{KxjyyB^(pWE-Cvl$#M<1p$=XJ{V3D>Dn;ko^>Sa6aI?oxd8M7_dq&%#? zKB(~8?Jbs1uESCuKMQ5`wn7J-=USS+qUh}wdc9sMajlNw$N3fN!=l6a`Rdd5loc}P z*QyV{y9^!9r}-8-4$rmDs~Z|TS{B6J@=y5lWrYm?`pP8uCk`$3&p!OP8N4@m{CkZq zvlH+eWrYm>2{M?+%V_$lCbZ-9C$)fIEh}X3`^nJkyz4nj$7t|3%f@B!-r&*WydP)& zY?Lt{xYpYq>Y&Scm{tF^X_L=4xO6`3qkUpH&v|&CKkTRc)3xt&j&rpBqnkFZ)|OZ_ zu02rOU1i5}Ky91234ay}R;cGZ*FIB5eVlhag^ms6f0efITnq0Fp8Aj0hco zew}wcI@Eh0;p_RrbKUlU)?81a{k_cpr26FBm(g~Jn@}HWvQqo#rxmi3WX$2djOKa@ z^{LO*+QM@!$Gr8`q5eDCfkIn&_Ycpt&zDgj@}{N!`w7prMIhoI@c7qPr)8s__%G}T z+ZW34zf4Bs9;Cfg=6VYC|H=HnNPX()yz9}S-mT^z-PE(-pfz~(jQIg=U4Owb<4;o; zJ&Xfw=eP-f!cVD3J!#~EPUf)l`ieNTJ@lvZ1NB`S>jvZHe5!wy`9EFf(Sv_)@aX5> z^ydDL)`35=+|N^=`Ybod*J*a)S7__{r_{ed#+-NF*IgYddHnyXX+zTjne(asx0?Sw zbsjxa+IoXWzptYlN6(nHu4je%SIC$H&z8}sqqjd)F!+^r0R9FUdYmUVjWv0r@pp$K zaeaQP`qamH*MGi_%^E+gJ`Mlg;L-0q$DJQ%M=j-_qkVXuXS6~#(E7~W+$bY=*4wwK z&-~#&rn&z@efW#Cb)IXz!Q+2#J5Xo~Kc=noT>EP>p6@5iX#2!XZ%SMEmD(0E*YjTW z7s{M>JySXc&$f`kbFDXc^z3T~rnGfEh5GOud%q0-r^skLk7<1wJesFuuIIPZPkGnV z*KzpMwVjf|bFDZ0qvwf!KwH<}SKs}ASbh9IS4MOHv=$(LdTt7 zt3JFpc*=FK9Y9Z^K0Mbte~9{mBsD5D({H@&NMo?wf$?|fhVgT+&S=bx!Q4gXWBKm3=}$A4e_Lu&p#C)Wf2 zezCu+YvB*C`PZq?PS>Bj&%^(T`pjo|Z|Kpby=U2hTXp{5#gA*>^>gjh1TE0^Z8934 zhiT}!*Z5DXkACr`mR>Q6qiz*C>SWcc@nfApVV z2fRMx+TS3SeBqyEdR7^CqHWjeJoo#U_C4Qm^-rwvON@8_@Tclp@`Yb2o;BP3>u_j` z^(Q&Pdwt-&!J~h|4v;VRyYqW$e_ZEI5yU$FtH$+hi)QCJ^iJ)s(Ducq$N8nk-=*`+ z&%XA#_E6)`5U>AA|I3WO23C5UXD;FY%o;B&v^Dy(NO)qwdqWR#ChdUxCtugoSN|1) z$oK6s+VZ#wpLKX=bDwDA`E{X;`R0676YqEUzFiOR*K!CRJ@SN~e`zgm47-#5{w%s(2q z7XN+q-z1(oIZ{}Rbe?Oy!K3Gec7XNGd9KC(xX%5WAf9*cHg1J&;r;Uzhq#ve@jWtf za$Z#@--oBQ&v!ou%E+5_H@)Uz z?PFZCi?)pM<76~+<2PkErxI=A+32m#pqHF}WP0FWHp)EK6V;ddP1M*M*QE@{9}&Yk z=y1Kj&*($Y8!B`wyMpQQPv}L;a1iLI!iM?8jpM zU6wL8>v$o9VZ8W^v6Hs6bWGW&hGW`d192R4W+OXlKA$$~{Lb-EV7Mk_c&B9!9nrur z7KiB^pS$)khWjn`1H<+Is#v=vOrgHVg5lf`#rXH8_(_@7I)AW?x$oZvrc9x0VUCsY zUFV*%LY8Bw?Z1eBNaoBc9Yc@%;rf5p`3DmQ)*1LW$o7`~qZn<-Ii_u+mr8q7*Vd$uR240 z?$`NoZ3`^p=NKLT2Ql}VpOhI`j@0J|I={cRG}lvvz3BTA^$(E!tvC*+4DZT?j$J4F zJN0Lc@mQR5hUeWuvO{Dv{H)N{b0d1 z>VL0=!OwSf{!ndcbv;u$c9HC_#PHszZpm=p`S zUzi{Ne*bPAqrtF_eMjg0dxads8ue{4_sCegsP~w*sh=gCk%B>V=*_V={LkoY3s2H^<6V@ z{!Hh;nJ`m2R>+(=S$*Phf6&~|l={w`sy?5MS%+var)xVUbB4UWt~h@x>#Lt*(EK%> z_s_M?z`LL4s{bcq!e=7&>&%pnq34gq9Iq`6W=z{D8Gg|A92uJUys1se^woEUHR?{C zKT%s6eyB$uGe7A0BQbX*eo|(I&a)P**ZJFxN#_qfnLoSWQ^q@Q`fj7$@_l2z`b%Y> zl^+<+E#eQghh_d8T&Di#bl%^k>dX^$en#gfwRML2(A*FGbz+ulTW6SO{@(IcG0$#b z$ZI>Dqt?_U=ZE^-E&Gc6@c9X*U@M~$3+8ee_wScwb!M$GY)_FfCiPf_j^XEi*_Y(w zi{?kWWvoJdm`};Rp!1)vF~6?;DPtH5=IhluRv#}NF2{n;46ipo{!YSqn!l6$fzC5$ z(d=_00LHNH@UH(>S&pI3-0Nt+!Wf_P+6@hcdGSlKSIF?g93Bg2;~y~O^di~IWX|B% z8w~5li)Am7IWwicH<-h;^BKiBOV z`twrRr*!_M+Pa@9^}P|7XA}4R? zcd_g&**UT%hB^OH^9gf^tnO!4%-OPAbZ%t>^BJB0h|c{^3&Z?6S9YN+)kBWNMRQ;F zVfCqjGvtvn@7Dgb7@pfXrmy4gk^Qz9#w}%dh7b$peXA;jb7Ne` zSlgVrRr|lG{a0)2ep1cwx5?fohI=SwxaZIVgXZ_D{~p<5nXeluLw$ck=T>TaO~Pom z{C=~q{%d5vrt>L7eGZka&^hj%cdLK7w$Aj`PnmaVpS5_ktj@4L;Acv9rR>cz@=Di3 z^t1l-b?&vYt7KCR49~yU$!^klXZqTwxgXZmcj^2)WwYj|51{7ExcXPi-XwEBj0Fv* zuPyh*>t)x<3iDHESbO-4@iv`zX0i5Z?g!29(D^rNJ8gdI3~S*vvQ;{tGPBxuKdjyS z{O4B_=2#s|8T4EyD=={*b=L8h$gY>YCGnHyg*IZ{sPk`4m_u}o7HX(1_u!j#{svhZ zYg+rx5bI4a+D>Wfdf+{;SLz)5*UNGYafx-E&c8+5bpF7HSeifQ=-jp1XAb6=ah=~^ z_C}q5gE8sz9c|1*8t+Low%*@O{jE8uwzrJ$9B6!=SrmHl^M>;iI_7=egPh@+?|q)@ z&aei1pEbf6?g@{}9P&PM*ZXfv`tMBozmfFcpY(q_>3=lo|6bDnbkhGq(*J7G-;nhG zB)||AeG}K+-=n z>GQKX_w$UTe|*yC=T**}l=NSa^!XW*GcQZ}XC?jflfM6qMCpZ}fB3chY{2_`&+dJ` zn%B^ykeV=D1DL-!bX$n)LTd`cJaHHob$A<4;YFKRr4A%;flq$??hL z_$kTp7beGlIXQkta{TP%_=U;wS0~4>NRGcgIevZ8e{0fzN7DcGr2m`N*QWPdN&k0} z{_iIJ-?u*V@N-H34-@A0r2oeW^Npnc7YXxMN&jyX<{y&&KPSw;CH?yY61we#^G^*!BwBzRI@S+xBkT*7bc($BF-W+kVNmU$(9Dym!ETMn>av z2W@TK^uD4k=hxYGVB7V!y~DP5+V(Ene$BRDx9vA=`$^m0sx9puGW_$-L33R^?`gNi zO^@dl?RRAGAF=JnZF{3_U!g6HcQ@LH;wHSG(|xb)f6%r(6Y+PmZF%=(|7zR5SX){@ z{24zB(0!ln^Q@)4J8r_y4!QmU+uuuD_8)KiOKt1(h|h~O-l1s+#ZCAgo$KoLIo!@4 zVcS)<<-LIRCK=b=VB5Fa*6VYW_GyR3P4CsVz0S68u$HeSV!`+-VK`OYJ!K8SR(jru@4Ybv^a@#yUbfIBv?n(BR6v=wnP z!t)+VJ0Wh$pXr}!`*l6_@$mWi+y?$6JASflpKaR%8~CMm-0z1c+y3Jk_!I5;F}AJq z$J_DUY+Juy>OAw0c3j+q@6G8RYx{Nn8FqY6+t%+VuaA${(~WD=vz476x9t;bySHth zYTGew=gaE+K6c*qbn%|AuSZ?{9_HWmck#O$zl&|V_+5?LZZrAqjoZ3`_xso9TUR~n zd{;f|d{;f|d{;f|d{;f|d{;f|d{;f|^SP^@e!uKs;Aj=y8o{EQ1{=(*Zp_RpSu4pzV5$kzSaGA@mtwE8*i9z^X&K`+HTJJ*~QoA zLl<8k-!8sBUR`{>{#|^%o^^gdt5?(d*~QoI$1c9Bo^?H4^{n$<^{n$<^{n$<^{mhT z`g%C8Hb3k0zl*Q?@2Y>@e-~f(-!)!!|6P3Df7kfd{de(o|6TK;?!Sw#`|p}Rb^l#_ z-GA47tNVYfywA7&ZC-9`TQ;npb^mqVKff=s>+1ZI?6|+bb@{LJb^rDH)b-T;ckx~R z>w3EU*ZI2t`g+o(zl-nkU)NLDU-w_<>;AX0@f^2pm;Nrk%YR)@U4Pwwov-`f$#gfp zuXgeE^|Q{q9_}5Q*T+BKb@5&PyY$!VQ|Igc>wK5~F22iuT~Gaa(N&)=zRQ1?{<{A< zU-w^s9(C#O;=BCU_0-3!?!V60{nwv&UHZHDF8_5sb^Ue!b-wPu{ygo{-^F+Nuj{Gn zuluj_b^rC}b(j7wzRQ1IPhEfAf1Upit!e)*`#0IY%DyH0bJ;g#-;jMx_9wEtWPdE< zdp(cAb?~sCl6_LPqm1LePyFALu|D%o!TU0CJ|W{c?-AbLM*V-0{j=b?W)~~%p$KWPpe0RK+jCQ$BKsW~*YZ8` zwlX-zXQ7PiwwBGx{#J(ezm^ST{2b{Wvg>7j9DldS_!-12WsJjDWp9+7BO}&_W#`Gx zmihUM)PF^?f2sQC%C3}MEc5GLrT&Gom9keS$6u%Z6*4rU?{3+j$-XZ8QyE&(fi~h_ zBYTbPS7hhQ&XWB^_G8(PWE*AQmHn0MJF;)fz9RcW+2>@Rl|4>I;~H4FY1#W_JIXjd zEn|N>89skownnzIYzNtV8OJ#`Ci|i6A=!hn2W0ol?vwpM_I=rxWnYqgQT7GdAILr< z`?PGW?AK(>*+XUcIm)*9!Vf-tPIBIlGuNIaV=nrfbe?m^$c~pCEu$@wO~}|!{owyN z*}=*2-&Fs7vR!55_#xSE$$et&At_&@|Av;C3RL1=KI~jHP8=1#_w))GH<9=OV z$KYY_mHnmc9@$^W$OFyjAjVa)m&sl%dx7jE*)rL+vNy`e35I;oO&;WQjO=I`HRJm9 zJbA-;{^U1bMw=(&+GohP{wNvsyF&IF*<~`;7S@J~WUM#m$!{%#GUhnjy=9ARe=lvfm2D^6UWRts<7GR`c9HEW z+b!Ap_`6ngJza*jr^(QFxD1U?m7$aM;vCu8vRBGpA!A%mv@LU+KJ(Y>xIq0wWZb9w z$;M^Z$*z&TL3XvwefqV|Ez`NBvc8OUWQpu}*>SRG$&QshQtz-*j+sJm8 zjhXGFE%rFsj-(^lf}kPPg0o3|Yq{Oq!KNBugNC|a(~#zdh8#N!D-EzKl@oLQGTB}- zZ$8%tV#v$=G8X)s*13Bq{Js(KuxrHhWmm|!kG#3xan^&|BYUgPua>=5#&~$kvFK$i z&ewLoiS*y6j`dA7;1~8+I#}|9B{$f;GJahzEE-_ZkYl}O>3n!aVAFs9x~dnJn&J1{ z4%X{SZm|3DZ*-Uc4tMnH8emt5fki`(ofYHr!P%VxoBsRV(XX>VRI@Fvc~%t^wBP!#06U|2^@j!N(2O=fjR2EP8!D!0r|P4RmV3%p9$Q&j&Q* z*zoV%GvDQx@x|7JdtS$a&!Jyf{#|z1X=Cr|V95=Zn!$3v<^1;K7nb<2XvngPUDvS6 zHR#&dY>dU?H_dHv2b*eu4H`0j(U9hbhAgX^d2ZvD8?SJ%v{;L(Ui=o$n%^dt++e92 zuZO-)rhd8B>%ct(J1q@4Hk}XS?ze^YI>0h+eQkaHE!6ipna&;e3(IqhR$-Gd=J=HC zIUQ_TGuSo_u;XIJWXH&OpY!JH`oJ`}U)U2`SRZ5ba?infZull@fAlyTrgAE-Tj(_5Ggbcfhz`#sQXlt&Jr&*ePTEz9_Wse$$$v z0TvBSEHxuHSjPEN%JGTXdh>ge->DORuaVeso#$D7^s>vavV zXvndyfjXd-+*ZnV*Y9Q;{{EdfA6x_Mt;Sy6!J+}SFN579J{GSQ zyL|oIViw4*|GJaiy*B2HIIo36}-__#2QlAeqQSTv|58?MD zNdsE%mfa!t@!EPTjOF(&U2XFW4*q*$_s_67z26mkV1{+SeQ9vNtLlCys(N8v?^Io{ zv0ew*amA$$E5zpR3)c(F824p2*7er>_RKFj)`;CT{4Oy6{Z~=1`-Szn^Z5=o)c_mj zS^0alqThOM=tV=0O~;t`7xZ2#+gZPRYxw)Ss(#T6hrO~r{V9}6c zeLkRJl^A@#O15+O-FyD~&Z1uT3(L9=%RSV_q5&2Su)Ajbx&~M@z@j0?rej=~h7W4H zEp;e=zgg7le$m@AmT_xi(Ey7E*zGfZT>~r{V9}6cy=Jg*7{AjhchBFwUTcT$dDv+& zh3quhE;4VvK1_)%jO9Kq^sfGLZT)iqyt7^h{0_|5>DuPj64y{98V*rz6%7;B`2b5^ z*dno&+>CV%u&&`)u@w!*dTy|uTVHG?x0;5&*jX{-(!ku=G5oGRerF$*h>3@1 z<$h~p(E!^scBhPAA7fZFz@j0?dVSGws~9v~Als3-TmC-5sMr0%t}vE+sEtJfEE-^U z%J_8+uxNlqLyq;aK*MS==siQWBk#)P?_-R5-7hTT1|uxNnYCgayNz@h;b4LNpveY}rh9~Ohp z<*LVy+7`0dVtru;y60h68oRKAoi-K?u(@||*8qzKSTy8VuNfM=W~`H2ah+P!Jk-k? z1-nA$$7QUOZ7dpK(E!W$OtjRmYk)-qEE;mG&q*{CVpfWMSljK)Z}2@c)a&)_>pU?R z%C>D_JvUeytbZ0^-`8K)i-vJy=Qpsfm)ufmgl*sxyT)UWF;@Tacw*6zW2u9W@d`0$I7hZs_+7RB`(&eD_lw?X%Z+Dj8;b^5G{A0?@#`93(Ey8v z9ILb8GkR}wzSw!iuZf$I&65=~oTWv*^up(3v5SqxZyP%n*!exaPk>z@%lW-X^8t3n zL}byBWhY$&&+A7woh9x^sz>GhVWzGZcGISR75l9g77ctawlrbKbd5K^-}qg|;dA$7 zuUIV&mx{!C`8NI?P-lk14O02_B!+ByW8WNVVkl!*l*h(Fo6*FXG&EDO? zG9P@5uT*{u_}m@l8akt1&yAX`nV28u4);YHi(au|KEN{9>wfVU^`arNXvndthLvJ^ z%IkF5w*L8TW~i5*k1>7NeLGn6dV}3Q%WYCQugdX4ouzwaAWY=23t0~&H{I(O)E51kou zv)>P{G57kqU-bIDc~b|A2ET`3x8Z){_l_riiBP$ZwU34@Ti$QOvEaEmk2=iw?< zb-#?;3LCdGJJ|T#jPndOj9cArc@GWeBpPyT_}RqcioD^HAbeJ`r4#e!syk2watQ4X}6F z+Be?8qW3Oa`(Se#()$f%193dBex}-n?@Jo=_Qb*N*}?Bz9OE_ zINp3s_4NV0tWi%B^L4TFbFAI-fz=xIHZjy<=M3w5oplX4)@w#?JTLHjYu&G}eM_s) z3-o@ruGiRPjP;#)tiwmdR_c(jjInD#FKk6`!ct#Y9}7MQ%;U4giPg`?USHT-#V~Gs z4rpVijit7*k1iSY>$$=DoJ2#8P3!CT#fRiOtkvP&gjumkO*tiF?Dre&Je~n{*2V%2 z*ZY0k!BPjFEzDbLk<-vuZO8QYgbVraDJRxz*4%K-jL+bx7j{+#J23Xy9c)h;VEZ!I z+-HB+0NXPb4LNo^qv1;B7WC?x*rHz8dTvK|u*@MeFwU^_i;Q3D06Q=i4LR0xOY3`! z*m6CaLGiG@=uPVYOK!Bdu2yq{?Mnl8TL+6?IC6u1q{T0KVW*63(n}puz5AvbbZwd& zY+47{(r?0!i*wdB9M-|6xxtpXW&EZ!gDvY@V|^^pFpym=5WY~*=t^&L;{O;~D{W62Gc_r<9n|{9`rJub&kdG$PS|J3+E_Heq5(Fi!Tt7(rDm}FEQgkAaJ{fJSaNG( zy=Jgz*jGMF4cf1-52;_+(r?11^MTyZz`I5po7Mp~r@{U9llsDz^-VNT2Xcd@F&5}; zV#_%xb|7|PqL=H-&#~)=^OxLUcWYtMK+Rx##^!#8?RsHDGYvV`=Y!V)4gBo(hs3bv z?@{iV~gtN{l`r~_or4fj>WIfNi+<^@jYf`eVD0^vHOMP9&Tc(gR|&>#k{5Kq_b#%MMI7yxAZ>6G{9Cgm|w3ie&H}Qbg{%AlU*slj75Eoa=(TA!r~V;+{ZK8 z53M>DuyEMX9jxm`18h!1Z~gFDh=y?)-$(KP*%aEw7B#^7T0(qS-XGf7R4*)Jk@L$~ zq;)_;6YCoKV$i^I9u2f~eW1S1_Kf8@-^QjjgMFyQZ<-sd&$Cp6*9;cD=s<51OC9>g zlG|=#OTAog>#vVn>KC^3o3PZu>q~r$cbGPIN*dspJFsOPxITFXxL(+CV{>}zV=)lB zpN#KUy!razHS;>a9@N6RhQ64dv8*TF+^^RGzi=2Da;$6c`VPeLbDxUdWPM( zZ(j_#@vZ?&OYeDSXN@h4E$@pNuBhe)OWv@&@3gUKfJFmrP6PGzn!yf?_4%9Ai*9m* zCAS>w^ zITpt28)_zw_0(Ca;OoxFScv7bW)ti81sZzBR_^(kICtu~!J;9@x`tWLO)P(};|Jos z`R78<4c2qpsfA6)4Gpl=&71r6`nm>KH00PqvAqrhG0gQVt!DK#%30or3| zj-_Tke?2!oU-+C%*9T{*FDx~KE$_D(va99>+c%caB5f?W!BSt?oCX-L87wt}rDn98 z-n72x&9V6Pnz;tv>q`ylr@6tVxxtpXB`h^_wx{FNjQg^UMGGu-fX!)eziDo;Wp3)% z^}>41(3@lN>-9y$K*rx|_`cN_ufCq4)7jux+)*tob#M*%hUI5T-rR3JH&`^}Sl8fV zf!@Pq6}=O2PI_*z_=G*Qg-yo|9=4*v{G!3xz8F|E)B>F)Z&~Z7dqliU!!6hQjntn}&f{-i@elp>1r{`oiKDmUpf;Hq`(dG-Ukx z+@TKBrXk1n6x-)yAr6-3JS;6;ADkWN8dwb0pM@z~m|s|OgXMYE#-_Q!<}|q9Y4bZU zztoJDYUr5;SmMK?H^<`FYlhxwalC8H%7R}V^Yy{mfoZ@uY?)ia_RKFV9LBpw8%rJF zrj4D|mR4l^P8&Ngw#+SI3)28gZm{Uhu{|-&htjY3FDGo!G1D-A#jbB*sjq852kfIV zZ@!+P!P$cIrXj}`rgz}EiT!+CgRMLG^?M$_1F@fKVbOqJ*8p45V1CiyELt%%9qNpW$FNH@`Pw z`97+RB{${{8ep>;#Grwi!BR6=YDUZH4fWMISaQp<^5->k4ZDdgHR#$jH`p{c*fO`2 z)m6^ofjTVgV8a*-r4F!J4LXnCG&k5Xw^W1WmevftS(dm~Uvq#8hK4M=K58g*4cxel&spdlSi@tfwxdtVzH=C2s$I_$j-etkZW8!Q^KtmTG=zL;?t@Ahb*Rn~{3 z4zN9AsV}XO0ll#3&9VOd zB)QSh%V(V2{f1x0IIY80^4rFe8yd(Bw#-faG&lUFbwEQC%NUn><>#AX88>f!Z+aa{ zy<(YCMQa{~4!8Nc(Cq8X?2fN7FLjNxrEc2|% zFS>(YF|glhVS``c_~kvdoWC=u8s2ZEUJk3lmUA*;sjstBmfHy(EE-_Z0NbRYw?To) zjV6xwzHezuD?+TO7Y)t^4Psv3!7>)E7nYjQQokX#G=y=Br-OzZ>viB7{0?ON+vlz~ z_?~IdtCO&mpNkml8p=9|<^L~iVtxJ+Z?QDM`p@z?=IaCg7&rHeUf3qR=@`SVtg{m_ zH}~r`gXRBmtoTh>zlYFgk_A~FFb6e4#qMT&Q8fE4ZS(m^@chSU)(|3-l_S} zbiet%iC@^e8rXsK`uzsWzlqw$`dFX=wn+ni%e7CjVee?rFzu`u?jiIxvG`3ju&!gY zSc~Bp*K>orJHs*-&iYvJY-wZB081TUn>3WUAy6Fa!`EE{`PzDh24{l?G2HWQY^nja z)R6dv!EahKG&Hf)!N;u-cZ%$5rWdt-Z+gxAnFagO7PgX`vA$NPeqFC?=!r!`j&%*? zT5a(wdXsyT+ z`?$=F!)mbQ{7qQqgR@hLP2-u>#-afh4X{lbJh$MK{I#z8+zI(6_p!4TgU+EL z$BvAfxZ9+4eX2LPkI{?&(8Rv0g+*`4stcQY&ih&d8|oky4LO#${th#ZM%ijvcCG`P zhC*6lKi0vHNkd?T!)EVeo%8pSph5d+XkuLh@iFr9`4D3b=b5vsP4AC7So8)~u3>X? zU8leD|vV-;{r!_CeXxWfOKC z*ZSX&TA8rZ@&|iB3(K!x#V;)XMg!~%WZv9wsBNMF77aNT4XNLsiQ}E%nKEzg7j_^^ zHNdh?d2<#G&Qb?7@D9_&x&~M@z~(gIw=fOvcfS_DsRsPQ=KR)kgWa`-bqziiuuZu+ z>$&|vy)y4w0NrSUqGc+`@{oCUh!`>*Z(iSeZq)3A2E92A3me!_hgxoH^lsEN7CgVvOUKcCj<<09rZqD}Qk9G3ZEv&C4u*?V8Ntw6OF9&MtBpR};G&qZ2#^MZFliq=N zYUUbXo9f`K&mBHPHnFY&77egX8j6|*zW-?Q>#S?oxrI&jq5-x^gR@>UG&HfU!Py&B z=O(sS%MG?EH)lP!TUuDx;Pr*gYGCe|250%~lx4*^>$&myqKWnO7nb!`Y>}Ijv+EHt zyf>Na{JWvqIwb5$aj+|z?qi&a_ciKfvE;T?TW{q(RP)PxKr8PX-kh!Lh2?Wgj-_6G zTPLXl>!jAF;>e6&SYLl($&IxvrV|B5!%t=_{H?fP8+;$TOo70=C8RL-EE1$krqk*-W z_v9yQ>#fX92b?80*w3`EzV>;|U~_t@t+Qxg+|ba()-~|#&gxD4x(3*sU&g^T_l$Fu0Grj2)WP-s zK)Rb)*Wh|#b9$+-vtHjvTUgf%i(c3+4W8QpO}VMyuEBHTb3oRw*rGNTu({mA+F-1Y z1sa;zx(434vKmygx(3*s->{CD2Cpygo=vQ4@cP1bY4G~;&P9IS()pX}W!!j|Yhrt~ z`obQS^Q$^I>-9xL7wdEK6%A~-=W8|N{VeCVUSD#9&E_V(^*W#-!w$ZtXAn)#?!gp_li*jD{${>frYU?|qG|y=%bUEDkoS zVVGO^JCm&ITgfuHP4BJTXv(eNdoZ!nx9EA8VZ$@i*zz+Z-&Z%XmYdIY*lccSsMUe@ zs3z9OLUjmZA+~WWzHRSDn!i(js~UW2N$<3Swe@EA(8zqyxSjrS)g!B+R$s>WeN8NZ z#4~pox9^JO``^ZAmTKWF;|80n??C>-`lszc4D5RvSU6|>K2{yFe#2b1nn`b{nck~& zH48r7FS((C_w}6KaL-$AzE1LUhDKJBse{iQ*vwdr%wLT~@pir2XW01f0h$J{*>Tdm zpUhjjrY=s#VmC3oD>sc>Sf`|yI`BRTo4q&X(^_M{fp>+9nNo70O=_e;(Eo%V3` zy?JhM?$^hH=hc%kES$4G#?+vRbq!tz*qjFLciuJD;J^1@oA|8teASNkPjCJ`c=#Ts zWH(*^fLQ)McoWMzB7UbP9xd6=XZ)gJ$UahPXkrKAquxgYd$DvhX&ACmFW+l4v0ev_ zPtXhdnFhT>Hr5OcO>9x>-4ygPCz~`RY|wCM3+uVLUf3oLLpIh74NYvXrgz&G4JCVY zuV6m>q-8!7=GXPYHfc!MP%|_%vCPvolSh2@_@m4klt>gKJSXB;Tms22^5?4;|B_vUT#*{IjpLhCOz@*rA5FmcZt6Bd;MlyXkG3=exJCLk$~CHmf0%+cr&l zD;nbZ3!BqR8C-*}JG}p9*+c`s2ZZPRjV_T z#W1&6-|Ji0p@t!QOU5tre8|S!(9pydwL0)#on<9=$i_PG4%5WC1|K)r8#?s*xS^qo z?e$}Byu;*J>gyVM{a7>JVVc;O8^5z}$llVR*X!VV(a^=ZUcO_>X<*!XVJzbB8)M91 zvwnp}HonWTK0GL`O|0wf_2am~Hfb2LvA#Svb1e0Zto}Zl)r{{Sn%Jl}=En0CHmf&N z2Q)OYD#e<~Va@JusRLz8pBHzwv0*H9K|el+U~{<*$1Sko`G$tgW%tt9ohRO#Ki|Uo zFw_u!X3BHEk(C3l1NDV%TqhHjd4`5YR^ySr?<{Yt*^mwI5BxkJJ8qV2Iu@{vHLK}G zLp!Sr`tkkjV`9&fd8^b{2dUX67@g*iE7!<^)OvWXpPm|wD44f2U^8@IsT_!w%&T*zr4e$>G4L>voa z0o$aZt``kC7S7*yR!;Qi?W`IwZ=7X((!am%Y;j^_dCs@7efcj=Jg@ko{J?r{sbALs zyK*A3XvneD77fww>Ulp9$6Wi6wzSl*vwZ(soVX}__J_^+bq%okeXN1y=L~IZ)EhOx zHfg|j)T{sZPIX{CS?+pgP_6rGlQ!fp3v);QRv63ArfKQ_69nfh?{Toa8?~`fZ`1&r z)8Kw%9rX9f`tjd%$g!?B);F?eH~4iH4TW-BEDpAuJ2PY$>);w(FD&0(i}M#YtHJz6HqISBiHNLFgN|4borEUt`nx3tQGVVfilJ*Wdn<_^!dTyNyL} ze@UFbusIFxmpb&9#OFMpopLOn$Iy_@Nj{Ucv2onuoP;fFmdP#ZjVv0PSf6M7ER=Qf zT-gcg)6#bW^g8Qvl6Q?ZHtLOY5_Vz6um21s>WwTKa;$4`zsz60KRYwy*V%Ob!sh(C zhB$6)dnjdi_ffX!*3UcUZL>31;CO6SL9u=W3s)mb!5O)Ti~^FmlO-9e6)$W7FJVvl=F2&3InuE(`yj z*!|M5Z*DBaPzTs4{qEs0`TSZ3i(c+yKAXY@4Ku`|4L^$v$1SpG=wb`=yF&{*tl9ja zfp?82zp&(193ATb+f)bi26iH{XvnhindZi4Q`WNVSco0Y^#$Q`>>8~(ZLE(4&n(!W zLH*dO_XR9t42y;)Hmw`aQ&R2sS%z^6PadG}mE&se@%rEi@md#KHD-P1CyGGj^r1A-9=P zy=dSWz;lQ?;ddi7YhZb{!18Va`#=jz&HVWW+f=i)(!g5X*WbZlt^Q@%gE_zA&>Q{6 zwQnEyt8HwE3GY#DtZU$Y zgU#M=ronTAMMIXI9O@-E?jgQ&Y`Wjb&F2pHF>F(Pd&cT-R)_b#TRPa@&{R^WLc}**f@7oXK}E6hRgY-4n8Mg z(U4`ur5d;|Sl5FFZDSj*JNWh7U|IXxSl7UOfDIb7t@}lT=LU<0EIS#mESCP@%ma?p z8ZD0Rt{Q75yg&Kk?f*l6caeDp`^AQu#X9i!`{JCO=wQdf-vx|w9X8B|8A~mBZsNCR z>`HO4*|8uuu|C(y4K|w_K1(*vN#3iQ#$uRToa?Zm4%)^x9AoyqW{Y*6&mwKCj|KMy zY*vHi)(>@vbDd{aHaA!`*YHJgJcruYHIvVObm`IYzJP73Z=92nMMIXQzKI6rI?thO zZm=@X4VLFn8|xaF>#*6mZW=r{STtnWNokP9HEPqQe-y`e3XL@r8`r)^HvO&c3)sf{ zBGe)Lz1MIL{ZM=x8|QDlH(|3HEH`JFJAB4ztXZOgISJc1C(GO(-SjlE4|cGzX7L_^ z&1#TOS{!4=mS3Lp+1y}D4VyN7K`hVtHkP%oZ}S&6J10$p&z&*TkYzViHT$9G*}+?GW4nhv%f z{(g0w>#&XUSAJt1B8!GByP>LKLEpYl;5n~6V@uXl)5|&LdU!8tVWWmPe_@;EI(3M- zMfQXyR$QEu+{bZFhUZnod&$smWFO|4*}|@w9MfmEM>g%G{9vjE8GuW6d^g;=Tx6=U7BG)F8vgxj8QmdvBp_x3=x6+P*-xm6_! zh_i#X9_NkfKVO!_As@96NBNB8uv9T-+T!4!e0Gs}oad=ui=!jdt~DRx%mAj%^U*vF zarV^KBs9R&4-7L-%lQ^%lIKz1`V0@I%md^w7pl%1sJfF+eCm)W-c`&Z~ zqX^^wd$-*-P+vKvtyucN_b#9KzylxF`cZ9Ow|#XOe@~HloJ-ZO#u+Tr-{Z4-6-yPX zFC=NjQpH$DtmI(mdYp0X5ofW?zl=?$KVP-Hky;id4?$R`6Zd>bC&w73Dr1@ zHz!Wxe0!Gs5$B~ckMnZ%Jr3v8bdOTEEx%7$oALDuna3d}aTds8oUsk_dd!6|4jPNT zjg$UQ`1ilR&+)%FWA}-@4mxS(ML)f?a<6*bxB!3W+E$z1v9jmNsGG-8S2>mA5aN8} z>tFBdxLUFFqibIs=G#9Xye=DOxhQ;HAoF~#RiAlL%SXP;d~Vn2(3H=BgtgP-@Z4s6nCCIh3dP{xl=OUb zqSDIvBe6Wr3i0G~wanujpuWe6$B469#_jArwHMmRekGR2IZZ5au9JD3tJG&6upjd| z$>Ic`gIuhKwz_WSp{*YMsxGNCk8`??66Y;4kHd3;IBDH(e*Zfai#Wy7;XD%*TCtRQ z=5x-Evh_G;s!yD^%RJ5z>Jw)>SzL>#+uF^o+l($No5z{b5#p?td7Qn}&&TO+@%Wr) zao(52IY|9eWTSP9^Pr(_>r}VO9FU8$ZWpOfJ|C2MKD>u;U$M_TnAbZnsasF?=*S#f zdc;r9n>pgA_ccCGxj%_>i_GIN4~Vl!7U$dIBu<5ug;DyxhQfRE2g^@($< z%;QkE<+25`7-!Xbd`kNah>QpDVlaS)zOz>vog;5$9Vn zkHb7j^U>cmv3!^Zw9$NYY$OJx*Nt_JIQPmt4);$rpF(lCe~43$A-Fs?8q4FnM-*|s zFY`FO11-0G#z^1ED-Pq6i&KwVeOL52YvhGE_sKjC_vWPSSK~})@)?O!sh`JT{UpwV z=2x5EYh~m!I^T+AE%R-r{!WE2k29@4aW=|4AJ$e05Bu}ArEYI3{xGfEARI3LD3jLs|G@n7T&9PKNuM!uiFzgeT9rfp&RM4EZRx z(OCAJYpmNH7U$q34)b}bj5*Et%gs-D z9)x^&zaDUG*vj|oO3gU$^ML0YagI#luwDq3z&MT8Ojy$L>%l!Dh@-zGS*|fJ)_y)ttXtw#-I2JJ+Iii0 zjuK~i5@)&gJq~sI!2AD7yFBLxJQoHT-{JFi+k43O@J2)D?Co(SFKVf`sw^$;)mGAMDnsL_acAxshc}WuIx!Pyq zERaz*we|Y>lE*;;mbCIcu`$jAqKR{65(fu38s0zSio^I!D=%6x5=X~IY6j`~JfuEx z&Pn1d*M2phzT$BI5T_nPaCxlxy^8k&;#`!(IYRq=8S{Xgw5K?pPd(1aekCu@=g0Cw zoJ*59hu~Q@8Yj$yOw9zBt(ud^d06L(b7c~T=P3g^oY%^ysN~bv>7<4CofF&McODT< zoHrzKo}~T0%;&-IKAmp6Px-vixV~x84{?el4&Nt|kH=9|%>jOQr;mki1%<|b`+Qqy zac)fFJYLsU@UVhT3&9G>=j4S7Ldb z$6K6_BypauOUP$94}5%xLmTnERL5rGx$2)DhxaM+xh;wFB<)w@^pf$xPc}{^4=9?) zd7}AWtL+Z)RFBUKUN>sj>v=x-ugBoL9~;fd;~Z#lzLLb@J!CY_+~&bkab96@hV#wm z!IsarV=T^hlQ^t9_MkvEhu5 zPv7D^l*D1(8I8kyBOmvP7jJ&7`i$eO$9azYGd>SnJkE{DJWiYkZTWZ%uB26S@;JO7 z66et*4nMCyUPj%jar&8jM&eY)$>W@AaTaPId>!RJCC=zPAP#NB_tJVEksH?SMHXkf zB+j{ls&R_-bDeMg9y+EtKHqpB^}12JUN4>3^;|gb$Est)8LykahwhuiS+4V=appGP z&at{3lGIJbF5|>`;Q5R%8)`=_&2x!+^>OliF0eR9Byo85cs|uQQ=1bfJKtVqagI*n zu6ec~|J zbXd*jHF4_W5za3SSZyufn8&%w;+&GiVci*x!+h(EGm?|XxyI`DBE|9bV1?rF^L6rx zIkx3f&#!t9c^uxW8K2XWIQ-qJe4HXvw~;uNoIK83EY2&FIIM5P8P0=&{PP~qmNw#h zsgBLWbJagR&MJ#@UJ~bAq18BiAJ!SCl7~n0IK00zF0WFYbba%>>9AUwPdyjT`?1lS zJkGl<&g+smtOuiUm~X^$pZNFY$C{s~YpiZJD2~rJ)&h?c=UZDo9;1v|87I%@gBE91 z5{JK!bgXPRuRYFeCZCZwm2vVo(-!AlNgUR<>O3gqzjGc`^6>0D4xdN3f8LYCVSOW? z(Rn?sc(jommg-m~kH`(zom&-$ak*J>e7-H$zSoV~$%o}boO+CKUU9;9Bqz`3Qx@lA zNgOU3jl+EFj5Csx$N8+{P`A%0j?Xv#{?0RH!+GHOtj*+8&#!t9d7Lj=oG&JEPHT*F z_vXZ@Y!Er%Y5_qWkF%s29JpZNFY$Est)S&zf#U*^FsisSR`YV98* zBNv-*>XFatOdO9PB(!Qy9%rM)*)xg5_r3Wzo3)Ns#>wOS*y21fiNkY_ILUllKO7%K zkL0ja$7VQFZytxw^VIFYBo6E7Gh|5|?fE=VHy@{x2NccYJfb*!UU-V)_*lGD`^3>9 z`@Gr#b(}mO zzBeGwvLw#6+CNf8-Kue>m*vMsVA-nokjHtP#W^{N!}?a62a|Cg%qk8oms2&5$PMSg zE{enWoT@lJ59Bn&S){EF(=5&)lMkmo?r1E}XAjHgrE=)&0na(|A&ySd6o+}x8E0f} zdz^hN&MT5QN6E3ru^bhL`)9M>cPew+<2=dYoSVcMZ;Z2X*|0`}Li3uH`<0wL&H;+U z_*|?wK0bSD-}BLFn(~?65V!1(`0iYH_+E#3eR+}(&xPhV^%!Z~=6P_K#ko3(^IBcX z&z`IEplA8C#;Mc{1zxwsR=4XE$LofFkHi0?qc%CLTRqOmes&%_!{WRxiG$;OoGt%* zLjL{HYQ^#AHh*t$v5fhl!!+eH*bx8T2D>9Ubk2jmjn4<=(C<_3AFMh)#35cihK`oU zn&-iCi*rj7hk3v{T8+cJW_@do(>M=Kwm7#Yan4KPP&+l{K|Y&xpJwO5^DWM&k~qBg zdOp=S;oqmxSfs6w43BXKHo+t;@XEY7{PILw1>WIAnaYJ)nHac9JL=X${Rd(`dwNgUQT z>c+Y{jMK9?^%#PN^PrLoXFQ)vEzW&OoM-Di`4ESkV)K0JaYpvD<8!&ic`%8yw;bo= zJht_kT6jKuUrF6ICUN+G&(4xD53u-mVg28;8_{21AIjRLao%8Yev-steLF)&jQH$Z zxk24K;y7$soa-#kBT1Z7vT+8AMT{+rbA!d1uj2Xhhq*SIPcf%BH!2SI{36APHPUa? z$;M$`Z+SlNusAy>adytf(a|lBv)bymcTzX<^12P@o3A-rHa>jMs}Q~MB+l!!&u3xc z#W>V#Zu5GL#W^^M^XepyjhW)iZC-!S;vAO5>1X52ZC+1XoFkJs)RBEHF>$>>_h-H=Gy5pVkB|2x8?Kt(~84=`eMZ~pS>4ppYJNz zAI`VA&FjzG_?(%H&tb%^#?ju~=Jl5?&N)dOKF<+{Tr8i-xyIppc!lU)l*Bnt`_wKO zAMMTU{#kEvE=}SbYjMcM#!UIlEzX@5=gK6`DHf-i&)n`)zVBCv-W!rQd=92=)qLg_ z=bIL%NaC=L@{Ul=XKw4kw=B+$Nt{b9PBovo#o^}{tUEU;j;}j?i&M>~H>Y*ydy2z) zutst4OCt~JR?TN_asEzmSl>RZIKI9eU~y{s=;)TOZ~UBueAXuUXg8P7oZ|e8#raYa zhxK}ejC`n_HQlRQp3jdIMD;>*6Z+Z0oLEA>-*Wy3qP|s-%R509>TMMIB^d8`}LN`dDP>u*oY!F&R7y> z|9l)B-SWEe^FQkL_#_VR2M5Z?WjNnhueUtT5f*3PBo6CaEsl=NZC)Q~arRH*NO*`t zE|!lD%`MI`7Uz&8&R$7AHV<@UZuie|7Uzg0&ed`{E+ZFMHc5pNu1{^#=f%Z_{=TNODxXmNgO_Bjmv8J=*X6@J1@65 zuT0_`Dvz0b<`n0Z7U#Sq&ZNa5A6s|So7*~ij>UOZ5@$b)Lkx>UXxWz6?E;IlGKs_b z=5-s!VIA!&R{i@#!R4{)&$2k<-;=+};=C@2gKv*xpC#p=IJwUyc=OoJaW1tu*CcUR zNBQohTDO&o)tOJ_vjPe{pUW-I4N07pNt|Im#OchZG0y8O&Z;C1>nQnD=iA)I=M5I; zT}d3C+aAaA(UG~0&vh2(JxLtaYmZ~|O-JT7J~vpLo0B*f%Ij`2>Nd=0ZsT*K#hFgx z@Sf;#ET0XrZk_Y3{MnCOgy!?@?H1=_NgSRF9>?;rIGyvYG0wXz&L@&MtOp)vn9tnC z=iL_PGf5nt3m#{f&)ml6eHQ17NgQ$@4%?W|N~_zJkI&5(XI&CU?#nnaACI%;AHHt&+s`<=qd~UHgUrXXlCUJ)O%x!!=W^wLL;_%$|d@PPa`?{l(<#T{OE!1;l z4dsmAKes6k_x*PihqZ?GN*Vr#>jAm&evLnxZ{yzP#Q6iu=Z8r?FVW@1VSI*l>&$25 z9z})c^976Z<0KC2Xf2-&I@Fad>NaC>_)zbk>vy&| zjE~)?ibFo!r?kgDJ{v5~)=3;6|0JIcn~_gz-Tv6(?2yFaJ|#wykK%OZ(;DYb6$gL2 zD2~tTi;_4t-zIzE?>%|Fd@C?C_DBEj4ySy+-DCMYCCTRnI`8=m=k-82xqsh=Ao?T3 zY0Zbf=dw`b;v~-UBo6W7y8&^wd>(wy;vAF2Idn9RVsyr7&1Y6|6ry*6{)jcIer6nw zPoY>|FW-_YE!1r$_N#yT_}p*#oSftXIXXU_aa!|v(Bhn$#9?03l6gHT&B|xX$LEI@ z=d>ga^O}~#QH;(wt@->&aTKEWa{b}sb5@8F;|#~AN31P=E<9}cT#)3$bAd)a7Dq8U z%jHB}JqWAP8&0y%CCtE;A?f>@U8sgBXhu6gG!uK(v{#&w9soKCkGzAD>&!`|U!UiXwQm2n@tZSGHi!A2s@r3opZY2%G_Lb&SySkDtu6nX>+ticzt@CZJzuX2 zTjF5!c^7iVkLyr4moop|HQyW!=%M|0y}$j!|8U%YQRm_3h+oXNaNGU=?!NP%H?R)U z_w|rIW6pX&pDpVXeYUKx^x3j5(r3&6-$tJ;?SI>Pf;QU!_Iw}f^XunT=l`j=KQI0N HQ-1y*qN~We literal 0 HcmV?d00001 diff --git a/data/sphere2.urdf b/data/sphere2.urdf new file mode 100644 index 000000000..35bf786a6 --- /dev/null +++ b/data/sphere2.urdf @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/HelloWorld/HelloWorld.cpp b/examples/HelloWorld/HelloWorld.cpp index f1934b269..bea045d0d 100644 --- a/examples/HelloWorld/HelloWorld.cpp +++ b/examples/HelloWorld/HelloWorld.cpp @@ -67,7 +67,7 @@ int main(int argc, char** argv) if (isDynamic) groundShape->calculateLocalInertia(mass,localInertia); - //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + //using motionstate is optional, it provides interpolation capabilities, and only synchronizes 'active' objects btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); btRigidBody* body = new btRigidBody(rbInfo); diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h b/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h index ba03015d4..9a6d16fbe 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h @@ -24,7 +24,7 @@ subject to the following restrictions: class btCollisionShape; class btCollisionObject; -class btBulletSerializedArrays; +struct btBulletSerializedArrays; struct ConstraintInput; diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp index ee57ca72b..cfa997bc5 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp @@ -772,14 +772,14 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( //info->m_lowerLimit[srow] = -SIMD_INFINITY; //info->m_upperLimit[srow] = SIMD_INFINITY; - btScalar dt = 1.0 / info->fps; + btScalar dt = BT_ONE / info->fps; btScalar kd = limot->m_springDamping; btScalar ks = limot->m_springStiffness; btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1); // btScalar erp = 0.1; - btScalar cfm = 0.0; - btScalar mA = 1.0 / m_rbA.getInvMass(); - btScalar mB = 1.0 / m_rbB.getInvMass(); + btScalar cfm = BT_ZERO; + btScalar mA = BT_ONE / m_rbA.getInvMass(); + btScalar mB = BT_ONE / m_rbB.getInvMass(); btScalar m = mA > mB ? mB : mA; btScalar angularfreq = sqrt(ks / m); @@ -787,7 +787,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency) if( 0.25 < angularfreq * dt) { - ks = 1.0 / dt / dt / 16.0 / m; + ks = BT_ONE / dt / dt / btScalar(16.0) / m; } //avoid overdamping if(kd * dt > m) diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 857fe9199..76a150947 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -303,7 +303,7 @@ void btHingeConstraint::buildJacobian() static inline btScalar btNormalizeAnglePositive(btScalar angle) { - return btFmod(btFmod(angle, 2.0*SIMD_PI) + 2.0*SIMD_PI, 2.0*SIMD_PI); + return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI)); } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 90acef7d3..1ae859aaa 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -36,311 +36,7 @@ enum btMultiBodyLinkFlags #ifdef TEST_SPATIAL_ALGEBRA_LAYER - struct btSpatialForceVector - { - btVector3 m_topVec, m_bottomVec; - // - btSpatialForceVector() { setZero(); } - btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {} - btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) - { - setValue(ax, ay, az, lx, ly, lz); - } - // - void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; } - void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) - { - m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz); - } - // - void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } - void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) - { - m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az; - m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz; - } - // - const btVector3 & getLinear() const { return m_topVec; } - const btVector3 & getAngular() const { return m_bottomVec; } - // - void setLinear(const btVector3 &linear) { m_topVec = linear; } - void setAngular(const btVector3 &angular) { m_bottomVec = angular; } - // - void addAngular(const btVector3 &angular) { m_bottomVec += angular; } - void addLinear(const btVector3 &linear) { m_topVec += linear; } - // - void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } - // - btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } - btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } - btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); } - btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); } - btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); } - btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); } - //btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; } - }; - - struct btSpatialMotionVector - { - btVector3 m_topVec, m_bottomVec; - // - btSpatialMotionVector() { setZero(); } - btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {} - // - void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; } - void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) - { - m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz); - } - // - void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } - void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) - { - m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az; - m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz; - } - // - const btVector3 & getAngular() const { return m_topVec; } - const btVector3 & getLinear() const { return m_bottomVec; } - // - void setAngular(const btVector3 &angular) { m_topVec = angular; } - void setLinear(const btVector3 &linear) { m_bottomVec = linear; } - // - void addAngular(const btVector3 &angular) { m_topVec += angular; } - void addLinear(const btVector3 &linear) { m_bottomVec += linear; } - // - void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } - // - btScalar dot(const btSpatialForceVector &b) const - { - return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec); - } - // - template - void cross(const SpatialVectorType &b, SpatialVectorType &out) const - { - out.m_topVec = m_topVec.cross(b.m_topVec); - out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); - } - template - SpatialVectorType cross(const SpatialVectorType &b) const - { - SpatialVectorType out; - out.m_topVec = m_topVec.cross(b.m_topVec); - out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); - return out; - } - // - btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } - btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } - btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; } - btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); } - btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); } - btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); } - btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); } - }; - - struct btSymmetricSpatialDyad - { - btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat; - // - btSymmetricSpatialDyad() { setIdentity(); } - btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); } - // - void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) - { - m_topLeftMat = topLeftMat; - m_topRightMat = topRightMat; - m_bottomLeftMat = bottomLeftMat; - } - // - void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) - { - m_topLeftMat += topLeftMat; - m_topRightMat += topRightMat; - m_bottomLeftMat += bottomLeftMat; - } - // - void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity(); } - // - btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat) - { - m_topLeftMat -= mat.m_topLeftMat; - m_topRightMat -= mat.m_topRightMat; - m_bottomLeftMat -= mat.m_bottomLeftMat; - return *this; - } - // - btSpatialForceVector operator * (const btSpatialMotionVector &vec) - { - return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec); - } - }; - - struct btSpatialTransformationMatrix - { - btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat; - btVector3 m_trnVec; - // - enum eOutputOperation - { - None = 0, - Add = 1, - Subtract = 2 - }; - // - template - void transform( const SpatialVectorType &inVec, - SpatialVectorType &outVec, - eOutputOperation outOp = None) - { - if(outOp == None) - { - outVec.m_topVec = m_rotMat * inVec.m_topVec; - outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; - } - else if(outOp == Add) - { - outVec.m_topVec += m_rotMat * inVec.m_topVec; - outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; - } - else if(outOp == Subtract) - { - outVec.m_topVec -= m_rotMat * inVec.m_topVec; - outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; - } - - } - - template - void transformRotationOnly( const SpatialVectorType &inVec, - SpatialVectorType &outVec, - eOutputOperation outOp = None) - { - if(outOp == None) - { - outVec.m_topVec = m_rotMat * inVec.m_topVec; - outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec; - } - else if(outOp == Add) - { - outVec.m_topVec += m_rotMat * inVec.m_topVec; - outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec; - } - else if(outOp == Subtract) - { - outVec.m_topVec -= m_rotMat * inVec.m_topVec; - outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec; - } - - } - - template - void transformInverse( const SpatialVectorType &inVec, - SpatialVectorType &outVec, - eOutputOperation outOp = None) - { - if(outOp == None) - { - outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; - outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); - } - else if(outOp == Add) - { - outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; - outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); - } - else if(outOp == Subtract) - { - outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; - outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); - } - } - - template - void transformInverseRotationOnly( const SpatialVectorType &inVec, - SpatialVectorType &outVec, - eOutputOperation outOp = None) - { - if(outOp == None) - { - outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; - outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec; - } - else if(outOp == Add) - { - outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; - outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec; - } - else if(outOp == Subtract) - { - outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; - outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec; - } - - } - - void transformInverse( const btSymmetricSpatialDyad &inMat, - btSymmetricSpatialDyad &outMat, - eOutputOperation outOp = None) - { - const btMatrix3x3 r_cross( 0, -m_trnVec[2], m_trnVec[1], - m_trnVec[2], 0, -m_trnVec[0], - -m_trnVec[1], m_trnVec[0], 0); - - - if(outOp == None) - { - outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; - outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; - outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; - } - else if(outOp == Add) - { - outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; - outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; - outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; - } - else if(outOp == Subtract) - { - outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; - outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; - outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; - } - } - - template - SpatialVectorType operator * (const SpatialVectorType &vec) - { - SpatialVectorType out; - transform(vec, out); - return out; - } - }; - - template - void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out) - { - //output op maybe? - - out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); - out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); - out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); - //maybe simple a*spatTranspose(a) would be nicer? - } - - template - btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b) - { - btSymmetricSpatialDyad out; - - out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); - out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); - out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); - - return out; - //maybe simple a*spatTranspose(a) would be nicer? - } +#include "LinearMath/btSpatialAlgebra.h" #endif //} diff --git a/src/LinearMath/btMatrixX.h b/src/LinearMath/btMatrixX.h index abaaf6fd7..a3e46b2d4 100644 --- a/src/LinearMath/btMatrixX.h +++ b/src/LinearMath/btMatrixX.h @@ -94,7 +94,7 @@ struct btVectorX { T temp; temp = scale / absxi; - ssq = ssq * (temp * temp) + 1.0; + ssq = ssq * (temp * temp) + BT_ONE; scale = absxi; } else diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index abed6cbdd..2523ef4ba 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -486,9 +486,17 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); } #ifdef BT_USE_DOUBLE_PRECISION #define SIMD_EPSILON DBL_EPSILON #define SIMD_INFINITY DBL_MAX +#define BT_ONE 1.0 +#define BT_ZERO 0.0 +#define BT_TWO 2.0 +#define BT_HALF 0.5 #else #define SIMD_EPSILON FLT_EPSILON #define SIMD_INFINITY FLT_MAX +#define BT_ONE 1.0f +#define BT_ZERO 0.0f +#define BT_TWO 2.0f +#define BT_HALF 0.5f #endif SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) diff --git a/src/LinearMath/btSpatialAlgebra.h b/src/LinearMath/btSpatialAlgebra.h new file mode 100644 index 000000000..8e59658bc --- /dev/null +++ b/src/LinearMath/btSpatialAlgebra.h @@ -0,0 +1,331 @@ +/* +Copyright (c) 2003-2015 Erwin Coumans, Jakub Stepien + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///These spatial algebra classes are used for btMultiBody, +///see BulletDynamics/Featherstone + +#ifndef BT_SPATIAL_ALGEBRA_H +#define BT_SPATIAL_ALGEBRA_H + + +#include "btMatrix3x3.h" + +struct btSpatialForceVector +{ + btVector3 m_topVec, m_bottomVec; + // + btSpatialForceVector() { setZero(); } + btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {} + btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + setValue(ax, ay, az, lx, ly, lz); + } + // + void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; } + void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz); + } + // + void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } + void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az; + m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz; + } + // + const btVector3 & getLinear() const { return m_topVec; } + const btVector3 & getAngular() const { return m_bottomVec; } + // + void setLinear(const btVector3 &linear) { m_topVec = linear; } + void setAngular(const btVector3 &angular) { m_bottomVec = angular; } + // + void addAngular(const btVector3 &angular) { m_bottomVec += angular; } + void addLinear(const btVector3 &linear) { m_topVec += linear; } + // + void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } + // + btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } + btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } + btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); } + btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); } + btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); } + btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); } + //btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; } +}; + +struct btSpatialMotionVector +{ + btVector3 m_topVec, m_bottomVec; + // + btSpatialMotionVector() { setZero(); } + btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {} + // + void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; } + void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz); + } + // + void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } + void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az; + m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz; + } + // + const btVector3 & getAngular() const { return m_topVec; } + const btVector3 & getLinear() const { return m_bottomVec; } + // + void setAngular(const btVector3 &angular) { m_topVec = angular; } + void setLinear(const btVector3 &linear) { m_bottomVec = linear; } + // + void addAngular(const btVector3 &angular) { m_topVec += angular; } + void addLinear(const btVector3 &linear) { m_bottomVec += linear; } + // + void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } + // + btScalar dot(const btSpatialForceVector &b) const + { + return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec); + } + // + template + void cross(const SpatialVectorType &b, SpatialVectorType &out) const + { + out.m_topVec = m_topVec.cross(b.m_topVec); + out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); + } + template + SpatialVectorType cross(const SpatialVectorType &b) const + { + SpatialVectorType out; + out.m_topVec = m_topVec.cross(b.m_topVec); + out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); + return out; + } + // + btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } + btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } + btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; } + btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); } + btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); } + btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); } + btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); } +}; + +struct btSymmetricSpatialDyad +{ + btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat; + // + btSymmetricSpatialDyad() { setIdentity(); } + btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); } + // + void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) + { + m_topLeftMat = topLeftMat; + m_topRightMat = topRightMat; + m_bottomLeftMat = bottomLeftMat; + } + // + void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) + { + m_topLeftMat += topLeftMat; + m_topRightMat += topRightMat; + m_bottomLeftMat += bottomLeftMat; + } + // + void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity(); } + // + btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat) + { + m_topLeftMat -= mat.m_topLeftMat; + m_topRightMat -= mat.m_topRightMat; + m_bottomLeftMat -= mat.m_bottomLeftMat; + return *this; + } + // + btSpatialForceVector operator * (const btSpatialMotionVector &vec) + { + return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec); + } +}; + +struct btSpatialTransformationMatrix +{ + btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat; + btVector3 m_trnVec; + // + enum eOutputOperation + { + None = 0, + Add = 1, + Subtract = 2 + }; + // + template + void transform( const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) + { + if(outOp == None) + { + outVec.m_topVec = m_rotMat * inVec.m_topVec; + outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; + } + else if(outOp == Add) + { + outVec.m_topVec += m_rotMat * inVec.m_topVec; + outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; + } + else if(outOp == Subtract) + { + outVec.m_topVec -= m_rotMat * inVec.m_topVec; + outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; + } + + } + + template + void transformRotationOnly( const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) + { + if(outOp == None) + { + outVec.m_topVec = m_rotMat * inVec.m_topVec; + outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec; + } + else if(outOp == Add) + { + outVec.m_topVec += m_rotMat * inVec.m_topVec; + outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec; + } + else if(outOp == Subtract) + { + outVec.m_topVec -= m_rotMat * inVec.m_topVec; + outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec; + } + + } + + template + void transformInverse( const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) + { + if(outOp == None) + { + outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); + } + else if(outOp == Add) + { + outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); + } + else if(outOp == Subtract) + { + outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); + } + } + + template + void transformInverseRotationOnly( const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) + { + if(outOp == None) + { + outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec; + } + else if(outOp == Add) + { + outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec; + } + else if(outOp == Subtract) + { + outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec; + } + + } + + void transformInverse( const btSymmetricSpatialDyad &inMat, + btSymmetricSpatialDyad &outMat, + eOutputOperation outOp = None) + { + const btMatrix3x3 r_cross( 0, -m_trnVec[2], m_trnVec[1], + m_trnVec[2], 0, -m_trnVec[0], + -m_trnVec[1], m_trnVec[0], 0); + + + if(outOp == None) + { + outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; + outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; + } + else if(outOp == Add) + { + outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; + outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; + } + else if(outOp == Subtract) + { + outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; + outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; + } + } + + template + SpatialVectorType operator * (const SpatialVectorType &vec) + { + SpatialVectorType out; + transform(vec, out); + return out; + } +}; + +template +void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out) +{ + //output op maybe? + + out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); + out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); + out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); + //maybe simple a*spatTranspose(a) would be nicer? +} + +template +btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b) +{ + btSymmetricSpatialDyad out; + + out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); + out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); + out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); + + return out; + //maybe simple a*spatTranspose(a) would be nicer? +} + +#endif //BT_SPATIAL_ALGEBRA_H + diff --git a/test/OpenCL/AllBullet3Kernels/testExecuteBullet3NarrowphaseKernels.cpp b/test/OpenCL/AllBullet3Kernels/testExecuteBullet3NarrowphaseKernels.cpp index d45b7d294..06f32b43a 100644 --- a/test/OpenCL/AllBullet3Kernels/testExecuteBullet3NarrowphaseKernels.cpp +++ b/test/OpenCL/AllBullet3Kernels/testExecuteBullet3NarrowphaseKernels.cpp @@ -12,7 +12,7 @@ #include "Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h" #ifdef B3_USE_ZLIB -#include "../btgui/minizip/unzip.h" +#include "minizip/unzip.h" #endif #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" diff --git a/test/TestBullet3OpenCL/premake4.lua b/test/TestBullet3OpenCL/premake4.lua index f2a6e0042..1a51d3499 100644 --- a/test/TestBullet3OpenCL/premake4.lua +++ b/test/TestBullet3OpenCL/premake4.lua @@ -20,6 +20,7 @@ function createProject(vendor) { ".","../gtest-1.7.0/include", "../../src", + "../../examples/ThirdPartyLibs", --for unzip } links {"gtest", @@ -34,8 +35,8 @@ function createProject(vendor) --you can comment out the following few lines, then you need to unzip the untest_data.zip manually defines {"B3_USE_ZLIB"} files { - "../../btgui/minizip/*.c", - "../../btgui/zlib/*.c", + "../../examples/ThirdPartyLibs/minizip/*.c", + "../../examples/ThirdPartyLibs/zlib/*.c", } files { diff --git a/test/collision/premake4.lua b/test/collision/premake4.lua index 0fdc92f65..f0a56f600 100644 --- a/test/collision/premake4.lua +++ b/test/collision/premake4.lua @@ -15,6 +15,12 @@ } + + if os.is("Windows") then + --see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012 + defines {"_VARIADIC_MAX=10"} + end + links {"LinearMath", "gtest"} files { diff --git a/test/gtest-1.7.0/premake4.lua b/test/gtest-1.7.0/premake4.lua index 5fdf84042..676f06e66 100644 --- a/test/gtest-1.7.0/premake4.lua +++ b/test/gtest-1.7.0/premake4.lua @@ -5,9 +5,11 @@ files{"src/gtest-all.cc"} --defines {"GTEST_HAS_PTHREAD=1"} - - --see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012 - defines {"_VARIADIC_MAX=10"} + + if os.is("Windows") then + --see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012 + defines {"_VARIADIC_MAX=10"} + end --targetdir "../../lib" diff --git a/test/hello_gtest/premake4.lua b/test/hello_gtest/premake4.lua index 983b9ad08..fb89c2035 100644 --- a/test/hello_gtest/premake4.lua +++ b/test/hello_gtest/premake4.lua @@ -12,6 +12,11 @@ ".","../gtest-1.7.0/include" } + if os.is("Windows") then + --see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012 + defines {"_VARIADIC_MAX=10"} + end + -- linkLib "gtest" links "gtest"