1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464246524662467246824692470247124722473247424752476247724782479248024812482248324842485248624872488248924902491249224932494249524962497249824992500250125022503250425052506250725082509251025112512251325142515251625172518251925202521252225232524252525262527252825292530253125322533253425352536253725382539254025412542254325442545254625472548254925502551255225532554255525562557255825592560256125622563256425652566256725682569257025712572257325742575257625772578257925802581258225832584258525862587258825892590259125922593259425952596259725982599260026012602260326042605260626072608260926102611261226132614261526162617261826192620262126222623262426252626262726282629263026312632263326342635263626372638263926402641264226432644264526462647264826492650265126522653265426552656265726582659266026612662266326642665266626672668266926702671267226732674267526762677267826792680268126822683268426852686268726882689269026912692269326942695269626972698269927002701270227032704270527062707270827092710271127122713271427152716271727182719272027212722272327242725272627272728272927302731273227332734273527362737273827392740274127422743274427452746274727482749275027512752275327542755275627572758275927602761276227632764276527662767276827692770277127722773277427752776277727782779278027812782278327842785278627872788278927902791279227932794279527962797279827992800280128022803280428052806280728082809281028112812281328142815281628172818281928202821282228232824282528262827282828292830283128322833283428352836283728382839284028412842284328442845284628472848284928502851285228532854285528562857285828592860286128622863286428652866286728682869287028712872287328742875287628772878287928802881288228832884288528862887288828892890289128922893289428952896289728982899290029012902290329042905290629072908290929102911291229132914291529162917291829192920292129222923292429252926292729282929293029312932293329342935293629372938293929402941294229432944294529462947294829492950295129522953295429552956295729582959296029612962296329642965296629672968296929702971297229732974297529762977297829792980298129822983298429852986298729882989299029912992299329942995299629972998299930003001300230033004300530063007300830093010301130123013301430153016301730183019302030213022302330243025302630273028302930303031303230333034303530363037303830393040304130423043304430453046304730483049305030513052305330543055305630573058305930603061306230633064306530663067306830693070307130723073307430753076307730783079308030813082308330843085308630873088308930903091309230933094309530963097309830993100310131023103310431053106310731083109311031113112311331143115311631173118311931203121312231233124312531263127312831293130313131323133313431353136313731383139314031413142314331443145314631473148314931503151315231533154315531563157315831593160316131623163316431653166316731683169317031713172317331743175317631773178317931803181318231833184318531863187318831893190319131923193319431953196319731983199320032013202320332043205320632073208320932103211321232133214321532163217321832193220322132223223322432253226322732283229323032313232323332343235323632373238323932403241324232433244324532463247324832493250325132523253325432553256325732583259326032613262326332643265326632673268326932703271327232733274327532763277327832793280328132823283328432853286328732883289329032913292329332943295329632973298329933003301330233033304330533063307330833093310331133123313331433153316331733183319332033213322332333243325332633273328332933303331333233333334333533363337333833393340334133423343334433453346334733483349335033513352335333543355335633573358335933603361336233633364336533663367336833693370337133723373337433753376337733783379338033813382338333843385338633873388338933903391339233933394339533963397339833993400340134023403340434053406340734083409341034113412341334143415341634173418341934203421342234233424342534263427342834293430343134323433343434353436343734383439344034413442344334443445344634473448344934503451345234533454345534563457345834593460346134623463346434653466346734683469347034713472347334743475347634773478347934803481348234833484348534863487348834893490349134923493349434953496349734983499350035013502350335043505350635073508350935103511351235133514351535163517351835193520352135223523352435253526352735283529353035313532353335343535353635373538353935403541354235433544354535463547354835493550355135523553355435553556355735583559356035613562356335643565356635673568356935703571357235733574357535763577357835793580358135823583358435853586358735883589359035913592359335943595359635973598359936003601360236033604360536063607360836093610361136123613361436153616361736183619362036213622362336243625362636273628362936303631363236333634363536363637363836393640364136423643364436453646364736483649365036513652365336543655365636573658365936603661366236633664366536663667366836693670367136723673367436753676367736783679368036813682368336843685368636873688368936903691369236933694369536963697369836993700370137023703370437053706370737083709371037113712371337143715371637173718371937203721372237233724372537263727372837293730373137323733373437353736373737383739374037413742374337443745374637473748374937503751375237533754375537563757375837593760376137623763376437653766376737683769377037713772377337743775377637773778377937803781378237833784378537863787378837893790379137923793379437953796379737983799380038013802380338043805380638073808380938103811381238133814381538163817381838193820382138223823382438253826382738283829383038313832383338343835383638373838383938403841384238433844384538463847384838493850385138523853385438553856385738583859386038613862386338643865386638673868386938703871387238733874387538763877387838793880388138823883388438853886388738883889389038913892389338943895389638973898389939003901390239033904390539063907390839093910391139123913391439153916391739183919392039213922392339243925392639273928392939303931393239333934393539363937393839393940394139423943394439453946394739483949395039513952395339543955395639573958395939603961396239633964396539663967396839693970397139723973397439753976397739783979398039813982398339843985398639873988398939903991399239933994399539963997399839994000400140024003400440054006400740084009401040114012401340144015401640174018401940204021402240234024402540264027402840294030403140324033403440354036403740384039404040414042404340444045404640474048404940504051405240534054405540564057405840594060406140624063406440654066406740684069407040714072407340744075407640774078407940804081408240834084408540864087408840894090409140924093409440954096409740984099410041014102410341044105410641074108410941104111411241134114411541164117411841194120412141224123412441254126412741284129413041314132413341344135413641374138413941404141414241434144414541464147414841494150415141524153415441554156415741584159416041614162416341644165416641674168416941704171417241734174417541764177417841794180418141824183418441854186418741884189419041914192419341944195419641974198419942004201420242034204420542064207420842094210421142124213421442154216421742184219422042214222422342244225422642274228422942304231423242334234423542364237423842394240424142424243424442454246424742484249425042514252425342544255425642574258425942604261426242634264426542664267426842694270427142724273427442754276427742784279428042814282428342844285428642874288428942904291429242934294429542964297429842994300430143024303430443054306430743084309431043114312431343144315431643174318431943204321432243234324432543264327432843294330433143324333433443354336433743384339434043414342434343444345434643474348434943504351435243534354435543564357435843594360436143624363436443654366436743684369437043714372437343744375437643774378437943804381438243834384438543864387438843894390439143924393439443954396439743984399440044014402440344044405440644074408 |
- /*
- Bullet Continuous Collision Detection and Physics Library
- Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org
- 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.
- */
- bool findSeparatingAxisOnGpu = true;
- bool splitSearchSepAxisConcave = false;
- bool splitSearchSepAxisConvex = true;
- bool useMprGpu = true; //use mpr for edge-edge (+contact point) or sat. Needs testing on main OpenCL platforms, before enabling...
- bool bvhTraversalKernelGPU = true;
- bool findConcaveSeparatingAxisKernelGPU = true;
- bool clipConcaveFacesAndFindContactsCPU = false; //false;//true;
- bool clipConvexFacesAndFindContactsCPU = false; //false;//true;
- bool reduceConcaveContactsOnGPU = true; //false;
- bool reduceConvexContactsOnGPU = true; //false;
- bool findConvexClippingFacesGPU = true;
- bool useGjk = false; ///option for CPU/host testing, when findSeparatingAxisOnGpu = false
- bool useGjkContacts = false; //////option for CPU/host testing when findSeparatingAxisOnGpu = false
- static int myframecount = 0; ///for testing
- ///This file was written by Erwin Coumans
- ///Separating axis rest based on work from Pierre Terdiman, see
- ///And contact clipping based on work from Simon Hobbs
- //#define B3_DEBUG_SAT_FACE
- //#define CHECK_ON_HOST
- #ifdef CHECK_ON_HOST
- //#define PERSISTENT_CONTACTS_HOST
- #endif
- int b3g_actualSATPairTests = 0;
- #include "b3ConvexHullContact.h"
- #include <string.h> //memcpy
- #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
- #include "Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h"
- #include "Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h"
- #include "Bullet3Geometry/b3AabbUtil.h"
- typedef b3AlignedObjectArray<b3Vector3> b3VertexArray;
- #include <float.h> //for FLT_MAX
- #include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
- #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
- //#include "AdlQuaternion.h"
- #include "kernels/satKernels.h"
- #include "kernels/mprKernels.h"
- #include "kernels/satConcaveKernels.h"
- #include "kernels/satClipHullContacts.h"
- #include "kernels/bvhTraversal.h"
- #include "kernels/primitiveContacts.h"
- #include "Bullet3Geometry/b3AabbUtil.h"
- #define BT_NARROWPHASE_SAT_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl"
- #define BT_NARROWPHASE_SAT_CONCAVE_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcave.cl"
- #define BT_NARROWPHASE_MPR_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl"
- #define BT_NARROWPHASE_CLIPHULL_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl"
- #define BT_NARROWPHASE_BVH_TRAVERSAL_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl"
- #define BT_NARROWPHASE_PRIMITIVE_CONTACT_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.cl"
- #ifndef __global
- #define __global
- #endif
- #ifndef __kernel
- #define __kernel
- #endif
- #include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h"
- #include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h"
- #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h"
- #include "Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h"
- #define dot3F4 b3Dot
- GpuSatCollision::GpuSatCollision(cl_context ctx, cl_device_id device, cl_command_queue q)
- : m_context(ctx),
- m_device(device),
- m_queue(q),
- m_findSeparatingAxisKernel(0),
- m_findSeparatingAxisVertexFaceKernel(0),
- m_findSeparatingAxisEdgeEdgeKernel(0),
- m_unitSphereDirections(m_context, m_queue),
- m_totalContactsOut(m_context, m_queue),
- m_sepNormals(m_context, m_queue),
- m_dmins(m_context, m_queue),
- m_hasSeparatingNormals(m_context, m_queue),
- m_concaveSepNormals(m_context, m_queue),
- m_concaveHasSeparatingNormals(m_context, m_queue),
- m_numConcavePairsOut(m_context, m_queue),
- m_gpuCompoundPairs(m_context, m_queue),
- m_gpuCompoundSepNormals(m_context, m_queue),
- m_gpuHasCompoundSepNormals(m_context, m_queue),
- m_numCompoundPairsOut(m_context, m_queue)
- {
- m_totalContactsOut.push_back(0);
- cl_int errNum = 0;
- if (1)
- {
- const char* mprSrc = mprKernelsCL;
- const char* srcConcave = satConcaveKernelsCL;
- char flags[1024] = {0};
- //#ifdef CL_PLATFORM_INTEL
- // sprintf(flags,"-g -s \"%s\"","C:/develop/bullet3_experiments2/opencl/gpu_narrowphase/kernels/sat.cl");
- //#endif
- m_mprPenetrationKernel = 0;
- m_findSeparatingAxisUnitSphereKernel = 0;
- if (useMprGpu)
- {
- cl_program mprProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, mprSrc, &errNum, flags, BT_NARROWPHASE_MPR_PATH);
- b3Assert(errNum == CL_SUCCESS);
- m_mprPenetrationKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, mprSrc, "mprPenetrationKernel", &errNum, mprProg);
- b3Assert(m_mprPenetrationKernel);
- b3Assert(errNum == CL_SUCCESS);
- m_findSeparatingAxisUnitSphereKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, mprSrc, "findSeparatingAxisUnitSphereKernel", &errNum, mprProg);
- b3Assert(m_findSeparatingAxisUnitSphereKernel);
- b3Assert(errNum == CL_SUCCESS);
- int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
- m_unitSphereDirections.resize(numDirections);
- m_unitSphereDirections.copyFromHostPointer(unitSphere162, numDirections, 0, true);
- }
- cl_program satProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, satKernelsCL, &errNum, flags, BT_NARROWPHASE_SAT_PATH);
- b3Assert(errNum == CL_SUCCESS);
- cl_program satConcaveProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, srcConcave, &errNum, flags, BT_NARROWPHASE_SAT_CONCAVE_PATH);
- b3Assert(errNum == CL_SUCCESS);
- m_findSeparatingAxisKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findSeparatingAxisKernel", &errNum, satProg);
- b3Assert(m_findSeparatingAxisKernel);
- b3Assert(errNum == CL_SUCCESS);
- m_findSeparatingAxisVertexFaceKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findSeparatingAxisVertexFaceKernel", &errNum, satProg);
- b3Assert(m_findSeparatingAxisVertexFaceKernel);
- m_findSeparatingAxisEdgeEdgeKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findSeparatingAxisEdgeEdgeKernel", &errNum, satProg);
- b3Assert(m_findSeparatingAxisVertexFaceKernel);
- m_findConcaveSeparatingAxisKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findConcaveSeparatingAxisKernel", &errNum, satProg);
- b3Assert(m_findConcaveSeparatingAxisKernel);
- b3Assert(errNum == CL_SUCCESS);
- m_findConcaveSeparatingAxisVertexFaceKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcConcave, "findConcaveSeparatingAxisVertexFaceKernel", &errNum, satConcaveProg);
- b3Assert(m_findConcaveSeparatingAxisVertexFaceKernel);
- b3Assert(errNum == CL_SUCCESS);
- m_findConcaveSeparatingAxisEdgeEdgeKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcConcave, "findConcaveSeparatingAxisEdgeEdgeKernel", &errNum, satConcaveProg);
- b3Assert(m_findConcaveSeparatingAxisEdgeEdgeKernel);
- b3Assert(errNum == CL_SUCCESS);
- m_findCompoundPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findCompoundPairsKernel", &errNum, satProg);
- b3Assert(m_findCompoundPairsKernel);
- b3Assert(errNum == CL_SUCCESS);
- m_processCompoundPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "processCompoundPairsKernel", &errNum, satProg);
- b3Assert(m_processCompoundPairsKernel);
- b3Assert(errNum == CL_SUCCESS);
- }
- if (1)
- {
- const char* srcClip = satClipKernelsCL;
- char flags[1024] = {0};
- //#ifdef CL_PLATFORM_INTEL
- // sprintf(flags,"-g -s \"%s\"","C:/develop/bullet3_experiments2/opencl/gpu_narrowphase/kernels/satClipHullContacts.cl");
- //#endif
- cl_program satClipContactsProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, srcClip, &errNum, flags, BT_NARROWPHASE_CLIPHULL_PATH);
- b3Assert(errNum == CL_SUCCESS);
- m_clipHullHullKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "clipHullHullKernel", &errNum, satClipContactsProg);
- b3Assert(errNum == CL_SUCCESS);
- m_clipCompoundsHullHullKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "clipCompoundsHullHullKernel", &errNum, satClipContactsProg);
- b3Assert(errNum == CL_SUCCESS);
- m_findClippingFacesKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "findClippingFacesKernel", &errNum, satClipContactsProg);
- b3Assert(errNum == CL_SUCCESS);
- m_clipFacesAndFindContacts = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "clipFacesAndFindContactsKernel", &errNum, satClipContactsProg);
- b3Assert(errNum == CL_SUCCESS);
- m_clipHullHullConcaveConvexKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "clipHullHullConcaveConvexKernel", &errNum, satClipContactsProg);
- b3Assert(errNum == CL_SUCCESS);
- // m_extractManifoldAndAddContactKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,srcClip, "extractManifoldAndAddContactKernel",&errNum,satClipContactsProg);
- // b3Assert(errNum==CL_SUCCESS);
- m_newContactReductionKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip,
- "newContactReductionKernel", &errNum, satClipContactsProg);
- b3Assert(errNum == CL_SUCCESS);
- }
- else
- {
- m_clipHullHullKernel = 0;
- m_clipCompoundsHullHullKernel = 0;
- m_findClippingFacesKernel = 0;
- m_newContactReductionKernel = 0;
- m_clipFacesAndFindContacts = 0;
- m_clipHullHullConcaveConvexKernel = 0;
- // m_extractManifoldAndAddContactKernel = 0;
- }
- if (1)
- {
- const char* srcBvh = bvhTraversalKernelCL;
- cl_program bvhTraversalProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, srcBvh, &errNum, "", BT_NARROWPHASE_BVH_TRAVERSAL_PATH);
- b3Assert(errNum == CL_SUCCESS);
- m_bvhTraversalKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcBvh, "bvhTraversalKernel", &errNum, bvhTraversalProg, "");
- b3Assert(errNum == CL_SUCCESS);
- }
- {
- const char* primitiveContactsSrc = primitiveContactsKernelsCL;
- cl_program primitiveContactsProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, primitiveContactsSrc, &errNum, "", BT_NARROWPHASE_PRIMITIVE_CONTACT_PATH);
- b3Assert(errNum == CL_SUCCESS);
- m_primitiveContactsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, primitiveContactsSrc, "primitiveContactsKernel", &errNum, primitiveContactsProg, "");
- b3Assert(errNum == CL_SUCCESS);
- m_findConcaveSphereContactsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, primitiveContactsSrc, "findConcaveSphereContactsKernel", &errNum, primitiveContactsProg);
- b3Assert(errNum == CL_SUCCESS);
- b3Assert(m_findConcaveSphereContactsKernel);
- m_processCompoundPairsPrimitivesKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, primitiveContactsSrc, "processCompoundPairsPrimitivesKernel", &errNum, primitiveContactsProg, "");
- b3Assert(errNum == CL_SUCCESS);
- b3Assert(m_processCompoundPairsPrimitivesKernel);
- }
- }
- GpuSatCollision::~GpuSatCollision()
- {
- if (m_findSeparatingAxisVertexFaceKernel)
- clReleaseKernel(m_findSeparatingAxisVertexFaceKernel);
- if (m_findSeparatingAxisEdgeEdgeKernel)
- clReleaseKernel(m_findSeparatingAxisEdgeEdgeKernel);
- if (m_findSeparatingAxisUnitSphereKernel)
- clReleaseKernel(m_findSeparatingAxisUnitSphereKernel);
- if (m_mprPenetrationKernel)
- clReleaseKernel(m_mprPenetrationKernel);
- if (m_findSeparatingAxisKernel)
- clReleaseKernel(m_findSeparatingAxisKernel);
- if (m_findConcaveSeparatingAxisVertexFaceKernel)
- clReleaseKernel(m_findConcaveSeparatingAxisVertexFaceKernel);
- if (m_findConcaveSeparatingAxisEdgeEdgeKernel)
- clReleaseKernel(m_findConcaveSeparatingAxisEdgeEdgeKernel);
- if (m_findConcaveSeparatingAxisKernel)
- clReleaseKernel(m_findConcaveSeparatingAxisKernel);
- if (m_findCompoundPairsKernel)
- clReleaseKernel(m_findCompoundPairsKernel);
- if (m_processCompoundPairsKernel)
- clReleaseKernel(m_processCompoundPairsKernel);
- if (m_findClippingFacesKernel)
- clReleaseKernel(m_findClippingFacesKernel);
- if (m_clipFacesAndFindContacts)
- clReleaseKernel(m_clipFacesAndFindContacts);
- if (m_newContactReductionKernel)
- clReleaseKernel(m_newContactReductionKernel);
- if (m_primitiveContactsKernel)
- clReleaseKernel(m_primitiveContactsKernel);
- if (m_findConcaveSphereContactsKernel)
- clReleaseKernel(m_findConcaveSphereContactsKernel);
- if (m_processCompoundPairsPrimitivesKernel)
- clReleaseKernel(m_processCompoundPairsPrimitivesKernel);
- if (m_clipHullHullKernel)
- clReleaseKernel(m_clipHullHullKernel);
- if (m_clipCompoundsHullHullKernel)
- clReleaseKernel(m_clipCompoundsHullHullKernel);
- if (m_clipHullHullConcaveConvexKernel)
- clReleaseKernel(m_clipHullHullConcaveConvexKernel);
- // if (m_extractManifoldAndAddContactKernel)
- // clReleaseKernel(m_extractManifoldAndAddContactKernel);
- if (m_bvhTraversalKernel)
- clReleaseKernel(m_bvhTraversalKernel);
- }
- struct MyTriangleCallback : public b3NodeOverlapCallback
- {
- int m_bodyIndexA;
- int m_bodyIndexB;
- virtual void processNode(int subPart, int triangleIndex)
- {
- printf("bodyIndexA %d, bodyIndexB %d\n", m_bodyIndexA, m_bodyIndexB);
- printf("triangleIndex %d\n", triangleIndex);
- }
- };
- #define float4 b3Vector3
- #define make_float4(x, y, z, w) b3MakeVector3(x, y, z, w)
- float signedDistanceFromPointToPlane(const float4& point, const float4& planeEqn, float4* closestPointOnFace)
- {
- float4 n = planeEqn;
- n[3] = 0.f;
- float dist = dot3F4(n, point) + planeEqn[3];
- *closestPointOnFace = point - dist * n;
- return dist;
- }
- #define cross3(a, b) (a.cross(b))
- b3Vector3 transform(const b3Vector3* v, const b3Vector3* pos, const b3Quaternion* orn)
- {
- b3Transform tr;
- tr.setIdentity();
- tr.setOrigin(*pos);
- tr.setRotation(*orn);
- b3Vector3 res = tr(*v);
- return res;
- }
- inline bool IsPointInPolygon(const float4& p,
- const b3GpuFace* face,
- const float4* baseVertex,
- const int* convexIndices,
- float4* out)
- {
- float4 a;
- float4 b;
- float4 ab;
- float4 ap;
- float4 v;
- float4 plane = b3MakeVector3(face->m_plane.x, face->m_plane.y, face->m_plane.z, 0.f);
- if (face->m_numIndices < 2)
- return false;
- float4 v0 = baseVertex[convexIndices[face->m_indexOffset + face->m_numIndices - 1]];
- b = v0;
- for (unsigned i = 0; i != face->m_numIndices; ++i)
- {
- a = b;
- float4 vi = baseVertex[convexIndices[face->m_indexOffset + i]];
- b = vi;
- ab = b - a;
- ap = p - a;
- v = cross3(ab, plane);
- if (b3Dot(ap, v) > 0.f)
- {
- float ab_m2 = b3Dot(ab, ab);
- float rt = ab_m2 != 0.f ? b3Dot(ab, ap) / ab_m2 : 0.f;
- if (rt <= 0.f)
- {
- *out = a;
- }
- else if (rt >= 1.f)
- {
- *out = b;
- }
- else
- {
- float s = 1.f - rt;
- out[0].x = s * a.x + rt * b.x;
- out[0].y = s * a.y + rt * b.y;
- out[0].z = s * a.z + rt * b.z;
- }
- return false;
- }
- }
- return true;
- }
- #define normalize3(a) (a.normalize())
- int extractManifoldSequentialGlobal(const float4* p, int nPoints, const float4& nearNormal, b3Int4* contactIdx)
- {
- if (nPoints == 0)
- return 0;
- if (nPoints <= 4)
- return nPoints;
- if (nPoints > 64)
- nPoints = 64;
- float4 center = b3MakeVector3(0, 0, 0, 0);
- {
- for (int i = 0; i < nPoints; i++)
- center += p[i];
- center /= (float)nPoints;
- }
- // sample 4 directions
- float4 aVector = p[0] - center;
- float4 u = cross3(nearNormal, aVector);
- float4 v = cross3(nearNormal, u);
- u = normalize3(u);
- v = normalize3(v);
- //keep point with deepest penetration
- float minW = FLT_MAX;
- int minIndex = -1;
- float4 maxDots;
- maxDots.x = FLT_MIN;
- maxDots.y = FLT_MIN;
- maxDots.z = FLT_MIN;
- maxDots.w = FLT_MIN;
- // idx, distance
- for (int ie = 0; ie < nPoints; ie++)
- {
- if (p[ie].w < minW)
- {
- minW = p[ie].w;
- minIndex = ie;
- }
- float f;
- float4 r = p[ie] - center;
- f = dot3F4(u, r);
- if (f < maxDots.x)
- {
- maxDots.x = f;
- contactIdx[0].x = ie;
- }
- f = dot3F4(-u, r);
- if (f < maxDots.y)
- {
- maxDots.y = f;
- contactIdx[0].y = ie;
- }
- f = dot3F4(v, r);
- if (f < maxDots.z)
- {
- maxDots.z = f;
- contactIdx[0].z = ie;
- }
- f = dot3F4(-v, r);
- if (f < maxDots.w)
- {
- maxDots.w = f;
- contactIdx[0].w = ie;
- }
- }
- if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
- {
- //replace the first contact with minimum (todo: replace contact with least penetration)
- contactIdx[0].x = minIndex;
- }
- return 4;
- }
- #define MAX_VERTS 1024
- inline void project(const b3ConvexPolyhedronData& hull, const float4& pos, const b3Quaternion& orn, const float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
- {
- min = FLT_MAX;
- max = -FLT_MAX;
- int numVerts = hull.m_numVertices;
- const float4 localDir = b3QuatRotate(orn.inverse(), dir);
- b3Scalar offset = dot3F4(pos, dir);
- for (int i = 0; i < numVerts; i++)
- {
- //b3Vector3 pt = trans * vertices[m_vertexOffset+i];
- //b3Scalar dp = pt.dot(dir);
- //b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
- b3Scalar dp = dot3F4((float4&)vertices[hull.m_vertexOffset + i], localDir);
- //b3Assert(dp==dpL);
- if (dp < min) min = dp;
- if (dp > max) max = dp;
- }
- if (min > max)
- {
- b3Scalar tmp = min;
- min = max;
- max = tmp;
- }
- min += offset;
- max += offset;
- }
- static bool TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
- const float4& posA, const b3Quaternion& ornA,
- const float4& posB, const b3Quaternion& ornB,
- const float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB, b3Scalar& depth)
- {
- b3Scalar Min0, Max0;
- b3Scalar Min1, Max1;
- project(hullA, posA, ornA, sep_axis, verticesA, Min0, Max0);
- project(hullB, posB, ornB, sep_axis, verticesB, Min1, Max1);
- if (Max0 < Min1 || Max1 < Min0)
- return false;
- b3Scalar d0 = Max0 - Min1;
- assert(d0 >= 0.0f);
- b3Scalar d1 = Max1 - Min0;
- assert(d1 >= 0.0f);
- depth = d0 < d1 ? d0 : d1;
- return true;
- }
- inline bool IsAlmostZero(const b3Vector3& v)
- {
- if (fabsf(v.x) > 1e-6 || fabsf(v.y) > 1e-6 || fabsf(v.z) > 1e-6) return false;
- return true;
- }
- static bool findSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
- const float4& posA1,
- const b3Quaternion& ornA,
- const float4& posB1,
- const b3Quaternion& ornB,
- const b3AlignedObjectArray<b3Vector3>& verticesA,
- const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
- const b3AlignedObjectArray<b3GpuFace>& facesA,
- const b3AlignedObjectArray<int>& indicesA,
- const b3AlignedObjectArray<b3Vector3>& verticesB,
- const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
- const b3AlignedObjectArray<b3GpuFace>& facesB,
- const b3AlignedObjectArray<int>& indicesB,
- b3Vector3& sep)
- {
- B3_PROFILE("findSeparatingAxis");
- b3g_actualSATPairTests++;
- float4 posA = posA1;
- posA.w = 0.f;
- float4 posB = posB1;
- posB.w = 0.f;
- //#ifdef TEST_INTERNAL_OBJECTS
- float4 c0local = (float4&)hullA.m_localCenter;
- float4 c0 = transform(&c0local, &posA, &ornA);
- float4 c1local = (float4&)hullB.m_localCenter;
- float4 c1 = transform(&c1local, &posB, &ornB);
- const float4 deltaC2 = c0 - c1;
- //#endif
- b3Scalar dmin = FLT_MAX;
- int curPlaneTests = 0;
- int numFacesA = hullA.m_numFaces;
- // Test normals from hullA
- for (int i = 0; i < numFacesA; i++)
- {
- const float4& normal = (float4&)facesA[hullA.m_faceOffset + i].m_plane;
- float4 faceANormalWS = b3QuatRotate(ornA, normal);
- if (dot3F4(deltaC2, faceANormalWS) < 0)
- faceANormalWS *= -1.f;
- curPlaneTests++;
- #ifdef TEST_INTERNAL_OBJECTS
- gExpectedNbTests++;
- if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
- continue;
- gActualNbTests++;
- #endif
- b3Scalar d;
- if (!TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d))
- return false;
- if (d < dmin)
- {
- dmin = d;
- sep = (b3Vector3&)faceANormalWS;
- }
- }
- int numFacesB = hullB.m_numFaces;
- // Test normals from hullB
- for (int i = 0; i < numFacesB; i++)
- {
- float4 normal = (float4&)facesB[hullB.m_faceOffset + i].m_plane;
- float4 WorldNormal = b3QuatRotate(ornB, normal);
- if (dot3F4(deltaC2, WorldNormal) < 0)
- {
- WorldNormal *= -1.f;
- }
- curPlaneTests++;
- #ifdef TEST_INTERNAL_OBJECTS
- gExpectedNbTests++;
- if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
- continue;
- gActualNbTests++;
- #endif
- b3Scalar d;
- if (!TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d))
- return false;
- if (d < dmin)
- {
- dmin = d;
- sep = (b3Vector3&)WorldNormal;
- }
- }
- int curEdgeEdge = 0;
- // Test edges
- for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++)
- {
- const float4& edge0 = (float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0];
- float4 edge0World = b3QuatRotate(ornA, (float4&)edge0);
- for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++)
- {
- const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1];
- float4 edge1World = b3QuatRotate(ornB, (float4&)edge1);
- float4 crossje = cross3(edge0World, edge1World);
- curEdgeEdge++;
- if (!IsAlmostZero((b3Vector3&)crossje))
- {
- crossje = normalize3(crossje);
- if (dot3F4(deltaC2, crossje) < 0)
- crossje *= -1.f;
- #ifdef TEST_INTERNAL_OBJECTS
- gExpectedNbTests++;
- if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
- continue;
- gActualNbTests++;
- #endif
- b3Scalar dist;
- if (!TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist))
- return false;
- if (dist < dmin)
- {
- dmin = dist;
- sep = (b3Vector3&)crossje;
- }
- }
- }
- }
- if ((dot3F4(-deltaC2, (float4&)sep)) > 0.0f)
- sep = -sep;
- return true;
- }
- bool findSeparatingAxisEdgeEdge(__global const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
- const b3Float4& posA1,
- const b3Quat& ornA,
- const b3Float4& posB1,
- const b3Quat& ornB,
- const b3Float4& DeltaC2,
- __global const b3AlignedObjectArray<float4>& vertices,
- __global const b3AlignedObjectArray<float4>& uniqueEdges,
- __global const b3AlignedObjectArray<b3GpuFace>& faces,
- __global const b3AlignedObjectArray<int>& indices,
- float4* sep,
- float* dmin)
- {
- // int i = get_global_id(0);
- float4 posA = posA1;
- posA.w = 0.f;
- float4 posB = posB1;
- posB.w = 0.f;
- //int curPlaneTests=0;
- int curEdgeEdge = 0;
- // Test edges
- for (int e0 = 0; e0 < hullA->m_numUniqueEdges; e0++)
- {
- const float4 edge0 = uniqueEdges[hullA->m_uniqueEdgesOffset + e0];
- float4 edge0World = b3QuatRotate(ornA, edge0);
- for (int e1 = 0; e1 < hullB->m_numUniqueEdges; e1++)
- {
- const float4 edge1 = uniqueEdges[hullB->m_uniqueEdgesOffset + e1];
- float4 edge1World = b3QuatRotate(ornB, edge1);
- float4 crossje = cross3(edge0World, edge1World);
- curEdgeEdge++;
- if (!IsAlmostZero(crossje))
- {
- crossje = normalize3(crossje);
- if (dot3F4(DeltaC2, crossje) < 0)
- crossje *= -1.f;
- float dist;
- bool result = true;
- {
- float Min0, Max0;
- float Min1, Max1;
- project(*hullA, posA, ornA, crossje, vertices, Min0, Max0);
- project(*hullB, posB, ornB, crossje, vertices, Min1, Max1);
- if (Max0 < Min1 || Max1 < Min0)
- result = false;
- float d0 = Max0 - Min1;
- float d1 = Max1 - Min0;
- dist = d0 < d1 ? d0 : d1;
- result = true;
- }
- if (dist < *dmin)
- {
- *dmin = dist;
- *sep = crossje;
- }
- }
- }
- }
- if ((dot3F4(-DeltaC2, *sep)) > 0.0f)
- {
- *sep = -(*sep);
- }
- return true;
- }
- __inline float4 lerp3(const float4& a, const float4& b, float t)
- {
- return b3MakeVector3(a.x + (b.x - a.x) * t,
- a.y + (b.y - a.y) * t,
- a.z + (b.z - a.z) * t,
- 0.f);
- }
- // Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
- int clipFace(const float4* pVtxIn, int numVertsIn, float4& planeNormalWS, float planeEqWS, float4* ppVtxOut)
- {
- int ve;
- float ds, de;
- int numVertsOut = 0;
- if (numVertsIn < 2)
- return 0;
- float4 firstVertex = pVtxIn[numVertsIn - 1];
- float4 endVertex = pVtxIn[0];
- ds = dot3F4(planeNormalWS, firstVertex) + planeEqWS;
- for (ve = 0; ve < numVertsIn; ve++)
- {
- endVertex = pVtxIn[ve];
- de = dot3F4(planeNormalWS, endVertex) + planeEqWS;
- if (ds < 0)
- {
- if (de < 0)
- {
- // Start < 0, end < 0, so output endVertex
- ppVtxOut[numVertsOut++] = endVertex;
- }
- else
- {
- // Start < 0, end >= 0, so output intersection
- ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
- }
- }
- else
- {
- if (de < 0)
- {
- // Start >= 0, end < 0 so output intersection and end
- ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
- ppVtxOut[numVertsOut++] = endVertex;
- }
- }
- firstVertex = endVertex;
- ds = de;
- }
- return numVertsOut;
- }
- int clipFaceAgainstHull(const float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
- const float4& posA, const b3Quaternion& ornA, float4* worldVertsB1, int numWorldVertsB1,
- float4* worldVertsB2, int capacityWorldVertsB2,
- const float minDist, float maxDist,
- const b3AlignedObjectArray<float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
- //const float4* verticesB, const b3GpuFace* facesB, const int* indicesB,
- float4* contactsOut,
- int contactCapacity)
- {
- int numContactsOut = 0;
- float4* pVtxIn = worldVertsB1;
- float4* pVtxOut = worldVertsB2;
- int numVertsIn = numWorldVertsB1;
- int numVertsOut = 0;
- int closestFaceA = -1;
- {
- float dmin = FLT_MAX;
- for (int face = 0; face < hullA->m_numFaces; face++)
- {
- const float4 Normal = b3MakeVector3(
- facesA[hullA->m_faceOffset + face].m_plane.x,
- facesA[hullA->m_faceOffset + face].m_plane.y,
- facesA[hullA->m_faceOffset + face].m_plane.z, 0.f);
- const float4 faceANormalWS = b3QuatRotate(ornA, Normal);
- float d = dot3F4(faceANormalWS, separatingNormal);
- if (d < dmin)
- {
- dmin = d;
- closestFaceA = face;
- }
- }
- }
- if (closestFaceA < 0)
- return numContactsOut;
- b3GpuFace polyA = facesA[hullA->m_faceOffset + closestFaceA];
- // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
- // int numContacts = numWorldVertsB1;
- int numVerticesA = polyA.m_numIndices;
- for (int e0 = 0; e0 < numVerticesA; e0++)
- {
- const float4 a = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + e0]];
- const float4 b = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + ((e0 + 1) % numVerticesA)]];
- const float4 edge0 = a - b;
- const float4 WorldEdge0 = b3QuatRotate(ornA, edge0);
- float4 planeNormalA = make_float4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
- float4 worldPlaneAnormal1 = b3QuatRotate(ornA, planeNormalA);
- float4 planeNormalWS1 = -cross3(WorldEdge0, worldPlaneAnormal1);
- float4 worldA1 = transform(&a, &posA, &ornA);
- float planeEqWS1 = -dot3F4(worldA1, planeNormalWS1);
- float4 planeNormalWS = planeNormalWS1;
- float planeEqWS = planeEqWS1;
- //clip face
- //clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
- numVertsOut = clipFace(pVtxIn, numVertsIn, planeNormalWS, planeEqWS, pVtxOut);
- //btSwap(pVtxIn,pVtxOut);
- float4* tmp = pVtxOut;
- pVtxOut = pVtxIn;
- pVtxIn = tmp;
- numVertsIn = numVertsOut;
- numVertsOut = 0;
- }
- // only keep points that are behind the witness face
- {
- float4 localPlaneNormal = make_float4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
- float localPlaneEq = polyA.m_plane.w;
- float4 planeNormalWS = b3QuatRotate(ornA, localPlaneNormal);
- float planeEqWS = localPlaneEq - dot3F4(planeNormalWS, posA);
- for (int i = 0; i < numVertsIn; i++)
- {
- float depth = dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS;
- if (depth <= minDist)
- {
- depth = minDist;
- }
- if (numContactsOut < contactCapacity)
- {
- if (depth <= maxDist)
- {
- float4 pointInWorld = pVtxIn[i];
- //resultOut.addContactPoint(separatingNormal,point,depth);
- contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
- //printf("depth=%f\n",depth);
- }
- }
- else
- {
- b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity);
- }
- }
- }
- return numContactsOut;
- }
- static int clipHullAgainstHull(const float4& separatingNormal,
- const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
- const float4& posA, const b3Quaternion& ornA, const float4& posB, const b3Quaternion& ornB,
- float4* worldVertsB1, float4* worldVertsB2, int capacityWorldVerts,
- const float minDist, float maxDist,
- const b3AlignedObjectArray<float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
- const b3AlignedObjectArray<float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
- float4* contactsOut,
- int contactCapacity)
- {
- int numContactsOut = 0;
- int numWorldVertsB1 = 0;
- B3_PROFILE("clipHullAgainstHull");
- // float curMaxDist=maxDist;
- int closestFaceB = -1;
- float dmax = -FLT_MAX;
- {
- //B3_PROFILE("closestFaceB");
- if (hullB.m_numFaces != 1)
- {
- //printf("wtf\n");
- }
- static bool once = true;
- //printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
- for (int face = 0; face < hullB.m_numFaces; face++)
- {
- #ifdef BT_DEBUG_SAT_FACE
- if (once)
- printf("face %d\n", face);
- const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face];
- if (once)
- {
- for (int i = 0; i < faceB->m_numIndices; i++)
- {
- float4 vert = verticesB[hullB.m_vertexOffset + indicesB[faceB->m_indexOffset + i]];
- printf("vert[%d] = %f,%f,%f\n", i, vert.x, vert.y, vert.z);
- }
- }
- #endif //BT_DEBUG_SAT_FACE \
- //if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
- {
- const float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset + face].m_plane.x,
- facesB[hullB.m_faceOffset + face].m_plane.y, facesB[hullB.m_faceOffset + face].m_plane.z, 0.f);
- const float4 WorldNormal = b3QuatRotate(ornB, Normal);
- #ifdef BT_DEBUG_SAT_FACE
- if (once)
- printf("faceNormal = %f,%f,%f\n", Normal.x, Normal.y, Normal.z);
- #endif
- float d = dot3F4(WorldNormal, separatingNormal);
- if (d > dmax)
- {
- dmax = d;
- closestFaceB = face;
- }
- }
- }
- once = false;
- }
- b3Assert(closestFaceB >= 0);
- {
- //B3_PROFILE("worldVertsB1");
- const b3GpuFace& polyB = facesB[hullB.m_faceOffset + closestFaceB];
- const int numVertices = polyB.m_numIndices;
- for (int e0 = 0; e0 < numVertices; e0++)
- {
- const float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
- worldVertsB1[numWorldVertsB1++] = transform(&b, &posB, &ornB);
- }
- }
- if (closestFaceB >= 0)
- {
- //B3_PROFILE("clipFaceAgainstHull");
- numContactsOut = clipFaceAgainstHull((float4&)separatingNormal, &hullA,
- posA, ornA,
- worldVertsB1, numWorldVertsB1, worldVertsB2, capacityWorldVerts, minDist, maxDist,
- verticesA, facesA, indicesA,
- contactsOut, contactCapacity);
- }
- return numContactsOut;
- }
- #define PARALLEL_SUM(v, n) \
- for (int j = 1; j < n; j++) v[0] += v[j];
- #define PARALLEL_DO(execution, n) \
- for (int ie = 0; ie < n; ie++) \
- { \
- execution; \
- }
- #define REDUCE_MAX(v, n) \
- { \
- int i = 0; \
- for (int offset = 0; offset < n; offset++) v[i] = (v[i].y > v[i + offset].y) ? v[i] : v[i + offset]; \
- }
- #define REDUCE_MIN(v, n) \
- { \
- int i = 0; \
- for (int offset = 0; offset < n; offset++) v[i] = (v[i].y < v[i + offset].y) ? v[i] : v[i + offset]; \
- }
- int extractManifold(const float4* p, int nPoints, const float4& nearNormal, b3Int4* contactIdx)
- {
- if (nPoints == 0)
- return 0;
- if (nPoints <= 4)
- return nPoints;
- if (nPoints > 64)
- nPoints = 64;
- float4 center = make_float4(0, 0, 0, 0);
- {
- for (int i = 0; i < nPoints; i++)
- center += p[i];
- center /= (float)nPoints;
- }
- // sample 4 directions
- float4 aVector = p[0] - center;
- float4 u = cross3(nearNormal, aVector);
- float4 v = cross3(nearNormal, u);
- u = normalize3(u);
- v = normalize3(v);
- //keep point with deepest penetration
- float minW = FLT_MAX;
- int minIndex = -1;
- float4 maxDots;
- maxDots.x = FLT_MIN;
- maxDots.y = FLT_MIN;
- maxDots.z = FLT_MIN;
- maxDots.w = FLT_MIN;
- // idx, distance
- for (int ie = 0; ie < nPoints; ie++)
- {
- if (p[ie].w < minW)
- {
- minW = p[ie].w;
- minIndex = ie;
- }
- float f;
- float4 r = p[ie] - center;
- f = dot3F4(u, r);
- if (f < maxDots.x)
- {
- maxDots.x = f;
- contactIdx[0].x = ie;
- }
- f = dot3F4(-u, r);
- if (f < maxDots.y)
- {
- maxDots.y = f;
- contactIdx[0].y = ie;
- }
- f = dot3F4(v, r);
- if (f < maxDots.z)
- {
- maxDots.z = f;
- contactIdx[0].z = ie;
- }
- f = dot3F4(-v, r);
- if (f < maxDots.w)
- {
- maxDots.w = f;
- contactIdx[0].w = ie;
- }
- }
- if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
- {
- //replace the first contact with minimum (todo: replace contact with least penetration)
- contactIdx[0].x = minIndex;
- }
- return 4;
- }
- int clipHullHullSingle(
- int bodyIndexA, int bodyIndexB,
- const float4& posA,
- const b3Quaternion& ornA,
- const float4& posB,
- const b3Quaternion& ornB,
- int collidableIndexA, int collidableIndexB,
- const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
- b3AlignedObjectArray<b3Contact4>* globalContactOut,
- int& nContacts,
- const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
- const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
- const b3AlignedObjectArray<b3Vector3>& verticesA,
- const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
- const b3AlignedObjectArray<b3GpuFace>& facesA,
- const b3AlignedObjectArray<int>& indicesA,
- const b3AlignedObjectArray<b3Vector3>& verticesB,
- const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
- const b3AlignedObjectArray<b3GpuFace>& facesB,
- const b3AlignedObjectArray<int>& indicesB,
- const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
- const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
- const b3Vector3& sepNormalWorldSpace,
- int maxContactCapacity)
- {
- int contactIndex = -1;
- b3ConvexPolyhedronData hullA, hullB;
- b3Collidable colA = hostCollidablesA[collidableIndexA];
- hullA = hostConvexDataA[colA.m_shapeIndex];
- //printf("numvertsA = %d\n",hullA.m_numVertices);
- b3Collidable colB = hostCollidablesB[collidableIndexB];
- hullB = hostConvexDataB[colB.m_shapeIndex];
- //printf("numvertsB = %d\n",hullB.m_numVertices);
- float4 contactsOut[MAX_VERTS];
- int localContactCapacity = MAX_VERTS;
- #ifdef _WIN32
- b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
- b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
- #endif
- {
- float4 worldVertsB1[MAX_VERTS];
- float4 worldVertsB2[MAX_VERTS];
- int capacityWorldVerts = MAX_VERTS;
- float4 hostNormal = make_float4(sepNormalWorldSpace.x, sepNormalWorldSpace.y, sepNormalWorldSpace.z, 0.f);
- int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
- int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
- b3Scalar minDist = -1;
- b3Scalar maxDist = 0.;
- b3Transform trA, trB;
- {
- //B3_PROFILE("transform computation");
- //trA.setIdentity();
- trA.setOrigin(b3MakeVector3(posA.x, posA.y, posA.z));
- trA.setRotation(b3Quaternion(ornA.x, ornA.y, ornA.z, ornA.w));
- //trB.setIdentity();
- trB.setOrigin(b3MakeVector3(posB.x, posB.y, posB.z));
- trB.setRotation(b3Quaternion(ornB.x, ornB.y, ornB.z, ornB.w));
- }
- b3Quaternion trAorn = trA.getRotation();
- b3Quaternion trBorn = trB.getRotation();
- int numContactsOut = clipHullAgainstHull(hostNormal,
- hostConvexDataA.at(shapeA),
- hostConvexDataB.at(shapeB),
- (float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
- (float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
- worldVertsB1, worldVertsB2, capacityWorldVerts,
- minDist, maxDist,
- verticesA, facesA, indicesA,
- verticesB, facesB, indicesB,
- contactsOut, localContactCapacity);
- if (numContactsOut > 0)
- {
- B3_PROFILE("overlap");
- float4 normalOnSurfaceB = (float4&)hostNormal;
- b3Int4 contactIdx;
- contactIdx.x = 0;
- contactIdx.y = 1;
- contactIdx.z = 2;
- contactIdx.w = 3;
- int numPoints = 0;
- {
- // B3_PROFILE("extractManifold");
- numPoints = extractManifold(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
- }
- b3Assert(numPoints);
- if (nContacts < maxContactCapacity)
- {
- contactIndex = nContacts;
- globalContactOut->expand();
- b3Contact4& contact = globalContactOut->at(nContacts);
- contact.m_batchIdx = 0; //i;
- contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
- contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
- contact.m_frictionCoeffCmp = 45874;
- contact.m_restituitionCoeffCmp = 0;
- // float distance = 0.f;
- for (int p = 0; p < numPoints; p++)
- {
- contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]]; //check if it is actually on B
- contact.m_worldNormalOnB = normalOnSurfaceB;
- }
- //printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
- contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
- nContacts++;
- }
- else
- {
- b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
- }
- }
- }
- return contactIndex;
- }
- void computeContactPlaneConvex(int pairIndex,
- int bodyIndexA, int bodyIndexB,
- int collidableIndexA, int collidableIndexB,
- const b3RigidBodyData* rigidBodies,
- const b3Collidable* collidables,
- const b3ConvexPolyhedronData* convexShapes,
- const b3Vector3* convexVertices,
- const int* convexIndices,
- const b3GpuFace* faces,
- b3Contact4* globalContactsOut,
- int& nGlobalContactsOut,
- int maxContactCapacity)
- {
- int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
- const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndex];
- b3Vector3 posB = rigidBodies[bodyIndexB].m_pos;
- b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
- b3Vector3 posA = rigidBodies[bodyIndexA].m_pos;
- b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
- // int numContactsOut = 0;
- // int numWorldVertsB1= 0;
- b3Vector3 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
- b3Vector3 planeNormal = b3MakeVector3(planeEq.x, planeEq.y, planeEq.z);
- b3Vector3 planeNormalWorld = b3QuatRotate(ornA, planeNormal);
- float planeConstant = planeEq.w;
- b3Transform convexWorldTransform;
- convexWorldTransform.setIdentity();
- convexWorldTransform.setOrigin(posB);
- convexWorldTransform.setRotation(ornB);
- b3Transform planeTransform;
- planeTransform.setIdentity();
- planeTransform.setOrigin(posA);
- planeTransform.setRotation(ornA);
- b3Transform planeInConvex;
- planeInConvex = convexWorldTransform.inverse() * planeTransform;
- b3Transform convexInPlane;
- convexInPlane = planeTransform.inverse() * convexWorldTransform;
- b3Vector3 planeNormalInConvex = planeInConvex.getBasis() * -planeNormal;
- float maxDot = -1e30;
- int hitVertex = -1;
- b3Vector3 hitVtx;
- #define MAX_PLANE_CONVEX_POINTS 64
- b3Vector3 contactPoints[MAX_PLANE_CONVEX_POINTS];
- int numPoints = 0;
- b3Int4 contactIdx;
- contactIdx.s[0] = 0;
- contactIdx.s[1] = 1;
- contactIdx.s[2] = 2;
- contactIdx.s[3] = 3;
- for (int i = 0; i < hullB->m_numVertices; i++)
- {
- b3Vector3 vtx = convexVertices[hullB->m_vertexOffset + i];
- float curDot = vtx.dot(planeNormalInConvex);
- if (curDot > maxDot)
- {
- hitVertex = i;
- maxDot = curDot;
- hitVtx = vtx;
- //make sure the deepest points is always included
- if (numPoints == MAX_PLANE_CONVEX_POINTS)
- numPoints--;
- }
- if (numPoints < MAX_PLANE_CONVEX_POINTS)
- {
- b3Vector3 vtxWorld = convexWorldTransform * vtx;
- b3Vector3 vtxInPlane = planeTransform.inverse() * vtxWorld;
- float dist = planeNormal.dot(vtxInPlane) - planeConstant;
- if (dist < 0.f)
- {
- vtxWorld.w = dist;
- contactPoints[numPoints] = vtxWorld;
- numPoints++;
- }
- }
- }
- int numReducedPoints = 0;
- numReducedPoints = numPoints;
- if (numPoints > 4)
- {
- numReducedPoints = extractManifoldSequentialGlobal(contactPoints, numPoints, planeNormalInConvex, &contactIdx);
- }
- int dstIdx;
- // dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
- if (numReducedPoints > 0)
- {
- if (nGlobalContactsOut < maxContactCapacity)
- {
- dstIdx = nGlobalContactsOut;
- nGlobalContactsOut++;
- b3Contact4* c = &globalContactsOut[dstIdx];
- c->m_worldNormalOnB = -planeNormalWorld;
- c->setFrictionCoeff(0.7);
- c->setRestituitionCoeff(0.f);
- c->m_batchIdx = pairIndex;
- c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
- c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
- for (int i = 0; i < numReducedPoints; i++)
- {
- b3Vector3 pOnB1 = contactPoints[contactIdx.s[i]];
- c->m_worldPosB[i] = pOnB1;
- }
- c->m_worldNormalOnB.w = (b3Scalar)numReducedPoints;
- } //if (dstIdx < numPairs)
- }
- // printf("computeContactPlaneConvex\n");
- }
- B3_FORCE_INLINE b3Vector3 MyUnQuantize(const unsigned short* vecIn, const b3Vector3& quantization, const b3Vector3& bvhAabbMin)
- {
- b3Vector3 vecOut;
- vecOut.setValue(
- (b3Scalar)(vecIn[0]) / (quantization.x),
- (b3Scalar)(vecIn[1]) / (quantization.y),
- (b3Scalar)(vecIn[2]) / (quantization.z));
- vecOut += bvhAabbMin;
- return vecOut;
- }
- void traverseTreeTree()
- {
- }
- #include "Bullet3Common/shared/b3Mat3x3.h"
- int numAabbChecks = 0;
- int maxNumAabbChecks = 0;
- int maxDepth = 0;
- // work-in-progress
- __kernel void findCompoundPairsKernel(
- int pairIndex,
- int bodyIndexA,
- int bodyIndexB,
- int collidableIndexA,
- int collidableIndexB,
- __global const b3RigidBodyData* rigidBodies,
- __global const b3Collidable* collidables,
- __global const b3ConvexPolyhedronData* convexShapes,
- __global const b3AlignedObjectArray<b3Float4>& vertices,
- __global const b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace,
- __global const b3AlignedObjectArray<b3Aabb>& aabbsLocalSpace,
- __global const b3GpuChildShape* gpuChildShapes,
- __global b3Int4* gpuCompoundPairsOut,
- __global int* numCompoundPairsOut,
- int maxNumCompoundPairsCapacity,
- b3AlignedObjectArray<b3QuantizedBvhNode>& treeNodesCPU,
- b3AlignedObjectArray<b3BvhSubtreeInfo>& subTreesCPU,
- b3AlignedObjectArray<b3BvhInfo>& bvhInfoCPU)
- {
- numAabbChecks = 0;
- maxNumAabbChecks = 0;
- // int i = pairIndex;
- {
- int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
- int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
- //once the broadphase avoids static-static pairs, we can remove this test
- if ((rigidBodies[bodyIndexA].m_invMass == 0) && (rigidBodies[bodyIndexB].m_invMass == 0))
- {
- return;
- }
- if ((collidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) && (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS))
- {
- int bvhA = collidables[collidableIndexA].m_compoundBvhIndex;
- int bvhB = collidables[collidableIndexB].m_compoundBvhIndex;
- int numSubTreesA = bvhInfoCPU[bvhA].m_numSubTrees;
- int subTreesOffsetA = bvhInfoCPU[bvhA].m_subTreeOffset;
- int subTreesOffsetB = bvhInfoCPU[bvhB].m_subTreeOffset;
- int numSubTreesB = bvhInfoCPU[bvhB].m_numSubTrees;
- float4 posA = rigidBodies[bodyIndexA].m_pos;
- b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
- b3Transform transA;
- transA.setIdentity();
- transA.setOrigin(posA);
- transA.setRotation(ornA);
- b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
- float4 posB = rigidBodies[bodyIndexB].m_pos;
- b3Transform transB;
- transB.setIdentity();
- transB.setOrigin(posB);
- transB.setRotation(ornB);
- for (int p = 0; p < numSubTreesA; p++)
- {
- b3BvhSubtreeInfo subtreeA = subTreesCPU[subTreesOffsetA + p];
- //bvhInfoCPU[bvhA].m_quantization
- b3Vector3 treeAminLocal = MyUnQuantize(subtreeA.m_quantizedAabbMin, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin);
- b3Vector3 treeAmaxLocal = MyUnQuantize(subtreeA.m_quantizedAabbMax, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin);
- b3Vector3 aabbAMinOut, aabbAMaxOut;
- float margin = 0.f;
- b3TransformAabb2(treeAminLocal, treeAmaxLocal, margin, transA.getOrigin(), transA.getRotation(), &aabbAMinOut, &aabbAMaxOut);
- for (int q = 0; q < numSubTreesB; q++)
- {
- b3BvhSubtreeInfo subtreeB = subTreesCPU[subTreesOffsetB + q];
- b3Vector3 treeBminLocal = MyUnQuantize(subtreeB.m_quantizedAabbMin, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin);
- b3Vector3 treeBmaxLocal = MyUnQuantize(subtreeB.m_quantizedAabbMax, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin);
- b3Vector3 aabbBMinOut, aabbBMaxOut;
- float margin = 0.f;
- b3TransformAabb2(treeBminLocal, treeBmaxLocal, margin, transB.getOrigin(), transB.getRotation(), &aabbBMinOut, &aabbBMaxOut);
- numAabbChecks = 0;
- bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinOut, aabbAMaxOut, aabbBMinOut, aabbBMaxOut);
- if (aabbOverlap)
- {
- int startNodeIndexA = subtreeA.m_rootNodeIndex + bvhInfoCPU[bvhA].m_nodeOffset;
- // int endNodeIndexA = startNodeIndexA+subtreeA.m_subtreeSize;
- int startNodeIndexB = subtreeB.m_rootNodeIndex + bvhInfoCPU[bvhB].m_nodeOffset;
- // int endNodeIndexB = startNodeIndexB+subtreeB.m_subtreeSize;
- b3AlignedObjectArray<b3Int2> nodeStack;
- b3Int2 node0;
- node0.x = startNodeIndexA;
- node0.y = startNodeIndexB;
- int maxStackDepth = 1024;
- nodeStack.resize(maxStackDepth);
- int depth = 0;
- nodeStack[depth++] = node0;
- do
- {
- if (depth > maxDepth)
- {
- maxDepth = depth;
- printf("maxDepth=%d\n", maxDepth);
- }
- b3Int2 node = nodeStack[--depth];
- b3Vector3 aMinLocal = MyUnQuantize(treeNodesCPU[node.x].m_quantizedAabbMin, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin);
- b3Vector3 aMaxLocal = MyUnQuantize(treeNodesCPU[node.x].m_quantizedAabbMax, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin);
- b3Vector3 bMinLocal = MyUnQuantize(treeNodesCPU[node.y].m_quantizedAabbMin, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin);
- b3Vector3 bMaxLocal = MyUnQuantize(treeNodesCPU[node.y].m_quantizedAabbMax, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin);
- float margin = 0.f;
- b3Vector3 aabbAMinOut, aabbAMaxOut;
- b3TransformAabb2(aMinLocal, aMaxLocal, margin, transA.getOrigin(), transA.getRotation(), &aabbAMinOut, &aabbAMaxOut);
- b3Vector3 aabbBMinOut, aabbBMaxOut;
- b3TransformAabb2(bMinLocal, bMaxLocal, margin, transB.getOrigin(), transB.getRotation(), &aabbBMinOut, &aabbBMaxOut);
- numAabbChecks++;
- bool nodeOverlap = b3TestAabbAgainstAabb(aabbAMinOut, aabbAMaxOut, aabbBMinOut, aabbBMaxOut);
- if (nodeOverlap)
- {
- bool isLeafA = treeNodesCPU[node.x].isLeafNode();
- bool isLeafB = treeNodesCPU[node.y].isLeafNode();
- bool isInternalA = !isLeafA;
- bool isInternalB = !isLeafB;
- //fail, even though it might hit two leaf nodes
- if (depth + 4 > maxStackDepth && !(isLeafA && isLeafB))
- {
- b3Error("Error: traversal exceeded maxStackDepth\n");
- continue;
- }
- if (isInternalA)
- {
- int nodeAleftChild = node.x + 1;
- bool isNodeALeftChildLeaf = treeNodesCPU[node.x + 1].isLeafNode();
- int nodeArightChild = isNodeALeftChildLeaf ? node.x + 2 : node.x + 1 + treeNodesCPU[node.x + 1].getEscapeIndex();
- if (isInternalB)
- {
- int nodeBleftChild = node.y + 1;
- bool isNodeBLeftChildLeaf = treeNodesCPU[node.y + 1].isLeafNode();
- int nodeBrightChild = isNodeBLeftChildLeaf ? node.y + 2 : node.y + 1 + treeNodesCPU[node.y + 1].getEscapeIndex();
- nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBleftChild);
- nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBleftChild);
- nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBrightChild);
- nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBrightChild);
- }
- else
- {
- nodeStack[depth++] = b3MakeInt2(nodeAleftChild, node.y);
- nodeStack[depth++] = b3MakeInt2(nodeArightChild, node.y);
- }
- }
- else
- {
- if (isInternalB)
- {
- int nodeBleftChild = node.y + 1;
- bool isNodeBLeftChildLeaf = treeNodesCPU[node.y + 1].isLeafNode();
- int nodeBrightChild = isNodeBLeftChildLeaf ? node.y + 2 : node.y + 1 + treeNodesCPU[node.y + 1].getEscapeIndex();
- nodeStack[depth++] = b3MakeInt2(node.x, nodeBleftChild);
- nodeStack[depth++] = b3MakeInt2(node.x, nodeBrightChild);
- }
- else
- {
- int compoundPairIdx = b3AtomicInc(numCompoundPairsOut);
- if (compoundPairIdx < maxNumCompoundPairsCapacity)
- {
- int childShapeIndexA = treeNodesCPU[node.x].getTriangleIndex();
- int childShapeIndexB = treeNodesCPU[node.y].getTriangleIndex();
- gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, childShapeIndexA, childShapeIndexB);
- }
- }
- }
- }
- } while (depth);
- maxNumAabbChecks = b3Max(numAabbChecks, maxNumAabbChecks);
- }
- }
- }
- return;
- }
- if ((collidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) || (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS))
- {
- if (collidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
- {
- int numChildrenA = collidables[collidableIndexA].m_numChildShapes;
- for (int c = 0; c < numChildrenA; c++)
- {
- int childShapeIndexA = collidables[collidableIndexA].m_shapeIndex + c;
- int childColIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;
- float4 posA = rigidBodies[bodyIndexA].m_pos;
- b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
- float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
- b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
- float4 newPosA = b3QuatRotate(ornA, childPosA) + posA;
- b3Quat newOrnA = b3QuatMul(ornA, childOrnA);
- b3Aabb aabbA = aabbsLocalSpace[childColIndexA];
- b3Transform transA;
- transA.setIdentity();
- transA.setOrigin(newPosA);
- transA.setRotation(newOrnA);
- b3Scalar margin = 0.0f;
- b3Vector3 aabbAMinOut, aabbAMaxOut;
- b3TransformAabb2((const b3Float4&)aabbA.m_min, (const b3Float4&)aabbA.m_max, margin, transA.getOrigin(), transA.getRotation(), &aabbAMinOut, &aabbAMaxOut);
- if (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
- {
- int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
- for (int b = 0; b < numChildrenB; b++)
- {
- int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
- int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
- b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
- float4 posB = rigidBodies[bodyIndexB].m_pos;
- float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
- b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
- float4 newPosB = transform(&childPosB, &posB, &ornB);
- b3Quat newOrnB = b3QuatMul(ornB, childOrnB);
- b3Aabb aabbB = aabbsLocalSpace[childColIndexB];
- b3Transform transB;
- transB.setIdentity();
- transB.setOrigin(newPosB);
- transB.setRotation(newOrnB);
- b3Vector3 aabbBMinOut, aabbBMaxOut;
- b3TransformAabb2((const b3Float4&)aabbB.m_min, (const b3Float4&)aabbB.m_max, margin, transB.getOrigin(), transB.getRotation(), &aabbBMinOut, &aabbBMaxOut);
- numAabbChecks++;
- bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinOut, aabbAMaxOut, aabbBMinOut, aabbBMaxOut);
- if (aabbOverlap)
- {
- /*
- int numFacesA = convexShapes[shapeIndexA].m_numFaces;
- float dmin = FLT_MAX;
- float4 posA = newPosA;
- posA.w = 0.f;
- float4 posB = newPosB;
- posB.w = 0.f;
- float4 c0local = convexShapes[shapeIndexA].m_localCenter;
- b3Quat ornA = newOrnA;
- float4 c0 = transform(&c0local, &posA, &ornA);
- float4 c1local = convexShapes[shapeIndexB].m_localCenter;
- b3Quat ornB =newOrnB;
- float4 c1 = transform(&c1local,&posB,&ornB);
- const float4 DeltaC2 = c0 - c1;
- */
- { //
- int compoundPairIdx = b3AtomicInc(numCompoundPairsOut);
- if (compoundPairIdx < maxNumCompoundPairsCapacity)
- {
- gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, childShapeIndexA, childShapeIndexB);
- }
- } //
- } //fi(1)
- } //for (int b=0
- } //if (collidables[collidableIndexB].
- else //if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
- {
- if (1)
- {
- // int numFacesA = convexShapes[shapeIndexA].m_numFaces;
- // float dmin = FLT_MAX;
- float4 posA = newPosA;
- posA.w = 0.f;
- float4 posB = rigidBodies[bodyIndexB].m_pos;
- posB.w = 0.f;
- float4 c0local = convexShapes[shapeIndexA].m_localCenter;
- b3Quat ornA = newOrnA;
- float4 c0;
- c0 = transform(&c0local, &posA, &ornA);
- float4 c1local = convexShapes[shapeIndexB].m_localCenter;
- b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
- float4 c1;
- c1 = transform(&c1local, &posB, &ornB);
- // const float4 DeltaC2 = c0 - c1;
- {
- int compoundPairIdx = b3AtomicInc(numCompoundPairsOut);
- if (compoundPairIdx < maxNumCompoundPairsCapacity)
- {
- gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, childShapeIndexA, -1);
- } //if (compoundPairIdx<maxNumCompoundPairsCapacity)
- } //
- } //fi (1)
- } //if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
- } //for (int b=0;b<numChildrenB;b++)
- return;
- } //if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
- if ((collidables[collidableIndexA].m_shapeType != SHAPE_CONCAVE_TRIMESH) && (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS))
- {
- int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
- for (int b = 0; b < numChildrenB; b++)
- {
- int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
- int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
- b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
- float4 posB = rigidBodies[bodyIndexB].m_pos;
- float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
- b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
- float4 newPosB = b3QuatRotate(ornB, childPosB) + posB;
- b3Quat newOrnB = b3QuatMul(ornB, childOrnB);
- int shapeIndexB = collidables[childColIndexB].m_shapeIndex;
- //////////////////////////////////////
- if (1)
- {
- // int numFacesA = convexShapes[shapeIndexA].m_numFaces;
- // float dmin = FLT_MAX;
- float4 posA = rigidBodies[bodyIndexA].m_pos;
- posA.w = 0.f;
- float4 posB = newPosB;
- posB.w = 0.f;
- float4 c0local = convexShapes[shapeIndexA].m_localCenter;
- b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
- float4 c0;
- c0 = transform(&c0local, &posA, &ornA);
- float4 c1local = convexShapes[shapeIndexB].m_localCenter;
- b3Quat ornB = newOrnB;
- float4 c1;
- c1 = transform(&c1local, &posB, &ornB);
- // const float4 DeltaC2 = c0 - c1;
- { //
- int compoundPairIdx = b3AtomicInc(numCompoundPairsOut);
- if (compoundPairIdx < maxNumCompoundPairsCapacity)
- {
- gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, -1, childShapeIndexB);
- } //fi (compoundPairIdx<maxNumCompoundPairsCapacity)
- } //
- } //fi (1)
- } //for (int b=0;b<numChildrenB;b++)
- return;
- } //if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
- return;
- } //fi ((collidables[collidableIndexA].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) ||(collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS))
- } //i<numPairs
- }
- __kernel void processCompoundPairsKernel(__global const b3Int4* gpuCompoundPairs,
- __global const b3RigidBodyData* rigidBodies,
- __global const b3Collidable* collidables,
- __global const b3ConvexPolyhedronData* convexShapes,
- __global const b3AlignedObjectArray<b3Float4>& vertices,
- __global const b3AlignedObjectArray<b3Float4>& uniqueEdges,
- __global const b3AlignedObjectArray<b3GpuFace>& faces,
- __global const b3AlignedObjectArray<int>& indices,
- __global b3Aabb* aabbs,
- __global const b3GpuChildShape* gpuChildShapes,
- __global b3AlignedObjectArray<b3Float4>& gpuCompoundSepNormalsOut,
- __global b3AlignedObjectArray<int>& gpuHasCompoundSepNormalsOut,
- int numCompoundPairs,
- int i)
- {
- // int i = get_global_id(0);
- if (i < numCompoundPairs)
- {
- int bodyIndexA = gpuCompoundPairs[i].x;
- int bodyIndexB = gpuCompoundPairs[i].y;
- int childShapeIndexA = gpuCompoundPairs[i].z;
- int childShapeIndexB = gpuCompoundPairs[i].w;
- int collidableIndexA = -1;
- int collidableIndexB = -1;
- b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
- float4 posA = rigidBodies[bodyIndexA].m_pos;
- b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
- float4 posB = rigidBodies[bodyIndexB].m_pos;
- if (childShapeIndexA >= 0)
- {
- collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;
- float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
- b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
- float4 newPosA = b3QuatRotate(ornA, childPosA) + posA;
- b3Quat newOrnA = b3QuatMul(ornA, childOrnA);
- posA = newPosA;
- ornA = newOrnA;
- }
- else
- {
- collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
- }
- if (childShapeIndexB >= 0)
- {
- collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
- float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
- b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
- float4 newPosB = b3QuatRotate(ornB, childPosB) + posB;
- b3Quat newOrnB = b3QuatMul(ornB, childOrnB);
- posB = newPosB;
- ornB = newOrnB;
- }
- else
- {
- collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
- }
- gpuHasCompoundSepNormalsOut[i] = 0;
- int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
- int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
- int shapeTypeA = collidables[collidableIndexA].m_shapeType;
- int shapeTypeB = collidables[collidableIndexB].m_shapeType;
- if ((shapeTypeA != SHAPE_CONVEX_HULL) || (shapeTypeB != SHAPE_CONVEX_HULL))
- {
- return;
- }
- int hasSeparatingAxis = 5;
- // int numFacesA = convexShapes[shapeIndexA].m_numFaces;
- float dmin = FLT_MAX;
- posA.w = 0.f;
- posB.w = 0.f;
- float4 c0local = convexShapes[shapeIndexA].m_localCenter;
- float4 c0 = transform(&c0local, &posA, &ornA);
- float4 c1local = convexShapes[shapeIndexB].m_localCenter;
- float4 c1 = transform(&c1local, &posB, &ornB);
- const float4 DeltaC2 = c0 - c1;
- float4 sepNormal = make_float4(1, 0, 0, 0);
- // bool sepA = findSeparatingAxis( convexShapes[shapeIndexA], convexShapes[shapeIndexB],posA,ornA,posB,ornB,DeltaC2,vertices,uniqueEdges,faces,indices,&sepNormal,&dmin);
- bool sepA = findSeparatingAxis(convexShapes[shapeIndexA], convexShapes[shapeIndexB], posA, ornA, posB, ornB, vertices, uniqueEdges, faces, indices, vertices, uniqueEdges, faces, indices, sepNormal); //,&dmin);
- hasSeparatingAxis = 4;
- if (!sepA)
- {
- hasSeparatingAxis = 0;
- }
- else
- {
- bool sepB = findSeparatingAxis(convexShapes[shapeIndexB], convexShapes[shapeIndexA], posB, ornB, posA, ornA, vertices, uniqueEdges, faces, indices, vertices, uniqueEdges, faces, indices, sepNormal); //,&dmin);
- if (!sepB)
- {
- hasSeparatingAxis = 0;
- }
- else //(!sepB)
- {
- bool sepEE = findSeparatingAxisEdgeEdge(&convexShapes[shapeIndexA], &convexShapes[shapeIndexB], posA, ornA, posB, ornB, DeltaC2, vertices, uniqueEdges, faces, indices, &sepNormal, &dmin);
- if (sepEE)
- {
- gpuCompoundSepNormalsOut[i] = sepNormal; //fastNormalize4(sepNormal);
- gpuHasCompoundSepNormalsOut[i] = 1;
- } //sepEE
- } //(!sepB)
- } //(!sepA)
- }
- }
- __kernel void clipCompoundsHullHullKernel(__global const b3Int4* gpuCompoundPairs,
- __global const b3RigidBodyData* rigidBodies,
- __global const b3Collidable* collidables,
- __global const b3ConvexPolyhedronData* convexShapes,
- __global const b3AlignedObjectArray<b3Float4>& vertices,
- __global const b3AlignedObjectArray<b3Float4>& uniqueEdges,
- __global const b3AlignedObjectArray<b3GpuFace>& faces,
- __global const b3AlignedObjectArray<int>& indices,
- __global const b3GpuChildShape* gpuChildShapes,
- __global const b3AlignedObjectArray<b3Float4>& gpuCompoundSepNormalsOut,
- __global const b3AlignedObjectArray<int>& gpuHasCompoundSepNormalsOut,
- __global struct b3Contact4Data* globalContactsOut,
- int* nGlobalContactsOut,
- int numCompoundPairs, int maxContactCapacity, int i)
- {
- // int i = get_global_id(0);
- int pairIndex = i;
- float4 worldVertsB1[64];
- float4 worldVertsB2[64];
- int capacityWorldVerts = 64;
- float4 localContactsOut[64];
- int localContactCapacity = 64;
- float minDist = -1e30f;
- float maxDist = 0.0f;
- if (i < numCompoundPairs)
- {
- if (gpuHasCompoundSepNormalsOut[i])
- {
- int bodyIndexA = gpuCompoundPairs[i].x;
- int bodyIndexB = gpuCompoundPairs[i].y;
- int childShapeIndexA = gpuCompoundPairs[i].z;
- int childShapeIndexB = gpuCompoundPairs[i].w;
- int collidableIndexA = -1;
- int collidableIndexB = -1;
- b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
- float4 posA = rigidBodies[bodyIndexA].m_pos;
- b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
- float4 posB = rigidBodies[bodyIndexB].m_pos;
- if (childShapeIndexA >= 0)
- {
- collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;
- float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
- b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
- float4 newPosA = b3QuatRotate(ornA, childPosA) + posA;
- b3Quat newOrnA = b3QuatMul(ornA, childOrnA);
- posA = newPosA;
- ornA = newOrnA;
- }
- else
- {
- collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
- }
- if (childShapeIndexB >= 0)
- {
- collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
- float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
- b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
- float4 newPosB = b3QuatRotate(ornB, childPosB) + posB;
- b3Quat newOrnB = b3QuatMul(ornB, childOrnB);
- posB = newPosB;
- ornB = newOrnB;
- }
- else
- {
- collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
- }
- int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
- int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
- int numLocalContactsOut = clipHullAgainstHull(gpuCompoundSepNormalsOut[i],
- convexShapes[shapeIndexA], convexShapes[shapeIndexB],
- posA, ornA,
- posB, ornB,
- worldVertsB1, worldVertsB2, capacityWorldVerts,
- minDist, maxDist,
- vertices, faces, indices,
- vertices, faces, indices,
- localContactsOut, localContactCapacity);
- if (numLocalContactsOut > 0)
- {
- float4 normal = -gpuCompoundSepNormalsOut[i];
- int nPoints = numLocalContactsOut;
- float4* pointsIn = localContactsOut;
- b3Int4 contactIdx; // = {-1,-1,-1,-1};
- contactIdx.s[0] = 0;
- contactIdx.s[1] = 1;
- contactIdx.s[2] = 2;
- contactIdx.s[3] = 3;
- int nReducedContacts = extractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
- int dstIdx;
- dstIdx = b3AtomicInc(nGlobalContactsOut);
- if ((dstIdx + nReducedContacts) < maxContactCapacity)
- {
- __global struct b3Contact4Data* c = globalContactsOut + dstIdx;
- c->m_worldNormalOnB = -normal;
- c->m_restituitionCoeffCmp = (0.f * 0xffff);
- c->m_frictionCoeffCmp = (0.7f * 0xffff);
- c->m_batchIdx = pairIndex;
- int bodyA = gpuCompoundPairs[pairIndex].x;
- int bodyB = gpuCompoundPairs[pairIndex].y;
- c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass == 0 ? -bodyA : bodyA;
- c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass == 0 ? -bodyB : bodyB;
- c->m_childIndexA = childShapeIndexA;
- c->m_childIndexB = childShapeIndexB;
- for (int i = 0; i < nReducedContacts; i++)
- {
- c->m_worldPosB[i] = pointsIn[contactIdx.s[i]];
- }
- b3Contact4Data_setNumPoints(c, nReducedContacts);
- }
- } // if (numContactsOut>0)
- } // if (gpuHasCompoundSepNormalsOut[i])
- } // if (i<numCompoundPairs)
- }
- void computeContactCompoundCompound(int pairIndex,
- int bodyIndexA, int bodyIndexB,
- int collidableIndexA, int collidableIndexB,
- const b3RigidBodyData* rigidBodies,
- const b3Collidable* collidables,
- const b3ConvexPolyhedronData* convexShapes,
- const b3GpuChildShape* cpuChildShapes,
- const b3AlignedObjectArray<b3Aabb>& hostAabbsWorldSpace,
- const b3AlignedObjectArray<b3Aabb>& hostAabbsLocalSpace,
- const b3AlignedObjectArray<b3Vector3>& convexVertices,
- const b3AlignedObjectArray<b3Vector3>& hostUniqueEdges,
- const b3AlignedObjectArray<int>& convexIndices,
- const b3AlignedObjectArray<b3GpuFace>& faces,
- b3Contact4* globalContactsOut,
- int& nGlobalContactsOut,
- int maxContactCapacity,
- b3AlignedObjectArray<b3QuantizedBvhNode>& treeNodesCPU,
- b3AlignedObjectArray<b3BvhSubtreeInfo>& subTreesCPU,
- b3AlignedObjectArray<b3BvhInfo>& bvhInfoCPU)
- {
- int shapeTypeB = collidables[collidableIndexB].m_shapeType;
- b3Assert(shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS);
- b3AlignedObjectArray<b3Int4> cpuCompoundPairsOut;
- int numCompoundPairsOut = 0;
- int maxNumCompoundPairsCapacity = 8192; //1024;
- cpuCompoundPairsOut.resize(maxNumCompoundPairsCapacity);
- // work-in-progress
- findCompoundPairsKernel(
- pairIndex,
- bodyIndexA, bodyIndexB,
- collidableIndexA, collidableIndexB,
- rigidBodies,
- collidables,
- convexShapes,
- convexVertices,
- hostAabbsWorldSpace,
- hostAabbsLocalSpace,
- cpuChildShapes,
- &cpuCompoundPairsOut[0],
- &numCompoundPairsOut,
- maxNumCompoundPairsCapacity,
- treeNodesCPU,
- subTreesCPU,
- bvhInfoCPU);
- printf("maxNumAabbChecks=%d\n", maxNumAabbChecks);
- if (numCompoundPairsOut > maxNumCompoundPairsCapacity)
- {
- b3Error("numCompoundPairsOut exceeded maxNumCompoundPairsCapacity (%d)\n", maxNumCompoundPairsCapacity);
- numCompoundPairsOut = maxNumCompoundPairsCapacity;
- }
- b3AlignedObjectArray<b3Float4> cpuCompoundSepNormalsOut;
- b3AlignedObjectArray<int> cpuHasCompoundSepNormalsOut;
- cpuCompoundSepNormalsOut.resize(numCompoundPairsOut);
- cpuHasCompoundSepNormalsOut.resize(numCompoundPairsOut);
- for (int i = 0; i < numCompoundPairsOut; i++)
- {
- processCompoundPairsKernel(&cpuCompoundPairsOut[0], rigidBodies, collidables, convexShapes, convexVertices, hostUniqueEdges, faces, convexIndices, 0, cpuChildShapes,
- cpuCompoundSepNormalsOut, cpuHasCompoundSepNormalsOut, numCompoundPairsOut, i);
- }
- for (int i = 0; i < numCompoundPairsOut; i++)
- {
- clipCompoundsHullHullKernel(&cpuCompoundPairsOut[0], rigidBodies, collidables, convexShapes, convexVertices, hostUniqueEdges, faces, convexIndices, cpuChildShapes,
- cpuCompoundSepNormalsOut, cpuHasCompoundSepNormalsOut, globalContactsOut, &nGlobalContactsOut, numCompoundPairsOut, maxContactCapacity, i);
- }
- /*
- int childColIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;
- float4 posA = rigidBodies[bodyIndexA].m_pos;
- b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
- float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
- b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
- float4 newPosA = b3QuatRotate(ornA,childPosA)+posA;
- b3Quat newOrnA = b3QuatMul(ornA,childOrnA);
- int shapeIndexA = collidables[childColIndexA].m_shapeIndex;
- bool foundSepAxis = findSeparatingAxis(hullA,hullB,
- posA,
- ornA,
- posB,
- ornB,
- convexVertices,uniqueEdges,faces,convexIndices,
- convexVertices,uniqueEdges,faces,convexIndices,
-
- sepNormalWorldSpace
- );
- */
- /*
- if (foundSepAxis)
- {
-
-
- contactIndex = clipHullHullSingle(
- bodyIndexA, bodyIndexB,
- posA,ornA,
- posB,ornB,
- collidableIndexA, collidableIndexB,
- &rigidBodies,
- &globalContactsOut,
- nGlobalContactsOut,
-
- convexShapes,
- convexShapes,
-
- convexVertices,
- uniqueEdges,
- faces,
- convexIndices,
-
- convexVertices,
- uniqueEdges,
- faces,
- convexIndices,
- collidables,
- collidables,
- sepNormalWorldSpace,
- maxContactCapacity);
-
- }
- */
- // return contactIndex;
- /*
- int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
- for (int c=0;c<numChildrenB;c++)
- {
- int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+c;
- int childColIndexB = cpuChildShapes[childShapeIndexB].m_shapeIndex;
- float4 rootPosB = rigidBodies[bodyIndexB].m_pos;
- b3Quaternion rootOrnB = rigidBodies[bodyIndexB].m_quat;
- b3Vector3 childPosB = cpuChildShapes[childShapeIndexB].m_childPosition;
- b3Quaternion childOrnB = cpuChildShapes[childShapeIndexB].m_childOrientation;
- float4 posB = b3QuatRotate(rootOrnB,childPosB)+rootPosB;
- b3Quaternion ornB = b3QuatMul(rootOrnB,childOrnB);//b3QuatMul(ornB,childOrnB);
- int shapeIndexB = collidables[childColIndexB].m_shapeIndex;
- const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndexB];
- }
- */
- }
- void computeContactPlaneCompound(int pairIndex,
- int bodyIndexA, int bodyIndexB,
- int collidableIndexA, int collidableIndexB,
- const b3RigidBodyData* rigidBodies,
- const b3Collidable* collidables,
- const b3ConvexPolyhedronData* convexShapes,
- const b3GpuChildShape* cpuChildShapes,
- const b3Vector3* convexVertices,
- const int* convexIndices,
- const b3GpuFace* faces,
- b3Contact4* globalContactsOut,
- int& nGlobalContactsOut,
- int maxContactCapacity)
- {
- int shapeTypeB = collidables[collidableIndexB].m_shapeType;
- b3Assert(shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS);
- int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
- for (int c = 0; c < numChildrenB; c++)
- {
- int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + c;
- int childColIndexB = cpuChildShapes[childShapeIndexB].m_shapeIndex;
- float4 rootPosB = rigidBodies[bodyIndexB].m_pos;
- b3Quaternion rootOrnB = rigidBodies[bodyIndexB].m_quat;
- b3Vector3 childPosB = cpuChildShapes[childShapeIndexB].m_childPosition;
- b3Quaternion childOrnB = cpuChildShapes[childShapeIndexB].m_childOrientation;
- float4 posB = b3QuatRotate(rootOrnB, childPosB) + rootPosB;
- b3Quaternion ornB = rootOrnB * childOrnB; //b3QuatMul(ornB,childOrnB);
- int shapeIndexB = collidables[childColIndexB].m_shapeIndex;
- const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndexB];
- b3Vector3 posA = rigidBodies[bodyIndexA].m_pos;
- b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
- // int numContactsOut = 0;
- // int numWorldVertsB1= 0;
- b3Vector3 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
- b3Vector3 planeNormal = b3MakeVector3(planeEq.x, planeEq.y, planeEq.z);
- b3Vector3 planeNormalWorld = b3QuatRotate(ornA, planeNormal);
- float planeConstant = planeEq.w;
- b3Transform convexWorldTransform;
- convexWorldTransform.setIdentity();
- convexWorldTransform.setOrigin(posB);
- convexWorldTransform.setRotation(ornB);
- b3Transform planeTransform;
- planeTransform.setIdentity();
- planeTransform.setOrigin(posA);
- planeTransform.setRotation(ornA);
- b3Transform planeInConvex;
- planeInConvex = convexWorldTransform.inverse() * planeTransform;
- b3Transform convexInPlane;
- convexInPlane = planeTransform.inverse() * convexWorldTransform;
- b3Vector3 planeNormalInConvex = planeInConvex.getBasis() * -planeNormal;
- float maxDot = -1e30;
- int hitVertex = -1;
- b3Vector3 hitVtx;
- #define MAX_PLANE_CONVEX_POINTS 64
- b3Vector3 contactPoints[MAX_PLANE_CONVEX_POINTS];
- int numPoints = 0;
- b3Int4 contactIdx;
- contactIdx.s[0] = 0;
- contactIdx.s[1] = 1;
- contactIdx.s[2] = 2;
- contactIdx.s[3] = 3;
- for (int i = 0; i < hullB->m_numVertices; i++)
- {
- b3Vector3 vtx = convexVertices[hullB->m_vertexOffset + i];
- float curDot = vtx.dot(planeNormalInConvex);
- if (curDot > maxDot)
- {
- hitVertex = i;
- maxDot = curDot;
- hitVtx = vtx;
- //make sure the deepest points is always included
- if (numPoints == MAX_PLANE_CONVEX_POINTS)
- numPoints--;
- }
- if (numPoints < MAX_PLANE_CONVEX_POINTS)
- {
- b3Vector3 vtxWorld = convexWorldTransform * vtx;
- b3Vector3 vtxInPlane = planeTransform.inverse() * vtxWorld;
- float dist = planeNormal.dot(vtxInPlane) - planeConstant;
- if (dist < 0.f)
- {
- vtxWorld.w = dist;
- contactPoints[numPoints] = vtxWorld;
- numPoints++;
- }
- }
- }
- int numReducedPoints = 0;
- numReducedPoints = numPoints;
- if (numPoints > 4)
- {
- numReducedPoints = extractManifoldSequentialGlobal(contactPoints, numPoints, planeNormalInConvex, &contactIdx);
- }
- int dstIdx;
- // dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
- if (numReducedPoints > 0)
- {
- if (nGlobalContactsOut < maxContactCapacity)
- {
- dstIdx = nGlobalContactsOut;
- nGlobalContactsOut++;
- b3Contact4* c = &globalContactsOut[dstIdx];
- c->m_worldNormalOnB = -planeNormalWorld;
- c->setFrictionCoeff(0.7);
- c->setRestituitionCoeff(0.f);
- c->m_batchIdx = pairIndex;
- c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
- c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
- for (int i = 0; i < numReducedPoints; i++)
- {
- b3Vector3 pOnB1 = contactPoints[contactIdx.s[i]];
- c->m_worldPosB[i] = pOnB1;
- }
- c->m_worldNormalOnB.w = (b3Scalar)numReducedPoints;
- } //if (dstIdx < numPairs)
- }
- }
- }
- void computeContactSphereConvex(int pairIndex,
- int bodyIndexA, int bodyIndexB,
- int collidableIndexA, int collidableIndexB,
- const b3RigidBodyData* rigidBodies,
- const b3Collidable* collidables,
- const b3ConvexPolyhedronData* convexShapes,
- const b3Vector3* convexVertices,
- const int* convexIndices,
- const b3GpuFace* faces,
- b3Contact4* globalContactsOut,
- int& nGlobalContactsOut,
- int maxContactCapacity)
- {
- float radius = collidables[collidableIndexA].m_radius;
- float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
- b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
- float4 pos = rigidBodies[bodyIndexB].m_pos;
- b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
- b3Transform tr;
- tr.setIdentity();
- tr.setOrigin(pos);
- tr.setRotation(quat);
- b3Transform trInv = tr.inverse();
- float4 spherePos = trInv(spherePos1);
- int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
- int shapeIndex = collidables[collidableIndex].m_shapeIndex;
- int numFaces = convexShapes[shapeIndex].m_numFaces;
- float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
- // float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
- float minDist = -1000000.f; // TODO: What is the largest/smallest float?
- bool bCollide = true;
- int region = -1;
- float4 localHitNormal;
- for (int f = 0; f < numFaces; f++)
- {
- b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset + f];
- float4 planeEqn;
- float4 localPlaneNormal = b3MakeVector3(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
- float4 n1 = localPlaneNormal; //quatRotate(quat,localPlaneNormal);
- planeEqn = n1;
- planeEqn[3] = face.m_plane.w;
- float4 pntReturn;
- float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
- if (dist > radius)
- {
- bCollide = false;
- break;
- }
- if (dist > 0)
- {
- //might hit an edge or vertex
- b3Vector3 out;
- bool isInPoly = IsPointInPolygon(spherePos,
- &face,
- &convexVertices[convexShapes[shapeIndex].m_vertexOffset],
- convexIndices,
- &out);
- if (isInPoly)
- {
- if (dist > minDist)
- {
- minDist = dist;
- closestPnt = pntReturn;
- localHitNormal = planeEqn;
- region = 1;
- }
- }
- else
- {
- b3Vector3 tmp = spherePos - out;
- b3Scalar l2 = tmp.length2();
- if (l2 < radius * radius)
- {
- dist = b3Sqrt(l2);
- if (dist > minDist)
- {
- minDist = dist;
- closestPnt = out;
- localHitNormal = tmp / dist;
- region = 2;
- }
- }
- else
- {
- bCollide = false;
- break;
- }
- }
- }
- else
- {
- if (dist > minDist)
- {
- minDist = dist;
- closestPnt = pntReturn;
- localHitNormal = planeEqn;
- region = 3;
- }
- }
- }
- static int numChecks = 0;
- numChecks++;
- if (bCollide && minDist > -10000)
- {
- float4 normalOnSurfaceB1 = tr.getBasis() * localHitNormal; //-hitNormalWorld;
- float4 pOnB1 = tr(closestPnt);
- //printf("dist ,%f,",minDist);
- float actualDepth = minDist - radius;
- if (actualDepth < 0)
- {
- //printf("actualDepth = ,%f,", actualDepth);
- //printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
- //printf("region=,%d,\n", region);
- pOnB1[3] = actualDepth;
- int dstIdx;
- // dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
- if (nGlobalContactsOut < maxContactCapacity)
- {
- dstIdx = nGlobalContactsOut;
- nGlobalContactsOut++;
- b3Contact4* c = &globalContactsOut[dstIdx];
- c->m_worldNormalOnB = normalOnSurfaceB1;
- c->setFrictionCoeff(0.7);
- c->setRestituitionCoeff(0.f);
- c->m_batchIdx = pairIndex;
- c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
- c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
- c->m_worldPosB[0] = pOnB1;
- int numPoints = 1;
- c->m_worldNormalOnB.w = (b3Scalar)numPoints;
- } //if (dstIdx < numPairs)
- }
- } //if (hasCollision)
- }
- int computeContactConvexConvex2(
- int pairIndex,
- int bodyIndexA, int bodyIndexB,
- int collidableIndexA, int collidableIndexB,
- const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
- const b3AlignedObjectArray<b3Collidable>& collidables,
- const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
- const b3AlignedObjectArray<b3Vector3>& convexVertices,
- const b3AlignedObjectArray<b3Vector3>& uniqueEdges,
- const b3AlignedObjectArray<int>& convexIndices,
- const b3AlignedObjectArray<b3GpuFace>& faces,
- b3AlignedObjectArray<b3Contact4>& globalContactsOut,
- int& nGlobalContactsOut,
- int maxContactCapacity,
- const b3AlignedObjectArray<b3Contact4>& oldContacts)
- {
- int contactIndex = -1;
- b3Vector3 posA = rigidBodies[bodyIndexA].m_pos;
- b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
- b3Vector3 posB = rigidBodies[bodyIndexB].m_pos;
- b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
- b3ConvexPolyhedronData hullA, hullB;
- b3Vector3 sepNormalWorldSpace;
- b3Collidable colA = collidables[collidableIndexA];
- hullA = convexShapes[colA.m_shapeIndex];
- //printf("numvertsA = %d\n",hullA.m_numVertices);
- b3Collidable colB = collidables[collidableIndexB];
- hullB = convexShapes[colB.m_shapeIndex];
- //printf("numvertsB = %d\n",hullB.m_numVertices);
- // int contactCapacity = MAX_VERTS;
- //int numContactsOut=0;
- #ifdef _WIN32
- b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
- b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
- #endif
- bool foundSepAxis = findSeparatingAxis(hullA, hullB,
- posA,
- ornA,
- posB,
- ornB,
- convexVertices, uniqueEdges, faces, convexIndices,
- convexVertices, uniqueEdges, faces, convexIndices,
- sepNormalWorldSpace);
- if (foundSepAxis)
- {
- contactIndex = clipHullHullSingle(
- bodyIndexA, bodyIndexB,
- posA, ornA,
- posB, ornB,
- collidableIndexA, collidableIndexB,
- &rigidBodies,
- &globalContactsOut,
- nGlobalContactsOut,
- convexShapes,
- convexShapes,
- convexVertices,
- uniqueEdges,
- faces,
- convexIndices,
- convexVertices,
- uniqueEdges,
- faces,
- convexIndices,
- collidables,
- collidables,
- sepNormalWorldSpace,
- maxContactCapacity);
- }
- return contactIndex;
- }
- void GpuSatCollision::computeConvexConvexContactsGPUSAT(b3OpenCLArray<b3Int4>* pairs, int nPairs,
- const b3OpenCLArray<b3RigidBodyData>* bodyBuf,
- b3OpenCLArray<b3Contact4>* contactOut, int& nContacts,
- const b3OpenCLArray<b3Contact4>* oldContacts,
- int maxContactCapacity,
- int compoundPairCapacity,
- const b3OpenCLArray<b3ConvexPolyhedronData>& convexData,
- const b3OpenCLArray<b3Vector3>& gpuVertices,
- const b3OpenCLArray<b3Vector3>& gpuUniqueEdges,
- const b3OpenCLArray<b3GpuFace>& gpuFaces,
- const b3OpenCLArray<int>& gpuIndices,
- const b3OpenCLArray<b3Collidable>& gpuCollidables,
- const b3OpenCLArray<b3GpuChildShape>& gpuChildShapes,
- const b3OpenCLArray<b3Aabb>& clAabbsWorldSpace,
- const b3OpenCLArray<b3Aabb>& clAabbsLocalSpace,
- b3OpenCLArray<b3Vector3>& worldVertsB1GPU,
- b3OpenCLArray<b3Int4>& clippingFacesOutGPU,
- b3OpenCLArray<b3Vector3>& worldNormalsAGPU,
- b3OpenCLArray<b3Vector3>& worldVertsA1GPU,
- b3OpenCLArray<b3Vector3>& worldVertsB2GPU,
- b3AlignedObjectArray<class b3OptimizedBvh*>& bvhDataUnused,
- b3OpenCLArray<b3QuantizedBvhNode>* treeNodesGPU,
- b3OpenCLArray<b3BvhSubtreeInfo>* subTreesGPU,
- b3OpenCLArray<b3BvhInfo>* bvhInfo,
- int numObjects,
- int maxTriConvexPairCapacity,
- b3OpenCLArray<b3Int4>& triangleConvexPairsOut,
- int& numTriConvexPairsOut)
- {
- myframecount++;
- if (!nPairs)
- return;
- #ifdef CHECK_ON_HOST
- b3AlignedObjectArray<b3QuantizedBvhNode> treeNodesCPU;
- treeNodesGPU->copyToHost(treeNodesCPU);
- b3AlignedObjectArray<b3BvhSubtreeInfo> subTreesCPU;
- subTreesGPU->copyToHost(subTreesCPU);
- b3AlignedObjectArray<b3BvhInfo> bvhInfoCPU;
- bvhInfo->copyToHost(bvhInfoCPU);
- b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
- clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
- b3AlignedObjectArray<b3Aabb> hostAabbsLocalSpace;
- clAabbsLocalSpace.copyToHost(hostAabbsLocalSpace);
- b3AlignedObjectArray<b3Int4> hostPairs;
- pairs->copyToHost(hostPairs);
- b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
- bodyBuf->copyToHost(hostBodyBuf);
- b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
- convexData.copyToHost(hostConvexData);
- b3AlignedObjectArray<b3Vector3> hostVertices;
- gpuVertices.copyToHost(hostVertices);
- b3AlignedObjectArray<b3Vector3> hostUniqueEdges;
- gpuUniqueEdges.copyToHost(hostUniqueEdges);
- b3AlignedObjectArray<b3GpuFace> hostFaces;
- gpuFaces.copyToHost(hostFaces);
- b3AlignedObjectArray<int> hostIndices;
- gpuIndices.copyToHost(hostIndices);
- b3AlignedObjectArray<b3Collidable> hostCollidables;
- gpuCollidables.copyToHost(hostCollidables);
- b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
- gpuChildShapes.copyToHost(cpuChildShapes);
- b3AlignedObjectArray<b3Int4> hostTriangleConvexPairs;
- b3AlignedObjectArray<b3Contact4> hostContacts;
- if (nContacts)
- {
- contactOut->copyToHost(hostContacts);
- }
- b3AlignedObjectArray<b3Contact4> oldHostContacts;
- if (oldContacts->size())
- {
- oldContacts->copyToHost(oldHostContacts);
- }
- hostContacts.resize(maxContactCapacity);
- for (int i = 0; i < nPairs; i++)
- {
- int bodyIndexA = hostPairs[i].x;
- int bodyIndexB = hostPairs[i].y;
- int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx;
- int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx;
- if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
- hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
- {
- computeContactSphereConvex(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, &hostBodyBuf[0],
- &hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
- }
- if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
- hostCollidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)
- {
- computeContactSphereConvex(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0],
- &hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
- //printf("convex-sphere\n");
- }
- if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
- hostCollidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
- {
- computeContactPlaneConvex(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0],
- &hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
- // printf("convex-plane\n");
- }
- if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
- hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
- {
- computeContactPlaneConvex(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, &hostBodyBuf[0],
- &hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
- // printf("plane-convex\n");
- }
- if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
- hostCollidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
- {
- computeContactCompoundCompound(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0],
- &hostCollidables[0], &hostConvexData[0], &cpuChildShapes[0], hostAabbsWorldSpace, hostAabbsLocalSpace, hostVertices, hostUniqueEdges, hostIndices, hostFaces, &hostContacts[0],
- nContacts, maxContactCapacity, treeNodesCPU, subTreesCPU, bvhInfoCPU);
- // printf("convex-plane\n");
- }
- if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
- hostCollidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
- {
- computeContactPlaneCompound(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0],
- &hostCollidables[0], &hostConvexData[0], &cpuChildShapes[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
- // printf("convex-plane\n");
- }
- if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
- hostCollidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
- {
- computeContactPlaneCompound(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, &hostBodyBuf[0],
- &hostCollidables[0], &hostConvexData[0], &cpuChildShapes[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
- // printf("plane-convex\n");
- }
- if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
- hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
- {
- //printf("hostPairs[i].z=%d\n",hostPairs[i].z);
- int contactIndex = computeContactConvexConvex2(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, hostBodyBuf, hostCollidables, hostConvexData, hostVertices, hostUniqueEdges, hostIndices, hostFaces, hostContacts, nContacts, maxContactCapacity, oldHostContacts);
- //int contactIndex = computeContactConvexConvex(hostPairs,i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
- if (contactIndex >= 0)
- {
- // printf("convex convex contactIndex = %d\n",contactIndex);
- hostPairs[i].z = contactIndex;
- }
- // printf("plane-convex\n");
- }
- }
- if (hostPairs.size())
- {
- pairs->copyFromHost(hostPairs);
- }
- hostContacts.resize(nContacts);
- if (nContacts)
- {
- contactOut->copyFromHost(hostContacts);
- }
- else
- {
- contactOut->resize(0);
- }
- m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
- //printf("(HOST) nContacts = %d\n",nContacts);
- #else
- {
- if (nPairs)
- {
- m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
- B3_PROFILE("primitiveContactsKernel");
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(pairs->getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(contactOut->getBufferCL()),
- b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_primitiveContactsKernel, "m_primitiveContactsKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(nPairs);
- launcher.setConst(maxContactCapacity);
- int num = nPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- nContacts = m_totalContactsOut.at(0);
- contactOut->resize(nContacts);
- }
- }
- #endif //CHECK_ON_HOST
- B3_PROFILE("computeConvexConvexContactsGPUSAT");
- // printf("nContacts = %d\n",nContacts);
- m_sepNormals.resize(nPairs);
- m_hasSeparatingNormals.resize(nPairs);
- int concaveCapacity = maxTriConvexPairCapacity;
- m_concaveSepNormals.resize(concaveCapacity);
- m_concaveHasSeparatingNormals.resize(concaveCapacity);
- m_numConcavePairsOut.resize(0);
- m_numConcavePairsOut.push_back(0);
- m_gpuCompoundPairs.resize(compoundPairCapacity);
- m_gpuCompoundSepNormals.resize(compoundPairCapacity);
- m_gpuHasCompoundSepNormals.resize(compoundPairCapacity);
- m_numCompoundPairsOut.resize(0);
- m_numCompoundPairsOut.push_back(0);
- int numCompoundPairs = 0;
- int numConcavePairs = 0;
- {
- clFinish(m_queue);
- if (findSeparatingAxisOnGpu)
- {
- m_dmins.resize(nPairs);
- if (splitSearchSepAxisConvex)
- {
- if (useMprGpu)
- {
- nContacts = m_totalContactsOut.at(0);
- {
- B3_PROFILE("mprPenetrationKernel");
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(pairs->getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(m_sepNormals.getBufferCL()),
- b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
- b3BufferInfoCL(contactOut->getBufferCL()),
- b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_mprPenetrationKernel, "mprPenetrationKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(maxContactCapacity);
- launcher.setConst(nPairs);
- int num = nPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- /*
- b3AlignedObjectArray<int>hostHasSepAxis;
- m_hasSeparatingNormals.copyToHost(hostHasSepAxis);
- b3AlignedObjectArray<b3Vector3>hostSepAxis;
- m_sepNormals.copyToHost(hostSepAxis);
- */
- nContacts = m_totalContactsOut.at(0);
- contactOut->resize(nContacts);
- // printf("nContacts (after mprPenetrationKernel) = %d\n",nContacts);
- if (nContacts > maxContactCapacity)
- {
- b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity);
- nContacts = maxContactCapacity;
- }
- }
- }
- if (1)
- {
- if (1)
- {
- {
- B3_PROFILE("findSeparatingAxisVertexFaceKernel");
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(pairs->getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
- b3BufferInfoCL(m_sepNormals.getBufferCL()),
- b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
- b3BufferInfoCL(m_dmins.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_findSeparatingAxisVertexFaceKernel, "findSeparatingAxisVertexFaceKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(nPairs);
- int num = nPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- }
- int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
- {
- B3_PROFILE("findSeparatingAxisEdgeEdgeKernel");
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(pairs->getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
- b3BufferInfoCL(m_sepNormals.getBufferCL()),
- b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
- b3BufferInfoCL(m_dmins.getBufferCL()),
- b3BufferInfoCL(m_unitSphereDirections.getBufferCL(), true)
- };
- b3LauncherCL launcher(m_queue, m_findSeparatingAxisEdgeEdgeKernel, "findSeparatingAxisEdgeEdgeKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(numDirections);
- launcher.setConst(nPairs);
- int num = nPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- }
- }
- if (useMprGpu)
- {
- B3_PROFILE("findSeparatingAxisUnitSphereKernel");
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(pairs->getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(m_unitSphereDirections.getBufferCL(), true),
- b3BufferInfoCL(m_sepNormals.getBufferCL()),
- b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
- b3BufferInfoCL(m_dmins.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_findSeparatingAxisUnitSphereKernel, "findSeparatingAxisUnitSphereKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
- launcher.setConst(numDirections);
- launcher.setConst(nPairs);
- int num = nPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- }
- }
- }
- else
- {
- B3_PROFILE("findSeparatingAxisKernel");
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(pairs->getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
- b3BufferInfoCL(m_sepNormals.getBufferCL()),
- b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_findSeparatingAxisKernel, "m_findSeparatingAxisKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(nPairs);
- int num = nPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- }
- }
- else
- {
- B3_PROFILE("findSeparatingAxisKernel CPU");
- b3AlignedObjectArray<b3Int4> hostPairs;
- pairs->copyToHost(hostPairs);
- b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
- bodyBuf->copyToHost(hostBodyBuf);
- b3AlignedObjectArray<b3Collidable> hostCollidables;
- gpuCollidables.copyToHost(hostCollidables);
- b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
- gpuChildShapes.copyToHost(cpuChildShapes);
- b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexShapeData;
- convexData.copyToHost(hostConvexShapeData);
- b3AlignedObjectArray<b3Vector3> hostVertices;
- gpuVertices.copyToHost(hostVertices);
- b3AlignedObjectArray<int> hostHasSepAxis;
- hostHasSepAxis.resize(nPairs);
- b3AlignedObjectArray<b3Vector3> hostSepAxis;
- hostSepAxis.resize(nPairs);
- b3AlignedObjectArray<b3Vector3> hostUniqueEdges;
- gpuUniqueEdges.copyToHost(hostUniqueEdges);
- b3AlignedObjectArray<b3GpuFace> hostFaces;
- gpuFaces.copyToHost(hostFaces);
- b3AlignedObjectArray<int> hostIndices;
- gpuIndices.copyToHost(hostIndices);
- b3AlignedObjectArray<b3Contact4> hostContacts;
- if (nContacts)
- {
- contactOut->copyToHost(hostContacts);
- }
- hostContacts.resize(maxContactCapacity);
- int nGlobalContactsOut = nContacts;
- for (int i = 0; i < nPairs; i++)
- {
- int bodyIndexA = hostPairs[i].x;
- int bodyIndexB = hostPairs[i].y;
- int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx;
- int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx;
- int shapeIndexA = hostCollidables[collidableIndexA].m_shapeIndex;
- int shapeIndexB = hostCollidables[collidableIndexB].m_shapeIndex;
- hostHasSepAxis[i] = 0;
- //once the broadphase avoids static-static pairs, we can remove this test
- if ((hostBodyBuf[bodyIndexA].m_invMass == 0) && (hostBodyBuf[bodyIndexB].m_invMass == 0))
- {
- continue;
- }
- if ((hostCollidables[collidableIndexA].m_shapeType != SHAPE_CONVEX_HULL) || (hostCollidables[collidableIndexB].m_shapeType != SHAPE_CONVEX_HULL))
- {
- continue;
- }
- float dmin = FLT_MAX;
- b3ConvexPolyhedronData* convexShapeA = &hostConvexShapeData[shapeIndexA];
- b3ConvexPolyhedronData* convexShapeB = &hostConvexShapeData[shapeIndexB];
- b3Vector3 posA = hostBodyBuf[bodyIndexA].m_pos;
- b3Vector3 posB = hostBodyBuf[bodyIndexB].m_pos;
- b3Quaternion ornA = hostBodyBuf[bodyIndexA].m_quat;
- b3Quaternion ornB = hostBodyBuf[bodyIndexB].m_quat;
- if (useGjk)
- {
- //first approximate the separating axis, to 'fail-proof' GJK+EPA or MPR
- {
- b3Vector3 c0local = hostConvexShapeData[shapeIndexA].m_localCenter;
- b3Vector3 c0 = b3TransformPoint(c0local, posA, ornA);
- b3Vector3 c1local = hostConvexShapeData[shapeIndexB].m_localCenter;
- b3Vector3 c1 = b3TransformPoint(c1local, posB, ornB);
- b3Vector3 DeltaC2 = c0 - c1;
- b3Vector3 sepAxis;
- bool hasSepAxisA = b3FindSeparatingAxis(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
- &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
- &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
- &sepAxis, &dmin);
- if (hasSepAxisA)
- {
- bool hasSepAxisB = b3FindSeparatingAxis(convexShapeB, convexShapeA, posB, ornB, posA, ornA, DeltaC2,
- &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
- &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
- &sepAxis, &dmin);
- if (hasSepAxisB)
- {
- bool hasEdgeEdge = b3FindSeparatingAxisEdgeEdge(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
- &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
- &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
- &sepAxis, &dmin, false);
- if (hasEdgeEdge)
- {
- hostHasSepAxis[i] = 1;
- hostSepAxis[i] = sepAxis;
- hostSepAxis[i].w = dmin;
- }
- }
- }
- }
- if (hostHasSepAxis[i])
- {
- int pairIndex = i;
- bool useMpr = true;
- if (useMpr)
- {
- int res = 0;
- float depth = 0.f;
- b3Vector3 sepAxis2 = b3MakeVector3(1, 0, 0);
- b3Vector3 resultPointOnBWorld = b3MakeVector3(0, 0, 0);
- float depthOut;
- b3Vector3 dirOut;
- b3Vector3 posOut;
- //res = b3MprPenetration(bodyIndexA,bodyIndexB,hostBodyBuf,hostConvexShapeData,hostCollidables,hostVertices,&mprConfig,&depthOut,&dirOut,&posOut);
- res = b3MprPenetration(pairIndex, bodyIndexA, bodyIndexB, &hostBodyBuf[0], &hostConvexShapeData[0], &hostCollidables[0], &hostVertices[0], &hostSepAxis[0], &hostHasSepAxis[0], &depthOut, &dirOut, &posOut);
- depth = depthOut;
- sepAxis2 = b3MakeVector3(-dirOut.x, -dirOut.y, -dirOut.z);
- resultPointOnBWorld = posOut;
- //hostHasSepAxis[i] = 0;
- if (res == 0)
- {
- //add point?
- //printf("depth = %f\n",depth);
- //printf("normal = %f,%f,%f\n",dir.v[0],dir.v[1],dir.v[2]);
- //qprintf("pos = %f,%f,%f\n",pos.v[0],pos.v[1],pos.v[2]);
- float dist = 0.f;
- const b3ConvexPolyhedronData& hullA = hostConvexShapeData[hostCollidables[hostBodyBuf[bodyIndexA].m_collidableIdx].m_shapeIndex];
- const b3ConvexPolyhedronData& hullB = hostConvexShapeData[hostCollidables[hostBodyBuf[bodyIndexB].m_collidableIdx].m_shapeIndex];
- if (b3TestSepAxis(&hullA, &hullB, posA, ornA, posB, ornB, &sepAxis2, &hostVertices[0], &hostVertices[0], &dist))
- {
- if (depth > dist)
- {
- float diff = depth - dist;
- static float maxdiff = 0.f;
- if (maxdiff < diff)
- {
- maxdiff = diff;
- printf("maxdiff = %20.10f\n", maxdiff);
- }
- }
- }
- if (depth > dmin)
- {
- b3Vector3 oldAxis = hostSepAxis[i];
- depth = dmin;
- sepAxis2 = oldAxis;
- }
- if (b3TestSepAxis(&hullA, &hullB, posA, ornA, posB, ornB, &sepAxis2, &hostVertices[0], &hostVertices[0], &dist))
- {
- if (depth > dist)
- {
- float diff = depth - dist;
- //printf("?diff = %f\n",diff );
- static float maxdiff = 0.f;
- if (maxdiff < diff)
- {
- maxdiff = diff;
- printf("maxdiff = %20.10f\n", maxdiff);
- }
- }
- //this is used for SAT
- //hostHasSepAxis[i] = 1;
- //hostSepAxis[i] = sepAxis2;
- //add contact point
- //int contactIndex = nGlobalContactsOut;
- b3Contact4& newContact = hostContacts.at(nGlobalContactsOut);
- nGlobalContactsOut++;
- newContact.m_batchIdx = 0; //i;
- newContact.m_bodyAPtrAndSignBit = (hostBodyBuf.at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
- newContact.m_bodyBPtrAndSignBit = (hostBodyBuf.at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
- newContact.m_frictionCoeffCmp = 45874;
- newContact.m_restituitionCoeffCmp = 0;
- static float maxDepth = 0.f;
- if (depth > maxDepth)
- {
- maxDepth = depth;
- printf("MPR maxdepth = %f\n", maxDepth);
- }
- resultPointOnBWorld.w = -depth;
- newContact.m_worldPosB[0] = resultPointOnBWorld;
- //b3Vector3 resultPointOnAWorld = resultPointOnBWorld+depth*sepAxis2;
- newContact.m_worldNormalOnB = sepAxis2;
- newContact.m_worldNormalOnB.w = (b3Scalar)1;
- }
- else
- {
- printf("rejected\n");
- }
- }
- }
- else
- {
- //int contactIndex = computeContactConvexConvex2( i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf, hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
- b3AlignedObjectArray<b3Contact4> oldHostContacts;
- int result;
- result = computeContactConvexConvex2( //hostPairs,
- pairIndex,
- bodyIndexA, bodyIndexB,
- collidableIndexA, collidableIndexB,
- hostBodyBuf,
- hostCollidables,
- hostConvexShapeData,
- hostVertices,
- hostUniqueEdges,
- hostIndices,
- hostFaces,
- hostContacts,
- nGlobalContactsOut,
- maxContactCapacity,
- oldHostContacts
- //hostHasSepAxis,
- //hostSepAxis
- );
- } //mpr
- } //hostHasSepAxis[i] = 1;
- }
- else
- {
- b3Vector3 c0local = hostConvexShapeData[shapeIndexA].m_localCenter;
- b3Vector3 c0 = b3TransformPoint(c0local, posA, ornA);
- b3Vector3 c1local = hostConvexShapeData[shapeIndexB].m_localCenter;
- b3Vector3 c1 = b3TransformPoint(c1local, posB, ornB);
- b3Vector3 DeltaC2 = c0 - c1;
- b3Vector3 sepAxis;
- bool hasSepAxisA = b3FindSeparatingAxis(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
- &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
- &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
- &sepAxis, &dmin);
- if (hasSepAxisA)
- {
- bool hasSepAxisB = b3FindSeparatingAxis(convexShapeB, convexShapeA, posB, ornB, posA, ornA, DeltaC2,
- &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
- &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
- &sepAxis, &dmin);
- if (hasSepAxisB)
- {
- bool hasEdgeEdge = b3FindSeparatingAxisEdgeEdge(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
- &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
- &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
- &sepAxis, &dmin, true);
- if (hasEdgeEdge)
- {
- hostHasSepAxis[i] = 1;
- hostSepAxis[i] = sepAxis;
- }
- }
- }
- }
- }
- if (useGjkContacts) //nGlobalContactsOut>0)
- {
- //printf("nGlobalContactsOut=%d\n",nGlobalContactsOut);
- nContacts = nGlobalContactsOut;
- contactOut->copyFromHost(hostContacts);
- m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
- }
- m_hasSeparatingNormals.copyFromHost(hostHasSepAxis);
- m_sepNormals.copyFromHost(hostSepAxis);
- /*
- //double-check results from GPU (comment-out the 'else' so both paths are executed
- b3AlignedObjectArray<int> checkHasSepAxis;
- m_hasSeparatingNormals.copyToHost(checkHasSepAxis);
- static int frameCount = 0;
- frameCount++;
- for (int i=0;i<nPairs;i++)
- {
- if (hostHasSepAxis[i] != checkHasSepAxis[i])
- {
- printf("at frameCount %d hostHasSepAxis[%d] = %d but checkHasSepAxis[i] = %d\n",
- frameCount,i,hostHasSepAxis[i],checkHasSepAxis[i]);
- }
- }
- //m_hasSeparatingNormals.copyFromHost(hostHasSepAxis);
- // m_sepNormals.copyFromHost(hostSepAxis);
- */
- }
- numCompoundPairs = m_numCompoundPairsOut.at(0);
- bool useGpuFindCompoundPairs = true;
- if (useGpuFindCompoundPairs)
- {
- B3_PROFILE("findCompoundPairsKernel");
- b3BufferInfoCL bInfo[] =
- {
- b3BufferInfoCL(pairs->getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(clAabbsLocalSpace.getBufferCL(), true),
- b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
- b3BufferInfoCL(m_gpuCompoundPairs.getBufferCL()),
- b3BufferInfoCL(m_numCompoundPairsOut.getBufferCL()),
- b3BufferInfoCL(subTreesGPU->getBufferCL()),
- b3BufferInfoCL(treeNodesGPU->getBufferCL()),
- b3BufferInfoCL(bvhInfo->getBufferCL())};
- b3LauncherCL launcher(m_queue, m_findCompoundPairsKernel, "m_findCompoundPairsKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(nPairs);
- launcher.setConst(compoundPairCapacity);
- int num = nPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- numCompoundPairs = m_numCompoundPairsOut.at(0);
- //printf("numCompoundPairs =%d\n",numCompoundPairs );
- if (numCompoundPairs)
- {
- //printf("numCompoundPairs=%d\n",numCompoundPairs);
- }
- }
- else
- {
- b3AlignedObjectArray<b3QuantizedBvhNode> treeNodesCPU;
- treeNodesGPU->copyToHost(treeNodesCPU);
- b3AlignedObjectArray<b3BvhSubtreeInfo> subTreesCPU;
- subTreesGPU->copyToHost(subTreesCPU);
- b3AlignedObjectArray<b3BvhInfo> bvhInfoCPU;
- bvhInfo->copyToHost(bvhInfoCPU);
- b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
- clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
- b3AlignedObjectArray<b3Aabb> hostAabbsLocalSpace;
- clAabbsLocalSpace.copyToHost(hostAabbsLocalSpace);
- b3AlignedObjectArray<b3Int4> hostPairs;
- pairs->copyToHost(hostPairs);
- b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
- bodyBuf->copyToHost(hostBodyBuf);
- b3AlignedObjectArray<b3Int4> cpuCompoundPairsOut;
- cpuCompoundPairsOut.resize(compoundPairCapacity);
- b3AlignedObjectArray<b3Collidable> hostCollidables;
- gpuCollidables.copyToHost(hostCollidables);
- b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
- gpuChildShapes.copyToHost(cpuChildShapes);
- b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
- convexData.copyToHost(hostConvexData);
- b3AlignedObjectArray<b3Vector3> hostVertices;
- gpuVertices.copyToHost(hostVertices);
- for (int pairIndex = 0; pairIndex < nPairs; pairIndex++)
- {
- int bodyIndexA = hostPairs[pairIndex].x;
- int bodyIndexB = hostPairs[pairIndex].y;
- int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx;
- int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx;
- if (cpuChildShapes.size())
- {
- findCompoundPairsKernel(
- pairIndex,
- bodyIndexA,
- bodyIndexB,
- collidableIndexA,
- collidableIndexB,
- &hostBodyBuf[0],
- &hostCollidables[0],
- &hostConvexData[0],
- hostVertices,
- hostAabbsWorldSpace,
- hostAabbsLocalSpace,
- &cpuChildShapes[0],
- &cpuCompoundPairsOut[0],
- &numCompoundPairs,
- compoundPairCapacity,
- treeNodesCPU,
- subTreesCPU,
- bvhInfoCPU);
- }
- }
- m_numCompoundPairsOut.copyFromHostPointer(&numCompoundPairs, 1, 0, true);
- if (numCompoundPairs)
- {
- b3CompoundOverlappingPair* ptr = (b3CompoundOverlappingPair*)&cpuCompoundPairsOut[0];
- m_gpuCompoundPairs.copyFromHostPointer(ptr, numCompoundPairs, 0, true);
- }
- //cpuCompoundPairsOut
- }
- if (numCompoundPairs)
- {
- printf("numCompoundPairs=%d\n", numCompoundPairs);
- }
- if (numCompoundPairs > compoundPairCapacity)
- {
- b3Error("Exceeded compound pair capacity (%d/%d)\n", numCompoundPairs, compoundPairCapacity);
- numCompoundPairs = compoundPairCapacity;
- }
- m_gpuCompoundPairs.resize(numCompoundPairs);
- m_gpuHasCompoundSepNormals.resize(numCompoundPairs);
- m_gpuCompoundSepNormals.resize(numCompoundPairs);
- if (numCompoundPairs)
- {
- B3_PROFILE("processCompoundPairsPrimitivesKernel");
- b3BufferInfoCL bInfo[] =
- {
- b3BufferInfoCL(m_gpuCompoundPairs.getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
- b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
- b3BufferInfoCL(contactOut->getBufferCL()),
- b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_processCompoundPairsPrimitivesKernel, "m_processCompoundPairsPrimitivesKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(numCompoundPairs);
- launcher.setConst(maxContactCapacity);
- int num = numCompoundPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- nContacts = m_totalContactsOut.at(0);
- //printf("nContacts (after processCompoundPairsPrimitivesKernel) = %d\n",nContacts);
- if (nContacts > maxContactCapacity)
- {
- b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity);
- nContacts = maxContactCapacity;
- }
- }
- if (numCompoundPairs)
- {
- B3_PROFILE("processCompoundPairsKernel");
- b3BufferInfoCL bInfo[] =
- {
- b3BufferInfoCL(m_gpuCompoundPairs.getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
- b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
- b3BufferInfoCL(m_gpuCompoundSepNormals.getBufferCL()),
- b3BufferInfoCL(m_gpuHasCompoundSepNormals.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_processCompoundPairsKernel, "m_processCompoundPairsKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(numCompoundPairs);
- int num = numCompoundPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- }
- //printf("numConcave = %d\n",numConcave);
- // printf("hostNormals.size()=%d\n",hostNormals.size());
- //int numPairs = pairCount.at(0);
- }
- int vertexFaceCapacity = 64;
- {
- //now perform the tree query on GPU
- if (treeNodesGPU->size() && treeNodesGPU->size())
- {
- if (bvhTraversalKernelGPU)
- {
- B3_PROFILE("m_bvhTraversalKernel");
- numConcavePairs = m_numConcavePairsOut.at(0);
- b3LauncherCL launcher(m_queue, m_bvhTraversalKernel, "m_bvhTraversalKernel");
- launcher.setBuffer(pairs->getBufferCL());
- launcher.setBuffer(bodyBuf->getBufferCL());
- launcher.setBuffer(gpuCollidables.getBufferCL());
- launcher.setBuffer(clAabbsWorldSpace.getBufferCL());
- launcher.setBuffer(triangleConvexPairsOut.getBufferCL());
- launcher.setBuffer(m_numConcavePairsOut.getBufferCL());
- launcher.setBuffer(subTreesGPU->getBufferCL());
- launcher.setBuffer(treeNodesGPU->getBufferCL());
- launcher.setBuffer(bvhInfo->getBufferCL());
- launcher.setConst(nPairs);
- launcher.setConst(maxTriConvexPairCapacity);
- int num = nPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- numConcavePairs = m_numConcavePairsOut.at(0);
- }
- else
- {
- b3AlignedObjectArray<b3Int4> hostPairs;
- pairs->copyToHost(hostPairs);
- b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
- bodyBuf->copyToHost(hostBodyBuf);
- b3AlignedObjectArray<b3Collidable> hostCollidables;
- gpuCollidables.copyToHost(hostCollidables);
- b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
- clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
- //int maxTriConvexPairCapacity,
- b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
- triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity);
- //int numTriConvexPairsOutHost=0;
- numConcavePairs = 0;
- //m_numConcavePairsOut
- b3AlignedObjectArray<b3QuantizedBvhNode> treeNodesCPU;
- treeNodesGPU->copyToHost(treeNodesCPU);
- b3AlignedObjectArray<b3BvhSubtreeInfo> subTreesCPU;
- subTreesGPU->copyToHost(subTreesCPU);
- b3AlignedObjectArray<b3BvhInfo> bvhInfoCPU;
- bvhInfo->copyToHost(bvhInfoCPU);
- //compute it...
- volatile int hostNumConcavePairsOut = 0;
- //
- for (int i = 0; i < nPairs; i++)
- {
- b3BvhTraversal(&hostPairs.at(0),
- &hostBodyBuf.at(0),
- &hostCollidables.at(0),
- &hostAabbsWorldSpace.at(0),
- &triangleConvexPairsOutHost.at(0),
- &hostNumConcavePairsOut,
- &subTreesCPU.at(0),
- &treeNodesCPU.at(0),
- &bvhInfoCPU.at(0),
- nPairs,
- maxTriConvexPairCapacity,
- i);
- }
- numConcavePairs = hostNumConcavePairsOut;
- if (hostNumConcavePairsOut)
- {
- triangleConvexPairsOutHost.resize(hostNumConcavePairsOut);
- triangleConvexPairsOut.copyFromHost(triangleConvexPairsOutHost);
- }
- //
- m_numConcavePairsOut.resize(0);
- m_numConcavePairsOut.push_back(numConcavePairs);
- }
- //printf("numConcavePairs=%d (max = %d\n",numConcavePairs,maxTriConvexPairCapacity);
- if (numConcavePairs > maxTriConvexPairCapacity)
- {
- static int exceeded_maxTriConvexPairCapacity_count = 0;
- b3Error("Exceeded the maxTriConvexPairCapacity (found %d but max is %d, it happened %d times)\n",
- numConcavePairs, maxTriConvexPairCapacity, exceeded_maxTriConvexPairCapacity_count++);
- numConcavePairs = maxTriConvexPairCapacity;
- }
- triangleConvexPairsOut.resize(numConcavePairs);
- if (numConcavePairs)
- {
- clippingFacesOutGPU.resize(numConcavePairs);
- worldNormalsAGPU.resize(numConcavePairs);
- worldVertsA1GPU.resize(vertexFaceCapacity * (numConcavePairs));
- worldVertsB1GPU.resize(vertexFaceCapacity * (numConcavePairs));
- if (findConcaveSeparatingAxisKernelGPU)
- {
- /*
- m_concaveHasSeparatingNormals.copyFromHost(concaveHasSeparatingNormalsCPU);
- clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
- worldVertsA1GPU.copyFromHost(worldVertsA1CPU);
- worldNormalsAGPU.copyFromHost(worldNormalsACPU);
- worldVertsB1GPU.copyFromHost(worldVertsB1CPU);
- */
- //now perform a SAT test for each triangle-convex element (stored in triangleConvexPairsOut)
- if (splitSearchSepAxisConcave)
- {
- //printf("numConcavePairs = %d\n",numConcavePairs);
- m_dmins.resize(numConcavePairs);
- {
- B3_PROFILE("findConcaveSeparatingAxisVertexFaceKernel");
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(triangleConvexPairsOut.getBufferCL()),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
- b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
- b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
- b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
- b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
- b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsB1GPU.getBufferCL()),
- b3BufferInfoCL(m_dmins.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisVertexFaceKernel, "m_findConcaveSeparatingAxisVertexFaceKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(vertexFaceCapacity);
- launcher.setConst(numConcavePairs);
- int num = numConcavePairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- }
- // numConcavePairs = 0;
- if (1)
- {
- B3_PROFILE("findConcaveSeparatingAxisEdgeEdgeKernel");
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(triangleConvexPairsOut.getBufferCL()),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
- b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
- b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
- b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
- b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
- b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsB1GPU.getBufferCL()),
- b3BufferInfoCL(m_dmins.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisEdgeEdgeKernel, "m_findConcaveSeparatingAxisEdgeEdgeKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(vertexFaceCapacity);
- launcher.setConst(numConcavePairs);
- int num = numConcavePairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- }
- // numConcavePairs = 0;
- }
- else
- {
- B3_PROFILE("findConcaveSeparatingAxisKernel");
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(triangleConvexPairsOut.getBufferCL()),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
- b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
- b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
- b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
- b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
- b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsB1GPU.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisKernel, "m_findConcaveSeparatingAxisKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(vertexFaceCapacity);
- launcher.setConst(numConcavePairs);
- int num = numConcavePairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- }
- }
- else
- {
- b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
- b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
- b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
- b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;
- b3AlignedObjectArray<int> concaveHasSeparatingNormalsCPU;
- b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
- triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost);
- //triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity);
- b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
- bodyBuf->copyToHost(hostBodyBuf);
- b3AlignedObjectArray<b3Collidable> hostCollidables;
- gpuCollidables.copyToHost(hostCollidables);
- b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
- clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
- b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
- convexData.copyToHost(hostConvexData);
- b3AlignedObjectArray<b3Vector3> hostVertices;
- gpuVertices.copyToHost(hostVertices);
- b3AlignedObjectArray<b3Vector3> hostUniqueEdges;
- gpuUniqueEdges.copyToHost(hostUniqueEdges);
- b3AlignedObjectArray<b3GpuFace> hostFaces;
- gpuFaces.copyToHost(hostFaces);
- b3AlignedObjectArray<int> hostIndices;
- gpuIndices.copyToHost(hostIndices);
- b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
- gpuChildShapes.copyToHost(cpuChildShapes);
- b3AlignedObjectArray<b3Vector3> concaveSepNormalsHost;
- m_concaveSepNormals.copyToHost(concaveSepNormalsHost);
- concaveHasSeparatingNormalsCPU.resize(concaveSepNormalsHost.size());
- b3GpuChildShape* childShapePointerCPU = 0;
- if (cpuChildShapes.size())
- childShapePointerCPU = &cpuChildShapes.at(0);
- clippingFacesOutCPU.resize(clippingFacesOutGPU.size());
- worldVertsA1CPU.resize(worldVertsA1GPU.size());
- worldNormalsACPU.resize(worldNormalsAGPU.size());
- worldVertsB1CPU.resize(worldVertsB1GPU.size());
- for (int i = 0; i < numConcavePairs; i++)
- {
- b3FindConcaveSeparatingAxisKernel(&triangleConvexPairsOutHost.at(0),
- &hostBodyBuf.at(0),
- &hostCollidables.at(0),
- &hostConvexData.at(0), &hostVertices.at(0), &hostUniqueEdges.at(0),
- &hostFaces.at(0), &hostIndices.at(0), childShapePointerCPU,
- &hostAabbsWorldSpace.at(0),
- &concaveSepNormalsHost.at(0),
- &clippingFacesOutCPU.at(0),
- &worldVertsA1CPU.at(0),
- &worldNormalsACPU.at(0),
- &worldVertsB1CPU.at(0),
- &concaveHasSeparatingNormalsCPU.at(0),
- vertexFaceCapacity,
- numConcavePairs, i);
- };
- m_concaveSepNormals.copyFromHost(concaveSepNormalsHost);
- m_concaveHasSeparatingNormals.copyFromHost(concaveHasSeparatingNormalsCPU);
- clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
- worldVertsA1GPU.copyFromHost(worldVertsA1CPU);
- worldNormalsAGPU.copyFromHost(worldNormalsACPU);
- worldVertsB1GPU.copyFromHost(worldVertsB1CPU);
- }
- // b3AlignedObjectArray<b3Vector3> cpuCompoundSepNormals;
- // m_concaveSepNormals.copyToHost(cpuCompoundSepNormals);
- // b3AlignedObjectArray<b3Int4> cpuConcavePairs;
- // triangleConvexPairsOut.copyToHost(cpuConcavePairs);
- }
- }
- }
- if (numConcavePairs)
- {
- if (numConcavePairs)
- {
- B3_PROFILE("findConcaveSphereContactsKernel");
- nContacts = m_totalContactsOut.at(0);
- // printf("nContacts1 = %d\n",nContacts);
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(triangleConvexPairsOut.getBufferCL()),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
- b3BufferInfoCL(contactOut->getBufferCL()),
- b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_findConcaveSphereContactsKernel, "m_findConcaveSphereContactsKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(numConcavePairs);
- launcher.setConst(maxContactCapacity);
- int num = numConcavePairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- nContacts = m_totalContactsOut.at(0);
- //printf("nContacts (after findConcaveSphereContactsKernel) = %d\n",nContacts);
- //printf("nContacts2 = %d\n",nContacts);
- if (nContacts >= maxContactCapacity)
- {
- b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity);
- nContacts = maxContactCapacity;
- }
- }
- }
- #ifdef __APPLE__
- bool contactClippingOnGpu = true;
- #else
- bool contactClippingOnGpu = true;
- #endif
- if (contactClippingOnGpu)
- {
- m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
- // printf("nContacts3 = %d\n",nContacts);
- //B3_PROFILE("clipHullHullKernel");
- bool breakupConcaveConvexKernel = true;
- #ifdef __APPLE__
- //actually, some Apple OpenCL platform/device combinations work fine...
- breakupConcaveConvexKernel = true;
- #endif
- //concave-convex contact clipping
- if (numConcavePairs)
- {
- // printf("numConcavePairs = %d\n", numConcavePairs);
- // nContacts = m_totalContactsOut.at(0);
- // printf("nContacts before = %d\n", nContacts);
- if (breakupConcaveConvexKernel)
- {
- worldVertsB2GPU.resize(vertexFaceCapacity * numConcavePairs);
- //clipFacesAndFindContacts
- if (clipConcaveFacesAndFindContactsCPU)
- {
- b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
- b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
- b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
- b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;
- clippingFacesOutGPU.copyToHost(clippingFacesOutCPU);
- worldVertsA1GPU.copyToHost(worldVertsA1CPU);
- worldNormalsAGPU.copyToHost(worldNormalsACPU);
- worldVertsB1GPU.copyToHost(worldVertsB1CPU);
- b3AlignedObjectArray<int> concaveHasSeparatingNormalsCPU;
- m_concaveHasSeparatingNormals.copyToHost(concaveHasSeparatingNormalsCPU);
- b3AlignedObjectArray<b3Vector3> concaveSepNormalsHost;
- m_concaveSepNormals.copyToHost(concaveSepNormalsHost);
- b3AlignedObjectArray<b3Vector3> worldVertsB2CPU;
- worldVertsB2CPU.resize(worldVertsB2GPU.size());
- for (int i = 0; i < numConcavePairs; i++)
- {
- clipFacesAndFindContactsKernel(&concaveSepNormalsHost.at(0),
- &concaveHasSeparatingNormalsCPU.at(0),
- &clippingFacesOutCPU.at(0),
- &worldVertsA1CPU.at(0),
- &worldNormalsACPU.at(0),
- &worldVertsB1CPU.at(0),
- &worldVertsB2CPU.at(0),
- vertexFaceCapacity,
- i);
- }
- clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
- worldVertsB2GPU.copyFromHost(worldVertsB2CPU);
- }
- else
- {
- if (1)
- {
- B3_PROFILE("clipFacesAndFindContacts");
- //nContacts = m_totalContactsOut.at(0);
- //int h = m_hasSeparatingNormals.at(0);
- //int4 p = clippingFacesOutGPU.at(0);
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
- b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
- b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
- b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsB1GPU.getBufferCL()),
- b3BufferInfoCL(worldVertsB2GPU.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_clipFacesAndFindContacts, "m_clipFacesAndFindContacts");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(vertexFaceCapacity);
- launcher.setConst(numConcavePairs);
- int debugMode = 0;
- launcher.setConst(debugMode);
- int num = numConcavePairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- //int bla = m_totalContactsOut.at(0);
- }
- }
- //contactReduction
- {
- int newContactCapacity = nContacts + numConcavePairs;
- contactOut->reserve(newContactCapacity);
- if (reduceConcaveContactsOnGPU)
- {
- // printf("newReservation = %d\n",newReservation);
- {
- B3_PROFILE("newContactReductionKernel");
- b3BufferInfoCL bInfo[] =
- {
- b3BufferInfoCL(triangleConvexPairsOut.getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
- b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
- b3BufferInfoCL(contactOut->getBufferCL()),
- b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsB2GPU.getBufferCL()),
- b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_newContactReductionKernel, "m_newContactReductionKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(vertexFaceCapacity);
- launcher.setConst(newContactCapacity);
- launcher.setConst(numConcavePairs);
- int num = numConcavePairs;
- launcher.launch1D(num);
- }
- nContacts = m_totalContactsOut.at(0);
- contactOut->resize(nContacts);
- //printf("contactOut4 (after newContactReductionKernel) = %d\n",nContacts);
- }
- else
- {
- volatile int nGlobalContactsOut = nContacts;
- b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
- triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost);
- b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
- bodyBuf->copyToHost(hostBodyBuf);
- b3AlignedObjectArray<int> concaveHasSeparatingNormalsCPU;
- m_concaveHasSeparatingNormals.copyToHost(concaveHasSeparatingNormalsCPU);
- b3AlignedObjectArray<b3Vector3> concaveSepNormalsHost;
- m_concaveSepNormals.copyToHost(concaveSepNormalsHost);
- b3AlignedObjectArray<b3Contact4> hostContacts;
- if (nContacts)
- {
- contactOut->copyToHost(hostContacts);
- }
- hostContacts.resize(newContactCapacity);
- b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
- b3AlignedObjectArray<b3Vector3> worldVertsB2CPU;
- clippingFacesOutGPU.copyToHost(clippingFacesOutCPU);
- worldVertsB2GPU.copyToHost(worldVertsB2CPU);
- for (int i = 0; i < numConcavePairs; i++)
- {
- b3NewContactReductionKernel(&triangleConvexPairsOutHost.at(0),
- &hostBodyBuf.at(0),
- &concaveSepNormalsHost.at(0),
- &concaveHasSeparatingNormalsCPU.at(0),
- &hostContacts.at(0),
- &clippingFacesOutCPU.at(0),
- &worldVertsB2CPU.at(0),
- &nGlobalContactsOut,
- vertexFaceCapacity,
- newContactCapacity,
- numConcavePairs,
- i);
- }
- nContacts = nGlobalContactsOut;
- m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
- // nContacts = m_totalContactsOut.at(0);
- //contactOut->resize(nContacts);
- hostContacts.resize(nContacts);
- //printf("contactOut4 (after newContactReductionKernel) = %d\n",nContacts);
- contactOut->copyFromHost(hostContacts);
- }
- }
- //re-use?
- }
- else
- {
- B3_PROFILE("clipHullHullConcaveConvexKernel");
- nContacts = m_totalContactsOut.at(0);
- int newContactCapacity = contactOut->capacity();
- //printf("contactOut5 = %d\n",nContacts);
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(triangleConvexPairsOut.getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
- b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
- b3BufferInfoCL(contactOut->getBufferCL()),
- b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_clipHullHullConcaveConvexKernel, "m_clipHullHullConcaveConvexKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(newContactCapacity);
- launcher.setConst(numConcavePairs);
- int num = numConcavePairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- nContacts = m_totalContactsOut.at(0);
- contactOut->resize(nContacts);
- //printf("contactOut6 = %d\n",nContacts);
- b3AlignedObjectArray<b3Contact4> cpuContacts;
- contactOut->copyToHost(cpuContacts);
- }
- // printf("nContacts after = %d\n", nContacts);
- } //numConcavePairs
- //convex-convex contact clipping
- bool breakupKernel = false;
- #ifdef __APPLE__
- breakupKernel = true;
- #endif
- #ifdef CHECK_ON_HOST
- bool computeConvexConvex = false;
- #else
- bool computeConvexConvex = true;
- #endif //CHECK_ON_HOST
- if (computeConvexConvex)
- {
- B3_PROFILE("clipHullHullKernel");
- if (breakupKernel)
- {
- worldVertsB1GPU.resize(vertexFaceCapacity * nPairs);
- clippingFacesOutGPU.resize(nPairs);
- worldNormalsAGPU.resize(nPairs);
- worldVertsA1GPU.resize(vertexFaceCapacity * nPairs);
- worldVertsB2GPU.resize(vertexFaceCapacity * nPairs);
- if (findConvexClippingFacesGPU)
- {
- B3_PROFILE("findClippingFacesKernel");
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(pairs->getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(m_sepNormals.getBufferCL()),
- b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
- b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
- b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsB1GPU.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_findClippingFacesKernel, "m_findClippingFacesKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(vertexFaceCapacity);
- launcher.setConst(nPairs);
- int num = nPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- }
- else
- {
- float minDist = -1e30f;
- float maxDist = 0.02f;
- b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
- convexData.copyToHost(hostConvexData);
- b3AlignedObjectArray<b3Collidable> hostCollidables;
- gpuCollidables.copyToHost(hostCollidables);
- b3AlignedObjectArray<int> hostHasSepNormals;
- m_hasSeparatingNormals.copyToHost(hostHasSepNormals);
- b3AlignedObjectArray<b3Vector3> cpuSepNormals;
- m_sepNormals.copyToHost(cpuSepNormals);
- b3AlignedObjectArray<b3Int4> hostPairs;
- pairs->copyToHost(hostPairs);
- b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
- bodyBuf->copyToHost(hostBodyBuf);
- //worldVertsB1GPU.resize(vertexFaceCapacity*nPairs);
- b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;
- worldVertsB1GPU.copyToHost(worldVertsB1CPU);
- b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
- clippingFacesOutGPU.copyToHost(clippingFacesOutCPU);
- b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
- worldNormalsACPU.resize(nPairs);
- b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
- worldVertsA1CPU.resize(worldVertsA1GPU.size());
- b3AlignedObjectArray<b3Vector3> hostVertices;
- gpuVertices.copyToHost(hostVertices);
- b3AlignedObjectArray<b3GpuFace> hostFaces;
- gpuFaces.copyToHost(hostFaces);
- b3AlignedObjectArray<int> hostIndices;
- gpuIndices.copyToHost(hostIndices);
- for (int i = 0; i < nPairs; i++)
- {
- int bodyIndexA = hostPairs[i].x;
- int bodyIndexB = hostPairs[i].y;
- int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx;
- int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx;
- int shapeIndexA = hostCollidables[collidableIndexA].m_shapeIndex;
- int shapeIndexB = hostCollidables[collidableIndexB].m_shapeIndex;
- if (hostHasSepNormals[i])
- {
- b3FindClippingFaces(cpuSepNormals[i],
- &hostConvexData[shapeIndexA],
- &hostConvexData[shapeIndexB],
- hostBodyBuf[bodyIndexA].m_pos, hostBodyBuf[bodyIndexA].m_quat,
- hostBodyBuf[bodyIndexB].m_pos, hostBodyBuf[bodyIndexB].m_quat,
- &worldVertsA1CPU.at(0), &worldNormalsACPU.at(0),
- &worldVertsB1CPU.at(0),
- vertexFaceCapacity, minDist, maxDist,
- &hostVertices.at(0), &hostFaces.at(0),
- &hostIndices.at(0),
- &hostVertices.at(0), &hostFaces.at(0),
- &hostIndices.at(0), &clippingFacesOutCPU.at(0), i);
- }
- }
- clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
- worldVertsA1GPU.copyFromHost(worldVertsA1CPU);
- worldNormalsAGPU.copyFromHost(worldNormalsACPU);
- worldVertsB1GPU.copyFromHost(worldVertsB1CPU);
- }
- ///clip face B against face A, reduce contacts and append them to a global contact array
- if (1)
- {
- if (clipConvexFacesAndFindContactsCPU)
- {
- //b3AlignedObjectArray<b3Int4> hostPairs;
- //pairs->copyToHost(hostPairs);
- b3AlignedObjectArray<b3Vector3> hostSepNormals;
- m_sepNormals.copyToHost(hostSepNormals);
- b3AlignedObjectArray<int> hostHasSepAxis;
- m_hasSeparatingNormals.copyToHost(hostHasSepAxis);
- b3AlignedObjectArray<b3Int4> hostClippingFaces;
- clippingFacesOutGPU.copyToHost(hostClippingFaces);
- b3AlignedObjectArray<b3Vector3> worldVertsB2CPU;
- worldVertsB2CPU.resize(vertexFaceCapacity * nPairs);
- b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
- worldVertsA1GPU.copyToHost(worldVertsA1CPU);
- b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
- worldNormalsAGPU.copyToHost(worldNormalsACPU);
- b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;
- worldVertsB1GPU.copyToHost(worldVertsB1CPU);
- /*
- __global const b3Float4* separatingNormals,
- __global const int* hasSeparatingAxis,
- __global b3Int4* clippingFacesOut,
- __global b3Float4* worldVertsA1,
- __global b3Float4* worldNormalsA1,
- __global b3Float4* worldVertsB1,
- __global b3Float4* worldVertsB2,
- int vertexFaceCapacity,
- int pairIndex
- */
- for (int i = 0; i < nPairs; i++)
- {
- clipFacesAndFindContactsKernel(
- &hostSepNormals.at(0),
- &hostHasSepAxis.at(0),
- &hostClippingFaces.at(0),
- &worldVertsA1CPU.at(0),
- &worldNormalsACPU.at(0),
- &worldVertsB1CPU.at(0),
- &worldVertsB2CPU.at(0),
- vertexFaceCapacity,
- i);
- }
- clippingFacesOutGPU.copyFromHost(hostClippingFaces);
- worldVertsB2GPU.copyFromHost(worldVertsB2CPU);
- }
- else
- {
- B3_PROFILE("clipFacesAndFindContacts");
- //nContacts = m_totalContactsOut.at(0);
- //int h = m_hasSeparatingNormals.at(0);
- //int4 p = clippingFacesOutGPU.at(0);
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(m_sepNormals.getBufferCL()),
- b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
- b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
- b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsB1GPU.getBufferCL()),
- b3BufferInfoCL(worldVertsB2GPU.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_clipFacesAndFindContacts, "m_clipFacesAndFindContacts");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(vertexFaceCapacity);
- launcher.setConst(nPairs);
- int debugMode = 0;
- launcher.setConst(debugMode);
- int num = nPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- }
- {
- nContacts = m_totalContactsOut.at(0);
- //printf("nContacts = %d\n",nContacts);
- int newContactCapacity = nContacts + nPairs;
- contactOut->reserve(newContactCapacity);
- if (reduceConvexContactsOnGPU)
- {
- {
- B3_PROFILE("newContactReductionKernel");
- b3BufferInfoCL bInfo[] =
- {
- b3BufferInfoCL(pairs->getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(m_sepNormals.getBufferCL()),
- b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
- b3BufferInfoCL(contactOut->getBufferCL()),
- b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
- b3BufferInfoCL(worldVertsB2GPU.getBufferCL()),
- b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_newContactReductionKernel, "m_newContactReductionKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(vertexFaceCapacity);
- launcher.setConst(newContactCapacity);
- launcher.setConst(nPairs);
- int num = nPairs;
- launcher.launch1D(num);
- }
- nContacts = m_totalContactsOut.at(0);
- contactOut->resize(nContacts);
- }
- else
- {
- volatile int nGlobalContactsOut = nContacts;
- b3AlignedObjectArray<b3Int4> hostPairs;
- pairs->copyToHost(hostPairs);
- b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
- bodyBuf->copyToHost(hostBodyBuf);
- b3AlignedObjectArray<b3Vector3> hostSepNormals;
- m_sepNormals.copyToHost(hostSepNormals);
- b3AlignedObjectArray<int> hostHasSepAxis;
- m_hasSeparatingNormals.copyToHost(hostHasSepAxis);
- b3AlignedObjectArray<b3Contact4> hostContactsOut;
- contactOut->copyToHost(hostContactsOut);
- hostContactsOut.resize(newContactCapacity);
- b3AlignedObjectArray<b3Int4> hostClippingFaces;
- clippingFacesOutGPU.copyToHost(hostClippingFaces);
- b3AlignedObjectArray<b3Vector3> worldVertsB2CPU;
- worldVertsB2GPU.copyToHost(worldVertsB2CPU);
- for (int i = 0; i < nPairs; i++)
- {
- b3NewContactReductionKernel(&hostPairs.at(0),
- &hostBodyBuf.at(0),
- &hostSepNormals.at(0),
- &hostHasSepAxis.at(0),
- &hostContactsOut.at(0),
- &hostClippingFaces.at(0),
- &worldVertsB2CPU.at(0),
- &nGlobalContactsOut,
- vertexFaceCapacity,
- newContactCapacity,
- nPairs,
- i);
- }
- nContacts = nGlobalContactsOut;
- m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
- hostContactsOut.resize(nContacts);
- //printf("contactOut4 (after newContactReductionKernel) = %d\n",nContacts);
- contactOut->copyFromHost(hostContactsOut);
- }
- // b3Contact4 pt = contactOut->at(0);
- // printf("nContacts = %d\n",nContacts);
- }
- }
- }
- else //breakupKernel
- {
- if (nPairs)
- {
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(pairs->getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(m_sepNormals.getBufferCL()),
- b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
- b3BufferInfoCL(contactOut->getBufferCL()),
- b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_clipHullHullKernel, "m_clipHullHullKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(nPairs);
- launcher.setConst(maxContactCapacity);
- int num = nPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- nContacts = m_totalContactsOut.at(0);
- if (nContacts >= maxContactCapacity)
- {
- b3Error("Exceeded contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
- nContacts = maxContactCapacity;
- }
- contactOut->resize(nContacts);
- }
- }
- int nCompoundsPairs = m_gpuCompoundPairs.size();
- if (nCompoundsPairs)
- {
- b3BufferInfoCL bInfo[] = {
- b3BufferInfoCL(m_gpuCompoundPairs.getBufferCL(), true),
- b3BufferInfoCL(bodyBuf->getBufferCL(), true),
- b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
- b3BufferInfoCL(convexData.getBufferCL(), true),
- b3BufferInfoCL(gpuVertices.getBufferCL(), true),
- b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
- b3BufferInfoCL(gpuFaces.getBufferCL(), true),
- b3BufferInfoCL(gpuIndices.getBufferCL(), true),
- b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
- b3BufferInfoCL(m_gpuCompoundSepNormals.getBufferCL(), true),
- b3BufferInfoCL(m_gpuHasCompoundSepNormals.getBufferCL(), true),
- b3BufferInfoCL(contactOut->getBufferCL()),
- b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
- b3LauncherCL launcher(m_queue, m_clipCompoundsHullHullKernel, "m_clipCompoundsHullHullKernel");
- launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
- launcher.setConst(nCompoundsPairs);
- launcher.setConst(maxContactCapacity);
- int num = nCompoundsPairs;
- launcher.launch1D(num);
- clFinish(m_queue);
- nContacts = m_totalContactsOut.at(0);
- if (nContacts > maxContactCapacity)
- {
- b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity);
- nContacts = maxContactCapacity;
- }
- contactOut->resize(nContacts);
- } //if nCompoundsPairs
- }
- } //contactClippingOnGpu
- //printf("nContacts end = %d\n",nContacts);
- //printf("frameCount = %d\n",frameCount++);
- }
|