From 271f458837c0bc0e9ded1275307d0743ca16d6bd Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 4 Jan 2014 20:54:27 -0800 Subject: [PATCH] Ported Minkowski Portal Refinement mpr.c from libccd to OpenCL, for bettwe edge-edge performance (and additional contact point for degenerate/high detailed convex shapes) Removed b3RigidBodyCL, replace by b3RigidBodyData and b3RigidBodyData_t shared between C++ host and OpenCL, Same for b3InertiaCL -> b3InertiaData --- Demos3/GpuDemos/GpuDemo.h | 6 +- .../GpuDemos/rigidbody/GpuRigidBodyDemo.cpp | 3 +- build3/stringify.bat | 1 + .../NarrowPhaseCollision/b3ConvexUtility.cpp | 2 +- .../NarrowPhaseCollision/b3ConvexUtility.h | 2 +- .../NarrowPhaseCollision/b3CpuNarrowPhase.h | 4 +- .../NarrowPhaseCollision/b3RigidBodyCL.h | 18 +- .../NarrowPhaseCollision/shared/b3ClipFaces.h | 14 +- .../shared/b3ContactSphereSphere.h | 4 +- .../shared/b3FindConcaveSatAxis.h | 302 +++- .../shared/b3MprPenetration.h | 919 ++++++++++++ src/Bullet3Common/b3Vector3.h | 7 +- src/Bullet3Common/shared/b3Float4.h | 25 + .../shared/b3PlatformDefinitions.h | 7 + src/Bullet3Common/shared/b3Quat.h | 9 + .../ConstraintSolver/b3FixedConstraint.cpp | 6 +- .../ConstraintSolver/b3FixedConstraint.h | 4 +- .../b3Generic6DofConstraint.cpp | 24 +- .../b3Generic6DofConstraint.h | 28 +- .../ConstraintSolver/b3PgsJacobiSolver.cpp | 90 +- .../ConstraintSolver/b3PgsJacobiSolver.h | 36 +- .../b3Point2PointConstraint.cpp | 8 +- .../b3Point2PointConstraint.h | 6 +- .../ConstraintSolver/b3TypedConstraint.h | 6 +- .../Initialize/b3OpenCLUtils.cpp | 2 +- .../b3ConvexHullContact.cpp | 534 +++++-- .../b3ConvexHullContact.h | 9 +- .../b3ConvexPolyhedronCL.h | 35 - .../NarrowphaseCollision/b3GjkEpa.cpp | 15 +- .../NarrowphaseCollision/b3GjkEpa.h | 21 +- .../b3GjkPairDetector.cpp | 271 +--- .../NarrowphaseCollision/b3GjkPairDetector.h | 4 +- .../NarrowphaseCollision/b3SupportMappings.h | 8 +- .../NarrowphaseCollision/b3VectorFloat4.h | 13 +- .../NarrowphaseCollision/kernels/mpr.cl | 89 ++ .../NarrowphaseCollision/kernels/mprKernels.h | 1244 +++++++++++++++++ src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp | 10 +- src/Bullet3OpenCL/Raycast/b3GpuRaycast.h | 29 +- .../RigidBody/b3GpuGenericConstraint.cpp | 8 +- .../RigidBody/b3GpuGenericConstraint.h | 6 +- .../RigidBody/b3GpuJacobiContactSolver.cpp | 56 +- .../RigidBody/b3GpuJacobiContactSolver.h | 8 +- .../RigidBody/b3GpuNarrowPhase.cpp | 22 +- .../RigidBody/b3GpuNarrowPhase.h | 4 +- .../RigidBody/b3GpuNarrowPhaseInternalData.h | 16 +- .../RigidBody/b3GpuPgsConstraintSolver.cpp | 50 +- .../RigidBody/b3GpuPgsConstraintSolver.h | 16 +- .../RigidBody/b3GpuPgsContactSolver.cpp | 24 +- .../RigidBody/b3GpuPgsContactSolver.h | 6 +- .../RigidBody/b3GpuRigidBodyPipeline.cpp | 18 +- src/Bullet3OpenCL/RigidBody/b3Solver.cpp | 36 +- src/Bullet3OpenCL/RigidBody/b3Solver.h | 10 +- 52 files changed, 3368 insertions(+), 727 deletions(-) create mode 100644 src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h create mode 100644 src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl create mode 100644 src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h diff --git a/Demos3/GpuDemos/GpuDemo.h b/Demos3/GpuDemos/GpuDemo.h index 72786ef0f..0cfe70ef0 100644 --- a/Demos3/GpuDemos/GpuDemo.h +++ b/Demos3/GpuDemos/GpuDemo.h @@ -48,9 +48,9 @@ public: arraySizeZ(10), #else - arraySizeX(10), - arraySizeY(10), - arraySizeZ(10), + arraySizeX(30), + arraySizeY(30), + arraySizeZ(30), #endif m_useConcaveMesh(false), gapX(16.3), diff --git a/Demos3/GpuDemos/rigidbody/GpuRigidBodyDemo.cpp b/Demos3/GpuDemos/rigidbody/GpuRigidBodyDemo.cpp index 6bd0bb5c5..ed8e54b58 100644 --- a/Demos3/GpuDemos/rigidbody/GpuRigidBodyDemo.cpp +++ b/Demos3/GpuDemos/rigidbody/GpuRigidBodyDemo.cpp @@ -16,7 +16,7 @@ #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" #include "GpuRigidBodyDemoInternalData.h" #include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h" @@ -439,6 +439,7 @@ bool GpuRigidBodyDemo::mouseButtonCallback(int button, int state, float x, float int numVertices = sizeof(point_sphere_vertices)/strideInBytes; int numIndices = sizeof(point_sphere_indices)/sizeof(int); m_data->m_pickGraphicsShapeIndex = m_instancingRenderer->registerShape(&point_sphere_vertices[0],numVertices,point_sphere_indices,numIndices,B3_GL_POINTS); + float color[4] ={1,0,0,1}; float scaling[4]={1,1,1,1}; diff --git a/build3/stringify.bat b/build3/stringify.bat index 2c5c7a263..dddceef20 100644 --- a/build3/stringify.bat +++ b/build3/stringify.bat @@ -18,6 +18,7 @@ premake4 --file=stringifyKernel.lua --kernelfile="../src/Bullet3OpenCL/Broadphas premake4 --file=stringifyKernel.lua --kernelfile="../src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl" --headerfile="../src/Bullet3OpenCL/NarrowphaseCollision/kernels/satKernels.h" --stringname="satKernelsCL" stringify premake4 --file=stringifyKernel.lua --kernelfile="../src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcave.cl" --headerfile="../src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcaveKernels.h" --stringname="satConcaveKernelsCL" stringify +premake4 --file=stringifyKernel.lua --kernelfile="../src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl" --headerfile="../src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h" --stringname="mprKernelsCL" stringify diff --git a/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp b/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp index 87dfebddb..55706fa63 100644 --- a/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp +++ b/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp @@ -20,7 +20,7 @@ subject to the following restrictions: #include "Bullet3Common/b3Quaternion.h" #include "Bullet3Common/b3HashMap.h" -//#include "b3ConvexPolyhedronCL.h" + diff --git a/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h b/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h index 88f433921..86c4151f8 100644 --- a/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h +++ b/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h @@ -20,7 +20,7 @@ subject to the following restrictions: #include "Bullet3Common/b3AlignedObjectArray.h" #include "Bullet3Common/b3Transform.h" -//#include "b3ConvexPolyhedronCL.h" + struct b3MyFace diff --git a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h index 3b91acb82..528be3346 100644 --- a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h +++ b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h @@ -60,8 +60,8 @@ public: - const struct b3RigidBodyCL* getBodiesCpu() const; - //struct b3RigidBodyCL* getBodiesCpu(); + const struct b3RigidBodyData* getBodiesCpu() const; + //struct b3RigidBodyData* getBodiesCpu(); int getNumBodiesGpu() const; diff --git a/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h b/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h index 6d0c0337e..d58f71802 100644 --- a/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h +++ b/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h @@ -21,22 +21,10 @@ subject to the following restrictions: #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" -B3_ATTRIBUTE_ALIGNED16(struct) b3RigidBodyCL : public b3RigidBodyData +inline float b3GetInvMass(const b3RigidBodyData& body) { - B3_DECLARE_ALIGNED_ALLOCATOR(); + return body.m_invMass; +} - - - float getInvMass() const - { - return m_invMass; - } -}; - - -struct b3InertiaCL : public b3InertiaData -{ - -}; #endif//B3_RIGID_BODY_CL diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h index 50a785b07..b5633ecd8 100644 --- a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h +++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h @@ -150,7 +150,19 @@ __kernel void clipFacesAndFindContactsKernel( __global const b3Float4* sepa { depth = minDist; } - +/* + static float maxDepth = 0.f; + if (depth < maxDepth) + { + maxDepth = depth; + if (maxDepth < -10) + { + printf("error at framecount %d?\n",myframecount); + } + printf("maxDepth = %f\n", maxDepth); + + } +*/ if (depth <=maxDist) { b3Float4 pointInWorld = pVtxIn[i]; diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h index dab27ba61..a3fa82287 100644 --- a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h +++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h @@ -9,9 +9,9 @@ void computeContactSphereConvex(int pairIndex, int bodyIndexA, int bodyIndexB, int collidableIndexA, int collidableIndexB, - const b3RigidBodyCL* rigidBodies, + const b3RigidBodyData* rigidBodies, const b3Collidable* collidables, - const b3ConvexPolyhedronCL* convexShapes, + const b3ConvexPolyhedronData* convexShapes, const b3Vector3* convexVertices, const int* convexIndices, const b3GpuFace* faces, diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h index 0026df238..5f301d547 100644 --- a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h +++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h @@ -86,6 +86,22 @@ bool b3FindSeparatingAxis( const b3ConvexPolyhedronData* hullA, __global const b posA.w = 0.f; b3Float4 posB = posB1; posB.w = 0.f; +/* + static int maxFaceVertex = 0; + + int curFaceVertexAB = hullA->m_numFaces*hullB->m_numVertices; + curFaceVertexAB+= hullB->m_numFaces*hullA->m_numVertices; + + if (curFaceVertexAB>maxFaceVertex) + { + maxFaceVertex = curFaceVertexAB; + printf("curFaceVertexAB = %d\n",curFaceVertexAB); + printf("hullA->m_numFaces = %d\n",hullA->m_numFaces); + printf("hullA->m_numVertices = %d\n",hullA->m_numVertices); + printf("hullB->m_numVertices = %d\n",hullB->m_numVertices); + } +*/ + int curPlaneTests=0; { int numFacesA = hullA->m_numFaces; @@ -115,7 +131,171 @@ bool b3FindSeparatingAxis( const b3ConvexPolyhedronData* hullA, __global const b } - +b3Vector3 unitSphere162[]= +{ + b3MakeVector3(0.000000,-1.000000,0.000000), +b3MakeVector3(0.203181,-0.967950,0.147618), +b3MakeVector3(-0.077607,-0.967950,0.238853), +b3MakeVector3(0.723607,-0.447220,0.525725), +b3MakeVector3(0.609547,-0.657519,0.442856), +b3MakeVector3(0.812729,-0.502301,0.295238), +b3MakeVector3(-0.251147,-0.967949,0.000000), +b3MakeVector3(-0.077607,-0.967950,-0.238853), +b3MakeVector3(0.203181,-0.967950,-0.147618), +b3MakeVector3(0.860698,-0.251151,0.442858), +b3MakeVector3(-0.276388,-0.447220,0.850649), +b3MakeVector3(-0.029639,-0.502302,0.864184), +b3MakeVector3(-0.155215,-0.251152,0.955422), +b3MakeVector3(-0.894426,-0.447216,0.000000), +b3MakeVector3(-0.831051,-0.502299,0.238853), +b3MakeVector3(-0.956626,-0.251149,0.147618), +b3MakeVector3(-0.276388,-0.447220,-0.850649), +b3MakeVector3(-0.483971,-0.502302,-0.716565), +b3MakeVector3(-0.436007,-0.251152,-0.864188), +b3MakeVector3(0.723607,-0.447220,-0.525725), +b3MakeVector3(0.531941,-0.502302,-0.681712), +b3MakeVector3(0.687159,-0.251152,-0.681715), +b3MakeVector3(0.687159,-0.251152,0.681715), +b3MakeVector3(-0.436007,-0.251152,0.864188), +b3MakeVector3(-0.956626,-0.251149,-0.147618), +b3MakeVector3(-0.155215,-0.251152,-0.955422), +b3MakeVector3(0.860698,-0.251151,-0.442858), +b3MakeVector3(0.276388,0.447220,0.850649), +b3MakeVector3(0.483971,0.502302,0.716565), +b3MakeVector3(0.232822,0.657519,0.716563), +b3MakeVector3(-0.723607,0.447220,0.525725), +b3MakeVector3(-0.531941,0.502302,0.681712), +b3MakeVector3(-0.609547,0.657519,0.442856), +b3MakeVector3(-0.723607,0.447220,-0.525725), +b3MakeVector3(-0.812729,0.502301,-0.295238), +b3MakeVector3(-0.609547,0.657519,-0.442856), +b3MakeVector3(0.276388,0.447220,-0.850649), +b3MakeVector3(0.029639,0.502302,-0.864184), +b3MakeVector3(0.232822,0.657519,-0.716563), +b3MakeVector3(0.894426,0.447216,0.000000), +b3MakeVector3(0.831051,0.502299,-0.238853), +b3MakeVector3(0.753442,0.657515,0.000000), +b3MakeVector3(-0.232822,-0.657519,0.716563), +b3MakeVector3(-0.162456,-0.850654,0.499995), +b3MakeVector3(0.052790,-0.723612,0.688185), +b3MakeVector3(0.138199,-0.894429,0.425321), +b3MakeVector3(0.262869,-0.525738,0.809012), +b3MakeVector3(0.361805,-0.723611,0.587779), +b3MakeVector3(0.531941,-0.502302,0.681712), +b3MakeVector3(0.425323,-0.850654,0.309011), +b3MakeVector3(0.812729,-0.502301,-0.295238), +b3MakeVector3(0.609547,-0.657519,-0.442856), +b3MakeVector3(0.850648,-0.525736,0.000000), +b3MakeVector3(0.670817,-0.723611,-0.162457), +b3MakeVector3(0.670817,-0.723610,0.162458), +b3MakeVector3(0.425323,-0.850654,-0.309011), +b3MakeVector3(0.447211,-0.894428,0.000001), +b3MakeVector3(-0.753442,-0.657515,0.000000), +b3MakeVector3(-0.525730,-0.850652,0.000000), +b3MakeVector3(-0.638195,-0.723609,0.262864), +b3MakeVector3(-0.361801,-0.894428,0.262864), +b3MakeVector3(-0.688189,-0.525736,0.499997), +b3MakeVector3(-0.447211,-0.723610,0.525729), +b3MakeVector3(-0.483971,-0.502302,0.716565), +b3MakeVector3(-0.232822,-0.657519,-0.716563), +b3MakeVector3(-0.162456,-0.850654,-0.499995), +b3MakeVector3(-0.447211,-0.723611,-0.525727), +b3MakeVector3(-0.361801,-0.894429,-0.262863), +b3MakeVector3(-0.688189,-0.525736,-0.499997), +b3MakeVector3(-0.638195,-0.723609,-0.262863), +b3MakeVector3(-0.831051,-0.502299,-0.238853), +b3MakeVector3(0.361804,-0.723612,-0.587779), +b3MakeVector3(0.138197,-0.894429,-0.425321), +b3MakeVector3(0.262869,-0.525738,-0.809012), +b3MakeVector3(0.052789,-0.723611,-0.688186), +b3MakeVector3(-0.029639,-0.502302,-0.864184), +b3MakeVector3(0.956626,0.251149,0.147618), +b3MakeVector3(0.956626,0.251149,-0.147618), +b3MakeVector3(0.951058,-0.000000,0.309013), +b3MakeVector3(1.000000,0.000000,0.000000), +b3MakeVector3(0.947213,-0.276396,0.162458), +b3MakeVector3(0.951058,0.000000,-0.309013), +b3MakeVector3(0.947213,-0.276396,-0.162458), +b3MakeVector3(0.155215,0.251152,0.955422), +b3MakeVector3(0.436007,0.251152,0.864188), +b3MakeVector3(-0.000000,-0.000000,1.000000), +b3MakeVector3(0.309017,0.000000,0.951056), +b3MakeVector3(0.138199,-0.276398,0.951055), +b3MakeVector3(0.587786,0.000000,0.809017), +b3MakeVector3(0.447216,-0.276398,0.850648), +b3MakeVector3(-0.860698,0.251151,0.442858), +b3MakeVector3(-0.687159,0.251152,0.681715), +b3MakeVector3(-0.951058,-0.000000,0.309013), +b3MakeVector3(-0.809018,0.000000,0.587783), +b3MakeVector3(-0.861803,-0.276396,0.425324), +b3MakeVector3(-0.587786,0.000000,0.809017), +b3MakeVector3(-0.670819,-0.276397,0.688191), +b3MakeVector3(-0.687159,0.251152,-0.681715), +b3MakeVector3(-0.860698,0.251151,-0.442858), +b3MakeVector3(-0.587786,-0.000000,-0.809017), +b3MakeVector3(-0.809018,-0.000000,-0.587783), +b3MakeVector3(-0.670819,-0.276397,-0.688191), +b3MakeVector3(-0.951058,0.000000,-0.309013), +b3MakeVector3(-0.861803,-0.276396,-0.425324), +b3MakeVector3(0.436007,0.251152,-0.864188), +b3MakeVector3(0.155215,0.251152,-0.955422), +b3MakeVector3(0.587786,-0.000000,-0.809017), +b3MakeVector3(0.309017,-0.000000,-0.951056), +b3MakeVector3(0.447216,-0.276398,-0.850648), +b3MakeVector3(0.000000,0.000000,-1.000000), +b3MakeVector3(0.138199,-0.276398,-0.951055), +b3MakeVector3(0.670820,0.276396,0.688190), +b3MakeVector3(0.809019,-0.000002,0.587783), +b3MakeVector3(0.688189,0.525736,0.499997), +b3MakeVector3(0.861804,0.276394,0.425323), +b3MakeVector3(0.831051,0.502299,0.238853), +b3MakeVector3(-0.447216,0.276397,0.850649), +b3MakeVector3(-0.309017,-0.000001,0.951056), +b3MakeVector3(-0.262869,0.525738,0.809012), +b3MakeVector3(-0.138199,0.276397,0.951055), +b3MakeVector3(0.029639,0.502302,0.864184), +b3MakeVector3(-0.947213,0.276396,-0.162458), +b3MakeVector3(-1.000000,0.000001,0.000000), +b3MakeVector3(-0.850648,0.525736,-0.000000), +b3MakeVector3(-0.947213,0.276397,0.162458), +b3MakeVector3(-0.812729,0.502301,0.295238), +b3MakeVector3(-0.138199,0.276397,-0.951055), +b3MakeVector3(-0.309016,-0.000000,-0.951057), +b3MakeVector3(-0.262869,0.525738,-0.809012), +b3MakeVector3(-0.447215,0.276397,-0.850649), +b3MakeVector3(-0.531941,0.502302,-0.681712), +b3MakeVector3(0.861804,0.276396,-0.425322), +b3MakeVector3(0.809019,0.000000,-0.587782), +b3MakeVector3(0.688189,0.525736,-0.499997), +b3MakeVector3(0.670821,0.276397,-0.688189), +b3MakeVector3(0.483971,0.502302,-0.716565), +b3MakeVector3(0.077607,0.967950,0.238853), +b3MakeVector3(0.251147,0.967949,0.000000), +b3MakeVector3(0.000000,1.000000,0.000000), +b3MakeVector3(0.162456,0.850654,0.499995), +b3MakeVector3(0.361800,0.894429,0.262863), +b3MakeVector3(0.447209,0.723612,0.525728), +b3MakeVector3(0.525730,0.850652,0.000000), +b3MakeVector3(0.638194,0.723610,0.262864), +b3MakeVector3(-0.203181,0.967950,0.147618), +b3MakeVector3(-0.425323,0.850654,0.309011), +b3MakeVector3(-0.138197,0.894430,0.425320), +b3MakeVector3(-0.361804,0.723612,0.587778), +b3MakeVector3(-0.052790,0.723612,0.688185), +b3MakeVector3(-0.203181,0.967950,-0.147618), +b3MakeVector3(-0.425323,0.850654,-0.309011), +b3MakeVector3(-0.447210,0.894429,0.000000), +b3MakeVector3(-0.670817,0.723611,-0.162457), +b3MakeVector3(-0.670817,0.723611,0.162457), +b3MakeVector3(0.077607,0.967950,-0.238853), +b3MakeVector3(0.162456,0.850654,-0.499995), +b3MakeVector3(-0.138197,0.894430,-0.425320), +b3MakeVector3(-0.052790,0.723612,-0.688185), +b3MakeVector3(-0.361804,0.723612,-0.587778), +b3MakeVector3(0.361800,0.894429,-0.262863), +b3MakeVector3(0.638194,0.723610,-0.262864), +b3MakeVector3(0.447209,0.723612,-0.525728) +}; bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, @@ -133,7 +313,8 @@ bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global __global const b3GpuFace* facesB, __global const int* indicesB, b3Float4* sep, - float* dmin) + float* dmin, + bool searchAllEdgeEdge) { @@ -146,49 +327,104 @@ bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global int curEdgeEdge = 0; // Test edges - for(int e0=0;e0m_numUniqueEdges;e0++) + static int maxEdgeTests = 0; + int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges; + if (curEdgeTests >maxEdgeTests ) { - const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0]; - b3Float4 edge0World = b3QuatRotate(ornA,edge0); + maxEdgeTests = curEdgeTests ; + printf("maxEdgeTests = %d\n",maxEdgeTests ); + printf("hullA->m_numUniqueEdges = %d\n",hullA->m_numUniqueEdges); + printf("hullB->m_numUniqueEdges = %d\n",hullB->m_numUniqueEdges); - for(int e1=0;e1m_numUniqueEdges;e1++) + } + + + if (searchAllEdgeEdge) + { + for(int e0=0;e0m_numUniqueEdges;e0++) { - const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1]; - b3Float4 edge1World = b3QuatRotate(ornB,edge1); + const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0]; + b3Float4 edge0World = b3QuatRotate(ornA,edge0); - - b3Float4 crossje = b3Cross(edge0World,edge1World); - - curEdgeEdge++; - if(!b3IsAlmostZero(crossje)) + for(int e1=0;e1m_numUniqueEdges;e1++) { - crossje = b3Normalized(crossje); - if (b3Dot(DeltaC2,crossje)<0) - crossje *= -1.f; + const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1]; + b3Float4 edge1World = b3QuatRotate(ornB,edge1); - float dist; - bool result = true; + + b3Float4 crossje = b3Cross(edge0World,edge1World); + + curEdgeEdge++; + if(!b3IsAlmostZero(crossje)) { - float Min0,Max0; - float Min1,Max1; - b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); - b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); + crossje = b3Normalized(crossje); + if (b3Dot(DeltaC2,crossje)<0) + crossje *= -1.f; + + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); - if(Max00) { - *dmin = dist; - *sep = crossje; + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); + + if(Max0 + * + * This file was ported from mpr.c file, part of libccd. + * The Minkoski Portal Refinement implementation was ported + * to OpenCL by Erwin Coumans for the Bullet 3 Physics library. + * at http://github.com/erwincoumans/bullet3 + * + * Distributed under the OSI-approved BSD License (the "License"); + * see . + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + + + + +#ifndef B3_MPR_PENETRATION_H +#define B3_MPR_PENETRATION_H + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" + + + + +#ifdef __cplusplus +#define B3_MPR_SQRT sqrtf +#else +#define B3_MPR_SQRT sqrt +#endif +#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y)) +#define B3_MPR_FABS fabs + +#define B3_MPR_TOLERANCE 1E-6f +#define B3_MPR_MAX_ITERATIONS 1000 + +struct _b3MprSupport_t +{ + b3Float4 v; //!< Support point in minkowski sum + b3Float4 v1; //!< Support point in obj1 + b3Float4 v2; //!< Support point in obj2 +}; +typedef struct _b3MprSupport_t b3MprSupport_t; + +struct _b3MprSimplex_t +{ + b3MprSupport_t ps[4]; + int last; //!< index of last added point +}; +typedef struct _b3MprSimplex_t b3MprSimplex_t; + +inline b3MprSupport_t* b3MprSimplexPointW(b3MprSimplex_t *s, int idx) +{ + return &s->ps[idx]; +} + +inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size) +{ + s->last = size - 1; +} + + +inline int b3MprSimplexSize(const b3MprSimplex_t *s) +{ + return s->last + 1; +} + + +inline const b3MprSupport_t* b3MprSimplexPoint(const b3MprSimplex_t* s, int idx) +{ + // here is no check on boundaries + return &s->ps[idx]; +} + +inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s) +{ + *d = *s; +} + +inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a) +{ + b3MprSupportCopy(s->ps + pos, a); +} + + +inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2) +{ + b3MprSupport_t supp; + + b3MprSupportCopy(&supp, &s->ps[pos1]); + b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]); + b3MprSupportCopy(&s->ps[pos2], &supp); +} + + +inline int b3MprIsZero(float val) +{ + return B3_MPR_FABS(val) < FLT_EPSILON; +} + + + +inline int b3MprEq(float _a, float _b) +{ + float ab; + float a, b; + + ab = B3_MPR_FABS(_a - _b); + if (B3_MPR_FABS(ab) < FLT_EPSILON) + return 1; + + a = B3_MPR_FABS(_a); + b = B3_MPR_FABS(_b); + if (b > a){ + return ab < FLT_EPSILON * b; + }else{ + return ab < FLT_EPSILON * a; + } +} + + +inline int b3MprVec3Eq(const b3Float4* a, const b3Float4 *b) +{ + return b3MprEq((*a).x, (*b).x) + && b3MprEq((*a).y, (*b).y) + && b3MprEq((*a).z, (*b).z); +} + + + +inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global const b3ConvexPolyhedronData_t* hull, b3ConstArray(b3Float4) verticesA) +{ + b3Float4 supVec = b3MakeFloat4(0,0,0,0); + float maxDot = -B3_LARGE_FLOAT; + + if( 0 < hull->m_numVertices ) + { + const b3Float4 scaled = supportVec; + int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot); + return verticesA[hull->m_vertexOffset+index]; + } + + return supVec; + +} + + +static void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + const b3Float4* _dir, b3Float4* outp, int logme) +{ + //dir is in worldspace, move to local space + + b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos; + b3Quat orn = cpuBodyBuf[bodyIndex].m_quat; + + b3Float4 dir = b3MakeFloat4((*_dir).x,(*_dir).y,(*_dir).z,0.f); + + const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),dir); + + + //find local support vertex + int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx; + + b3Assert(cpuCollidables[colIndex].m_shapeType==SHAPE_CONVEX_HULL); + __global const b3ConvexPolyhedronData_t* hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex]; + + b3Float4 pInA; + if (logme) + { + + + b3Float4 supVec = b3MakeFloat4(0,0,0,0); + float maxDot = -B3_LARGE_FLOAT; + + if( 0 < hull->m_numVertices ) + { + const b3Float4 scaled = localDir; + int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot); + pInA = cpuVertices[hull->m_vertexOffset+index]; + + } + + + } else + { + pInA = b3LocalGetSupportVertex(localDir,hull,cpuVertices); + } + + //move vertex to world space + *outp = b3TransformPoint(pInA,pos,orn); + +} + +inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + const b3Float4* _dir, b3MprSupport_t *supp) +{ + b3Float4 dir; + dir = *_dir; + b3MprConvexSupport(pairIndex,bodyIndexA,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v1,0); + dir = *_dir*-1.f; + b3MprConvexSupport(pairIndex,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v2,0); + supp->v = supp->v1 - supp->v2; +} + + + + + + + + + +inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center) +{ + + center->v1 = cpuBodyBuf[bodyIndexA].m_pos; + center->v2 = cpuBodyBuf[bodyIndexB].m_pos; + center->v = center->v1 - center->v2; +} + +inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z) +{ + (*v).x = x; + (*v).y = y; + (*v).z = z; + (*v).w = 0.f; +} + +inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w) +{ + (*v).x += (*w).x; + (*v).y += (*w).y; + (*v).z += (*w).z; +} + +inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w) +{ + *v = *w; +} + +inline void b3MprVec3Scale(b3Float4 *d, float k) +{ + *d *= k; +} + +inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b) +{ + float dot; + + dot = b3Dot3F4(*a,*b); + return dot; +} + + +inline float b3MprVec3Len2(const b3Float4 *v) +{ + return b3MprVec3Dot(v, v); +} + +inline void b3MprVec3Normalize(b3Float4 *d) +{ + float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d)); + b3MprVec3Scale(d, k); +} + +inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b) +{ + *d = b3Cross3(*a,*b); + +} + + +inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w) +{ + *d = *v - *w; +} + +inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir) +{ + b3Float4 v2v1, v3v1; + + b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 1)->v); + b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 1)->v); + b3MprVec3Cross(dir, &v2v1, &v3v1); + b3MprVec3Normalize(dir); +} + + +inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal, + const b3Float4 *dir) +{ + float dot; + dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v); + return b3MprIsZero(dot) || dot > 0.f; +} + +inline int portalReachTolerance(const b3MprSimplex_t *portal, + const b3MprSupport_t *v4, + const b3Float4 *dir) +{ + float dv1, dv2, dv3, dv4; + float dot1, dot2, dot3; + + // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} + + dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir); + dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir); + dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir); + dv4 = b3MprVec3Dot(&v4->v, dir); + + dot1 = dv4 - dv1; + dot2 = dv4 - dv2; + dot3 = dv4 - dv3; + + dot1 = B3_MPR_FMIN(dot1, dot2); + dot1 = B3_MPR_FMIN(dot1, dot3); + + return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE; +} + +inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal, + const b3MprSupport_t *v4, + const b3Float4 *dir) +{ + float dot; + dot = b3MprVec3Dot(&v4->v, dir); + return b3MprIsZero(dot) || dot > 0.f; +} + +inline void b3ExpandPortal(b3MprSimplex_t *portal, + const b3MprSupport_t *v4) +{ + float dot; + b3Float4 v4v0; + + b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v); + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0); + if (dot > 0.f){ + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0); + if (dot > 0.f){ + b3MprSimplexSet(portal, 1, v4); + }else{ + b3MprSimplexSet(portal, 3, v4); + } + }else{ + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0); + if (dot > 0.f){ + b3MprSimplexSet(portal, 2, v4); + }else{ + b3MprSimplexSet(portal, 1, v4); + } + } +} + + + +static int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + __global int* hasSepAxis, + b3MprSimplex_t *portal) +{ + b3Float4 dir, va, vb; + float dot; + int cont; + + + + // vertex 0 is center of portal + b3FindOrigin(bodyIndexA,bodyIndexB,cpuBodyBuf, b3MprSimplexPointW(portal, 0)); + // vertex 0 is center of portal + b3MprSimplexSetSize(portal, 1); + + + + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){ + // Portal's center lies on origin (0,0,0) => we know that objects + // intersect but we would need to know penetration info. + // So move center little bit... + b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f); + b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va); + } + + + // vertex 1 = support in direction of origin + b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Scale(&dir, -1.f); + b3MprVec3Normalize(&dir); + + + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 1)); + + b3MprSimplexSetSize(portal, 2); + + // test if origin isn't outside of v1 + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir); + + + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + + // vertex 2 + b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v, + &b3MprSimplexPoint(portal, 1)->v); + if (b3MprIsZero(b3MprVec3Len2(&dir))){ + if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){ + // origin lies on v1 + return 1; + }else{ + // origin lies on v0-v1 segment + return 2; + } + } + + b3MprVec3Normalize(&dir); + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 2)); + + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir); + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + b3MprSimplexSetSize(portal, 3); + + // vertex 3 direction + b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Cross(&dir, &va, &vb); + b3MprVec3Normalize(&dir); + + // it is better to form portal faces to be oriented "outside" origin + dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v); + if (dot > 0.f){ + b3MprSimplexSwap(portal, 1, 2); + b3MprVec3Scale(&dir, -1.f); + } + + while (b3MprSimplexSize(portal) < 4){ + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 3)); + + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir); + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + cont = 0; + + // test if origin is outside (v1, v0, v3) - set v2 as v3 and + // continue + b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 3)->v); + dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !b3MprIsZero(dot)){ + b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3)); + cont = 1; + } + + if (!cont){ + // test if origin is outside (v3, v0, v2) - set v1 as v3 and + // continue + b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 2)->v); + dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !b3MprIsZero(dot)){ + b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3)); + cont = 1; + } + } + + if (cont){ + b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Cross(&dir, &va, &vb); + b3MprVec3Normalize(&dir); + }else{ + b3MprSimplexSetSize(portal, 4); + } + } + + return 0; +} + + +static int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + b3MprSimplex_t *portal) +{ + b3Float4 dir; + b3MprSupport_t v4; + + for (int i=0;iv, + &b3MprSimplexPoint(portal, 2)->v); + b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 2)->v); + b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v); + + sum = b[0] + b[1] + b[2] + b[3]; + + if (b3MprIsZero(sum) || sum < 0.f){ + b[0] = 0.f; + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 3)->v); + b[1] = b3MprVec3Dot(&vec, &dir); + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[2] = b3MprVec3Dot(&vec, &dir); + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 2)->v); + b[3] = b3MprVec3Dot(&vec, &dir); + + sum = b[1] + b[2] + b[3]; + } + + inv = 1.f / sum; + + b3MprVec3Copy(&p1, b3mpr_vec3_origin); + b3MprVec3Copy(&p2, b3mpr_vec3_origin); + for (i = 0; i < 4; i++){ + b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1); + b3MprVec3Scale(&vec, b[i]); + b3MprVec3Add(&p1, &vec); + + b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2); + b3MprVec3Scale(&vec, b[i]); + b3MprVec3Add(&p2, &vec); + } + b3MprVec3Scale(&p1, inv); + b3MprVec3Scale(&p2, inv); + + b3MprVec3Copy(pos, &p1); + b3MprVec3Add(pos, &p2); + b3MprVec3Scale(pos, 0.5); +} + +inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b) +{ + b3Float4 ab; + b3MprVec3Sub2(&ab, a, b); + return b3MprVec3Len2(&ab); +} + +inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P, + const b3Float4 *x0, + const b3Float4 *b, + b3Float4 *witness) +{ + // The computation comes from solving equation of segment: + // S(t) = x0 + t.d + // where - x0 is initial point of segment + // - d is direction of segment from x0 (|d| > 0) + // - t belongs to <0, 1> interval + // + // Than, distance from a segment to some point P can be expressed: + // D(t) = |x0 + t.d - P|^2 + // which is distance from any point on segment. Minimization + // of this function brings distance from P to segment. + // Minimization of D(t) leads to simple quadratic equation that's + // solving is straightforward. + // + // Bonus of this method is witness point for free. + + float dist, t; + b3Float4 d, a; + + // direction of segment + b3MprVec3Sub2(&d, b, x0); + + // precompute vector from P to x0 + b3MprVec3Sub2(&a, x0, P); + + t = -1.f * b3MprVec3Dot(&a, &d); + t /= b3MprVec3Len2(&d); + + if (t < 0.f || b3MprIsZero(t)){ + dist = b3MprVec3Dist2(x0, P); + if (witness) + b3MprVec3Copy(witness, x0); + }else if (t > 1.f || b3MprEq(t, 1.f)){ + dist = b3MprVec3Dist2(b, P); + if (witness) + b3MprVec3Copy(witness, b); + }else{ + if (witness){ + b3MprVec3Copy(witness, &d); + b3MprVec3Scale(witness, t); + b3MprVec3Add(witness, x0); + dist = b3MprVec3Dist2(witness, P); + }else{ + // recycling variables + b3MprVec3Scale(&d, t); + b3MprVec3Add(&d, &a); + dist = b3MprVec3Len2(&d); + } + } + + return dist; +} + + +inline float b3MprVec3PointTriDist2(const b3Float4 *P, + const b3Float4 *x0, const b3Float4 *B, + const b3Float4 *C, + b3Float4 *witness) +{ + // Computation comes from analytic expression for triangle (x0, B, C) + // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and + // Then equation for distance is: + // D(s, t) = | T(s, t) - P |^2 + // This leads to minimization of quadratic function of two variables. + // The solution from is taken only if s is between 0 and 1, t is + // between 0 and 1 and t + s < 1, otherwise distance from segment is + // computed. + + b3Float4 d1, d2, a; + float u, v, w, p, q, r; + float s, t, dist, dist2; + b3Float4 witness2; + + b3MprVec3Sub2(&d1, B, x0); + b3MprVec3Sub2(&d2, C, x0); + b3MprVec3Sub2(&a, x0, P); + + u = b3MprVec3Dot(&a, &a); + v = b3MprVec3Dot(&d1, &d1); + w = b3MprVec3Dot(&d2, &d2); + p = b3MprVec3Dot(&a, &d1); + q = b3MprVec3Dot(&a, &d2); + r = b3MprVec3Dot(&d1, &d2); + + s = (q * r - w * p) / (w * v - r * r); + t = (-s * r - q) / w; + + if ((b3MprIsZero(s) || s > 0.f) + && (b3MprEq(s, 1.f) || s < 1.f) + && (b3MprIsZero(t) || t > 0.f) + && (b3MprEq(t, 1.f) || t < 1.f) + && (b3MprEq(t + s, 1.f) || t + s < 1.f)){ + + if (witness){ + b3MprVec3Scale(&d1, s); + b3MprVec3Scale(&d2, t); + b3MprVec3Copy(witness, x0); + b3MprVec3Add(witness, &d1); + b3MprVec3Add(witness, &d2); + + dist = b3MprVec3Dist2(witness, P); + }else{ + dist = s * s * v; + dist += t * t * w; + dist += 2.f * s * t * r; + dist += 2.f * s * p; + dist += 2.f * t * q; + dist += u; + } + }else{ + dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness); + + dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + b3MprVec3Copy(witness, &witness2); + } + + dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + b3MprVec3Copy(witness, &witness2); + } + } + + return dist; +} + + +static void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + b3MprSimplex_t *portal, + float *depth, b3Float4 *pdir, b3Float4 *pos) +{ + b3Float4 dir; + b3MprSupport_t v4; + unsigned long iterations; + + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + + iterations = 1UL; + for (int i=0;i find penetration info + if (portalReachTolerance(portal, &v4, &dir) + || iterations ==B3_MPR_MAX_ITERATIONS) + { + *depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin,&b3MprSimplexPoint(portal, 1)->v,&b3MprSimplexPoint(portal, 2)->v,&b3MprSimplexPoint(portal, 3)->v,pdir); + *depth = B3_MPR_SQRT(*depth); + + if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z)) + { + + *pdir = dir; + } + b3MprVec3Normalize(pdir); + + // barycentric coordinates: + b3FindPos(portal, pos); + + + return; + } + + b3ExpandPortal(portal, &v4); + + iterations++; + } +} + +static void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos) +{ + // Touching contact on portal's v1 - so depth is zero and direction + // is unimportant and pos can be guessed + *depth = 0.f; + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + + b3MprVec3Copy(dir, b3mpr_vec3_origin); + + b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1); + b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2); + b3MprVec3Scale(pos, 0.5); +} + +static void b3FindPenetrSegment(b3MprSimplex_t *portal, + float *depth, b3Float4 *dir, b3Float4 *pos) +{ + + // Origin lies on v0-v1 segment. + // Depth is distance to v1, direction also and position must be + // computed + + b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1); + b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2); + b3MprVec3Scale(pos, 0.5f); + + + b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v); + *depth = B3_MPR_SQRT(b3MprVec3Len2(dir)); + b3MprVec3Normalize(dir); +} + + + +inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB, + b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + __global int* hasSepAxis, + float *depthOut, b3Float4* dirOut, b3Float4* posOut) +{ + + b3MprSimplex_t portal; + + + if (!hasSepAxis[pairIndex]) + return -1; + + hasSepAxis[pairIndex] = 0; + int res; + + // Phase 1: Portal discovery + res = b3DiscoverPortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,hasSepAxis, &portal); + + + //sepAxis[pairIndex] = *pdir;//or -dir? + + switch (res) + { + case 0: + { + // Phase 2: Portal refinement + + res = b3RefinePortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal); + if (res < 0) + return -1; + + // Phase 3. Penetration info + b3FindPenetr(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal, depthOut, dirOut, posOut); + hasSepAxis[pairIndex] = 1; + sepAxis[pairIndex] = -*dirOut; + break; + } + case 1: + { + // Touching contact on portal's v1. + b3FindPenetrTouch(&portal, depthOut, dirOut, posOut); + break; + } + case 2: + { + + b3FindPenetrSegment( &portal, depthOut, dirOut, posOut); + break; + } + default: + { + hasSepAxis[pairIndex]=0; + //if (res < 0) + //{ + // Origin isn't inside portal - no collision. + return -1; + //} + } + }; + + return 0; +}; + + + +#endif //B3_MPR_PENETRATION_H diff --git a/src/Bullet3Common/b3Vector3.h b/src/Bullet3Common/b3Vector3.h index 521935962..ed45ffaa9 100644 --- a/src/Bullet3Common/b3Vector3.h +++ b/src/Bullet3Common/b3Vector3.h @@ -990,7 +990,12 @@ B3_FORCE_INLINE long b3Vector3::maxDot( const b3Vector3 *array, long array_ ptIndex = i; } } - + + b3Assert(ptIndex>=0); + if (ptIndex<0) + { + ptIndex = 0; + } dotOut = maxDot; return ptIndex; } diff --git a/src/Bullet3Common/shared/b3Float4.h b/src/Bullet3Common/shared/b3Float4.h index fd92b641c..5e4b95bce 100644 --- a/src/Bullet3Common/shared/b3Float4.h +++ b/src/Bullet3Common/shared/b3Float4.h @@ -68,5 +68,30 @@ inline bool b3IsAlmostZero(b3Float4ConstArg v) } +inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut ) +{ + float maxDot = -B3_INFINITY; + int i = 0; + int ptIndex = -1; + for( i = 0; i < vecLen; i++ ) + { + float dot = b3Dot3F4(vecArray[i],vec); + + if( dot > maxDot ) + { + maxDot = dot; + ptIndex = i; + } + } + b3Assert(ptIndex>=0); + if (ptIndex<0) + { + ptIndex = 0; + } + *dotOut = maxDot; + return ptIndex; +} + + #endif //B3_FLOAT4_H diff --git a/src/Bullet3Common/shared/b3PlatformDefinitions.h b/src/Bullet3Common/shared/b3PlatformDefinitions.h index 01243ed1f..a3287ae08 100644 --- a/src/Bullet3Common/shared/b3PlatformDefinitions.h +++ b/src/Bullet3Common/shared/b3PlatformDefinitions.h @@ -7,6 +7,8 @@ struct MyTest }; #ifdef __cplusplus +//#define b3ConstArray(a) const b3AlignedObjectArray& +#define b3ConstArray(a) const a* #define b3AtomicInc(a) ((*a)++) inline int b3AtomicAdd (volatile int *p, int val) @@ -19,6 +21,11 @@ inline int b3AtomicAdd (volatile int *p, int val) #define __global #else +//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX +#define B3_LARGE_FLOAT 1e18f +#define B3_INFINITY 1e18f +#define b3Assert(a) +#define b3ConstArray(a) __global const a* #define b3AtomicInc atomic_inc #define b3AtomicAdd atomic_add #define b3Fabs fabs diff --git a/src/Bullet3Common/shared/b3Quat.h b/src/Bullet3Common/shared/b3Quat.h index 81b688108..f262d5e08 100644 --- a/src/Bullet3Common/shared/b3Quat.h +++ b/src/Bullet3Common/shared/b3Quat.h @@ -38,6 +38,8 @@ inline b3Quat b3QuatMul(b3Quat a, b3Quat b); inline b3Quat b3QuatNormalized(b3QuatConstArg in); inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec); inline b3Quat b3QuatInvert(b3QuatConstArg q); +inline b3Quat b3QuatInverse(b3QuatConstArg q); + inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b) { b3Quat ans; @@ -74,6 +76,13 @@ inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec) return out; } + + +inline b3Quat b3QuatInverse(b3QuatConstArg q) +{ + return (b3Quat)(-q.xyz, q.w); +} + inline b3Quat b3QuatInvert(b3QuatConstArg q) { return (b3Quat)(-q.xyz, q.w); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp index bc7aa3976..5e11e7493 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp +++ b/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp @@ -1,6 +1,6 @@ #include "b3FixedConstraint.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include "Bullet3Common/b3TransformUtil.h" #include @@ -19,13 +19,13 @@ b3FixedConstraint::~b3FixedConstraint () } -void b3FixedConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies) +void b3FixedConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) { info->m_numConstraintRows = 6; info->nub = 6; } -void b3FixedConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies) +void b3FixedConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies) { //fix the 3 linear degrees of freedom diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h index c811f095e..e884a8291 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h @@ -16,9 +16,9 @@ public: virtual ~b3FixedConstraint(); - virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies); + virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); - virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies); + virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies); virtual void setParam(int num, b3Scalar value, int axis = -1) { diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp index 7295e9b81..b2398f450 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp @@ -20,7 +20,7 @@ http://gimpact.sf.net */ #include "b3Generic6DofConstraint.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include "Bullet3Common/b3TransformUtil.h" #include "Bullet3Common/b3TransformUtil.h" @@ -36,7 +36,7 @@ http://gimpact.sf.net -b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyCL* bodies) +b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies) : b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB) , m_frameInA(frameInA) , m_frameInB(frameInB), @@ -214,13 +214,13 @@ void b3Generic6DofConstraint::calculateAngleInfo() } -static b3Transform getCenterOfMassTransform(const b3RigidBodyCL& body) +static b3Transform getCenterOfMassTransform(const b3RigidBodyData& body) { b3Transform tr(body.m_quat,body.m_pos); return tr; } -void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyCL* bodies) +void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies) { b3Transform transA; b3Transform transB; @@ -229,7 +229,7 @@ void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyCL* bodies) calculateTransforms(transA,transB,bodies); } -void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyCL* bodies) +void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies) { m_calculatedTransformA = transA * m_frameInA; m_calculatedTransformB = transB * m_frameInB; @@ -272,7 +272,7 @@ bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index) -void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies) +void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) { //prepare constraint calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies); @@ -300,7 +300,7 @@ void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBod // printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows); } -void b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies) +void b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) { //pre-allocate all 6 info->m_numConstraintRows = 6; @@ -308,7 +308,7 @@ void b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const } -void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyCL* bodies) +void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies) { b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]); @@ -332,7 +332,7 @@ void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBod } -void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyCL* bodies) +void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies) { //prepare constraint @@ -447,7 +447,7 @@ void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep) } -void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB,const b3RigidBodyCL* bodies) +void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB,const b3RigidBodyData* bodies) { m_frameInA = frameA; m_frameInB = frameB; @@ -476,7 +476,7 @@ b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const -void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyCL* bodies) +void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies) { b3Scalar imA = bodies[m_rbA].m_invMass; b3Scalar imB = bodies[m_rbB].m_invMass; @@ -787,7 +787,7 @@ b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const -void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1,const b3Vector3& axis2, const b3RigidBodyCL* bodies) +void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1,const b3Vector3& axis2, const b3RigidBodyData* bodies) { b3Vector3 zAxis = axis1.normalized(); b3Vector3 yAxis = axis2.normalized(); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h index 02bb1a82d..084d36055 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h @@ -31,7 +31,7 @@ http://gimpact.sf.net #include "b3JacobianEntry.h" #include "b3TypedConstraint.h" -struct b3RigidBodyCL; +struct b3RigidBodyData; @@ -123,7 +123,7 @@ public: int testLimitValue(b3Scalar test_value); //! apply the correction impulses for two bodies - b3Scalar solveAngularLimits(b3Scalar timeStep,b3Vector3& axis, b3Scalar jacDiagABInv,b3RigidBodyCL * body0, b3RigidBodyCL * body1); + b3Scalar solveAngularLimits(b3Scalar timeStep,b3Vector3& axis, b3Scalar jacDiagABInv,b3RigidBodyData * body0, b3RigidBodyData * body1); }; @@ -214,8 +214,8 @@ public: b3Scalar solveLinearAxis( b3Scalar timeStep, b3Scalar jacDiagABInv, - b3RigidBodyCL& body1,const b3Vector3 &pointInA, - b3RigidBodyCL& body2,const b3Vector3 &pointInB, + b3RigidBodyData& body1,const b3Vector3 &pointInA, + b3RigidBodyData& body2,const b3Vector3 &pointInB, int limit_index, const b3Vector3 & axis_normal_on_a, const b3Vector3 & anchorPos); @@ -343,16 +343,16 @@ public: B3_DECLARE_ALIGNED_ALLOCATOR(); - b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB ,bool useLinearReferenceFrameA,const b3RigidBodyCL* bodies); + b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB ,bool useLinearReferenceFrameA,const b3RigidBodyData* bodies); //! Calcs global transform of the offsets /*! Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. \sa b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo */ - void calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyCL* bodies); + void calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies); - void calculateTransforms(const b3RigidBodyCL* bodies); + void calculateTransforms(const b3RigidBodyData* bodies); //! Gets the global transform of the offset for body A /*! @@ -395,13 +395,13 @@ public: - virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies); + virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); - void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies); + void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); - virtual void getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyCL* bodies); + virtual void getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies); - void getInfo2NonVirtual (b3ConstraintInfo2* info,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyCL* bodies); + void getInfo2NonVirtual (b3ConstraintInfo2* info,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies); void updateRHS(b3Scalar timeStep); @@ -421,7 +421,7 @@ public: */ b3Scalar getRelativePivotPosition(int axis_index) const; - void setFrames(const b3Transform & frameA, const b3Transform & frameB, const b3RigidBodyCL* bodies); + void setFrames(const b3Transform & frameA, const b3Transform & frameB, const b3RigidBodyData* bodies); //! Test angular limit. /*! @@ -520,7 +520,7 @@ public: return m_angularLimits[limitIndex-3].isLimited(); } - virtual void calcAnchorPos(const b3RigidBodyCL* bodies); // overridable + virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable int get_limit_motor_info2( b3RotationalLimitMotor * limot, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB, @@ -536,7 +536,7 @@ public: ///return the local value of parameter virtual b3Scalar getParam(int num, int axis = -1) const; - void setAxis( const b3Vector3& axis1, const b3Vector3& axis2,const b3RigidBodyCL* bodies); + void setAxis( const b3Vector3& axis1, const b3Vector3& axis2,const b3RigidBodyData* bodies); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp index d568158b2..b5fddef61 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp +++ b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp @@ -34,9 +34,9 @@ subject to the following restrictions: #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" -static b3Transform getWorldTransform(b3RigidBodyCL* rb) +static b3Transform getWorldTransform(b3RigidBodyData* rb) { b3Transform newTrans; newTrans.setOrigin(rb->m_pos); @@ -44,24 +44,24 @@ static b3Transform getWorldTransform(b3RigidBodyCL* rb) return newTrans; } -static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaCL* inertia) +static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaData* inertia) { return inertia->m_invInertiaWorld; } -static const b3Vector3& getLinearVelocity(b3RigidBodyCL* rb) +static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb) { return rb->m_linVel; } -static const b3Vector3& getAngularVelocity(b3RigidBodyCL* rb) +static const b3Vector3& getAngularVelocity(b3RigidBodyData* rb) { return rb->m_angVel; } -static b3Vector3 getVelocityInLocalPoint(b3RigidBodyCL* rb, const b3Vector3& rel_pos) +static b3Vector3 getVelocityInLocalPoint(b3RigidBodyData* rb, const b3Vector3& rel_pos) { //we also calculate lin/ang velocity for kinematic objects return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos); @@ -152,7 +152,7 @@ b3PgsJacobiSolver::~b3PgsJacobiSolver() { } -void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints) +void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints) { b3ContactSolverInfo infoGlobal; infoGlobal.m_splitImpulse = false; @@ -176,8 +176,8 @@ void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, b3In /// b3PgsJacobiSolver Sequentially applies impulses -b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyCL* bodies, - b3InertiaCL* inertias, +b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyData* bodies, + b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, @@ -439,7 +439,7 @@ int b3PgsJacobiSolver::b3RandInt2 (int n) -void b3PgsJacobiSolver::initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyCL* rb) +void b3PgsJacobiSolver::initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* rb) { solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); @@ -450,7 +450,7 @@ void b3PgsJacobiSolver::initSolverBody(int bodyIndex, b3SolverBody* solverBody, if (rb) { solverBody->m_worldTransform = getWorldTransform(rb); - solverBody->internalSetInvMass(b3MakeVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())); + solverBody->internalSetInvMass(b3MakeVector3(rb->m_invMass,rb->m_invMass,rb->m_invMass)); solverBody->m_originalBodyIndex = bodyIndex; solverBody->m_angularFactor = b3MakeVector3(1,1,1); solverBody->m_linearFactor = b3MakeVector3(1,1,1); @@ -486,7 +486,7 @@ b3Scalar b3PgsJacobiSolver::restitutionCurve(b3Scalar rel_vel, b3Scalar restitut -void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) +void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) { @@ -494,8 +494,8 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaC b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; - b3RigidBodyCL* body0 = &bodies[solverBodyA.m_originalBodyIndex]; - b3RigidBodyCL* body1 = &bodies[solverBodyB.m_originalBodyIndex]; + b3RigidBodyData* body0 = &bodies[solverBodyA.m_originalBodyIndex]; + b3RigidBodyData* body1 = &bodies[solverBodyB.m_originalBodyIndex]; solverConstraint.m_solverBodyIdA = solverBodyIdA; @@ -527,12 +527,12 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaC if (body0) { vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = body0->getInvMass() + normalAxis.dot(vec); + denom0 = body0->m_invMass + normalAxis.dot(vec); } if (body1) { vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = body1->getInvMass() + normalAxis.dot(vec); + denom1 = body1->m_invMass + normalAxis.dot(vec); } b3Scalar denom; @@ -542,8 +542,8 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaC } else { denom = relaxation/(denom0+denom1); - b3Scalar countA = body0->getInvMass() ? b3Scalar(m_bodyCount[solverBodyA.m_originalBodyIndex]): 1.f; - b3Scalar countB = body1->getInvMass() ? b3Scalar(m_bodyCount[solverBodyB.m_originalBodyIndex]): 1.f; + b3Scalar countA = body0->m_invMass ? b3Scalar(m_bodyCount[solverBodyA.m_originalBodyIndex]): 1.f; + b3Scalar countB = body1->m_invMass ? b3Scalar(m_bodyCount[solverBodyB.m_originalBodyIndex]): 1.f; scaledDenom = relaxation/(denom0*countA+denom1*countB); } @@ -574,7 +574,7 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaC } } -b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) +b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) { b3SolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; @@ -584,9 +584,9 @@ b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyCL* bodi } -void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, +void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, - b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, + b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) { @@ -597,8 +597,8 @@ void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyCL* bodies,b3I b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; - b3RigidBodyCL* body0 = &bodies[m_tmpSolverBodyPool[solverBodyIdA].m_originalBodyIndex]; - b3RigidBodyCL* body1 = &bodies[m_tmpSolverBodyPool[solverBodyIdB].m_originalBodyIndex]; + b3RigidBodyData* body0 = &bodies[m_tmpSolverBodyPool[solverBodyIdA].m_originalBodyIndex]; + b3RigidBodyData* body1 = &bodies[m_tmpSolverBodyPool[solverBodyIdB].m_originalBodyIndex]; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -660,7 +660,7 @@ void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyCL* bodies,b3I -b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) +b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) { b3SolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; @@ -670,13 +670,13 @@ b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyC } -int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,b3InertiaCL* inertias) +int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias) { //b3Assert(bodyIndex< m_tmpSolverBodyPool.size()); - b3RigidBodyCL& body = bodies[bodyIndex]; + b3RigidBodyData& body = bodies[bodyIndex]; int curIndex = -1; - if (m_usePgs || body.getInvMass()==0.f) + if (m_usePgs || body.m_invMass==0.f) { if (m_bodyCount[bodyIndex]<0) { @@ -706,7 +706,7 @@ int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies, #include -void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3SolverConstraint& solverConstraint, +void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, @@ -719,8 +719,8 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyCL* bodies, b3InertiaC b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; - b3RigidBodyCL* rb0 = &bodies[bodyA->m_originalBodyIndex]; - b3RigidBodyCL* rb1 = &bodies[bodyB->m_originalBodyIndex]; + b3RigidBodyData* rb0 = &bodies[bodyA->m_originalBodyIndex]; + b3RigidBodyData* rb1 = &bodies[bodyB->m_originalBodyIndex]; // b3Vector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); // b3Vector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); @@ -746,12 +746,12 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyCL* bodies, b3InertiaC if (rb0) { vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); + denom0 = rb0->m_invMass + cp.m_normalWorldOnB.dot(vec); } if (rb1) { vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); + denom1 = rb1->m_invMass + cp.m_normalWorldOnB.dot(vec); } #endif //COMPUTE_IMPULSE_DENOM @@ -870,7 +870,7 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyCL* bodies, b3InertiaC -void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3SolverConstraint& solverConstraint, +void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal) { @@ -914,9 +914,9 @@ void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyCL* bodies, b3I -void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal) +void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal) { - b3RigidBodyCL* colObj0=0,*colObj1=0; + b3RigidBodyData* colObj0=0,*colObj1=0; int solverBodyIdA = getOrInitSolverBody(manifold->getBodyA(),bodies,inertias); @@ -1062,7 +1062,7 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, b3InertiaCL* inert } } -b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) { B3_PROFILE("solveGroupCacheFriendlySetup"); @@ -1111,7 +1111,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, m_bodyCount[bodyIndexB]=-1; } else { - if (bodies[bodyIndexA].getInvMass()) + if (bodies[bodyIndexA].m_invMass) { //m_bodyCount[bodyIndexA]+=manifoldPtr[i].getNPoints(); m_bodyCount[bodyIndexA]++; @@ -1119,7 +1119,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, else m_bodyCount[bodyIndexA]=-1; - if (bodies[bodyIndexB].getInvMass()) + if (bodies[bodyIndexB].m_invMass) // m_bodyCount[bodyIndexB]+=manifoldPtr[i].getNPoints(); m_bodyCount[bodyIndexB]++; else @@ -1194,10 +1194,10 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, b3SolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; b3TypedConstraint* constraint = constraints[i]; - b3RigidBodyCL& rbA = bodies[ constraint->getRigidBodyA()]; + b3RigidBodyData& rbA = bodies[ constraint->getRigidBodyA()]; //b3RigidBody& rbA = constraint->getRigidBodyA(); // b3RigidBody& rbB = constraint->getRigidBodyB(); - b3RigidBodyCL& rbB = bodies[ constraint->getRigidBodyB()]; + b3RigidBodyData& rbB = bodies[ constraint->getRigidBodyB()]; int solverBodyIdA = getOrInitSolverBody(constraint->getRigidBodyA(),bodies,inertias); int solverBodyIdB = getOrInitSolverBody(constraint->getRigidBodyB(),bodies,inertias); @@ -1290,9 +1290,9 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, { //it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal //because it gets multiplied iMJlB - b3Vector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass(); + b3Vector3 iMJlA = solverConstraint.m_contactNormal*rbA.m_invMass; b3Vector3 iMJaA = invInertiaWorldA*solverConstraint.m_relpos1CrossNormal; - b3Vector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal? + b3Vector3 iMJlB = solverConstraint.m_contactNormal*rbB.m_invMass;//sign of normal? b3Vector3 iMJaB = invInertiaWorldB*solverConstraint.m_relpos2CrossNormal; b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal); @@ -1701,7 +1701,7 @@ void b3PgsJacobiSolver::averageVelocities() } } -b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,b3InertiaCL* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal) +b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal) { B3_PROFILE("solveGroupCacheFriendlyFinish"); int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); @@ -1759,8 +1759,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies, int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; //b3Assert(i==bodyIndex); - b3RigidBodyCL* body = &bodies[bodyIndex]; - if (body->getInvMass()) + b3RigidBodyData* body = &bodies[bodyIndex]; + if (body->m_invMass) { if (infoGlobal.m_splitImpulse) m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h index 337413856..d2ca307fa 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h @@ -13,8 +13,8 @@ class b3Dispatcher; #include "b3SolverBody.h" #include "b3SolverConstraint.h" -struct b3RigidBodyCL; -struct b3InertiaCL; +struct b3RigidBodyData; +struct b3InertiaData; class b3PgsJacobiSolver { @@ -48,26 +48,26 @@ protected: { return 0.02f; } - void setupFrictionConstraint( b3RigidBodyCL* bodies,b3InertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB, + void setupFrictionConstraint( b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB, b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, - b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, + b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.); - void setupRollingFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB, + void setupRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB, b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, - b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, + b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.); - b3SolverConstraint& addFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.); - b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f); + b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.); + b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f); - void setupContactConstraint(b3RigidBodyCL* bodies, b3InertiaCL* inertias, + void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, b3Vector3& rel_pos1, b3Vector3& rel_pos2); - void setFrictionConstraintImpulse( b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, + void setFrictionConstraintImpulse( b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal); ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction @@ -76,7 +76,7 @@ protected: b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution); - void convertContact(b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal); + void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal); void resolveSplitPenetrationSIMD( @@ -88,8 +88,8 @@ protected: const b3SolverConstraint& contactConstraint); //internal method - int getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,b3InertiaCL* inertias); - void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyCL* collisionObject); + int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias); + void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject); void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint); @@ -101,7 +101,7 @@ protected: protected: - virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, b3InertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); @@ -109,7 +109,7 @@ protected: b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); - virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies, b3InertiaCL* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal); + virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal); public: @@ -119,10 +119,10 @@ public: b3PgsJacobiSolver(bool usePgs); virtual ~b3PgsJacobiSolver(); -// void solveContacts(int numBodies, b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numContacts, b3Contact4* contacts); - void solveContacts(int numBodies, b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints); +// void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts); + void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints); - b3Scalar solveGroup(b3RigidBodyCL* bodies,b3InertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + b3Scalar solveGroup(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); ///clear internal cached data and reset random seed virtual void reset(); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp index 6a8112815..637c5b5a2 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp @@ -15,7 +15,7 @@ subject to the following restrictions: #include "b3Point2PointConstraint.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include @@ -41,12 +41,12 @@ m_useSolveConstraintObsolete(false) */ -void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies) +void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) { getInfo1NonVirtual(info,bodies); } -void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies) +void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) { info->m_numConstraintRows = 3; info->nub = 3; @@ -55,7 +55,7 @@ void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const -void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies) +void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies) { b3Transform trA; trA.setIdentity(); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h index 5499a409e..681b48733 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h @@ -76,11 +76,11 @@ public: - virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies); + virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); - void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies); + void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); - virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies); + virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies); void getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h index 8b3147e6e..cf9cec0d5 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h @@ -62,7 +62,7 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3JointFeedback }; -struct b3RigidBodyCL; +struct b3RigidBodyData; ///TypedConstraint is the baseclass for Bullet constraints and vehicles @@ -170,10 +170,10 @@ public: } ///internal method used by the constraint solver, don't use them directly - virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)=0; + virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)=0; ///internal method used by the constraint solver, don't use them directly - virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies)=0; + virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)=0; ///internal method used by the constraint solver, don't use them directly void internalSetAppliedImpulse(b3Scalar appliedImpulse) diff --git a/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp b/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp index 8ddaa8e82..e9619e776 100644 --- a/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp +++ b/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp @@ -597,7 +597,7 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev if (disableBinaryCaching) { - kernelSourceOrg = 0; + //kernelSourceOrg = 0; } cl_program m_cpProgram=0; diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp index 55061af57..733a53ced 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp @@ -16,6 +16,7 @@ subject to the following restrictions: bool findSeparatingAxisOnGpu = true; bool splitSearchSepAxisConcave = false; bool splitSearchSepAxisConvex = true; +bool useMprGpu = false;//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; @@ -23,6 +24,11 @@ bool clipConvexFacesAndFindContactsCPU = false;//false;//true; bool reduceConcaveContactsOnGPU = true;//false; bool reduceConvexContactsOnGPU = true;//false; bool findConvexClippingFacesGPU = true; +bool useGjk = true;///option for CPU/host testing, when findSeparatingAxisOnGpu = false +bool useGjkContacts = true;//////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 @@ -40,7 +46,9 @@ int b3g_actualSATPairTests=0; #include "b3ConvexHullContact.h" #include //memcpy -#include "b3ConvexPolyhedronCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h" + #include "Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h" #include "Bullet3Geometry/b3AabbUtil.h" @@ -53,6 +61,8 @@ typedef b3AlignedObjectArray b3VertexArray; //#include "AdlQuaternion.h" #include "kernels/satKernels.h" +#include "kernels/mprKernels.h" + #include "kernels/satConcaveKernels.h" #include "kernels/satClipHullContacts.h" @@ -65,6 +75,7 @@ typedef b3AlignedObjectArray b3VertexArray; #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" @@ -115,12 +126,25 @@ m_dmins(m_context,m_queue) if (1) { + const char* mprSrc = mprKernelsCL; const char* src = satKernelsCL; 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; + + 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); + } + cl_program satProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,src,&errNum,flags,BT_NARROWPHASE_SAT_PATH); b3Assert(errNum==CL_SUCCESS); @@ -251,6 +275,9 @@ GpuSatCollision::~GpuSatCollision() clReleaseKernel(m_findSeparatingAxisEdgeEdgeKernel); + if (m_mprPenetrationKernel) + clReleaseKernel(m_mprPenetrationKernel); + if (m_findSeparatingAxisKernel) clReleaseKernel(m_findSeparatingAxisKernel); @@ -1158,12 +1185,12 @@ int clipHullHullSingle( int collidableIndexA, int collidableIndexB, - const b3AlignedObjectArray* bodyBuf, + const b3AlignedObjectArray* bodyBuf, b3AlignedObjectArray* globalContactOut, int& nContacts, - const b3AlignedObjectArray& hostConvexDataA, - const b3AlignedObjectArray& hostConvexDataB, + const b3AlignedObjectArray& hostConvexDataA, + const b3AlignedObjectArray& hostConvexDataB, const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& uniqueEdgesA, @@ -1181,7 +1208,7 @@ int clipHullHullSingle( int maxContactCapacity ) { int contactIndex = -1; - b3ConvexPolyhedronCL hullA, hullB; + b3ConvexPolyhedronData hullA, hullB; b3Collidable colA = hostCollidablesA[collidableIndexA]; hullA = hostConvexDataA[colA.m_shapeIndex]; @@ -1303,9 +1330,9 @@ int clipHullHullSingle( void computeContactPlaneConvex(int pairIndex, int bodyIndexA, int bodyIndexB, int collidableIndexA, int collidableIndexB, - const b3RigidBodyCL* rigidBodies, + const b3RigidBodyData* rigidBodies, const b3Collidable* collidables, - const b3ConvexPolyhedronCL* convexShapes, + const b3ConvexPolyhedronData* convexShapes, const b3Vector3* convexVertices, const int* convexIndices, const b3GpuFace* faces, @@ -1315,7 +1342,7 @@ void computeContactPlaneConvex(int pairIndex, { int shapeIndex = collidables[collidableIndexB].m_shapeIndex; - const b3ConvexPolyhedronCL* hullB = &convexShapes[shapeIndex]; + const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndex]; b3Vector3 posB = rigidBodies[bodyIndexB].m_pos; b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat; @@ -2085,7 +2112,7 @@ __kernel void clipCompoundsHullHullKernel( __global const b3Int4* gpuCompoundP void computeContactCompoundCompound(int pairIndex, int bodyIndexA, int bodyIndexB, int collidableIndexA, int collidableIndexB, - const b3RigidBodyCL* rigidBodies, + const b3RigidBodyData* rigidBodies, const b3Collidable* collidables, const b3ConvexPolyhedronData* convexShapes, const b3GpuChildShape* cpuChildShapes, @@ -2238,7 +2265,7 @@ void computeContactCompoundCompound(int pairIndex, int shapeIndexB = collidables[childColIndexB].m_shapeIndex; - const b3ConvexPolyhedronCL* hullB = &convexShapes[shapeIndexB]; + const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndexB]; } */ @@ -2248,9 +2275,9 @@ void computeContactCompoundCompound(int pairIndex, void computeContactPlaneCompound(int pairIndex, int bodyIndexA, int bodyIndexB, int collidableIndexA, int collidableIndexB, - const b3RigidBodyCL* rigidBodies, + const b3RigidBodyData* rigidBodies, const b3Collidable* collidables, - const b3ConvexPolyhedronCL* convexShapes, + const b3ConvexPolyhedronData* convexShapes, const b3GpuChildShape* cpuChildShapes, const b3Vector3* convexVertices, const int* convexIndices, @@ -2280,7 +2307,7 @@ void computeContactPlaneCompound(int pairIndex, int shapeIndexB = collidables[childColIndexB].m_shapeIndex; - const b3ConvexPolyhedronCL* hullB = &convexShapes[shapeIndexB]; + const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndexB]; b3Vector3 posA = rigidBodies[bodyIndexA].m_pos; @@ -2401,9 +2428,9 @@ void computeContactPlaneCompound(int pairIndex, void computeContactSphereConvex(int pairIndex, int bodyIndexA, int bodyIndexB, int collidableIndexA, int collidableIndexB, - const b3RigidBodyCL* rigidBodies, + const b3RigidBodyData* rigidBodies, const b3Collidable* collidables, - const b3ConvexPolyhedronCL* convexShapes, + const b3ConvexPolyhedronData* convexShapes, const b3Vector3* convexVertices, const int* convexIndices, const b3GpuFace* faces, @@ -2562,9 +2589,9 @@ int computeContactConvexConvex( b3AlignedObjectArray& pairs, int pairIndex, int bodyIndexA, int bodyIndexB, int collidableIndexA, int collidableIndexB, - const b3AlignedObjectArray& rigidBodies, + const b3AlignedObjectArray& rigidBodies, const b3AlignedObjectArray& collidables, - const b3AlignedObjectArray& convexShapes, + const b3AlignedObjectArray& convexShapes, const b3AlignedObjectArray& convexVertices, const b3AlignedObjectArray& uniqueEdges, const b3AlignedObjectArray& convexIndices, @@ -2572,7 +2599,11 @@ int computeContactConvexConvex( b3AlignedObjectArray& pairs, b3AlignedObjectArray& globalContactsOut, int& nGlobalContactsOut, int maxContactCapacity, - const b3AlignedObjectArray& oldContacts + b3AlignedObjectArray&hostHasSepAxis, + b3AlignedObjectArray&hostSepAxis + + + //,const b3AlignedObjectArray& oldContacts ) { int contactIndex = -1; @@ -2629,10 +2660,10 @@ int computeContactConvexConvex( b3AlignedObjectArray& pairs, //printf("add existing points?\n"); //refresh - int numOldPoints = oldContacts[pairs[pairIndex].z].getNPoints(); + int numOldPoints = 0;//oldContacts[pairs[pairIndex].z].getNPoints(); if (numOldPoints) { - newContact = oldContacts[pairs[pairIndex].z]; + //newContact = oldContacts[pairs[pairIndex].z]; #ifdef PERSISTENT_CONTACTS_HOST b3ContactCache::refreshContactPoints(transA,transB,newContact); #endif //PERSISTENT_CONTACTS_HOST @@ -2670,7 +2701,12 @@ int computeContactConvexConvex( b3AlignedObjectArray& pairs, newContact.m_localPosA[p] = transA.inverse()*resultPointOnAWorld; newContact.m_localPosB[p] = transB.inverse()*resultPointOnBWorld; #endif - newContact.m_worldNormalOnB = sepAxis2; + newContact.m_worldNormalOnB = sepAxis2; + + + hostHasSepAxis[pairIndex] = 1; + hostSepAxis[pairIndex] =sepAxis2; + //printf("sepAxis[%d]=%f,%f,%f,%f\n",pairIndex,sepAxis2.x,sepAxis2.y,sepAxis2.z,sepAxis2.w); } //printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints); newContact.m_worldNormalOnB.w = (b3Scalar)numPoints; @@ -2690,9 +2726,9 @@ int computeContactConvexConvex2( int pairIndex, int bodyIndexA, int bodyIndexB, int collidableIndexA, int collidableIndexB, - const b3AlignedObjectArray& rigidBodies, + const b3AlignedObjectArray& rigidBodies, const b3AlignedObjectArray& collidables, - const b3AlignedObjectArray& convexShapes, + const b3AlignedObjectArray& convexShapes, const b3AlignedObjectArray& convexVertices, const b3AlignedObjectArray& uniqueEdges, const b3AlignedObjectArray& convexIndices, @@ -2710,7 +2746,7 @@ int computeContactConvexConvex2( b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat; - b3ConvexPolyhedronCL hullA, hullB; + b3ConvexPolyhedronData hullA, hullB; b3Vector3 sepNormalWorldSpace; @@ -2787,15 +2823,17 @@ int computeContactConvexConvex2( + + void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* pairs, int nPairs, - const b3OpenCLArray* bodyBuf, + const b3OpenCLArray* bodyBuf, b3OpenCLArray* contactOut, int& nContacts, const b3OpenCLArray* oldContacts, int maxContactCapacity, int compoundPairCapacity, - const b3OpenCLArray& convexData, + const b3OpenCLArray& convexData, const b3OpenCLArray& gpuVertices, const b3OpenCLArray& gpuUniqueEdges, const b3OpenCLArray& gpuFaces, @@ -2822,6 +2860,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* int& numTriConvexPairsOut ) { + myframecount++; + if (!nPairs) return; @@ -2846,12 +2886,12 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* b3AlignedObjectArray hostPairs; pairs->copyToHost(hostPairs); - b3AlignedObjectArray hostBodyBuf; + b3AlignedObjectArray hostBodyBuf; bodyBuf->copyToHost(hostBodyBuf); - b3AlignedObjectArray hostConvexData; + b3AlignedObjectArray hostConvexData; convexData.copyToHost(hostConvexData); b3AlignedObjectArray hostVertices; @@ -2962,8 +3002,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* 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); + //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) @@ -3068,56 +3108,109 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* m_dmins.resize(nPairs); if (splitSearchSepAxisConvex) { + { - 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()) - }; + 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 ); + 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 num = nPairs; + launcher.launch1D( num); + clFinish(m_queue); } + + + + if (useMprGpu) { - 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()) - }; + 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_findSeparatingAxisEdgeEdgeKernel,"findSeparatingAxisEdgeEdgeKernel"); - launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); - launcher.setConst( nPairs ); + 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); + /* + b3AlignedObjectArrayhostHasSepAxis; + m_hasSeparatingNormals.copyToHost(hostHasSepAxis); + b3AlignedObjectArrayhostSepAxis; + m_sepNormals.copyToHost(hostSepAxis); + */ + nContacts = m_totalContactsOut.at(0); + contactOut->resize(nContacts); + //printf("nContacts (after processCompoundPairsPrimitivesKernel) = %d\n",nContacts); + if (nContacts>maxContactCapacity) + { + + b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity); + nContacts = maxContactCapacity; + } + + } + } else + { + + { + 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()) + }; + + b3LauncherCL launcher(m_queue, m_findSeparatingAxisEdgeEdgeKernel,"findSeparatingAxisEdgeEdgeKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( nPairs ); + + int num = nPairs; + launcher.launch1D( num); + clFinish(m_queue); - int num = nPairs; - launcher.launch1D( num); - clFinish(m_queue); } + } + } else { @@ -3150,11 +3243,12 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* else { + B3_PROFILE("findSeparatingAxisKernel CPU"); b3AlignedObjectArray hostPairs; pairs->copyToHost(hostPairs); - b3AlignedObjectArray hostBodyBuf; + b3AlignedObjectArray hostBodyBuf; bodyBuf->copyToHost(hostBodyBuf); b3AlignedObjectArray hostCollidables; @@ -3163,7 +3257,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* b3AlignedObjectArray cpuChildShapes; gpuChildShapes.copyToHost(cpuChildShapes); - b3AlignedObjectArray hostConvexShapeData; + b3AlignedObjectArray hostConvexShapeData; convexData.copyToHost(hostConvexShapeData); b3AlignedObjectArray hostVertices; @@ -3181,6 +3275,15 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* b3AlignedObjectArray hostIndices; gpuIndices.copyToHost(hostIndices); + + b3AlignedObjectArray hostContacts; + if (nContacts) + { + contactOut->copyToHost(hostContacts); + } + hostContacts.resize(maxContactCapacity); + int nGlobalContactsOut = nContacts; + for (int i=0;i* 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 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; - 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); - 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 result = computeContactConvexConvex( hostPairs, + pairIndex, + bodyIndexA, bodyIndexB, + collidableIndexA, collidableIndexB, + hostBodyBuf, + hostCollidables, + hostConvexShapeData, + hostVertices, + hostUniqueEdges, + hostIndices, + hostFaces, + hostContacts, + nGlobalContactsOut, + maxContactCapacity, + 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; - 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); - if (hasEdgeEdge) - { - hostHasSepAxis[i] = 1; - hostSepAxis[i] = sepAxis; - } - } - } + 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); @@ -3337,7 +3636,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* b3AlignedObjectArray hostPairs; pairs->copyToHost(hostPairs); - b3AlignedObjectArray hostBodyBuf; + b3AlignedObjectArray hostBodyBuf; bodyBuf->copyToHost(hostBodyBuf); @@ -3350,7 +3649,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* b3AlignedObjectArray cpuChildShapes; gpuChildShapes.copyToHost(cpuChildShapes); - b3AlignedObjectArray hostConvexData; + b3AlignedObjectArray hostConvexData; convexData.copyToHost(hostConvexData); b3AlignedObjectArray hostVertices; @@ -3537,7 +3836,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* { b3AlignedObjectArray hostPairs; pairs->copyToHost(hostPairs); - b3AlignedObjectArray hostBodyBuf; + b3AlignedObjectArray hostBodyBuf; bodyBuf->copyToHost(hostBodyBuf); b3AlignedObjectArray hostCollidables; gpuCollidables.copyToHost(hostCollidables); @@ -3750,14 +4049,14 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* b3AlignedObjectArray triangleConvexPairsOutHost; triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost); //triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity); - b3AlignedObjectArray hostBodyBuf; + b3AlignedObjectArray hostBodyBuf; bodyBuf->copyToHost(hostBodyBuf); b3AlignedObjectArray hostCollidables; gpuCollidables.copyToHost(hostCollidables); b3AlignedObjectArray hostAabbsWorldSpace; clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace); - b3AlignedObjectArray hostConvexData; + b3AlignedObjectArray hostConvexData; convexData.copyToHost(hostConvexData); b3AlignedObjectArray hostVertices; @@ -3887,7 +4186,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* //B3_PROFILE("clipHullHullKernel"); - bool breakupConcaveConvexKernel = false; + bool breakupConcaveConvexKernel = true; #ifdef __APPLE__ //actually, some Apple OpenCL platform/device combinations work fine... @@ -4025,7 +4324,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* volatile int nGlobalContactsOut = nContacts; b3AlignedObjectArray triangleConvexPairsOutHost; triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost); - b3AlignedObjectArray hostBodyBuf; + b3AlignedObjectArray hostBodyBuf; bodyBuf->copyToHost(hostBodyBuf); b3AlignedObjectArrayconcaveHasSeparatingNormalsCPU; @@ -4123,7 +4422,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* //convex-convex contact clipping B3_PROFILE("clipHullHullKernel"); - bool breakupKernel = false; + bool breakupKernel = true; #ifdef __APPLE__ breakupKernel = true; @@ -4182,7 +4481,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* float minDist = -1e30f; float maxDist = 0.02f; - b3AlignedObjectArray hostConvexData; + b3AlignedObjectArray hostConvexData; convexData.copyToHost(hostConvexData); b3AlignedObjectArray hostCollidables; gpuCollidables.copyToHost(hostCollidables); @@ -4194,7 +4493,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* b3AlignedObjectArray hostPairs; pairs->copyToHost(hostPairs); - b3AlignedObjectArray hostBodyBuf; + b3AlignedObjectArray hostBodyBuf; bodyBuf->copyToHost(hostBodyBuf); @@ -4267,6 +4566,9 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* if (clipConvexFacesAndFindContactsCPU) { + //b3AlignedObjectArray hostPairs; + //pairs->copyToHost(hostPairs); + b3AlignedObjectArray hostSepNormals; m_sepNormals.copyToHost(hostSepNormals); b3AlignedObjectArray hostHasSepAxis; @@ -4343,8 +4645,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* } { - // nContacts = m_totalContactsOut.at(0); - // printf("nContacts = %d\n",nContacts); + nContacts = m_totalContactsOut.at(0); + //printf("nContacts = %d\n",nContacts); int newContactCapacity = nContacts+nPairs; contactOut->reserve(newContactCapacity); @@ -4382,7 +4684,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* volatile int nGlobalContactsOut = nContacts; b3AlignedObjectArray hostPairs; pairs->copyToHost(hostPairs); - b3AlignedObjectArray hostBodyBuf; + b3AlignedObjectArray hostBodyBuf; bodyBuf->copyToHost(hostBodyBuf); b3AlignedObjectArray hostSepNormals; m_sepNormals.copyToHost(hostSepNormals); diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h index d257d9bc4..ff75ccd91 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h @@ -3,10 +3,10 @@ #define _CONVEX_HULL_CONTACT_H #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include "Bullet3Common/b3AlignedObjectArray.h" -#include "b3ConvexPolyhedronCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" #include "Bullet3Common/shared/b3Int2.h" @@ -26,6 +26,7 @@ struct GpuSatCollision cl_device_id m_device; cl_command_queue m_queue; cl_kernel m_findSeparatingAxisKernel; + cl_kernel m_mprPenetrationKernel; cl_kernel m_findSeparatingAxisVertexFaceKernel; cl_kernel m_findSeparatingAxisEdgeEdgeKernel; @@ -77,12 +78,12 @@ struct GpuSatCollision void computeConvexConvexContactsGPUSAT( b3OpenCLArray* pairs, int nPairs, - const b3OpenCLArray* bodyBuf, + const b3OpenCLArray* bodyBuf, b3OpenCLArray* contactOut, int& nContacts, const b3OpenCLArray* oldContacts, int maxContactCapacity, int compoundPairCapacity, - const b3OpenCLArray& hostConvexData, + const b3OpenCLArray& hostConvexData, const b3OpenCLArray& vertices, const b3OpenCLArray& uniqueEdges, const b3OpenCLArray& faces, diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h index b13dcd945..337100fb1 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h @@ -6,39 +6,4 @@ -B3_ATTRIBUTE_ALIGNED16(struct) b3ConvexPolyhedronCL : public b3ConvexPolyhedronData -{ - - inline void project(const b3Transform& trans, const b3Vector3& dir, const b3AlignedObjectArray& vertices, b3Scalar& min, b3Scalar& max) const - { - min = FLT_MAX; - max = -FLT_MAX; - int numVerts = m_numVertices; - - const b3Vector3 localDir = trans.getBasis().transpose()*dir; - const b3Vector3 localDi2 = b3QuatRotate(trans.getRotation().inverse(),dir); - - b3Scalar offset = trans.getOrigin().dot(dir); - - for(int i=0;i max) max = dp; - } - if(min>max) - { - b3Scalar tmp = min; - min = max; - max = tmp; - } - min += offset; - max += offset; - } - -}; - #endif //CONVEX_POLYHEDRON_CL diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp index 003a02570..b96844c7c 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp @@ -59,7 +59,7 @@ namespace gjkepa2_impl { - const b3ConvexPolyhedronCL* m_shapes[2]; + const b3ConvexPolyhedronData* m_shapes[2]; b3Matrix3x3 m_toshape1; @@ -631,7 +631,10 @@ namespace gjkepa2_impl append(m_stock,best); best=findbest(); outer=*best; - } else { m_status=eStatus::InvalidHull;break; } + } else { + m_status=eStatus::Failed; + //m_status=eStatus::InvalidHull; + break; } } else { m_status=eStatus::AccuraryReached;break; } } else { m_status=eStatus::OutOfVertices;break; } } @@ -804,7 +807,7 @@ namespace gjkepa2_impl // static void Initialize(const b3Transform& transA, const b3Transform& transB, - const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB, + const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& verticesB, b3GjkEpaSolver2::sResults& results, @@ -839,7 +842,7 @@ int b3GjkEpaSolver2::StackSizeRequirement() // bool b3GjkEpaSolver2::Distance( const b3Transform& transA, const b3Transform& transB, - const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB, + const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& verticesB, const b3Vector3& guess, @@ -877,7 +880,7 @@ bool b3GjkEpaSolver2::Distance( const b3Transform& transA, const b3Transform& t // bool b3GjkEpaSolver2::Penetration( const b3Transform& transA, const b3Transform& transB, - const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB, + const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& verticesB, const b3Vector3& guess, @@ -927,7 +930,7 @@ bool b3GjkEpaSolver2::Penetration( const b3Transform& transA, const b3Transform& b3Scalar b3GjkEpaSolver2::SignedDistance(const b3Vector3& position, b3Scalar margin, const b3Transform& transA, - const b3ConvexPolyhedronCL& hullA, + const b3ConvexPolyhedronData& hullA, const b3AlignedObjectArray& verticesA, sResults& results) { diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h index 367c782bb..976238a04 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h @@ -27,7 +27,8 @@ GJK-EPA collision solver by Nathanael Presson, 2008 #include "Bullet3Common/b3AlignedObjectArray.h" #include "Bullet3Common/b3Transform.h" -#include "b3ConvexPolyhedronCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" + ///btGjkEpaSolver contributed under zlib by Nathanael Presson struct b3GjkEpaSolver2 @@ -49,14 +50,14 @@ struct sResults static int StackSizeRequirement(); static bool Distance( const b3Transform& transA, const b3Transform& transB, - const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB, + const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& verticesB, const b3Vector3& guess, sResults& results); static bool Penetration( const b3Transform& transA, const b3Transform& transB, - const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB, + const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& verticesB, const b3Vector3& guess, @@ -65,19 +66,15 @@ static bool Penetration( const b3Transform& transA, const b3Transform& transB, #if 0 static b3Scalar SignedDistance( const b3Vector3& position, b3Scalar margin, - const b3Transform& transA, - const b3ConvexPolyhedronCL& hullA, - const b3AlignedObjectArray& verticesA, - + const btConvexShape* shape, + const btTransform& wtrs, sResults& results); -static bool SignedDistance( const b3Transform& transA, const b3Transform& transB, - const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, - const b3AlignedObjectArray& verticesA, - const b3AlignedObjectArray& verticesB, +static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, const b3Vector3& guess, sResults& results); -#endif //0 +#endif }; diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.cpp index a6ba5c0ee..8e78a19e5 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.cpp +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: #include "b3GjkPairDetector.h" #include "Bullet3Common/b3Transform.h" #include "b3VoronoiSimplexSolver.h" -#include "b3ConvexPolyhedronCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" #include "b3VectorFloat4.h" #include "b3GjkEpa.h" #include "b3SupportMappings.h" @@ -48,7 +48,7 @@ m_fixContactNormalDirection(1) bool calcPenDepth( b3VoronoiSimplexSolver& simplexSolver, const b3Transform& transformA, const b3Transform& transformB, - const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, + const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& verticesB, b3Vector3& v, b3Vector3& wWitnessOnA, b3Vector3& wWitnessOnB) @@ -83,7 +83,7 @@ bool calcPenDepth( b3VoronoiSimplexSolver& simplexSolver, } #define dot3F4 b3Dot -inline void project(const b3ConvexPolyhedronCL& hull, const float4& pos, const b3Quaternion& orn, const float4& dir, const b3AlignedObjectArray& vertices, b3Scalar& min, b3Scalar& max) +inline void project(const b3ConvexPolyhedronData& hull, const float4& pos, const b3Quaternion& orn, const float4& dir, const b3AlignedObjectArray& vertices, b3Scalar& min, b3Scalar& max) { min = FLT_MAX; max = -FLT_MAX; @@ -114,7 +114,7 @@ inline void project(const b3ConvexPolyhedronCL& hull, const float4& pos, const } -static bool TestSepAxis(const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, +static bool TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, const float4& posA,const b3Quaternion& ornA, const float4& posB,const b3Quaternion& ornB, float4& sep_axis, const b3AlignedObjectArray& verticesA,const b3AlignedObjectArray& verticesB,b3Scalar& depth) @@ -146,7 +146,7 @@ static bool TestSepAxis(const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhed bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, const b3Transform& transB, - const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, + const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& verticesB, b3Scalar maximumDistanceSquared, @@ -203,9 +203,7 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, b3Scalar margin = marginA + marginB; b3Scalar bestDeltaN = -1e30f; b3Vector3 bestSepAxis= b3MakeVector3(0,0,0); - b3Vector3 bestPointOnA; - b3Vector3 bestPointOnB; - + gjkDetector->m_simplexSolver->reset(); @@ -224,33 +222,10 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, b3Vector3 pWorld = localTransA(pInA); b3Vector3 qWorld = localTransB(qInB); - { - b3Scalar l2 = gjkDetector->m_cachedSeparatingAxis.length2(); - if (l2>B3_EPSILON*B3_EPSILON) - { - - b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1.f/b3Sqrt(l2)); - float computedDepth=1e30f; - if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(), - transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth)) - { - return false; - } - - - - if(computedDepthB3_EPSILON*B3_EPSILON) - { - resultSepDistance = computedDepth; - resultSepNormal = testAxis; - } - } - } - } - +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("got local supporting vertices\n"); +#endif if (check2d) { @@ -260,26 +235,7 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, b3Vector3 w = pWorld - qWorld; delta = gjkDetector->m_cachedSeparatingAxis.dot(w); - if (delta>0) - return false; - b3Scalar deltaN = gjkDetector->m_cachedSeparatingAxis.normalized().dot(w.normalized()); - - if (deltaN < bestDeltaN) - { - bestDeltaN = deltaN; - //printf("new solution?\n"); - bestSepAxis = gjkDetector->m_cachedSeparatingAxis; - gjkDetector->m_simplexSolver->compute_points(bestPointOnA, bestPointOnB); - } - prevDelta = delta; - b3Scalar dist = 0; - if (delta<0) - dist = -b3Sqrt(b3Fabs(delta)); - else - dist = b3Sqrt(delta); - - //printf("gjkDetector->m_cachedSeparatingAxis = %f,%f,%f delta/dist = %f\n",gjkDetector->m_cachedSeparatingAxis.x,gjkDetector->m_cachedSeparatingAxis.y,gjkDetector->m_cachedSeparatingAxis.z,dist); // potential exit, they don't overlap if ((delta > b3Scalar(0.0)) && (delta * delta > squaredDistance * maximumDistanceSquared)) { @@ -313,8 +269,14 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, break; } +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("addVertex 1\n"); +#endif //add current vertex to simplex gjkDetector->m_simplexSolver->addVertex(w, pWorld, qWorld); +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("addVertex 2\n"); +#endif b3Vector3 newCachedSeparatingAxis; //calculate the closest point to the origin (update vector v) @@ -325,12 +287,9 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, break; } - if(0)//newCachedSeparatingAxis.length2()m_cachedSeparatingAxis = newCachedSeparatingAxis; - } + gjkDetector->m_cachedSeparatingAxis = newCachedSeparatingAxis; gjkDetector->m_degenerateSimplex = 6; checkSimplex = true; break; @@ -338,17 +297,24 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, b3Scalar previousSquaredDistance = squaredDistance; squaredDistance = newCachedSeparatingAxis.length2(); - - b3Vector3 sepAxis=newCachedSeparatingAxis.normalized(); - +#if 0 +///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo + if (squaredDistance>previousSquaredDistance) + { + gjkDetector->m_degenerateSimplex = 7; + squaredDistance = previousSquaredDistance; + checkSimplex = false; + break; + } +#endif // - - //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); + //redundant gjkDetector->m_simplexSolver->compute_points(pointOnA, pointOnB); //are we getting any closer ? if (previousSquaredDistance - squaredDistance <= B3_EPSILON * previousSquaredDistance) { +// gjkDetector->m_simplexSolver->backup_closest(gjkDetector->m_cachedSeparatingAxis); checkSimplex = true; gjkDetector->m_degenerateSimplex = 12; @@ -357,55 +323,32 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, gjkDetector->m_cachedSeparatingAxis = newCachedSeparatingAxis; - { - b3Scalar l2 = gjkDetector->m_cachedSeparatingAxis.length2(); - if (l2>B3_EPSILON*B3_EPSILON) - { + //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject + if (gjkDetector->m_curIter++ > gGjkMaxIter) + { + #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION) - b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1.f/b3Sqrt(l2)); - float computedDepth=1e30f; - if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(), - transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth)) - { - return false; - } - + printf("btGjkPairDetector maxIter exceeded:%i\n",gjkDetector->m_curIter); + printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n", + gjkDetector->m_cachedSeparatingAxis.getX(), + gjkDetector->m_cachedSeparatingAxis.getY(), + gjkDetector->m_cachedSeparatingAxis.getZ(), + squaredDistance); + - - if(computedDepthB3_EPSILON*B3_EPSILON) - { - resultSepDistance = computedDepth; - resultSepNormal = testAxis; - } - } - } - } + #endif + break; - //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject - if (gjkDetector->m_curIter++ > gGjkMaxIter) - { - break; - } + } bool check = (!gjkDetector->m_simplexSolver->fullSimplex()); - - float projectedDepth = 0; - - if (delta<0) - { - projectedDepth = -b3Sqrt(b3Fabs(delta)); - } else - { - projectedDepth = b3Sqrt(delta); - } - - //printf("dist2 = %f dist= %f projectedDepth = %f\n", squaredDistance,b3Sqrt(squaredDistance),projectedDepth); + //bool check = (!gjkDetector->m_simplexSolver->fullSimplex() && squaredDistance > B3_EPSILON * gjkDetector->m_simplexSolver->maxVertex()); if (!check) { + //do we need this backup_closest here ? +// gjkDetector->m_simplexSolver->backup_closest(gjkDetector->m_cachedSeparatingAxis); gjkDetector->m_degenerateSimplex = 13; break; } @@ -413,16 +356,7 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, if (checkSimplex) { - if (bestSepAxis.length2()) - { - pointOnA = bestPointOnA; - pointOnB = bestPointOnB; - gjkDetector->m_cachedSeparatingAxis = bestSepAxis; - } else - { - gjkDetector->m_simplexSolver->compute_points(pointOnA, pointOnB); - } - + gjkDetector->m_simplexSolver->compute_points(pointOnA, pointOnB); normalInB = gjkDetector->m_cachedSeparatingAxis; b3Scalar lenSqr =gjkDetector->m_cachedSeparatingAxis.length2(); @@ -450,7 +384,8 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, } } - bool catchDegeneratePenetrationCase = (gjkDetector->m_catchDegeneracies && gjkDetector->m_penetrationDepthSolver && gjkDetector->m_degenerateSimplex && ((distance+margin) < 0.01)); + bool catchDegeneratePenetrationCase = + (gjkDetector->m_catchDegeneracies && gjkDetector->m_penetrationDepthSolver && gjkDetector->m_degenerateSimplex && ((distance+margin) < 0.01)); //if (checkPenetration && !isValid) if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) @@ -543,10 +478,11 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, - if (isValid && ((distance < 0) || (distance*distance < maximumDistanceSquared))) + if (isValid && (distance < 0)) + //if (isValid && ((distance < 0) || (distance*distance < maximumDistanceSquared))) { - if (gjkDetector->m_fixContactNormalDirection) + if (1)//m_fixContactNormalDirection) { ///@workaround for sticky convex collisions //in some degenerate cases (usually when the use uses very small margins) @@ -556,104 +492,37 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, //We like to use a dot product of the normal against the difference of the centroids, //once the centroid is available in the API //until then we use the center of the aabb to approximate the centroid - b3Vector3 aabbMin,aabbMax; - //m_minkowskiA->getAabb(localTransA,aabbMin,aabbMax); - //b3Vector3 posA = (aabbMax+aabbMin)*b3Scalar(0.5); - - //m_minkowskiB->getAabb(localTransB,aabbMin,aabbMax); - //b3Vector3 posB = (aabbMin+aabbMax)*b3Scalar(0.5); + b3Vector3 posA = localTransA*hullA.m_localCenter; + b3Vector3 posB = localTransB*hullB.m_localCenter; - - b3Vector3 diff = transA.getOrigin()-transB.getOrigin(); + + b3Vector3 diff = posA-posB; if (diff.dot(normalInB) < 0.f) normalInB *= -1.f; } gjkDetector->m_cachedSeparatingAxis = normalInB; gjkDetector->m_cachedSeparatingDistance = distance; - { - b3Scalar l2 = gjkDetector->m_cachedSeparatingAxis.length2(); - if (l2>B3_EPSILON*B3_EPSILON) - { - - b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1.f/b3Sqrt(l2)); - float computedDepth=1e30f; - if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(), - transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth)) - { - return false; - } - - - - if(computedDepthB3_EPSILON*B3_EPSILON) - { - resultSepDistance = computedDepth; - resultSepNormal = testAxis; - } - } - } - } - + /*output.addContactPoint( + normalInB, + pointOnB+positionOffset, + distance); + */ + static float maxPenetrationDistance = 0.f; + if (distanceB3_EPSILON*B3_EPSILON); - if (lsqrB3_EPSILON*B3_EPSILON) - { - - b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1./b3Sqrt(l2)); - float computedDepth=1e30f; - if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(), - transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth)) - { - return false; - } - - - - if(computedDepthB3_EPSILON*B3_EPSILON) - { - resultSepDistance = computedDepth; - resultSepNormal = testAxis; - } - } - } - } - } -#endif resultPointOnB = pointOnB+positionOffset; return true; } + + + return false; } diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.h index d71477c66..2a0f85a88 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.h +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.h @@ -12,7 +12,7 @@ class b3Transform; struct b3GjkEpaSolver2; class b3VoronoiSimplexSolver; -struct b3ConvexPolyhedronCL; +struct b3ConvexPolyhedronData; B3_ATTRIBUTE_ALIGNED16(struct) b3GjkPairDetector { @@ -73,7 +73,7 @@ public: bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, const b3Transform& transB, - const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, + const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& verticesB, b3Scalar maximumDistanceSquared, diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h index 828d3fa0f..d073ee57c 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h @@ -6,10 +6,12 @@ #include "Bullet3Common/b3AlignedObjectArray.h" #include "b3VectorFloat4.h" -struct b3ConvexPolyhedronCL; + struct b3GjkPairDetector; -inline b3Vector3 localGetSupportVertexWithMargin(const float4& supportVec,const struct b3ConvexPolyhedronCL* hull, + + +inline b3Vector3 localGetSupportVertexWithMargin(const float4& supportVec,const struct b3ConvexPolyhedronData* hull, const b3AlignedObjectArray& verticesA, b3Scalar margin) { b3Vector3 supVec = b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); @@ -27,7 +29,7 @@ inline b3Vector3 localGetSupportVertexWithMargin(const float4& supportVec,const } -inline b3Vector3 localGetSupportVertexWithoutMargin(const float4& supportVec,const struct b3ConvexPolyhedronCL* hull, +inline b3Vector3 localGetSupportVertexWithoutMargin(const float4& supportVec,const struct b3ConvexPolyhedronData* hull, const b3AlignedObjectArray& verticesA) { return localGetSupportVertexWithMargin(supportVec,hull,verticesA,0.f); diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h index 3ae074f28..f6f65f771 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h @@ -3,18 +3,9 @@ #include "Bullet3Common/b3Transform.h" -#define cross3(a,b) (a.cross(b)) +//#define cross3(a,b) (a.cross(b)) #define float4 b3Vector3 -#define make_float4(x,y,z,w) b3Vector4(x,y,z,w) +//#define make_float4(x,y,z,w) b3Vector4(x,y,z,w) -inline 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; -} #endif //B3_VECTOR_FLOAT4_H diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl new file mode 100644 index 000000000..f132f11a3 --- /dev/null +++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl @@ -0,0 +1,89 @@ + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +#define AppendInc(x, out) out = atomic_inc(x) +#define GET_NPOINTS(x) (x).m_worldNormalOnB.w +#ifdef cl_ext_atomic_counters_32 + #pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable +#else + #define counter32_t volatile __global int* +#endif + + +__kernel void mprPenetrationKernel( __global int4* pairs, + __global const b3RigidBodyData_t* rigidBodies, + __global const b3Collidable_t* collidables, + __global const b3ConvexPolyhedronData_t* convexShapes, + __global const float4* vertices, + __global float4* separatingNormals, + __global int* hasSeparatingAxis, + __global struct b3Contact4Data* restrict globalContactsOut, + counter32_t nGlobalContactsOut, + int contactCapacity, + int numPairs) +{ + int i = get_global_id(0); + int pairIndex = i; + if (im_worldNormalOnB = -dirOut;//normal; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + int bodyA = pairs[pairIndex].x; + int bodyB = pairs[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 = -1; + c->m_childIndexB = -1; + //for (int i=0;im_worldPosB[0] = posOut;//localPoints[contactIdx[i]]; + GET_NPOINTS(*c) = 1;//nContacts; + } + } + + } +} diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h new file mode 100644 index 000000000..07a7ca218 --- /dev/null +++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h @@ -0,0 +1,1244 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* mprKernelsCL= \ +"/***\n" +" * ---------------------------------\n" +" * Copyright (c)2012 Daniel Fiser \n" +" *\n" +" * This file was ported from mpr.c file, part of libccd.\n" +" * The Minkoski Portal Refinement implementation was ported \n" +" * to OpenCL by Erwin Coumans for the Bullet 3 Physics library.\n" +" * at http://github.com/erwincoumans/bullet3\n" +" *\n" +" * Distributed under the OSI-approved BSD License (the \"License\");\n" +" * see .\n" +" * This software is distributed WITHOUT ANY WARRANTY; without even the\n" +" * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\n" +" * See the License for more information.\n" +" */\n" +"#ifndef B3_MPR_PENETRATION_H\n" +"#define B3_MPR_PENETRATION_H\n" +"#ifndef B3_FLOAT4_H\n" +"#define B3_FLOAT4_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#define B3_PLATFORM_DEFINITIONS_H\n" +"struct MyTest\n" +"{\n" +" int bla;\n" +"};\n" +"#ifdef __cplusplus\n" +"#else\n" +"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" +"#define B3_LARGE_FLOAT 1e18f\n" +"#define B3_INFINITY 1e18f\n" +"#define b3Assert(a)\n" +"#define b3ConstArray(a) __global const a*\n" +"#define b3AtomicInc atomic_inc\n" +"#define b3AtomicAdd atomic_add\n" +"#define b3Fabs fabs\n" +"#define b3Sqrt native_sqrt\n" +"#define b3Sin native_sin\n" +"#define b3Cos native_cos\n" +"#endif\n" +"#endif\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Float4;\n" +" #define b3Float4ConstArg const b3Float4\n" +" #define b3MakeFloat4 (float4)\n" +" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return dot(a1, b1);\n" +" }\n" +" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return cross(a1, b1);\n" +" }\n" +" #define b3MinFloat4 min\n" +" #define b3MaxFloat4 max\n" +" #define b3Normalized(a) normalize(a)\n" +"#endif \n" +" \n" +"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" +"{\n" +" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" +" return false;\n" +" return true;\n" +"}\n" +"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" +"{\n" +" float maxDot = -B3_INFINITY;\n" +" int i = 0;\n" +" int ptIndex = -1;\n" +" for( i = 0; i < vecLen; i++ )\n" +" {\n" +" float dot = b3Dot3F4(vecArray[i],vec);\n" +" \n" +" if( dot > maxDot )\n" +" {\n" +" maxDot = dot;\n" +" ptIndex = i;\n" +" }\n" +" }\n" +" b3Assert(ptIndex>=0)\n" +" if (ptIndex<0)\n" +" {\n" +" //ptIndex = 0;\n" +" }\n" +" *dotOut = maxDot;\n" +" return ptIndex;\n" +"}\n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_RIGIDBODY_DATA_H\n" +"#define B3_RIGIDBODY_DATA_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_QUAT_H\n" +"#define B3_QUAT_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif\n" +"#endif\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Quat;\n" +" #define b3QuatConstArg const b3Quat\n" +" \n" +" \n" +"inline float4 b3FastNormalize4(float4 v)\n" +"{\n" +" v = (float4)(v.xyz,0.f);\n" +" return fast_normalize(v);\n" +"}\n" +" \n" +"inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" +"inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" +"inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" +"{\n" +" b3Quat ans;\n" +" ans = b3Cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - b3Dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" +"{\n" +" b3Quat q;\n" +" q=in;\n" +" //return b3FastNormalize4(in);\n" +" float len = native_sqrt(dot(q, q));\n" +" if(len > 0.f)\n" +" {\n" +" q *= 1.f / len;\n" +" }\n" +" else\n" +" {\n" +" q.x = q.y = q.z = 0.f;\n" +" q.w = 1.f;\n" +" }\n" +" return q;\n" +"}\n" +"inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" b3Quat qInv = b3QuatInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" return b3QuatRotate( b3QuatInvert( q ), vec );\n" +"}\n" +"inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" +"{\n" +" return b3QuatRotate( orientation, point ) + (translation);\n" +"}\n" +" \n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifndef B3_MAT3x3_H\n" +"#define B3_MAT3x3_H\n" +"#ifndef B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"typedef struct\n" +"{\n" +" b3Float4 m_row[3];\n" +"}b3Mat3x3;\n" +"#define b3Mat3x3ConstArg const b3Mat3x3\n" +"#define b3GetRow(m,row) (m.m_row[row])\n" +"inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" +"{\n" +" b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" +" b3Mat3x3 out;\n" +" out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" +" out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" +" out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" +" out.m_row[0].w = 0.f;\n" +" out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" +" out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" +" out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" +" out.m_row[1].w = 0.f;\n" +" out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" +" out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" +" out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" +" out.m_row[2].w = 0.f;\n" +" return out;\n" +"}\n" +"inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = fabs(matIn.m_row[0]);\n" +" out.m_row[1] = fabs(matIn.m_row[1]);\n" +" out.m_row[2] = fabs(matIn.m_row[2]);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtZero();\n" +"__inline\n" +"b3Mat3x3 mtIdentity();\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Mat3x3 mtZero()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(0.f);\n" +" m.m_row[1] = (b3Float4)(0.f);\n" +" m.m_row[2] = (b3Float4)(0.f);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtIdentity()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(1,0,0,0);\n" +" m.m_row[1] = (b3Float4)(0,1,0,0);\n" +" m.m_row[2] = (b3Float4)(0,0,1,0);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" +" out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" +" out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" +"{\n" +" b3Mat3x3 transB;\n" +" transB = mtTranspose( b );\n" +" b3Mat3x3 ans;\n" +" // why this doesn't run when 0ing in the for{}\n" +" a.m_row[0].w = 0.f;\n" +" a.m_row[1].w = 0.f;\n" +" a.m_row[2].w = 0.f;\n" +" for(int i=0; i<3; i++)\n" +" {\n" +"// a.m_row[i].w = 0.f;\n" +" ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" +" ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" +" ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" +" ans.m_row[i].w = 0.f;\n" +" }\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" +"{\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a.m_row[0], b );\n" +" ans.y = b3Dot3F4( a.m_row[1], b );\n" +" ans.z = b3Dot3F4( a.m_row[2], b );\n" +" ans.w = 0.f;\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" +"{\n" +" b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" +" b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" +" b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a, colx );\n" +" ans.y = b3Dot3F4( a, coly );\n" +" ans.z = b3Dot3F4( a, colz );\n" +" return ans;\n" +"}\n" +"#endif\n" +"#endif //B3_MAT3x3_H\n" +"typedef struct b3RigidBodyData b3RigidBodyData_t;\n" +"struct b3RigidBodyData\n" +"{\n" +" b3Float4 m_pos;\n" +" b3Quat m_quat;\n" +" b3Float4 m_linVel;\n" +" b3Float4 m_angVel;\n" +" int m_collidableIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"};\n" +"typedef struct b3InertiaData b3InertiaData_t;\n" +"struct b3InertiaData\n" +"{\n" +" b3Mat3x3 m_invInertiaWorld;\n" +" b3Mat3x3 m_initInvInertia;\n" +"};\n" +"#endif //B3_RIGIDBODY_DATA_H\n" +" \n" +"#ifndef B3_CONVEX_POLYHEDRON_DATA_H\n" +"#define B3_CONVEX_POLYHEDRON_DATA_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"typedef struct b3GpuFace b3GpuFace_t;\n" +"struct b3GpuFace\n" +"{\n" +" b3Float4 m_plane;\n" +" int m_indexOffset;\n" +" int m_numIndices;\n" +" int m_unusedPadding1;\n" +" int m_unusedPadding2;\n" +"};\n" +"typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;\n" +"struct b3ConvexPolyhedronData\n" +"{\n" +" b3Float4 m_localCenter;\n" +" b3Float4 m_extents;\n" +" b3Float4 mC;\n" +" b3Float4 mE;\n" +" float m_radius;\n" +" int m_faceOffset;\n" +" int m_numFaces;\n" +" int m_numVertices;\n" +" int m_vertexOffset;\n" +" int m_uniqueEdgesOffset;\n" +" int m_numUniqueEdges;\n" +" int m_unused;\n" +"};\n" +"#endif //B3_CONVEX_POLYHEDRON_DATA_H\n" +"#ifndef B3_COLLIDABLE_H\n" +"#define B3_COLLIDABLE_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"enum b3ShapeTypes\n" +"{\n" +" SHAPE_HEIGHT_FIELD=1,\n" +" SHAPE_CONVEX_HULL=3,\n" +" SHAPE_PLANE=4,\n" +" SHAPE_CONCAVE_TRIMESH=5,\n" +" SHAPE_COMPOUND_OF_CONVEX_HULLS=6,\n" +" SHAPE_SPHERE=7,\n" +" MAX_NUM_SHAPE_TYPES,\n" +"};\n" +"typedef struct b3Collidable b3Collidable_t;\n" +"struct b3Collidable\n" +"{\n" +" union {\n" +" int m_numChildShapes;\n" +" int m_bvhIndex;\n" +" };\n" +" union\n" +" {\n" +" float m_radius;\n" +" int m_compoundBvhIndex;\n" +" };\n" +" int m_shapeType;\n" +" int m_shapeIndex;\n" +"};\n" +"typedef struct b3GpuChildShape b3GpuChildShape_t;\n" +"struct b3GpuChildShape\n" +"{\n" +" b3Float4 m_childPosition;\n" +" b3Quat m_childOrientation;\n" +" int m_shapeIndex;\n" +" int m_unused0;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"};\n" +"struct b3CompoundOverlappingPair\n" +"{\n" +" int m_bodyIndexA;\n" +" int m_bodyIndexB;\n" +"// int m_pairType;\n" +" int m_childShapeIndexA;\n" +" int m_childShapeIndexB;\n" +"};\n" +"#endif //B3_COLLIDABLE_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#define B3_MPR_SQRT sqrt\n" +"#endif\n" +"#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))\n" +"#define B3_MPR_FABS fabs\n" +"#define B3_MPR_TOLERANCE 1E-6f\n" +"#define B3_MPR_MAX_ITERATIONS 1000\n" +"struct _b3MprSupport_t \n" +"{\n" +" b3Float4 v; //!< Support point in minkowski sum\n" +" b3Float4 v1; //!< Support point in obj1\n" +" b3Float4 v2; //!< Support point in obj2\n" +"};\n" +"typedef struct _b3MprSupport_t b3MprSupport_t;\n" +"struct _b3MprSimplex_t \n" +"{\n" +" b3MprSupport_t ps[4];\n" +" int last; //!< index of last added point\n" +"};\n" +"typedef struct _b3MprSimplex_t b3MprSimplex_t;\n" +"inline b3MprSupport_t* b3MprSimplexPointW(b3MprSimplex_t *s, int idx)\n" +"{\n" +" return &s->ps[idx];\n" +"}\n" +"inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size)\n" +"{\n" +" s->last = size - 1;\n" +"}\n" +"inline int b3MprSimplexSize(const b3MprSimplex_t *s)\n" +"{\n" +" return s->last + 1;\n" +"}\n" +"inline const b3MprSupport_t* b3MprSimplexPoint(const b3MprSimplex_t* s, int idx)\n" +"{\n" +" // here is no check on boundaries\n" +" return &s->ps[idx];\n" +"}\n" +"inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s)\n" +"{\n" +" *d = *s;\n" +"}\n" +"inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a)\n" +"{\n" +" b3MprSupportCopy(s->ps + pos, a);\n" +"}\n" +"inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)\n" +"{\n" +" b3MprSupport_t supp;\n" +" b3MprSupportCopy(&supp, &s->ps[pos1]);\n" +" b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]);\n" +" b3MprSupportCopy(&s->ps[pos2], &supp);\n" +"}\n" +"inline int b3MprIsZero(float val)\n" +"{\n" +" return B3_MPR_FABS(val) < FLT_EPSILON;\n" +"}\n" +"inline int b3MprEq(float _a, float _b)\n" +"{\n" +" float ab;\n" +" float a, b;\n" +" ab = B3_MPR_FABS(_a - _b);\n" +" if (B3_MPR_FABS(ab) < FLT_EPSILON)\n" +" return 1;\n" +" a = B3_MPR_FABS(_a);\n" +" b = B3_MPR_FABS(_b);\n" +" if (b > a){\n" +" return ab < FLT_EPSILON * b;\n" +" }else{\n" +" return ab < FLT_EPSILON * a;\n" +" }\n" +"}\n" +"inline int b3MprVec3Eq(const b3Float4* a, const b3Float4 *b)\n" +"{\n" +" return b3MprEq((*a).x, (*b).x)\n" +" && b3MprEq((*a).y, (*b).y)\n" +" && b3MprEq((*a).z, (*b).z);\n" +"}\n" +"inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global const b3ConvexPolyhedronData_t* hull, b3ConstArray(b3Float4) verticesA)\n" +"{\n" +" b3Float4 supVec = b3MakeFloat4(0,0,0,0);\n" +" float maxDot = -B3_LARGE_FLOAT;\n" +" if( 0 < hull->m_numVertices )\n" +" {\n" +" const b3Float4 scaled = supportVec;\n" +" int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot);\n" +" return verticesA[hull->m_vertexOffset+index];\n" +" }\n" +" return supVec;\n" +"}\n" +"static void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" +" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" +" b3ConstArray(b3Collidable_t) cpuCollidables,\n" +" b3ConstArray(b3Float4) cpuVertices,\n" +" __global b3Float4* sepAxis,\n" +" const b3Float4* _dir, b3Float4* outp, int logme)\n" +"{\n" +" //dir is in worldspace, move to local space\n" +" \n" +" b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos;\n" +" b3Quat orn = cpuBodyBuf[bodyIndex].m_quat;\n" +" \n" +" b3Float4 dir = b3MakeFloat4((*_dir).x,(*_dir).y,(*_dir).z,0.f);\n" +" \n" +" const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),dir);\n" +" \n" +" //find local support vertex\n" +" int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx;\n" +" \n" +" b3Assert(cpuCollidables[colIndex].m_shapeType==SHAPE_CONVEX_HULL);\n" +" __global const b3ConvexPolyhedronData_t* hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex];\n" +" \n" +" b3Float4 pInA;\n" +" if (logme)\n" +" {\n" +" b3Float4 supVec = b3MakeFloat4(0,0,0,0);\n" +" float maxDot = -B3_LARGE_FLOAT;\n" +" if( 0 < hull->m_numVertices )\n" +" {\n" +" const b3Float4 scaled = localDir;\n" +" int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot);\n" +" pInA = cpuVertices[hull->m_vertexOffset+index];\n" +" \n" +" }\n" +" } else\n" +" {\n" +" pInA = b3LocalGetSupportVertex(localDir,hull,cpuVertices);\n" +" }\n" +" //move vertex to world space\n" +" *outp = b3TransformPoint(pInA,pos,orn);\n" +" \n" +"}\n" +"inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" +" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" +" b3ConstArray(b3Collidable_t) cpuCollidables,\n" +" b3ConstArray(b3Float4) cpuVertices,\n" +" __global b3Float4* sepAxis,\n" +" const b3Float4* _dir, b3MprSupport_t *supp)\n" +"{\n" +" b3Float4 dir;\n" +" dir = *_dir;\n" +" b3MprConvexSupport(pairIndex,bodyIndexA,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v1,0);\n" +" dir = *_dir*-1.f;\n" +" b3MprConvexSupport(pairIndex,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v2,0);\n" +" supp->v = supp->v1 - supp->v2;\n" +"}\n" +"inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center)\n" +"{\n" +" center->v1 = cpuBodyBuf[bodyIndexA].m_pos;\n" +" center->v2 = cpuBodyBuf[bodyIndexB].m_pos;\n" +" center->v = center->v1 - center->v2;\n" +"}\n" +"inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z)\n" +"{\n" +" (*v).x = x;\n" +" (*v).y = y;\n" +" (*v).z = z;\n" +" (*v).w = 0.f;\n" +"}\n" +"inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w)\n" +"{\n" +" (*v).x += (*w).x;\n" +" (*v).y += (*w).y;\n" +" (*v).z += (*w).z;\n" +"}\n" +"inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w)\n" +"{\n" +" *v = *w;\n" +"}\n" +"inline void b3MprVec3Scale(b3Float4 *d, float k)\n" +"{\n" +" *d *= k;\n" +"}\n" +"inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b)\n" +"{\n" +" float dot;\n" +" dot = b3Dot3F4(*a,*b);\n" +" return dot;\n" +"}\n" +"inline float b3MprVec3Len2(const b3Float4 *v)\n" +"{\n" +" return b3MprVec3Dot(v, v);\n" +"}\n" +"inline void b3MprVec3Normalize(b3Float4 *d)\n" +"{\n" +" float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d));\n" +" b3MprVec3Scale(d, k);\n" +"}\n" +"inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b)\n" +"{\n" +" *d = b3Cross3(*a,*b);\n" +" \n" +"}\n" +"inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)\n" +"{\n" +" *d = *v - *w;\n" +"}\n" +"inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir)\n" +"{\n" +" b3Float4 v2v1, v3v1;\n" +" b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v,\n" +" &b3MprSimplexPoint(portal, 1)->v);\n" +" b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v,\n" +" &b3MprSimplexPoint(portal, 1)->v);\n" +" b3MprVec3Cross(dir, &v2v1, &v3v1);\n" +" b3MprVec3Normalize(dir);\n" +"}\n" +"inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal,\n" +" const b3Float4 *dir)\n" +"{\n" +" float dot;\n" +" dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v);\n" +" return b3MprIsZero(dot) || dot > 0.f;\n" +"}\n" +"inline int portalReachTolerance(const b3MprSimplex_t *portal,\n" +" const b3MprSupport_t *v4,\n" +" const b3Float4 *dir)\n" +"{\n" +" float dv1, dv2, dv3, dv4;\n" +" float dot1, dot2, dot3;\n" +" // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}\n" +" dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir);\n" +" dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir);\n" +" dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir);\n" +" dv4 = b3MprVec3Dot(&v4->v, dir);\n" +" dot1 = dv4 - dv1;\n" +" dot2 = dv4 - dv2;\n" +" dot3 = dv4 - dv3;\n" +" dot1 = B3_MPR_FMIN(dot1, dot2);\n" +" dot1 = B3_MPR_FMIN(dot1, dot3);\n" +" return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE;\n" +"}\n" +"inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal, \n" +" const b3MprSupport_t *v4,\n" +" const b3Float4 *dir)\n" +"{\n" +" float dot;\n" +" dot = b3MprVec3Dot(&v4->v, dir);\n" +" return b3MprIsZero(dot) || dot > 0.f;\n" +"}\n" +"inline void b3ExpandPortal(b3MprSimplex_t *portal,\n" +" const b3MprSupport_t *v4)\n" +"{\n" +" float dot;\n" +" b3Float4 v4v0;\n" +" b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);\n" +" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);\n" +" if (dot > 0.f){\n" +" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);\n" +" if (dot > 0.f){\n" +" b3MprSimplexSet(portal, 1, v4);\n" +" }else{\n" +" b3MprSimplexSet(portal, 3, v4);\n" +" }\n" +" }else{\n" +" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);\n" +" if (dot > 0.f){\n" +" b3MprSimplexSet(portal, 2, v4);\n" +" }else{\n" +" b3MprSimplexSet(portal, 1, v4);\n" +" }\n" +" }\n" +"}\n" +"static int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" +" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" +" b3ConstArray(b3Collidable_t) cpuCollidables,\n" +" b3ConstArray(b3Float4) cpuVertices,\n" +" __global b3Float4* sepAxis,\n" +" __global int* hasSepAxis,\n" +" b3MprSimplex_t *portal)\n" +"{\n" +" b3Float4 dir, va, vb;\n" +" float dot;\n" +" int cont;\n" +" \n" +" \n" +" // vertex 0 is center of portal\n" +" b3FindOrigin(bodyIndexA,bodyIndexB,cpuBodyBuf, b3MprSimplexPointW(portal, 0));\n" +" // vertex 0 is center of portal\n" +" b3MprSimplexSetSize(portal, 1);\n" +" \n" +" b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" +" b3Float4* b3mpr_vec3_origin = &zero;\n" +" if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){\n" +" // Portal's center lies on origin (0,0,0) => we know that objects\n" +" // intersect but we would need to know penetration info.\n" +" // So move center little bit...\n" +" b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);\n" +" b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);\n" +" }\n" +" // vertex 1 = support in direction of origin\n" +" b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);\n" +" b3MprVec3Scale(&dir, -1.f);\n" +" b3MprVec3Normalize(&dir);\n" +" b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 1));\n" +" b3MprSimplexSetSize(portal, 2);\n" +" // test if origin isn't outside of v1\n" +" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);\n" +" \n" +" if (b3MprIsZero(dot) || dot < 0.f)\n" +" return -1;\n" +" // vertex 2\n" +" b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,\n" +" &b3MprSimplexPoint(portal, 1)->v);\n" +" if (b3MprIsZero(b3MprVec3Len2(&dir))){\n" +" if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){\n" +" // origin lies on v1\n" +" return 1;\n" +" }else{\n" +" // origin lies on v0-v1 segment\n" +" return 2;\n" +" }\n" +" }\n" +" b3MprVec3Normalize(&dir);\n" +" b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 2));\n" +" \n" +" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir);\n" +" if (b3MprIsZero(dot) || dot < 0.f)\n" +" return -1;\n" +" b3MprSimplexSetSize(portal, 3);\n" +" // vertex 3 direction\n" +" b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,\n" +" &b3MprSimplexPoint(portal, 0)->v);\n" +" b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,\n" +" &b3MprSimplexPoint(portal, 0)->v);\n" +" b3MprVec3Cross(&dir, &va, &vb);\n" +" b3MprVec3Normalize(&dir);\n" +" // it is better to form portal faces to be oriented \"outside\" origin\n" +" dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);\n" +" if (dot > 0.f){\n" +" b3MprSimplexSwap(portal, 1, 2);\n" +" b3MprVec3Scale(&dir, -1.f);\n" +" }\n" +" while (b3MprSimplexSize(portal) < 4){\n" +" b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 3));\n" +" \n" +" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);\n" +" if (b3MprIsZero(dot) || dot < 0.f)\n" +" return -1;\n" +" cont = 0;\n" +" // test if origin is outside (v1, v0, v3) - set v2 as v3 and\n" +" // continue\n" +" b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,\n" +" &b3MprSimplexPoint(portal, 3)->v);\n" +" dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);\n" +" if (dot < 0.f && !b3MprIsZero(dot)){\n" +" b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));\n" +" cont = 1;\n" +" }\n" +" if (!cont){\n" +" // test if origin is outside (v3, v0, v2) - set v1 as v3 and\n" +" // continue\n" +" b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,\n" +" &b3MprSimplexPoint(portal, 2)->v);\n" +" dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);\n" +" if (dot < 0.f && !b3MprIsZero(dot)){\n" +" b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));\n" +" cont = 1;\n" +" }\n" +" }\n" +" if (cont){\n" +" b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,\n" +" &b3MprSimplexPoint(portal, 0)->v);\n" +" b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,\n" +" &b3MprSimplexPoint(portal, 0)->v);\n" +" b3MprVec3Cross(&dir, &va, &vb);\n" +" b3MprVec3Normalize(&dir);\n" +" }else{\n" +" b3MprSimplexSetSize(portal, 4);\n" +" }\n" +" }\n" +" return 0;\n" +"}\n" +"static int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" +" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" +" b3ConstArray(b3Collidable_t) cpuCollidables,\n" +" b3ConstArray(b3Float4) cpuVertices,\n" +" __global b3Float4* sepAxis,\n" +" b3MprSimplex_t *portal)\n" +"{\n" +" b3Float4 dir;\n" +" b3MprSupport_t v4;\n" +" for (int i=0;iv,\n" +" &b3MprSimplexPoint(portal, 2)->v);\n" +" b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);\n" +" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,\n" +" &b3MprSimplexPoint(portal, 2)->v);\n" +" b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);\n" +" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v,\n" +" &b3MprSimplexPoint(portal, 1)->v);\n" +" b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);\n" +" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,\n" +" &b3MprSimplexPoint(portal, 1)->v);\n" +" b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);\n" +" sum = b[0] + b[1] + b[2] + b[3];\n" +" if (b3MprIsZero(sum) || sum < 0.f){\n" +" b[0] = 0.f;\n" +" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,\n" +" &b3MprSimplexPoint(portal, 3)->v);\n" +" b[1] = b3MprVec3Dot(&vec, &dir);\n" +" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,\n" +" &b3MprSimplexPoint(portal, 1)->v);\n" +" b[2] = b3MprVec3Dot(&vec, &dir);\n" +" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,\n" +" &b3MprSimplexPoint(portal, 2)->v);\n" +" b[3] = b3MprVec3Dot(&vec, &dir);\n" +" sum = b[1] + b[2] + b[3];\n" +" }\n" +" inv = 1.f / sum;\n" +" b3MprVec3Copy(&p1, b3mpr_vec3_origin);\n" +" b3MprVec3Copy(&p2, b3mpr_vec3_origin);\n" +" for (i = 0; i < 4; i++){\n" +" b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);\n" +" b3MprVec3Scale(&vec, b[i]);\n" +" b3MprVec3Add(&p1, &vec);\n" +" b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2);\n" +" b3MprVec3Scale(&vec, b[i]);\n" +" b3MprVec3Add(&p2, &vec);\n" +" }\n" +" b3MprVec3Scale(&p1, inv);\n" +" b3MprVec3Scale(&p2, inv);\n" +" b3MprVec3Copy(pos, &p1);\n" +" b3MprVec3Add(pos, &p2);\n" +" b3MprVec3Scale(pos, 0.5);\n" +"}\n" +"inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b)\n" +"{\n" +" b3Float4 ab;\n" +" b3MprVec3Sub2(&ab, a, b);\n" +" return b3MprVec3Len2(&ab);\n" +"}\n" +"inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,\n" +" const b3Float4 *x0,\n" +" const b3Float4 *b,\n" +" b3Float4 *witness)\n" +"{\n" +" // The computation comes from solving equation of segment:\n" +" // S(t) = x0 + t.d\n" +" // where - x0 is initial point of segment\n" +" // - d is direction of segment from x0 (|d| > 0)\n" +" // - t belongs to <0, 1> interval\n" +" // \n" +" // Than, distance from a segment to some point P can be expressed:\n" +" // D(t) = |x0 + t.d - P|^2\n" +" // which is distance from any point on segment. Minimization\n" +" // of this function brings distance from P to segment.\n" +" // Minimization of D(t) leads to simple quadratic equation that's\n" +" // solving is straightforward.\n" +" //\n" +" // Bonus of this method is witness point for free.\n" +" float dist, t;\n" +" b3Float4 d, a;\n" +" // direction of segment\n" +" b3MprVec3Sub2(&d, b, x0);\n" +" // precompute vector from P to x0\n" +" b3MprVec3Sub2(&a, x0, P);\n" +" t = -1.f * b3MprVec3Dot(&a, &d);\n" +" t /= b3MprVec3Len2(&d);\n" +" if (t < 0.f || b3MprIsZero(t)){\n" +" dist = b3MprVec3Dist2(x0, P);\n" +" if (witness)\n" +" b3MprVec3Copy(witness, x0);\n" +" }else if (t > 1.f || b3MprEq(t, 1.f)){\n" +" dist = b3MprVec3Dist2(b, P);\n" +" if (witness)\n" +" b3MprVec3Copy(witness, b);\n" +" }else{\n" +" if (witness){\n" +" b3MprVec3Copy(witness, &d);\n" +" b3MprVec3Scale(witness, t);\n" +" b3MprVec3Add(witness, x0);\n" +" dist = b3MprVec3Dist2(witness, P);\n" +" }else{\n" +" // recycling variables\n" +" b3MprVec3Scale(&d, t);\n" +" b3MprVec3Add(&d, &a);\n" +" dist = b3MprVec3Len2(&d);\n" +" }\n" +" }\n" +" return dist;\n" +"}\n" +"inline float b3MprVec3PointTriDist2(const b3Float4 *P,\n" +" const b3Float4 *x0, const b3Float4 *B,\n" +" const b3Float4 *C,\n" +" b3Float4 *witness)\n" +"{\n" +" // Computation comes from analytic expression for triangle (x0, B, C)\n" +" // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and\n" +" // Then equation for distance is:\n" +" // D(s, t) = | T(s, t) - P |^2\n" +" // This leads to minimization of quadratic function of two variables.\n" +" // The solution from is taken only if s is between 0 and 1, t is\n" +" // between 0 and 1 and t + s < 1, otherwise distance from segment is\n" +" // computed.\n" +" b3Float4 d1, d2, a;\n" +" float u, v, w, p, q, r;\n" +" float s, t, dist, dist2;\n" +" b3Float4 witness2;\n" +" b3MprVec3Sub2(&d1, B, x0);\n" +" b3MprVec3Sub2(&d2, C, x0);\n" +" b3MprVec3Sub2(&a, x0, P);\n" +" u = b3MprVec3Dot(&a, &a);\n" +" v = b3MprVec3Dot(&d1, &d1);\n" +" w = b3MprVec3Dot(&d2, &d2);\n" +" p = b3MprVec3Dot(&a, &d1);\n" +" q = b3MprVec3Dot(&a, &d2);\n" +" r = b3MprVec3Dot(&d1, &d2);\n" +" s = (q * r - w * p) / (w * v - r * r);\n" +" t = (-s * r - q) / w;\n" +" if ((b3MprIsZero(s) || s > 0.f)\n" +" && (b3MprEq(s, 1.f) || s < 1.f)\n" +" && (b3MprIsZero(t) || t > 0.f)\n" +" && (b3MprEq(t, 1.f) || t < 1.f)\n" +" && (b3MprEq(t + s, 1.f) || t + s < 1.f)){\n" +" if (witness){\n" +" b3MprVec3Scale(&d1, s);\n" +" b3MprVec3Scale(&d2, t);\n" +" b3MprVec3Copy(witness, x0);\n" +" b3MprVec3Add(witness, &d1);\n" +" b3MprVec3Add(witness, &d2);\n" +" dist = b3MprVec3Dist2(witness, P);\n" +" }else{\n" +" dist = s * s * v;\n" +" dist += t * t * w;\n" +" dist += 2.f * s * t * r;\n" +" dist += 2.f * s * p;\n" +" dist += 2.f * t * q;\n" +" dist += u;\n" +" }\n" +" }else{\n" +" dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);\n" +" dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);\n" +" if (dist2 < dist){\n" +" dist = dist2;\n" +" if (witness)\n" +" b3MprVec3Copy(witness, &witness2);\n" +" }\n" +" dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);\n" +" if (dist2 < dist){\n" +" dist = dist2;\n" +" if (witness)\n" +" b3MprVec3Copy(witness, &witness2);\n" +" }\n" +" }\n" +" return dist;\n" +"}\n" +"static void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" +" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" +" b3ConstArray(b3Collidable_t) cpuCollidables,\n" +" b3ConstArray(b3Float4) cpuVertices,\n" +" __global b3Float4* sepAxis,\n" +" b3MprSimplex_t *portal,\n" +" float *depth, b3Float4 *pdir, b3Float4 *pos)\n" +"{\n" +" b3Float4 dir;\n" +" b3MprSupport_t v4;\n" +" unsigned long iterations;\n" +" b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" +" b3Float4* b3mpr_vec3_origin = &zero;\n" +" iterations = 1UL;\n" +" for (int i=0;i find penetration info\n" +" if (portalReachTolerance(portal, &v4, &dir)\n" +" || iterations ==B3_MPR_MAX_ITERATIONS)\n" +" {\n" +" *depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin,&b3MprSimplexPoint(portal, 1)->v,&b3MprSimplexPoint(portal, 2)->v,&b3MprSimplexPoint(portal, 3)->v,pdir);\n" +" *depth = B3_MPR_SQRT(*depth);\n" +" \n" +" if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z))\n" +" {\n" +" \n" +" *pdir = dir;\n" +" } \n" +" b3MprVec3Normalize(pdir);\n" +" \n" +" // barycentric coordinates:\n" +" b3FindPos(portal, pos);\n" +" return;\n" +" }\n" +" b3ExpandPortal(portal, &v4);\n" +" iterations++;\n" +" }\n" +"}\n" +"static void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos)\n" +"{\n" +" // Touching contact on portal's v1 - so depth is zero and direction\n" +" // is unimportant and pos can be guessed\n" +" *depth = 0.f;\n" +" b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" +" b3Float4* b3mpr_vec3_origin = &zero;\n" +" b3MprVec3Copy(dir, b3mpr_vec3_origin);\n" +" b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);\n" +" b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);\n" +" b3MprVec3Scale(pos, 0.5);\n" +"}\n" +"static void b3FindPenetrSegment(b3MprSimplex_t *portal,\n" +" float *depth, b3Float4 *dir, b3Float4 *pos)\n" +"{\n" +" \n" +" // Origin lies on v0-v1 segment.\n" +" // Depth is distance to v1, direction also and position must be\n" +" // computed\n" +" b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);\n" +" b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);\n" +" b3MprVec3Scale(pos, 0.5f);\n" +" \n" +" b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);\n" +" *depth = B3_MPR_SQRT(b3MprVec3Len2(dir));\n" +" b3MprVec3Normalize(dir);\n" +"}\n" +"inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,\n" +" b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,\n" +" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" +" b3ConstArray(b3Collidable_t) cpuCollidables,\n" +" b3ConstArray(b3Float4) cpuVertices,\n" +" __global b3Float4* sepAxis,\n" +" __global int* hasSepAxis,\n" +" float *depthOut, b3Float4* dirOut, b3Float4* posOut)\n" +"{\n" +" \n" +" b3MprSimplex_t portal;\n" +" \n" +" if (!hasSepAxis[pairIndex])\n" +" return -1;\n" +" \n" +" hasSepAxis[pairIndex] = 0;\n" +" int res;\n" +" // Phase 1: Portal discovery\n" +" res = b3DiscoverPortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,hasSepAxis, &portal);\n" +" \n" +" \n" +" //sepAxis[pairIndex] = *pdir;//or -dir?\n" +" switch (res)\n" +" {\n" +" case 0:\n" +" {\n" +" // Phase 2: Portal refinement\n" +" \n" +" res = b3RefinePortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal);\n" +" if (res < 0)\n" +" return -1;\n" +" // Phase 3. Penetration info\n" +" b3FindPenetr(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal, depthOut, dirOut, posOut);\n" +" hasSepAxis[pairIndex] = 1;\n" +" sepAxis[pairIndex] = -*dirOut;\n" +" break;\n" +" }\n" +" case 1:\n" +" {\n" +" // Touching contact on portal's v1.\n" +" b3FindPenetrTouch(&portal, depthOut, dirOut, posOut);\n" +" break;\n" +" }\n" +" case 2:\n" +" {\n" +" \n" +" b3FindPenetrSegment( &portal, depthOut, dirOut, posOut);\n" +" break;\n" +" }\n" +" default:\n" +" {\n" +" hasSepAxis[pairIndex]=0;\n" +" //if (res < 0)\n" +" //{\n" +" // Origin isn't inside portal - no collision.\n" +" return -1;\n" +" //}\n" +" }\n" +" };\n" +" \n" +" return 0;\n" +"};\n" +"#endif //B3_MPR_PENETRATION_H\n" +"#ifndef B3_CONTACT4DATA_H\n" +"#define B3_CONTACT4DATA_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"typedef struct b3Contact4Data b3Contact4Data_t;\n" +"struct b3Contact4Data\n" +"{\n" +" b3Float4 m_worldPosB[4];\n" +"// b3Float4 m_localPosA[4];\n" +"// b3Float4 m_localPosB[4];\n" +" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" +" unsigned short m_restituitionCoeffCmp;\n" +" unsigned short m_frictionCoeffCmp;\n" +" int m_batchIdx;\n" +" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" +" int m_bodyBPtrAndSignBit;\n" +" int m_childIndexA;\n" +" int m_childIndexB;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"};\n" +"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" +"{\n" +" return (int)contact->m_worldNormalOnB.w;\n" +"};\n" +"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" +"{\n" +" contact->m_worldNormalOnB.w = (float)numPoints;\n" +"};\n" +"#endif //B3_CONTACT4DATA_H\n" +"#define AppendInc(x, out) out = atomic_inc(x)\n" +"#define GET_NPOINTS(x) (x).m_worldNormalOnB.w\n" +"#ifdef cl_ext_atomic_counters_32\n" +" #pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" +"#else\n" +" #define counter32_t volatile __global int*\n" +"#endif\n" +"__kernel void mprPenetrationKernel( __global int4* pairs,\n" +" __global const b3RigidBodyData_t* rigidBodies, \n" +" __global const b3Collidable_t* collidables,\n" +" __global const b3ConvexPolyhedronData_t* convexShapes, \n" +" __global const float4* vertices,\n" +" __global float4* separatingNormals,\n" +" __global int* hasSeparatingAxis,\n" +" __global struct b3Contact4Data* restrict globalContactsOut,\n" +" counter32_t nGlobalContactsOut,\n" +" int contactCapacity,\n" +" int numPairs)\n" +"{\n" +" int i = get_global_id(0);\n" +" int pairIndex = i;\n" +" if (im_worldNormalOnB = -dirOut;//normal;\n" +" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" +" c->m_batchIdx = pairIndex;\n" +" int bodyA = pairs[pairIndex].x;\n" +" int bodyB = pairs[pairIndex].y;\n" +" c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0 ? -bodyA:bodyA;\n" +" c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0 ? -bodyB:bodyB;\n" +" c->m_childIndexA = -1;\n" +" c->m_childIndexB = -1;\n" +" //for (int i=0;im_worldPosB[0] = posOut;//localPoints[contactIdx[i]];\n" +" GET_NPOINTS(*c) = 1;//nContacts;\n" +" }\n" +" }\n" +" }\n" +"}\n" +; diff --git a/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp b/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp index c1b865bf8..cb57fbd5b 100644 --- a/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp +++ b/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp @@ -1,7 +1,7 @@ #include "b3GpuRaycast.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h" @@ -75,7 +75,7 @@ bool sphere_intersect(const b3Vector3& spherePos, b3Scalar radius, const b3Vect return false; } -bool rayConvex(const b3Vector3& rayFromLocal, const b3Vector3& rayToLocal, const b3ConvexPolyhedronCL& poly, +bool rayConvex(const b3Vector3& rayFromLocal, const b3Vector3& rayToLocal, const b3ConvexPolyhedronData& poly, const b3AlignedObjectArray& faces, float& hitFraction, b3Vector3& hitNormal) { float exitFraction = hitFraction; @@ -125,7 +125,7 @@ bool rayConvex(const b3Vector3& rayFromLocal, const b3Vector3& rayToLocal, const } void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray& rays, b3AlignedObjectArray& hitResults, - int numBodies,const struct b3RigidBodyCL* bodies, int numCollidables,const struct b3Collidable* collidables, const struct b3GpuNarrowPhaseInternalData* narrowphaseData) + int numBodies,const struct b3RigidBodyData* bodies, int numCollidables,const struct b3Collidable* collidables, const struct b3GpuNarrowPhaseInternalData* narrowphaseData) { // return castRays(rays,hitResults,numBodies,bodies,numCollidables,collidables); @@ -173,7 +173,7 @@ void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray& rays, b3A int shapeIndex = collidables[bodies[b].m_collidableIdx].m_shapeIndex; - const b3ConvexPolyhedronCL& poly = narrowphaseData->m_convexPolyhedra[shapeIndex]; + const b3ConvexPolyhedronData& poly = narrowphaseData->m_convexPolyhedra[shapeIndex]; if (rayConvex(rayFromLocal, rayToLocal,poly,narrowphaseData->m_convexFaces, hitFraction, hitNormal)) { hitBodyIndex = b; @@ -206,7 +206,7 @@ void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray& rays, b3A } ///todo: add some acceleration structure (AABBs, tree etc) void b3GpuRaycast::castRays(const b3AlignedObjectArray& rays, b3AlignedObjectArray& hitResults, - int numBodies,const struct b3RigidBodyCL* bodies, int numCollidables, const struct b3Collidable* collidables, const struct b3GpuNarrowPhaseInternalData* narrowphaseData) + int numBodies,const struct b3RigidBodyData* bodies, int numCollidables, const struct b3Collidable* collidables, const struct b3GpuNarrowPhaseInternalData* narrowphaseData) { //castRaysHost(rays,hitResults,numBodies,bodies,numCollidables,collidables,narrowphaseData); diff --git a/src/Bullet3OpenCL/Raycast/b3GpuRaycast.h b/src/Bullet3OpenCL/Raycast/b3GpuRaycast.h index 781a6414a..8b0cbd0be 100644 --- a/src/Bullet3OpenCL/Raycast/b3GpuRaycast.h +++ b/src/Bullet3OpenCL/Raycast/b3GpuRaycast.h @@ -18,40 +18,15 @@ public: virtual ~b3GpuRaycast(); void castRaysHost(const b3AlignedObjectArray& raysIn, b3AlignedObjectArray& hitResults, - int numBodies, const struct b3RigidBodyCL* bodies, int numCollidables, const struct b3Collidable* collidables, + int numBodies, const struct b3RigidBodyData* bodies, int numCollidables, const struct b3Collidable* collidables, const struct b3GpuNarrowPhaseInternalData* narrowphaseData); void castRays(const b3AlignedObjectArray& rays, b3AlignedObjectArray& hitResults, - int numBodies,const struct b3RigidBodyCL* bodies, int numCollidables, const struct b3Collidable* collidables, + int numBodies,const struct b3RigidBodyData* bodies, int numCollidables, const struct b3Collidable* collidables, const struct b3GpuNarrowPhaseInternalData* narrowphaseData ); -/* const b3OpenCLArray* bodyBuf, - b3OpenCLArray* contactOut, int& nContacts, - int maxContactCapacity, - const b3OpenCLArray& hostConvexData, - const b3OpenCLArray& vertices, - const b3OpenCLArray& uniqueEdges, - const b3OpenCLArray& faces, - const b3OpenCLArray& indices, - const b3OpenCLArray& gpuCollidables, - const b3OpenCLArray& gpuChildShapes, - const b3OpenCLArray& clAabbs, - b3OpenCLArray& worldVertsB1GPU, - b3OpenCLArray& clippingFacesOutGPU, - b3OpenCLArray& worldNormalsAGPU, - b3OpenCLArray& worldVertsA1GPU, - b3OpenCLArray& worldVertsB2GPU, - b3AlignedObjectArray& bvhData, - b3OpenCLArray* treeNodesGPU, - b3OpenCLArray* subTreesGPU, - b3OpenCLArray* bvhInfo, - int numObjects, - int maxTriConvexPairCapacity, - b3OpenCLArray& triangleConvexPairs, - int& numTriConvexPairsOut - */ }; diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp index f42fd9ba4..2189fd904 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp @@ -14,12 +14,12 @@ subject to the following restrictions: //Originally written by Erwin Coumans #include "b3GpuGenericConstraint.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include #include "Bullet3Common/b3Transform.h" -void b3GpuGenericConstraint::getInfo1 (unsigned int* info,const b3RigidBodyCL* bodies) +void b3GpuGenericConstraint::getInfo1 (unsigned int* info,const b3RigidBodyData* bodies) { switch (m_constraintType) { @@ -35,7 +35,7 @@ void b3GpuGenericConstraint::getInfo1 (unsigned int* info,const b3RigidBodyCL* b }; } -void getInfo2Point2Point(b3GpuGenericConstraint* constraint, b3GpuConstraintInfo2* info, const b3RigidBodyCL* bodies) +void getInfo2Point2Point(b3GpuGenericConstraint* constraint, b3GpuConstraintInfo2* info, const b3RigidBodyData* bodies) { b3Transform trA; trA.setIdentity(); @@ -120,7 +120,7 @@ void getInfo2Point2Point(b3GpuGenericConstraint* constraint, b3GpuConstraintInfo } -void b3GpuGenericConstraint::getInfo2 (b3GpuConstraintInfo2* info, const b3RigidBodyCL* bodies) +void b3GpuGenericConstraint::getInfo2 (b3GpuConstraintInfo2* info, const b3RigidBodyData* bodies) { switch (m_constraintType) { diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h b/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h index 3db24bc3c..14b3ba7fe 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h +++ b/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h @@ -17,7 +17,7 @@ subject to the following restrictions: #define B3_GPU_GENERIC_CONSTRAINT_H #include "Bullet3Common/b3Quaternion.h" -struct b3RigidBodyCL; +struct b3RigidBodyData; enum B3_CONSTRAINT_FLAGS { B3_CONSTRAINT_FLAG_ENABLED=1, @@ -121,10 +121,10 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3GpuGenericConstraint } ///internal method used by the constraint solver, don't use them directly - void getInfo1 (unsigned int* info,const b3RigidBodyCL* bodies); + void getInfo1 (unsigned int* info,const b3RigidBodyData* bodies); ///internal method used by the constraint solver, don't use them directly - void getInfo2 (b3GpuConstraintInfo2* info, const b3RigidBodyCL* bodies); + void getInfo2 (b3GpuConstraintInfo2* info, const b3RigidBodyData* bodies); }; diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp index 74fca91b5..b5b025efb 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp @@ -455,7 +455,7 @@ void setConstraint4( const b3Vector3& posA, const b3Vector3& linVelA, const b3Ve -void ContactToConstraintKernel(b3Contact4* gContact, b3RigidBodyCL* gBodies, b3InertiaCL* gShapes, b3GpuConstraint4* gConstraintOut, int nContacts, +void ContactToConstraintKernel(b3Contact4* gContact, b3RigidBodyData* gBodies, b3InertiaData* gShapes, b3GpuConstraint4* gConstraintOut, int nContacts, float dt, float positionDrift, float positionConstraintCoeff, int gIdx, b3AlignedObjectArray& bodyCount @@ -496,7 +496,7 @@ float positionConstraintCoeff, int gIdx, b3AlignedObjectArray& bod } -void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyCL* bodies,b3InertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,const b3JacobiSolverInfo& solverInfo) +void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,const b3JacobiSolverInfo& solverInfo) { B3_PROFILE("b3GpuJacobiContactSolver::solveGroup"); @@ -579,8 +579,8 @@ void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyCL* bodies,b3InertiaCL* float frictionCoeff = contactConstraints[i].getFrictionCoeff(); int aIdx = (int)contactConstraints[i].m_bodyA; int bIdx = (int)contactConstraints[i].m_bodyB; - b3RigidBodyCL& bodyA = bodies[aIdx]; - b3RigidBodyCL& bodyB = bodies[bIdx]; + b3RigidBodyData& bodyA = bodies[aIdx]; + b3RigidBodyData& bodyB = bodies[bIdx]; b3Vector3 zero = b3MakeVector3(0,0,0); @@ -589,7 +589,7 @@ void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyCL* bodies,b3InertiaCL* b3Vector3* dlvBPtr=&zero; b3Vector3* davBPtr=&zero; - if (bodyA.getInvMass()) + if (bodyA.m_invMass) { int bodyOffsetA = offsetSplitBodies[aIdx]; int constraintOffsetA = contactConstraintOffsets[i].x; @@ -598,7 +598,7 @@ void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyCL* bodies,b3InertiaCL* davAPtr = &deltaAngularVelocities[splitIndexA]; } - if (bodyB.getInvMass()) + if (bodyB.m_invMass) { int bodyOffsetB = offsetSplitBodies[bIdx]; int constraintOffsetB = contactConstraintOffsets[i].y; @@ -626,7 +626,7 @@ void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyCL* bodies,b3InertiaCL* //easy for (int i=0;i* bodies,b3OpenCLArray* inertias,b3OpenCLArray* manifoldPtr,const btJacobiSolverInfo& solverInfo) +//void b3GpuJacobiContactSolver::solveGroup(b3OpenCLArray* bodies,b3OpenCLArray* inertias,b3OpenCLArray* manifoldPtr,const btJacobiSolverInfo& solverInfo) { b3JacobiSolverInfo solverInfo; solverInfo.m_fixedBodyIndex = static0Index; @@ -947,12 +947,12 @@ void b3GpuJacobiContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_m #if 0 -void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray* bodiesGPU,b3OpenCLArray* inertiasGPU,b3OpenCLArray* manifoldPtrGPU,const btJacobiSolverInfo& solverInfo) +void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray* bodiesGPU,b3OpenCLArray* inertiasGPU,b3OpenCLArray* manifoldPtrGPU,const btJacobiSolverInfo& solverInfo) { - b3AlignedObjectArray bodiesCPU; + b3AlignedObjectArray bodiesCPU; bodiesGPU->copyToHost(bodiesCPU); - b3AlignedObjectArray inertiasCPU; + b3AlignedObjectArray inertiasCPU; inertiasGPU->copyToHost(inertiasCPU); b3AlignedObjectArray manifoldPtrCPU; manifoldPtrGPU->copyToHost(manifoldPtrCPU); @@ -1141,8 +1141,8 @@ void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray* bo float frictionCoeff = contactConstraints[i].getFrictionCoeff(); int aIdx = (int)contactConstraints[i].m_bodyA; int bIdx = (int)contactConstraints[i].m_bodyB; - b3RigidBodyCL& bodyA = bodiesCPU[aIdx]; - b3RigidBodyCL& bodyB = bodiesCPU[bIdx]; + b3RigidBodyData& bodyA = bodiesCPU[aIdx]; + b3RigidBodyData& bodyB = bodiesCPU[bIdx]; b3Vector3 zero(0,0,0); @@ -1151,7 +1151,7 @@ void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray* bo b3Vector3* dlvBPtr=&zero; b3Vector3* davBPtr=&zero; - if (bodyA.getInvMass()) + if (bodyA.m_invMass) { int bodyOffsetA = offsetSplitBodies[aIdx]; int constraintOffsetA = contactConstraintOffsets[i].x; @@ -1160,7 +1160,7 @@ void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray* bo davAPtr = &deltaAngularVelocities[splitIndexA]; } - if (bodyB.getInvMass()) + if (bodyB.m_invMass) { int bodyOffsetB = offsetSplitBodies[bIdx]; int constraintOffsetB = contactConstraintOffsets[i].y; @@ -1200,7 +1200,7 @@ void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray* bo //easy for (int i=0;i* bo float frictionCoeff = contactConstraints[i].getFrictionCoeff(); int aIdx = (int)contactConstraints[i].m_bodyA; int bIdx = (int)contactConstraints[i].m_bodyB; - b3RigidBodyCL& bodyA = bodiesCPU[aIdx]; - b3RigidBodyCL& bodyB = bodiesCPU[bIdx]; + b3RigidBodyData& bodyA = bodiesCPU[aIdx]; + b3RigidBodyData& bodyB = bodiesCPU[bIdx]; b3Vector3 zero(0,0,0); @@ -1273,7 +1273,7 @@ void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray* bo b3Vector3* dlvBPtr=&zero; b3Vector3* davBPtr=&zero; - if (bodyA.getInvMass()) + if (bodyA.m_invMass) { int bodyOffsetA = offsetSplitBodies[aIdx]; int constraintOffsetA = contactConstraintOffsets[i].x; @@ -1282,7 +1282,7 @@ void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray* bo davAPtr = &deltaAngularVelocities[splitIndexA]; } - if (bodyB.getInvMass()) + if (bodyB.m_invMass) { int bodyOffsetB = offsetSplitBodies[bIdx]; int constraintOffsetB = contactConstraintOffsets[i].y; @@ -1319,7 +1319,7 @@ void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray* bo //easy for (int i=0;i* bo //easy for (int i=0;i* gpuBodies,b3OpenCLArray* gpuInertias, int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + //b3Scalar solveGroup(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias, int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); //void solveGroup(btOpenCLArray* bodies,btOpenCLArray* inertias,btOpenCLArray* manifoldPtr,const btJacobiSolverInfo& solverInfo); //void solveGroupMixed(btOpenCLArray* bodies,btOpenCLArray* inertias,btOpenCLArray* manifoldPtr,const btJacobiSolverInfo& solverInfo); diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp index d086bc628..b512405d2 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp @@ -2,7 +2,7 @@ #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" -#include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" #include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h" #include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h" #include @@ -45,16 +45,16 @@ m_queue(queue) m_data->m_pBufContactOutCPU = new b3AlignedObjectArray(); m_data->m_pBufContactOutCPU->resize(config.m_maxBroadphasePairs); - m_data->m_bodyBufferCPU = new b3AlignedObjectArray(); + m_data->m_bodyBufferCPU = new b3AlignedObjectArray(); m_data->m_bodyBufferCPU->resize(config.m_maxConvexBodies); - m_data->m_inertiaBufferCPU = new b3AlignedObjectArray(); + m_data->m_inertiaBufferCPU = new b3AlignedObjectArray(); m_data->m_inertiaBufferCPU->resize(config.m_maxConvexBodies); m_data->m_pBufContactBuffersGPU[0] = new b3OpenCLArray(ctx,queue, config.m_maxContactCapacity,true); m_data->m_pBufContactBuffersGPU[1] = new b3OpenCLArray(ctx,queue, config.m_maxContactCapacity,true); - m_data->m_inertiaBufferGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexBodies,false); + m_data->m_inertiaBufferGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexBodies,false); m_data->m_collidablesGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexShapes); m_data->m_collidablesCPU.reserve(config.m_maxConvexShapes); @@ -63,14 +63,14 @@ m_queue(queue) //m_data->m_solverDataGPU = adl::Solver::allocate(ctx,queue, config.m_maxBroadphasePairs,false); - m_data->m_bodyBufferGPU = new b3OpenCLArray(ctx,queue, config.m_maxConvexBodies,false); + m_data->m_bodyBufferGPU = new b3OpenCLArray(ctx,queue, config.m_maxConvexBodies,false); m_data->m_convexFacesGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexShapes*config.m_maxFacesPerShape,false); m_data->m_convexFaces.reserve(config.m_maxConvexShapes*config.m_maxFacesPerShape); m_data->m_gpuChildShapes = new b3OpenCLArray(ctx,queue,config.m_maxCompoundChildShapes,false); - m_data->m_convexPolyhedraGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexShapes,false); + m_data->m_convexPolyhedraGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexShapes,false); m_data->m_convexPolyhedra.reserve(config.m_maxConvexShapes); m_data->m_uniqueEdgesGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexUniqueEdges,true); @@ -271,7 +271,7 @@ int b3GpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1); - b3ConvexPolyhedronCL& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1); + b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1); convex.mC = convexPtr->mC; convex.mE = convexPtr->mE; convex.m_extents= convexPtr->m_extents; @@ -673,7 +673,7 @@ int b3GpuNarrowPhase::registerConcaveMeshShape(b3AlignedObjectArray* m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1); - b3ConvexPolyhedronCL& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1); + b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1); convex.mC = b3MakeVector3(0,0,0); convex.mE = b3MakeVector3(0,0,0); convex.m_extents= b3MakeVector3(0,0,0); @@ -738,7 +738,7 @@ cl_mem b3GpuNarrowPhase::getBodiesGpu() return (cl_mem)m_data->m_bodyBufferGPU->getBufferCL(); } -const struct b3RigidBodyCL* b3GpuNarrowPhase::getBodiesCpu() const +const struct b3RigidBodyData* b3GpuNarrowPhase::getBodiesCpu() const { return &m_data->m_bodyBufferCPU->at(0); }; @@ -910,7 +910,7 @@ int b3GpuNarrowPhase::registerRigidBody(int collidableIndex, float mass, const f m_data->m_bodyBufferCPU->resize(m_data->m_numAcceleratedRigidBodies+1); - b3RigidBodyCL& body = m_data->m_bodyBufferCPU->at(m_data->m_numAcceleratedRigidBodies); + b3RigidBodyData& body = m_data->m_bodyBufferCPU->at(m_data->m_numAcceleratedRigidBodies); float friction = 1.f; float restitution = 0.f; @@ -940,7 +940,7 @@ int b3GpuNarrowPhase::registerRigidBody(int collidableIndex, float mass, const f m_data->m_bodyBufferGPU->copyFromHostPointer(&body,1,m_data->m_numAcceleratedRigidBodies); } - b3InertiaCL& shapeInfo = m_data->m_inertiaBufferCPU->at(m_data->m_numAcceleratedRigidBodies); + b3InertiaData& shapeInfo = m_data->m_inertiaBufferCPU->at(m_data->m_numAcceleratedRigidBodies); if (mass==0.f) { diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h index 6d315be6d..05ff3fd09 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h +++ b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h @@ -60,8 +60,8 @@ public: cl_mem getBodiesGpu(); - const struct b3RigidBodyCL* getBodiesCpu() const; - //struct b3RigidBodyCL* getBodiesCpu(); + const struct b3RigidBodyData* getBodiesCpu() const; + //struct b3RigidBodyData* getBodiesCpu(); int getNumBodiesGpu() const; diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h index 7b0dfe11b..8a7f1ea85 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h +++ b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h @@ -3,7 +3,7 @@ #define B3_GPU_NARROWPHASE_INTERNAL_DATA_H #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" -#include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" @@ -11,7 +11,7 @@ #include "Bullet3Common/b3AlignedObjectArray.h" #include "Bullet3Common/b3Vector3.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" #include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h" @@ -27,12 +27,12 @@ struct b3GpuNarrowPhaseInternalData { b3AlignedObjectArray* m_convexData; - b3AlignedObjectArray m_convexPolyhedra; + b3AlignedObjectArray m_convexPolyhedra; b3AlignedObjectArray m_uniqueEdges; b3AlignedObjectArray m_convexVertices; b3AlignedObjectArray m_convexIndices; - b3OpenCLArray* m_convexPolyhedraGPU; + b3OpenCLArray* m_convexPolyhedraGPU; b3OpenCLArray* m_uniqueEdgesGPU; b3OpenCLArray* m_convexVerticesGPU; b3OpenCLArray* m_convexIndicesGPU; @@ -60,11 +60,11 @@ struct b3GpuNarrowPhaseInternalData b3AlignedObjectArray* m_pBufContactOutCPU; - b3AlignedObjectArray* m_bodyBufferCPU; - b3OpenCLArray* m_bodyBufferGPU; + b3AlignedObjectArray* m_bodyBufferCPU; + b3OpenCLArray* m_bodyBufferGPU; - b3AlignedObjectArray* m_inertiaBufferCPU; - b3OpenCLArray* m_inertiaBufferGPU; + b3AlignedObjectArray* m_inertiaBufferCPU; + b3OpenCLArray* m_inertiaBufferGPU; int m_numAcceleratedShapes; int m_numAcceleratedRigidBodies; diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp index 334dc792e..4d14bc428 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp @@ -24,7 +24,7 @@ bool gpuBreakConstraints = true; #include "b3GpuPgsConstraintSolver.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h" #include @@ -72,8 +72,8 @@ struct b3GpuPgsJacobiSolverInternalData b3AlignedObjectArray m_cpuConstraintInfo1; b3AlignedObjectArray m_cpuConstraintRowOffsets; - b3AlignedObjectArray m_cpuBodies; - b3AlignedObjectArray m_cpuInertias; + b3AlignedObjectArray m_cpuBodies; + b3AlignedObjectArray m_cpuInertias; b3AlignedObjectArray m_cpuConstraints; @@ -85,7 +85,7 @@ struct b3GpuPgsJacobiSolverInternalData -static b3Transform getWorldTransform(b3RigidBodyCL* rb) +static b3Transform getWorldTransform(b3RigidBodyData* rb) { b3Transform newTrans; newTrans.setOrigin(rb->m_pos); @@ -93,24 +93,24 @@ static b3Transform getWorldTransform(b3RigidBodyCL* rb) return newTrans; } -static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaCL* inertia) +static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaData* inertia) { return inertia->m_invInertiaWorld; } -static const b3Vector3& getLinearVelocity(b3RigidBodyCL* rb) +static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb) { return rb->m_linVel; } -static const b3Vector3& getAngularVelocity(b3RigidBodyCL* rb) +static const b3Vector3& getAngularVelocity(b3RigidBodyData* rb) { return rb->m_angVel; } -b3Vector3 getVelocityInLocalPoint(b3RigidBodyCL* rb, const b3Vector3& rel_pos) +b3Vector3 getVelocityInLocalPoint(b3RigidBodyData* rb, const b3Vector3& rel_pos) { //we also calculate lin/ang velocity for kinematic objects return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos); @@ -204,7 +204,7 @@ void b3GpuPgsConstraintSolver::recomputeBatches() -b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, int numBodies, b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, int numBodies, b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) { B3_PROFILE("GPU solveGroupCacheFriendlySetup"); batchConstraints.resize(numConstraints); @@ -216,7 +216,7 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArraym_gpuBodies->resize(numBodies); m_gpuData->m_gpuBodies->copyFromHostPointer(bodies,numBodies); - b3OpenCLArray gpuInertias(m_gpuData->m_context,m_gpuData->m_queue); + b3OpenCLArray gpuInertias(m_gpuData->m_context,m_gpuData->m_queue); gpuInertias.resize(numBodies); gpuInertias.copyFromHostPointer(inertias,numBodies); */ @@ -245,7 +245,7 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArraym_cpuBodies[i]; + b3RigidBodyData& body = m_gpuData->m_cpuBodies[i]; b3GpuSolverBody& solverBody = m_tmpSolverBodyPool[i]; initSolverBody(i,&solverBody,&body); solverBody.m_originalBodyIndex = i; @@ -394,10 +394,10 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArraym_cpuConstraints[i]; - b3RigidBodyCL& rbA = m_gpuData->m_cpuBodies[ constraint.getRigidBodyA()]; + b3RigidBodyData& rbA = m_gpuData->m_cpuBodies[ constraint.getRigidBodyA()]; //b3RigidBody& rbA = constraint.getRigidBodyA(); // b3RigidBody& rbB = constraint.getRigidBodyB(); - b3RigidBodyCL& rbB = m_gpuData->m_cpuBodies[ constraint.getRigidBodyB()]; + b3RigidBodyData& rbB = m_gpuData->m_cpuBodies[ constraint.getRigidBodyB()]; @@ -407,7 +407,7 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArraym_deltaLinearVelocity.setValue(0.f,0.f,0.f); @@ -665,7 +665,7 @@ void b3GpuPgsConstraintSolver::initSolverBody(int bodyIndex, b3GpuSolverBody* so b3Assert(rb); // solverBody->m_worldTransform = getWorldTransform(rb); - solverBody->internalSetInvMass(b3MakeVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())); + solverBody->internalSetInvMass(b3MakeVector3(rb->m_invMass,rb->m_invMass,rb->m_invMass)); solverBody->m_originalBodyIndex = bodyIndex; solverBody->m_angularFactor = b3MakeVector3(1,1,1); solverBody->m_linearFactor = b3MakeVector3(1,1,1); @@ -991,7 +991,7 @@ inline int b3GpuPgsConstraintSolver::sortConstraintByBatch3( b3BatchConstraint* /// b3PgsJacobiSolver Sequentially applies impulses -b3Scalar b3GpuPgsConstraintSolver::solveGroup(b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, +b3Scalar b3GpuPgsConstraintSolver::solveGroup(b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, int numBodies, b3OpenCLArray* gpuConstraints,int numConstraints, const b3ContactSolverInfo& infoGlobal) { @@ -1007,7 +1007,7 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroup(b3OpenCLArray* gpuB return 0.f; } -void b3GpuPgsConstraintSolver::solveJoints(int numBodies, b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, +void b3GpuPgsConstraintSolver::solveJoints(int numBodies, b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, int numConstraints, b3OpenCLArray* gpuConstraints) { b3ContactSolverInfo infoGlobal; @@ -1026,10 +1026,10 @@ void b3GpuPgsConstraintSolver::solveJoints(int numBodies, b3OpenCLArray testBodies; +//b3AlignedObjectArray testBodies; -b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias,int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias,int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) { B3_PROFILE("solveGroupCacheFriendlyFinish"); int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); @@ -1113,8 +1113,8 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArraym_cpuBodies[bodyIndex]; - if (body->getInvMass()) + b3RigidBodyData* body = &m_gpuData->m_cpuBodies[bodyIndex]; + if (body->m_invMass) { if (infoGlobal.m_splitImpulse) m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h b/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h index fef4b6003..ec0e3f73d 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h +++ b/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h @@ -27,8 +27,8 @@ class b3Dispatcher; #include "b3GpuSolverBody.h" #include "b3GpuSolverConstraint.h" #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" -struct b3RigidBodyCL; -struct b3InertiaCL; +struct b3RigidBodyData; +struct b3InertiaData; #include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" #include "b3GpuGenericConstraint.h" @@ -55,20 +55,20 @@ protected: int m_numSplitImpulseRecoveries; -// int getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,b3InertiaCL* inertias); - void initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyCL* rb); +// int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias); + void initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyData* rb); public: b3GpuPgsConstraintSolver (cl_context ctx, cl_device_id device, cl_command_queue queue,bool usePgs); virtual~b3GpuPgsConstraintSolver (); virtual b3Scalar solveGroupCacheFriendlyIterations(b3OpenCLArray* gpuConstraints1,int numConstraints,const b3ContactSolverInfo& infoGlobal); - virtual b3Scalar solveGroupCacheFriendlySetup(b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); - b3Scalar solveGroupCacheFriendlyFinish(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias,int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + virtual b3Scalar solveGroupCacheFriendlySetup(b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + b3Scalar solveGroupCacheFriendlyFinish(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias,int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); - b3Scalar solveGroup(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias, int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); - void solveJoints(int numBodies, b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, + b3Scalar solveGroup(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias, int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + void solveJoints(int numBodies, b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, int numConstraints, b3OpenCLArray* gpuConstraints); int sortConstraintByBatch3( struct b3BatchConstraint* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies); diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp index 5c5acb1fe..ba9fffa50 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp @@ -82,8 +82,8 @@ struct b3GpuBatchingPgsSolverInternalData b3OpenCLArray* m_sortDataBuffer; b3OpenCLArray* m_contactBuffer; - b3OpenCLArray* m_bodyBufferGPU; - b3OpenCLArray* m_inertiaBufferGPU; + b3OpenCLArray* m_bodyBufferGPU; + b3OpenCLArray* m_inertiaBufferGPU; b3OpenCLArray* m_pBufContactOutGPU; b3OpenCLArray* m_pBufContactOutGPUCopy; @@ -111,8 +111,8 @@ b3GpuPgsContactSolver::b3GpuPgsContactSolver(cl_context ctx,cl_device_id device, m_data->m_pairCapacity = pairCapacity; m_data->m_nIterations = 4; m_data->m_batchSizesGpu = new b3OpenCLArray(ctx,q); - m_data->m_bodyBufferGPU = new b3OpenCLArray(ctx,q); - m_data->m_inertiaBufferGPU = new b3OpenCLArray(ctx,q); + m_data->m_bodyBufferGPU = new b3OpenCLArray(ctx,q); + m_data->m_inertiaBufferGPU = new b3OpenCLArray(ctx,q); m_data->m_pBufContactOutGPU = new b3OpenCLArray(ctx,q); m_data->m_pBufContactOutGPUCopy = new b3OpenCLArray(ctx,q); @@ -293,7 +293,7 @@ struct b3ConstraintCfg -void b3GpuPgsContactSolver::solveContactConstraintBatchSizes( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, +void b3GpuPgsContactSolver::solveContactConstraintBatchSizes( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches,int numIterations, const b3AlignedObjectArray* batchSizes)//const b3OpenCLArray* gpuBatchSizes) { B3_PROFILE("solveContactConstraintBatchSizes"); @@ -353,7 +353,7 @@ void b3GpuPgsContactSolver::solveContactConstraintBatchSizes( const b3OpenCLArr } } -void b3GpuPgsContactSolver::solveContactConstraint( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, +void b3GpuPgsContactSolver::solveContactConstraint( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches,int numIterations, const b3AlignedObjectArray* batchSizes)//,const b3OpenCLArray* gpuBatchSizes) { @@ -582,7 +582,7 @@ static const int gridTable8x8[] = #define USE_4x4_GRID 1 -void SetSortDataCPU(b3Contact4* gContact, b3RigidBodyCL* gBodies, b3SortData* gSortDataOut, int nContacts,float scale,const b3Int4& nSplit,int staticIdx) +void SetSortDataCPU(b3Contact4* gContact, b3RigidBodyData* gBodies, b3SortData* gSortDataOut, int nContacts,float scale,const b3Int4& nSplit,int staticIdx) { for (int gIdx=0;gIdx* bodyBuf = m_data->m_bodyBufferGPU; + b3OpenCLArray* bodyBuf = m_data->m_bodyBufferGPU; void* additionalData = 0;//m_data->m_frictionCGPU; - const b3OpenCLArray* shapeBuf = m_data->m_inertiaBufferGPU; + const b3OpenCLArray* shapeBuf = m_data->m_inertiaBufferGPU; b3OpenCLArray* contactConstraintOut = m_data->m_contactCGPU; int nContacts = nContactOut; @@ -795,12 +795,12 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem - const b3OpenCLArray* bodyNative = bodyBuf; + const b3OpenCLArray* bodyNative = bodyBuf; { - //b3OpenCLArray* bodyNative = b3OpenCLArrayUtils::map( data->m_device, bodyBuf ); + //b3OpenCLArray* bodyNative = b3OpenCLArrayUtils::map( data->m_device, bodyBuf ); //b3OpenCLArray* contactNative = b3OpenCLArrayUtils::map( data->m_device, contactsIn ); const int sortAlignment = 512; // todo. get this out of sort @@ -855,7 +855,7 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem b3AlignedObjectArray contactCPU; m_data->m_pBufContactOutGPU->copyToHost(contactCPU); - b3AlignedObjectArray bodiesCPU; + b3AlignedObjectArray bodiesCPU; bodyBuf->copyToHost(bodiesCPU); float scale = 1.f/csCfg.m_batchCellSize; b3Int4 nSplit; diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h b/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h index 642523126..98e2a5b8c 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h +++ b/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h @@ -4,7 +4,7 @@ #include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" #include "b3GpuConstraint4.h" @@ -24,10 +24,10 @@ protected: - void solveContactConstraintBatchSizes( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, + void solveContactConstraintBatchSizes( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches, int numIterations, const b3AlignedObjectArray* batchSizes);//const b3OpenCLArray* gpuBatchSizes); - void solveContactConstraint( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, + void solveContactConstraint( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches, int numIterations, const b3AlignedObjectArray* batchSizes);//const b3OpenCLArray* gpuBatchSizes); public: diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp index a9a79a9c5..8b07fedb2 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp @@ -48,7 +48,7 @@ bool gIntegrateOnCpu = false; #include "b3GpuJacobiContactSolver.h" #endif //TEST_OTHER_GPU_SOLVER -#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" #include "Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h" @@ -317,9 +317,9 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) //solve constraints - b3OpenCLArray gpuBodies(m_data->m_context,m_data->m_queue,0,true); + b3OpenCLArray gpuBodies(m_data->m_context,m_data->m_queue,0,true); gpuBodies.setFromOpenCLBuffer(m_data->m_narrowphase->getBodiesGpu(),m_data->m_narrowphase->getNumRigidBodies()); - b3OpenCLArray gpuInertias(m_data->m_context,m_data->m_queue,0,true); + b3OpenCLArray gpuInertias(m_data->m_context,m_data->m_queue,0,true); gpuInertias.setFromOpenCLBuffer(m_data->m_narrowphase->getBodyInertiasGpu(),m_data->m_narrowphase->getNumRigidBodies()); b3OpenCLArray gpuContacts(m_data->m_context,m_data->m_queue,0,true); gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(),m_data->m_narrowphase->getNumContactsGpu()); @@ -340,9 +340,9 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) m_data->m_gpuSolver->solveJoints(m_data->m_narrowphase->getNumRigidBodies(),&gpuBodies,&gpuInertias,numJoints, m_data->m_gpuConstraints); } else { - b3AlignedObjectArray hostBodies; + b3AlignedObjectArray hostBodies; gpuBodies.copyToHost(hostBodies); - b3AlignedObjectArray hostInertias; + b3AlignedObjectArray hostInertias; gpuInertias.copyToHost(hostInertias); b3TypedConstraint** joints = numJoints? &m_data->m_joints[0] : 0; @@ -366,8 +366,8 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) bool forceHost = false; if (forceHost) { - b3AlignedObjectArray hostBodies; - b3AlignedObjectArray hostInertias; + b3AlignedObjectArray hostBodies; + b3AlignedObjectArray hostInertias; b3AlignedObjectArray hostContacts; { @@ -399,9 +399,9 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) } } else { - b3AlignedObjectArray hostBodies; + b3AlignedObjectArray hostBodies; gpuBodies.copyToHost(hostBodies); - b3AlignedObjectArray hostInertias; + b3AlignedObjectArray hostInertias; gpuInertias.copyToHost(hostInertias); b3AlignedObjectArray hostContacts; gpuContacts.copyToHost(hostContacts); diff --git a/src/Bullet3OpenCL/RigidBody/b3Solver.cpp b/src/Bullet3OpenCL/RigidBody/b3Solver.cpp index de4aa794a..5aa7eb70b 100644 --- a/src/Bullet3OpenCL/RigidBody/b3Solver.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3Solver.cpp @@ -359,7 +359,7 @@ void solveContact(b3GpuConstraint4& cs, struct SolveTask// : public ThreadPool::Task { - SolveTask(b3AlignedObjectArray& bodies, b3AlignedObjectArray& shapes, b3AlignedObjectArray& constraints, + SolveTask(b3AlignedObjectArray& bodies, b3AlignedObjectArray& shapes, b3AlignedObjectArray& constraints, int start, int nConstraints,int maxNumBatches,b3AlignedObjectArray* wgUsedBodies, int curWgidx, b3AlignedObjectArray* batchSizes, int cellIndex) : m_bodies( bodies ), m_shapes( shapes ), m_constraints( constraints ), m_start( start ), m_nConstraints( nConstraints ), m_solveFriction( true ),m_maxNumBatches(maxNumBatches), @@ -388,8 +388,8 @@ struct SolveTask// : public ThreadPool::Task int aIdx = (int)m_constraints[i].m_bodyA; int bIdx = (int)m_constraints[i].m_bodyB; int localBatch = m_constraints[i].m_batchIdx; - b3RigidBodyCL& bodyA = m_bodies[aIdx]; - b3RigidBodyCL& bodyB = m_bodies[bIdx]; + b3RigidBodyData& bodyA = m_bodies[aIdx]; + b3RigidBodyData& bodyB = m_bodies[bIdx]; if( !m_solveFriction ) { @@ -439,8 +439,8 @@ struct SolveTask// : public ThreadPool::Task int aIdx = (int)m_constraints[i].m_bodyA; int bIdx = (int)m_constraints[i].m_bodyB; int localBatch = m_constraints[i].m_batchIdx; - b3RigidBodyCL& bodyA = m_bodies[aIdx]; - b3RigidBodyCL& bodyB = m_bodies[bIdx]; + b3RigidBodyData& bodyA = m_bodies[aIdx]; + b3RigidBodyData& bodyB = m_bodies[bIdx]; if( !m_solveFriction ) { @@ -479,8 +479,8 @@ struct SolveTask// : public ThreadPool::Task } - b3AlignedObjectArray& m_bodies; - b3AlignedObjectArray& m_shapes; + b3AlignedObjectArray& m_bodies; + b3AlignedObjectArray& m_shapes; b3AlignedObjectArray& m_constraints; b3AlignedObjectArray* m_batchSizes; int m_cellIndex; @@ -492,7 +492,7 @@ struct SolveTask// : public ThreadPool::Task }; -void b3Solver::solveContactConstraintHost( b3OpenCLArray* bodyBuf, b3OpenCLArray* shapeBuf, +void b3Solver::solveContactConstraintHost( b3OpenCLArray* bodyBuf, b3OpenCLArray* shapeBuf, b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches,b3AlignedObjectArray* batchSizes) { @@ -541,9 +541,9 @@ void b3Solver::solveContactConstraintHost( b3OpenCLArray* bodyBu } #endif - b3AlignedObjectArray bodyNative; + b3AlignedObjectArray bodyNative; bodyBuf->copyToHost(bodyNative); - b3AlignedObjectArray shapeNative; + b3AlignedObjectArray shapeNative; shapeBuf->copyToHost(shapeNative); b3AlignedObjectArray constraintNative; constraint->copyToHost(constraintNative); @@ -674,8 +674,8 @@ void b3Solver::solveContactConstraintHost( b3OpenCLArray* bodyBu } -void checkConstraintBatch(const b3OpenCLArray* bodyBuf, - const b3OpenCLArray* shapeBuf, +void checkConstraintBatch(const b3OpenCLArray* bodyBuf, + const b3OpenCLArray* shapeBuf, b3OpenCLArray* constraint, b3OpenCLArray* m_numConstraints, b3OpenCLArray* m_offsets, @@ -751,7 +751,7 @@ void checkConstraintBatch(const b3OpenCLArray* bodyBuf, static bool verify=false; -void b3Solver::solveContactConstraint( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, +void b3Solver::solveContactConstraint( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches) { @@ -926,8 +926,8 @@ void b3Solver::solveContactConstraint( const b3OpenCLArray* body } -void b3Solver::convertToConstraints( const b3OpenCLArray* bodyBuf, - const b3OpenCLArray* shapeBuf, +void b3Solver::convertToConstraints( const b3OpenCLArray* bodyBuf, + const b3OpenCLArray* shapeBuf, b3OpenCLArray* contactsIn, b3OpenCLArray* contactCOut, void* additionalData, int nContacts, const ConstraintCfg& cfg ) { @@ -952,13 +952,13 @@ void b3Solver::convertToConstraints( const b3OpenCLArray* bodyBuf if (gConvertConstraintOnCpu) { - b3AlignedObjectArray gBodies; + b3AlignedObjectArray gBodies; bodyBuf->copyToHost(gBodies); b3AlignedObjectArray gContact; contactsIn->copyToHost(gContact); - b3AlignedObjectArray gShapes; + b3AlignedObjectArray gShapes; shapeBuf->copyToHost(gShapes); b3AlignedObjectArray gConstraintOut; @@ -1021,7 +1021,7 @@ void b3Solver::convertToConstraints( const b3OpenCLArray* bodyBuf } /* -void b3Solver::sortContacts( const b3OpenCLArray* bodyBuf, +void b3Solver::sortContacts( const b3OpenCLArray* bodyBuf, b3OpenCLArray* contactsIn, void* additionalData, int nContacts, const b3Solver::ConstraintCfg& cfg ) { diff --git a/src/Bullet3OpenCL/RigidBody/b3Solver.h b/src/Bullet3OpenCL/RigidBody/b3Solver.h index 16cf16731..ce77c798d 100644 --- a/src/Bullet3OpenCL/RigidBody/b3Solver.h +++ b/src/Bullet3OpenCL/RigidBody/b3Solver.h @@ -20,7 +20,7 @@ subject to the following restrictions: #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" #include "b3GpuConstraint4.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" #include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h" @@ -103,15 +103,15 @@ class b3Solver : public b3SolverBase virtual ~b3Solver(); - void solveContactConstraint( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* inertiaBuf, + void solveContactConstraint( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* inertiaBuf, b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches); - void solveContactConstraintHost( b3OpenCLArray* bodyBuf, b3OpenCLArray* shapeBuf, + void solveContactConstraintHost( b3OpenCLArray* bodyBuf, b3OpenCLArray* shapeBuf, b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches, b3AlignedObjectArray* batchSizes); - void convertToConstraints( const b3OpenCLArray* bodyBuf, - const b3OpenCLArray* shapeBuf, + void convertToConstraints( const b3OpenCLArray* bodyBuf, + const b3OpenCLArray* shapeBuf, b3OpenCLArray* contactsIn, b3OpenCLArray* contactCOut, void* additionalData, int nContacts, const ConstraintCfg& cfg );