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
This commit is contained in:
erwincoumans
@@ -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),
|
||||
|
||||
@@ -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};
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -20,7 +20,7 @@ subject to the following restrictions:
|
||||
#include "Bullet3Common/b3Quaternion.h"
|
||||
#include "Bullet3Common/b3HashMap.h"
|
||||
|
||||
//#include "b3ConvexPolyhedronCL.h"
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -20,7 +20,7 @@ subject to the following restrictions:
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include "Bullet3Common/b3Transform.h"
|
||||
|
||||
//#include "b3ConvexPolyhedronCL.h"
|
||||
|
||||
|
||||
|
||||
struct b3MyFace
|
||||
|
||||
@@ -60,8 +60,8 @@ public:
|
||||
|
||||
|
||||
|
||||
const struct b3RigidBodyCL* getBodiesCpu() const;
|
||||
//struct b3RigidBodyCL* getBodiesCpu();
|
||||
const struct b3RigidBodyData* getBodiesCpu() const;
|
||||
//struct b3RigidBodyData* getBodiesCpu();
|
||||
|
||||
int getNumBodiesGpu() const;
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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];
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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;e0<hullA->m_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;e1<hullB->m_numUniqueEdges;e1++)
|
||||
}
|
||||
|
||||
|
||||
if (searchAllEdgeEdge)
|
||||
{
|
||||
for(int e0=0;e0<hullA->m_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;e1<hullB->m_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(Max0<Min1 || Max1<Min0)
|
||||
return false;
|
||||
if(Max0<Min1 || Max1<Min0)
|
||||
return false;
|
||||
|
||||
float d0 = Max0 - Min1;
|
||||
float d1 = Max1 - Min0;
|
||||
dist = d0<d1 ? d0:d1;
|
||||
result = true;
|
||||
float d0 = Max0 - Min1;
|
||||
float d1 = Max1 - Min0;
|
||||
dist = d0<d1 ? d0:d1;
|
||||
result = true;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if(dist<*dmin)
|
||||
if(dist<*dmin)
|
||||
{
|
||||
*dmin = dist;
|
||||
*sep = crossje;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
int numDirections = sizeof(unitSphere162)/sizeof(b3Vector3);
|
||||
//printf("numDirections =%d\n",numDirections );
|
||||
|
||||
|
||||
for(int i=0;i<numDirections;i++)
|
||||
{
|
||||
b3Float4 crossje = unitSphere162[i];
|
||||
{
|
||||
//if (b3Dot(DeltaC2,crossje)>0)
|
||||
{
|
||||
*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<Min1 || Max1<Min0)
|
||||
return false;
|
||||
|
||||
float d0 = Max0 - Min1;
|
||||
float d1 = Max1 - Min0;
|
||||
dist = d0<d1 ? d0:d1;
|
||||
result = true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
if(dist<*dmin)
|
||||
{
|
||||
*dmin = dist;
|
||||
*sep = crossje;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -536,7 +772,7 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
|
||||
DeltaC2,
|
||||
verticesA,uniqueEdgesA,facesA,indicesA,
|
||||
vertices,uniqueEdges,faces,indices,
|
||||
&sepAxis,&dmin);
|
||||
&sepAxis,&dmin,true);
|
||||
|
||||
if (!sepEE)
|
||||
{
|
||||
|
||||
@@ -0,0 +1,919 @@
|
||||
|
||||
/***
|
||||
* ---------------------------------
|
||||
* Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
|
||||
*
|
||||
* 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 <http://www.opensource.org/licenses/bsd-license.php>.
|
||||
* 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;i<B3_MPR_MAX_ITERATIONS;i++)
|
||||
//while (1)
|
||||
{
|
||||
// compute direction outside the portal (from v0 throught v1,v2,v3
|
||||
// face)
|
||||
b3PortalDir(portal, &dir);
|
||||
|
||||
// test if origin is inside the portal
|
||||
if (portalEncapsulesOrigin(portal, &dir))
|
||||
return 0;
|
||||
|
||||
// get next support point
|
||||
|
||||
b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);
|
||||
|
||||
|
||||
// test if v4 can expand portal to contain origin and if portal
|
||||
// expanding doesn't reach given tolerance
|
||||
if (!portalCanEncapsuleOrigin(portal, &v4, &dir)
|
||||
|| portalReachTolerance(portal, &v4, &dir))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// v1-v2-v3 triangle must be rearranged to face outside Minkowski
|
||||
// difference (direction from v0).
|
||||
b3ExpandPortal(portal, &v4);
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
static void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
|
||||
{
|
||||
|
||||
b3Float4 zero = b3MakeFloat4(0,0,0,0);
|
||||
b3Float4* b3mpr_vec3_origin = &zero;
|
||||
|
||||
b3Float4 dir;
|
||||
size_t i;
|
||||
float b[4], sum, inv;
|
||||
b3Float4 vec, p1, p2;
|
||||
|
||||
b3PortalDir(portal, &dir);
|
||||
|
||||
// use barycentric coordinates of tetrahedron to find origin
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
|
||||
&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<B3_MPR_MAX_ITERATIONS;i++)
|
||||
//while (1)
|
||||
{
|
||||
// compute portal direction and obtain next support point
|
||||
b3PortalDir(portal, &dir);
|
||||
|
||||
b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);
|
||||
|
||||
|
||||
// reached tolerance -> 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
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -7,6 +7,8 @@ struct MyTest
|
||||
};
|
||||
|
||||
#ifdef __cplusplus
|
||||
//#define b3ConstArray(a) const b3AlignedObjectArray<a>&
|
||||
#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
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
|
||||
#include "b3FixedConstraint.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Common/b3TransformUtil.h"
|
||||
#include <new>
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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)
|
||||
{
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -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 <stdio.h>
|
||||
|
||||
|
||||
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);
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -15,7 +15,7 @@ subject to the following restrictions:
|
||||
|
||||
|
||||
#include "b3Point2PointConstraint.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
#include <new>
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -597,7 +597,7 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
|
||||
|
||||
if (disableBinaryCaching)
|
||||
{
|
||||
kernelSourceOrg = 0;
|
||||
//kernelSourceOrg = 0;
|
||||
}
|
||||
|
||||
cl_program m_cpProgram=0;
|
||||
|
||||
@@ -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 <string.h>//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<b3Vector3> 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<b3Vector3> 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<b3RigidBodyCL>* bodyBuf,
|
||||
const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
|
||||
b3AlignedObjectArray<b3Contact4>* globalContactOut,
|
||||
int& nContacts,
|
||||
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronCL>& hostConvexDataA,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronCL>& hostConvexDataB,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
|
||||
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& 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<b3Int4>& pairs,
|
||||
int pairIndex,
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
const b3AlignedObjectArray<b3RigidBodyCL>& rigidBodies,
|
||||
const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
|
||||
const b3AlignedObjectArray<b3Collidable>& collidables,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronCL>& convexShapes,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
|
||||
const b3AlignedObjectArray<b3Vector3>& convexVertices,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdges,
|
||||
const b3AlignedObjectArray<int>& convexIndices,
|
||||
@@ -2572,7 +2599,11 @@ int computeContactConvexConvex( b3AlignedObjectArray<b3Int4>& pairs,
|
||||
b3AlignedObjectArray<b3Contact4>& globalContactsOut,
|
||||
int& nGlobalContactsOut,
|
||||
int maxContactCapacity,
|
||||
const b3AlignedObjectArray<b3Contact4>& oldContacts
|
||||
b3AlignedObjectArray<int>&hostHasSepAxis,
|
||||
b3AlignedObjectArray<b3Vector3>&hostSepAxis
|
||||
|
||||
|
||||
//,const b3AlignedObjectArray<b3Contact4>& oldContacts
|
||||
)
|
||||
{
|
||||
int contactIndex = -1;
|
||||
@@ -2629,10 +2660,10 @@ int computeContactConvexConvex( b3AlignedObjectArray<b3Int4>& 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<b3Int4>& 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<b3RigidBodyCL>& rigidBodies,
|
||||
const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
|
||||
const b3AlignedObjectArray<b3Collidable>& collidables,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronCL>& convexShapes,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
|
||||
const b3AlignedObjectArray<b3Vector3>& convexVertices,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdges,
|
||||
const b3AlignedObjectArray<int>& 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<b3Int4>* pairs, int nPairs,
|
||||
const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
|
||||
const b3OpenCLArray<b3RigidBodyData>* bodyBuf,
|
||||
b3OpenCLArray<b3Contact4>* contactOut, int& nContacts,
|
||||
const b3OpenCLArray<b3Contact4>* oldContacts,
|
||||
int maxContactCapacity,
|
||||
int compoundPairCapacity,
|
||||
const b3OpenCLArray<b3ConvexPolyhedronCL>& convexData,
|
||||
const b3OpenCLArray<b3ConvexPolyhedronData>& convexData,
|
||||
const b3OpenCLArray<b3Vector3>& gpuVertices,
|
||||
const b3OpenCLArray<b3Vector3>& gpuUniqueEdges,
|
||||
const b3OpenCLArray<b3GpuFace>& gpuFaces,
|
||||
@@ -2822,6 +2860,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
int& numTriConvexPairsOut
|
||||
)
|
||||
{
|
||||
myframecount++;
|
||||
|
||||
if (!nPairs)
|
||||
return;
|
||||
|
||||
@@ -2846,12 +2886,12 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
b3AlignedObjectArray<b3Int4> hostPairs;
|
||||
pairs->copyToHost(hostPairs);
|
||||
|
||||
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
|
||||
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
||||
bodyBuf->copyToHost(hostBodyBuf);
|
||||
|
||||
|
||||
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronCL> hostConvexData;
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
|
||||
convexData.copyToHost(hostConvexData);
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> hostVertices;
|
||||
@@ -2962,8 +3002,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
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<b3Int4>*
|
||||
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);
|
||||
/*
|
||||
b3AlignedObjectArray<int>hostHasSepAxis;
|
||||
m_hasSeparatingNormals.copyToHost(hostHasSepAxis);
|
||||
b3AlignedObjectArray<b3Vector3>hostSepAxis;
|
||||
m_sepNormals.copyToHost(hostSepAxis);
|
||||
*/
|
||||
nContacts = m_totalContactsOut.at(0);
|
||||
contactOut->resize(nContacts);
|
||||
//printf("nContacts (after 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<b3Int4>*
|
||||
else
|
||||
{
|
||||
|
||||
B3_PROFILE("findSeparatingAxisKernel CPU");
|
||||
|
||||
|
||||
b3AlignedObjectArray<b3Int4> hostPairs;
|
||||
pairs->copyToHost(hostPairs);
|
||||
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
|
||||
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
||||
bodyBuf->copyToHost(hostBodyBuf);
|
||||
|
||||
b3AlignedObjectArray<b3Collidable> hostCollidables;
|
||||
@@ -3163,7 +3257,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
|
||||
gpuChildShapes.copyToHost(cpuChildShapes);
|
||||
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronCL> hostConvexShapeData;
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexShapeData;
|
||||
convexData.copyToHost(hostConvexShapeData);
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> hostVertices;
|
||||
@@ -3181,6 +3275,15 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
|
||||
b3AlignedObjectArray<int> hostIndices;
|
||||
gpuIndices.copyToHost(hostIndices);
|
||||
|
||||
b3AlignedObjectArray<b3Contact4> hostContacts;
|
||||
if (nContacts)
|
||||
{
|
||||
contactOut->copyToHost(hostContacts);
|
||||
}
|
||||
hostContacts.resize(maxContactCapacity);
|
||||
int nGlobalContactsOut = nContacts;
|
||||
|
||||
|
||||
for (int i=0;i<nPairs;i++)
|
||||
{
|
||||
@@ -3215,41 +3318,237 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
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<b3Int4>*
|
||||
b3AlignedObjectArray<b3Int4> hostPairs;
|
||||
pairs->copyToHost(hostPairs);
|
||||
|
||||
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
|
||||
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
||||
bodyBuf->copyToHost(hostBodyBuf);
|
||||
|
||||
|
||||
@@ -3350,7 +3649,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
|
||||
gpuChildShapes.copyToHost(cpuChildShapes);
|
||||
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronCL> hostConvexData;
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
|
||||
convexData.copyToHost(hostConvexData);
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> hostVertices;
|
||||
@@ -3537,7 +3836,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
{
|
||||
b3AlignedObjectArray<b3Int4> hostPairs;
|
||||
pairs->copyToHost(hostPairs);
|
||||
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
|
||||
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
||||
bodyBuf->copyToHost(hostBodyBuf);
|
||||
b3AlignedObjectArray<b3Collidable> hostCollidables;
|
||||
gpuCollidables.copyToHost(hostCollidables);
|
||||
@@ -3750,14 +4049,14 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
|
||||
triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost);
|
||||
//triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity);
|
||||
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
|
||||
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
||||
bodyBuf->copyToHost(hostBodyBuf);
|
||||
b3AlignedObjectArray<b3Collidable> hostCollidables;
|
||||
gpuCollidables.copyToHost(hostCollidables);
|
||||
b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
|
||||
clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
|
||||
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronCL> hostConvexData;
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
|
||||
convexData.copyToHost(hostConvexData);
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> hostVertices;
|
||||
@@ -3887,7 +4186,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
|
||||
//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<b3Int4>*
|
||||
volatile int nGlobalContactsOut = nContacts;
|
||||
b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
|
||||
triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost);
|
||||
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
|
||||
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
||||
bodyBuf->copyToHost(hostBodyBuf);
|
||||
|
||||
b3AlignedObjectArray<int>concaveHasSeparatingNormalsCPU;
|
||||
@@ -4123,7 +4422,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
|
||||
//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<b3Int4>*
|
||||
float minDist = -1e30f;
|
||||
float maxDist = 0.02f;
|
||||
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronCL> hostConvexData;
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
|
||||
convexData.copyToHost(hostConvexData);
|
||||
b3AlignedObjectArray<b3Collidable> hostCollidables;
|
||||
gpuCollidables.copyToHost(hostCollidables);
|
||||
@@ -4194,7 +4493,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
|
||||
b3AlignedObjectArray<b3Int4> hostPairs;
|
||||
pairs->copyToHost(hostPairs);
|
||||
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
|
||||
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
||||
bodyBuf->copyToHost(hostBodyBuf);
|
||||
|
||||
|
||||
@@ -4267,6 +4566,9 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
if (clipConvexFacesAndFindContactsCPU)
|
||||
{
|
||||
|
||||
//b3AlignedObjectArray<b3Int4> hostPairs;
|
||||
//pairs->copyToHost(hostPairs);
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> hostSepNormals;
|
||||
m_sepNormals.copyToHost(hostSepNormals);
|
||||
b3AlignedObjectArray<int> hostHasSepAxis;
|
||||
@@ -4343,8 +4645,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
}
|
||||
|
||||
{
|
||||
// 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<b3Int4>*
|
||||
volatile int nGlobalContactsOut = nContacts;
|
||||
b3AlignedObjectArray<b3Int4> hostPairs;
|
||||
pairs->copyToHost(hostPairs);
|
||||
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
|
||||
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
||||
bodyBuf->copyToHost(hostBodyBuf);
|
||||
b3AlignedObjectArray<b3Vector3> hostSepNormals;
|
||||
m_sepNormals.copyToHost(hostSepNormals);
|
||||
|
||||
@@ -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<b3Int4>* pairs, int nPairs,
|
||||
const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
|
||||
const b3OpenCLArray<b3RigidBodyData>* bodyBuf,
|
||||
b3OpenCLArray<b3Contact4>* contactOut, int& nContacts,
|
||||
const b3OpenCLArray<b3Contact4>* oldContacts,
|
||||
int maxContactCapacity,
|
||||
int compoundPairCapacity,
|
||||
const b3OpenCLArray<b3ConvexPolyhedronCL>& hostConvexData,
|
||||
const b3OpenCLArray<b3ConvexPolyhedronData>& hostConvexData,
|
||||
const b3OpenCLArray<b3Vector3>& vertices,
|
||||
const b3OpenCLArray<b3Vector3>& uniqueEdges,
|
||||
const b3OpenCLArray<b3GpuFace>& faces,
|
||||
|
||||
@@ -6,39 +6,4 @@
|
||||
|
||||
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct) b3ConvexPolyhedronCL : public b3ConvexPolyhedronData
|
||||
{
|
||||
|
||||
inline void project(const b3Transform& trans, const b3Vector3& dir, const b3AlignedObjectArray<b3Vector3>& 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<numVerts;i++)
|
||||
{
|
||||
//b3Vector3 pt = trans * vertices[m_vertexOffset+i];
|
||||
//b3Scalar dp = pt.dot(dir);
|
||||
b3Scalar dp = vertices[m_vertexOffset+i].dot(localDir);
|
||||
//b3Assert(dp==dpL);
|
||||
if(dp < min) min = dp;
|
||||
if(dp > max) max = dp;
|
||||
}
|
||||
if(min>max)
|
||||
{
|
||||
b3Scalar tmp = min;
|
||||
min = max;
|
||||
max = tmp;
|
||||
}
|
||||
min += offset;
|
||||
max += offset;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //CONVEX_POLYHEDRON_CL
|
||||
|
||||
@@ -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<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& 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<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& 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<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& 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<b3Vector3>& verticesA,
|
||||
sResults& results)
|
||||
{
|
||||
|
||||
@@ -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<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& 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<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& 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<b3Vector3>& 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<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
||||
static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0,
|
||||
const btConvexShape* shape1,const btTransform& wtrs1,
|
||||
const b3Vector3& guess,
|
||||
sResults& results);
|
||||
#endif //0
|
||||
#endif
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -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<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& 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<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
|
||||
inline void project(const b3ConvexPolyhedronData& hull, const float4& pos, const b3Quaternion& orn, const float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
|
||||
{
|
||||
min = FLT_MAX;
|
||||
max = -FLT_MAX;
|
||||
@@ -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<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& 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<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& 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(computedDepth<resultSepDistance)
|
||||
{
|
||||
if (testAxis.length2()>B3_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()<REL_ERROR2)
|
||||
if(newCachedSeparatingAxis.length2()<REL_ERROR2)
|
||||
{
|
||||
if (delta<bestDeltaN)
|
||||
{
|
||||
gjkDetector->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(computedDepth<resultSepDistance)
|
||||
{
|
||||
if (testAxis.length2()>B3_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(computedDepth<resultSepDistance)
|
||||
{
|
||||
if (testAxis.length2()>B3_EPSILON*B3_EPSILON)
|
||||
{
|
||||
resultSepDistance = computedDepth;
|
||||
resultSepNormal = testAxis;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*output.addContactPoint(
|
||||
normalInB,
|
||||
pointOnB+positionOffset,
|
||||
distance);
|
||||
*/
|
||||
|
||||
static float maxPenetrationDistance = 0.f;
|
||||
if (distance<maxPenetrationDistance)
|
||||
{
|
||||
maxPenetrationDistance = distance;
|
||||
printf("maxPenetrationDistance = %f\n",maxPenetrationDistance);
|
||||
}
|
||||
resultSepNormal = normalInB;
|
||||
//printf("normalInB = %f,%f,%f, distance = %f\n",normalInB.x,normalInB.y,normalInB.z,distance);
|
||||
resultSepDistance = distance;
|
||||
|
||||
|
||||
b3Scalar lsqr = resultSepNormal.length2();
|
||||
b3Assert(lsqr>B3_EPSILON*B3_EPSILON);
|
||||
if (lsqr<B3_EPSILON*B3_EPSILON)
|
||||
return false;
|
||||
resultSepNormal *= 1.f/b3Sqrt(lsqr);
|
||||
|
||||
#if 0
|
||||
float dot = resultSepNormal.dot(b3Vector3(0,-1,0));
|
||||
//float dot = b3Vector3(-0.7,-0.6,-0.2).dot(b3Vector3(0,-1,0));
|
||||
|
||||
static float minDot = 1e30f;
|
||||
|
||||
if (dot<minDot)
|
||||
{
|
||||
//printf("minDot = %f\n", dot);
|
||||
minDot=dot;
|
||||
}
|
||||
|
||||
//printf("gNumGjkChecks = %d, gNumDeepPenetrationChecks = %d, minDot = %f\n", gNumGjkChecks,gNumDeepPenetrationChecks,minDot);
|
||||
if (0)//dot<0.64)
|
||||
{
|
||||
{
|
||||
b3Scalar l2 = resultSepNormal.length2();
|
||||
if (l2>B3_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(computedDepth<resultSepDistance)
|
||||
{
|
||||
if (testAxis.length2()>B3_EPSILON*B3_EPSILON)
|
||||
{
|
||||
resultSepDistance = computedDepth;
|
||||
resultSepNormal = testAxis;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
resultPointOnB = pointOnB+positionOffset;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
return false;
|
||||
|
||||
}
|
||||
|
||||
@@ -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<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
||||
b3Scalar maximumDistanceSquared,
|
||||
|
||||
@@ -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<b3Vector3>& 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<b3Vector3>& verticesA)
|
||||
{
|
||||
return localGetSupportVertexWithMargin(supportVec,hull,verticesA,0.f);
|
||||
|
||||
@@ -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
|
||||
|
||||
89
src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl
Normal file
89
src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl
Normal file
@@ -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 (i<numPairs)
|
||||
{
|
||||
int bodyIndexA = pairs[i].x;
|
||||
int bodyIndexB = pairs[i].y;
|
||||
|
||||
int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
|
||||
int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
|
||||
|
||||
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
|
||||
int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
|
||||
|
||||
|
||||
//once the broadphase avoids static-static pairs, we can remove this test
|
||||
if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
if ((collidables[collidableIndexA].m_shapeType!=SHAPE_CONVEX_HULL) ||(collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
float depthOut;
|
||||
b3Float4 dirOut;
|
||||
b3Float4 posOut;
|
||||
|
||||
|
||||
int res = b3MprPenetration(pairIndex, bodyIndexA, bodyIndexB,rigidBodies,convexShapes,collidables,vertices,separatingNormals,hasSeparatingAxis,&depthOut, &dirOut, &posOut);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if (res==0)
|
||||
{
|
||||
//add a contact
|
||||
|
||||
int dstIdx;
|
||||
AppendInc( nGlobalContactsOut, dstIdx );
|
||||
if (dstIdx<contactCapacity)
|
||||
{
|
||||
__global struct b3Contact4Data* c = globalContactsOut + dstIdx;
|
||||
c->m_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;i<nContacts;i++)
|
||||
posOut.w = -depthOut;
|
||||
c->m_worldPosB[0] = posOut;//localPoints[contactIdx[i]];
|
||||
GET_NPOINTS(*c) = 1;//nContacts;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
1244
src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h
Normal file
1244
src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h
Normal file
File diff suppressed because it is too large
Load Diff
@@ -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<b3GpuFace>& 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<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& 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<b3RayInfo>& 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<b3RayInfo>& rays, b3A
|
||||
}
|
||||
///todo: add some acceleration structure (AABBs, tree etc)
|
||||
void b3GpuRaycast::castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& 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);
|
||||
|
||||
@@ -18,40 +18,15 @@ public:
|
||||
virtual ~b3GpuRaycast();
|
||||
|
||||
void castRaysHost(const b3AlignedObjectArray<b3RayInfo>& raysIn, b3AlignedObjectArray<b3RayHit>& 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<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& 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<b3RigidBodyCL>* bodyBuf,
|
||||
b3OpenCLArray<b3Contact4>* contactOut, int& nContacts,
|
||||
int maxContactCapacity,
|
||||
const b3OpenCLArray<b3ConvexPolyhedronCL>& hostConvexData,
|
||||
const b3OpenCLArray<b3Vector3>& vertices,
|
||||
const b3OpenCLArray<b3Vector3>& uniqueEdges,
|
||||
const b3OpenCLArray<b3GpuFace>& faces,
|
||||
const b3OpenCLArray<int>& indices,
|
||||
const b3OpenCLArray<b3Collidable>& gpuCollidables,
|
||||
const b3OpenCLArray<b3GpuChildShape>& gpuChildShapes,
|
||||
|
||||
const b3OpenCLArray<b3YetAnotherAabb>& clAabbs,
|
||||
b3OpenCLArray<b3Vector3>& worldVertsB1GPU,
|
||||
b3OpenCLArray<b3Int4>& clippingFacesOutGPU,
|
||||
b3OpenCLArray<b3Vector3>& worldNormalsAGPU,
|
||||
b3OpenCLArray<b3Vector3>& worldVertsA1GPU,
|
||||
b3OpenCLArray<b3Vector3>& worldVertsB2GPU,
|
||||
b3AlignedObjectArray<class b3OptimizedBvh*>& bvhData,
|
||||
b3OpenCLArray<b3QuantizedBvhNode>* treeNodesGPU,
|
||||
b3OpenCLArray<b3BvhSubtreeInfo>* subTreesGPU,
|
||||
b3OpenCLArray<b3BvhInfo>* bvhInfo,
|
||||
int numObjects,
|
||||
int maxTriConvexPairCapacity,
|
||||
b3OpenCLArray<b3Int4>& triangleConvexPairs,
|
||||
int& numTriConvexPairsOut
|
||||
*/
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -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 <new>
|
||||
#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)
|
||||
{
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
};
|
||||
|
||||
@@ -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<unsigned int>& bodyCount
|
||||
@@ -496,7 +496,7 @@ float positionConstraintCoeff, int gIdx, b3AlignedObjectArray<unsigned int>& 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<numBodies;i++)
|
||||
{
|
||||
if (bodies[i].getInvMass())
|
||||
if (bodies[i].m_invMass)
|
||||
{
|
||||
int bodyOffset = offsetSplitBodies[i];
|
||||
int count = bodyCount[i];
|
||||
@@ -667,8 +667,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);
|
||||
|
||||
@@ -677,7 +677,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;
|
||||
@@ -686,7 +686,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;
|
||||
@@ -710,7 +710,7 @@ void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyCL* bodies,b3InertiaCL*
|
||||
//easy
|
||||
for (int i=0;i<numBodies;i++)
|
||||
{
|
||||
if (bodies[i].getInvMass())
|
||||
if (bodies[i].m_invMass)
|
||||
{
|
||||
int bodyOffset = offsetSplitBodies[i];
|
||||
int count = bodyCount[i];
|
||||
@@ -740,7 +740,7 @@ void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyCL* bodies,b3InertiaCL*
|
||||
//easy
|
||||
for (int i=0;i<numBodies;i++)
|
||||
{
|
||||
if (bodies[i].getInvMass())
|
||||
if (bodies[i].m_invMass)
|
||||
{
|
||||
int bodyOffset = offsetSplitBodies[i];
|
||||
int count = bodyCount[i];
|
||||
@@ -758,7 +758,7 @@ void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyCL* bodies,b3InertiaCL*
|
||||
void b3GpuJacobiContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem inertiaBuf, int numContacts, cl_mem contactBuf, const struct b3Config& config, int static0Index)
|
||||
//
|
||||
//
|
||||
//void b3GpuJacobiContactSolver::solveGroup(b3OpenCLArray<b3RigidBodyCL>* bodies,b3OpenCLArray<b3InertiaCL>* inertias,b3OpenCLArray<b3Contact4>* manifoldPtr,const btJacobiSolverInfo& solverInfo)
|
||||
//void b3GpuJacobiContactSolver::solveGroup(b3OpenCLArray<b3RigidBodyData>* bodies,b3OpenCLArray<b3InertiaData>* inertias,b3OpenCLArray<b3Contact4>* 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<b3RigidBodyCL>* bodiesGPU,b3OpenCLArray<b3InertiaCL>* inertiasGPU,b3OpenCLArray<b3Contact4>* manifoldPtrGPU,const btJacobiSolverInfo& solverInfo)
|
||||
void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray<b3RigidBodyData>* bodiesGPU,b3OpenCLArray<b3InertiaData>* inertiasGPU,b3OpenCLArray<b3Contact4>* manifoldPtrGPU,const btJacobiSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
b3AlignedObjectArray<b3RigidBodyCL> bodiesCPU;
|
||||
b3AlignedObjectArray<b3RigidBodyData> bodiesCPU;
|
||||
bodiesGPU->copyToHost(bodiesCPU);
|
||||
b3AlignedObjectArray<b3InertiaCL> inertiasCPU;
|
||||
b3AlignedObjectArray<b3InertiaData> inertiasCPU;
|
||||
inertiasGPU->copyToHost(inertiasCPU);
|
||||
b3AlignedObjectArray<b3Contact4> manifoldPtrCPU;
|
||||
manifoldPtrGPU->copyToHost(manifoldPtrCPU);
|
||||
@@ -1141,8 +1141,8 @@ void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray<b3RigidBodyCL>* 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<b3RigidBodyCL>* 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<b3RigidBodyCL>* 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<b3RigidBodyCL>* bo
|
||||
//easy
|
||||
for (int i=0;i<numBodiesCPU;i++)
|
||||
{
|
||||
if (bodiesCPU[i].getInvMass())
|
||||
if (bodiesCPU[i].m_invMass)
|
||||
{
|
||||
int bodyOffset = offsetSplitBodies[i];
|
||||
int count = bodyCount[i];
|
||||
@@ -1263,8 +1263,8 @@ void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray<b3RigidBodyCL>* 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<b3RigidBodyCL>* 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<b3RigidBodyCL>* 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<b3RigidBodyCL>* bo
|
||||
//easy
|
||||
for (int i=0;i<numBodiesCPU;i++)
|
||||
{
|
||||
if (bodiesCPU[i].getInvMass())
|
||||
if (bodiesCPU[i].m_invMass)
|
||||
{
|
||||
int bodyOffset = offsetSplitBodies[i];
|
||||
int count = bodyCount[i];
|
||||
@@ -1362,7 +1362,7 @@ void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray<b3RigidBodyCL>* bo
|
||||
//easy
|
||||
for (int i=0;i<numBodiesCPU;i++)
|
||||
{
|
||||
if (bodiesCPU[i].getInvMass())
|
||||
if (bodiesCPU[i].m_invMass)
|
||||
{
|
||||
int bodyOffset = offsetSplitBodies[i];
|
||||
int count = bodyCount[i];
|
||||
|
||||
@@ -3,13 +3,13 @@
|
||||
#define B3_GPU_JACOBI_CONTACT_SOLVER_H
|
||||
#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h"
|
||||
//#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
|
||||
#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
|
||||
|
||||
|
||||
//struct b3InertiaCL;
|
||||
//struct b3InertiaData;
|
||||
//b3InertiaData
|
||||
|
||||
class b3TypedConstraint;
|
||||
@@ -49,10 +49,10 @@ public:
|
||||
|
||||
|
||||
void solveContacts(int numBodies, cl_mem bodyBuf, cl_mem inertiaBuf, int numContacts, cl_mem contactBuf, const struct b3Config& config, int static0Index);
|
||||
void solveGroupHost(b3RigidBodyCL* bodies,b3InertiaCL* inertias,int numBodies,struct b3Contact4* manifoldPtr, int numManifolds,const b3JacobiSolverInfo& solverInfo);
|
||||
void solveGroupHost(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,struct b3Contact4* manifoldPtr, int numManifolds,const b3JacobiSolverInfo& solverInfo);
|
||||
//void solveGroupHost(btRigidBodyCL* bodies,b3InertiaData* inertias,int numBodies,btContact4* manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btJacobiSolverInfo& solverInfo);
|
||||
|
||||
//b3Scalar solveGroup(b3OpenCLArray<b3RigidBodyCL>* gpuBodies,b3OpenCLArray<b3InertiaCL>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||||
//b3Scalar solveGroup(b3OpenCLArray<b3RigidBodyData>* gpuBodies,b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||||
|
||||
//void solveGroup(btOpenCLArray<btRigidBodyCL>* bodies,btOpenCLArray<btInertiaCL>* inertias,btOpenCLArray<btContact4>* manifoldPtr,const btJacobiSolverInfo& solverInfo);
|
||||
//void solveGroupMixed(btOpenCLArray<btRigidBodyCL>* bodies,btOpenCLArray<btInertiaCL>* inertias,btOpenCLArray<btContact4>* manifoldPtr,const btJacobiSolverInfo& solverInfo);
|
||||
|
||||
@@ -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 <string.h>
|
||||
@@ -45,16 +45,16 @@ m_queue(queue)
|
||||
|
||||
m_data->m_pBufContactOutCPU = new b3AlignedObjectArray<b3Contact4>();
|
||||
m_data->m_pBufContactOutCPU->resize(config.m_maxBroadphasePairs);
|
||||
m_data->m_bodyBufferCPU = new b3AlignedObjectArray<b3RigidBodyCL>();
|
||||
m_data->m_bodyBufferCPU = new b3AlignedObjectArray<b3RigidBodyData>();
|
||||
m_data->m_bodyBufferCPU->resize(config.m_maxConvexBodies);
|
||||
|
||||
m_data->m_inertiaBufferCPU = new b3AlignedObjectArray<b3InertiaCL>();
|
||||
m_data->m_inertiaBufferCPU = new b3AlignedObjectArray<b3InertiaData>();
|
||||
m_data->m_inertiaBufferCPU->resize(config.m_maxConvexBodies);
|
||||
|
||||
m_data->m_pBufContactBuffersGPU[0] = new b3OpenCLArray<b3Contact4>(ctx,queue, config.m_maxContactCapacity,true);
|
||||
m_data->m_pBufContactBuffersGPU[1] = new b3OpenCLArray<b3Contact4>(ctx,queue, config.m_maxContactCapacity,true);
|
||||
|
||||
m_data->m_inertiaBufferGPU = new b3OpenCLArray<b3InertiaCL>(ctx,queue,config.m_maxConvexBodies,false);
|
||||
m_data->m_inertiaBufferGPU = new b3OpenCLArray<b3InertiaData>(ctx,queue,config.m_maxConvexBodies,false);
|
||||
m_data->m_collidablesGPU = new b3OpenCLArray<b3Collidable>(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<adl::TYPE_CL>::allocate(ctx,queue, config.m_maxBroadphasePairs,false);
|
||||
m_data->m_bodyBufferGPU = new b3OpenCLArray<b3RigidBodyCL>(ctx,queue, config.m_maxConvexBodies,false);
|
||||
m_data->m_bodyBufferGPU = new b3OpenCLArray<b3RigidBodyData>(ctx,queue, config.m_maxConvexBodies,false);
|
||||
|
||||
m_data->m_convexFacesGPU = new b3OpenCLArray<b3GpuFace>(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<b3GpuChildShape>(ctx,queue,config.m_maxCompoundChildShapes,false);
|
||||
|
||||
m_data->m_convexPolyhedraGPU = new b3OpenCLArray<b3ConvexPolyhedronCL>(ctx,queue,config.m_maxConvexShapes,false);
|
||||
m_data->m_convexPolyhedraGPU = new b3OpenCLArray<b3ConvexPolyhedronData>(ctx,queue,config.m_maxConvexShapes,false);
|
||||
m_data->m_convexPolyhedra.reserve(config.m_maxConvexShapes);
|
||||
|
||||
m_data->m_uniqueEdgesGPU = new b3OpenCLArray<b3Vector3>(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<b3Vector3>*
|
||||
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)
|
||||
{
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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<b3ConvexUtility*>* m_convexData;
|
||||
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronCL> m_convexPolyhedra;
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
|
||||
b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
|
||||
b3AlignedObjectArray<b3Vector3> m_convexVertices;
|
||||
b3AlignedObjectArray<int> m_convexIndices;
|
||||
|
||||
b3OpenCLArray<b3ConvexPolyhedronCL>* m_convexPolyhedraGPU;
|
||||
b3OpenCLArray<b3ConvexPolyhedronData>* m_convexPolyhedraGPU;
|
||||
b3OpenCLArray<b3Vector3>* m_uniqueEdgesGPU;
|
||||
b3OpenCLArray<b3Vector3>* m_convexVerticesGPU;
|
||||
b3OpenCLArray<int>* m_convexIndicesGPU;
|
||||
@@ -60,11 +60,11 @@ struct b3GpuNarrowPhaseInternalData
|
||||
b3AlignedObjectArray<b3Contact4>* m_pBufContactOutCPU;
|
||||
|
||||
|
||||
b3AlignedObjectArray<b3RigidBodyCL>* m_bodyBufferCPU;
|
||||
b3OpenCLArray<b3RigidBodyCL>* m_bodyBufferGPU;
|
||||
b3AlignedObjectArray<b3RigidBodyData>* m_bodyBufferCPU;
|
||||
b3OpenCLArray<b3RigidBodyData>* m_bodyBufferGPU;
|
||||
|
||||
b3AlignedObjectArray<b3InertiaCL>* m_inertiaBufferCPU;
|
||||
b3OpenCLArray<b3InertiaCL>* m_inertiaBufferGPU;
|
||||
b3AlignedObjectArray<b3InertiaData>* m_inertiaBufferCPU;
|
||||
b3OpenCLArray<b3InertiaData>* m_inertiaBufferGPU;
|
||||
|
||||
int m_numAcceleratedShapes;
|
||||
int m_numAcceleratedRigidBodies;
|
||||
|
||||
@@ -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 <new>
|
||||
@@ -72,8 +72,8 @@ struct b3GpuPgsJacobiSolverInternalData
|
||||
b3AlignedObjectArray<unsigned int> m_cpuConstraintInfo1;
|
||||
b3AlignedObjectArray<unsigned int> m_cpuConstraintRowOffsets;
|
||||
|
||||
b3AlignedObjectArray<b3RigidBodyCL> m_cpuBodies;
|
||||
b3AlignedObjectArray<b3InertiaCL> m_cpuInertias;
|
||||
b3AlignedObjectArray<b3RigidBodyData> m_cpuBodies;
|
||||
b3AlignedObjectArray<b3InertiaData> m_cpuInertias;
|
||||
|
||||
|
||||
b3AlignedObjectArray<b3GpuGenericConstraint> 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<b3RigidBodyCL>* gpuBodies, b3OpenCLArray<b3InertiaCL>* gpuInertias, int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
|
||||
b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
|
||||
{
|
||||
B3_PROFILE("GPU solveGroupCacheFriendlySetup");
|
||||
batchConstraints.resize(numConstraints);
|
||||
@@ -216,7 +216,7 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3
|
||||
/* m_gpuData->m_gpuBodies->resize(numBodies);
|
||||
m_gpuData->m_gpuBodies->copyFromHostPointer(bodies,numBodies);
|
||||
|
||||
b3OpenCLArray<b3InertiaCL> gpuInertias(m_gpuData->m_context,m_gpuData->m_queue);
|
||||
b3OpenCLArray<b3InertiaData> gpuInertias(m_gpuData->m_context,m_gpuData->m_queue);
|
||||
gpuInertias.resize(numBodies);
|
||||
gpuInertias.copyFromHostPointer(inertias,numBodies);
|
||||
*/
|
||||
@@ -245,7 +245,7 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3
|
||||
for (int i=0;i<numBodies;i++)
|
||||
{
|
||||
|
||||
b3RigidBodyCL& body = m_gpuData->m_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(b3OpenCLArray<b3
|
||||
b3GpuSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[constraintRowOffset];
|
||||
b3GpuGenericConstraint& constraint = m_gpuData->m_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(b3OpenCLArray<b3
|
||||
b3GpuSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
|
||||
b3GpuSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
|
||||
|
||||
if (rbA.getInvMass())
|
||||
if (rbA.m_invMass)
|
||||
{
|
||||
batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA;
|
||||
} else
|
||||
@@ -417,7 +417,7 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3
|
||||
batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA;
|
||||
}
|
||||
|
||||
if (rbB.getInvMass())
|
||||
if (rbB.m_invMass)
|
||||
{
|
||||
batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB;
|
||||
} else
|
||||
@@ -531,9 +531,9 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3
|
||||
{
|
||||
//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);
|
||||
@@ -655,7 +655,7 @@ void resolveSingleConstraintRowGeneric2( b3GpuSolverBody* body1, b3GpuSolverBod
|
||||
|
||||
|
||||
|
||||
void b3GpuPgsConstraintSolver::initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyCL* rb)
|
||||
void b3GpuPgsConstraintSolver::initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyData* rb)
|
||||
{
|
||||
|
||||
solverBody->m_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<b3RigidBodyCL>* gpuBodies, b3OpenCLArray<b3InertiaCL>* gpuInertias,
|
||||
b3Scalar b3GpuPgsConstraintSolver::solveGroup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias,
|
||||
int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints, const b3ContactSolverInfo& infoGlobal)
|
||||
{
|
||||
|
||||
@@ -1007,7 +1007,7 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroup(b3OpenCLArray<b3RigidBodyCL>* gpuB
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
void b3GpuPgsConstraintSolver::solveJoints(int numBodies, b3OpenCLArray<b3RigidBodyCL>* gpuBodies, b3OpenCLArray<b3InertiaCL>* gpuInertias,
|
||||
void b3GpuPgsConstraintSolver::solveJoints(int numBodies, b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias,
|
||||
int numConstraints, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints)
|
||||
{
|
||||
b3ContactSolverInfo infoGlobal;
|
||||
@@ -1026,10 +1026,10 @@ void b3GpuPgsConstraintSolver::solveJoints(int numBodies, b3OpenCLArray<b3RigidB
|
||||
|
||||
}
|
||||
|
||||
//b3AlignedObjectArray<b3RigidBodyCL> testBodies;
|
||||
//b3AlignedObjectArray<b3RigidBodyData> testBodies;
|
||||
|
||||
|
||||
b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyCL>* gpuBodies,b3OpenCLArray<b3InertiaCL>* gpuInertias,int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
|
||||
b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyData>* gpuBodies,b3OpenCLArray<b3InertiaData>* gpuInertias,int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
|
||||
{
|
||||
B3_PROFILE("solveGroupCacheFriendlyFinish");
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
@@ -1113,8 +1113,8 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b
|
||||
//printf("bodyIndex=%d\n",bodyIndex);
|
||||
b3Assert(i==bodyIndex);
|
||||
|
||||
b3RigidBodyCL* body = &m_gpuData->m_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);
|
||||
|
||||
@@ -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<b3GpuGenericConstraint>* gpuConstraints1,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||||
virtual b3Scalar solveGroupCacheFriendlySetup(b3OpenCLArray<b3RigidBodyCL>* gpuBodies, b3OpenCLArray<b3InertiaCL>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||||
b3Scalar solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyCL>* gpuBodies,b3OpenCLArray<b3InertiaCL>* gpuInertias,int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||||
virtual b3Scalar solveGroupCacheFriendlySetup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||||
b3Scalar solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyData>* gpuBodies,b3OpenCLArray<b3InertiaData>* gpuInertias,int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||||
|
||||
|
||||
b3Scalar solveGroup(b3OpenCLArray<b3RigidBodyCL>* gpuBodies,b3OpenCLArray<b3InertiaCL>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||||
void solveJoints(int numBodies, b3OpenCLArray<b3RigidBodyCL>* gpuBodies, b3OpenCLArray<b3InertiaCL>* gpuInertias,
|
||||
b3Scalar solveGroup(b3OpenCLArray<b3RigidBodyData>* gpuBodies,b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||||
void solveJoints(int numBodies, b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias,
|
||||
int numConstraints, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints);
|
||||
|
||||
int sortConstraintByBatch3( struct b3BatchConstraint* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies);
|
||||
|
||||
@@ -82,8 +82,8 @@ struct b3GpuBatchingPgsSolverInternalData
|
||||
b3OpenCLArray<b3SortData>* m_sortDataBuffer;
|
||||
b3OpenCLArray<b3Contact4>* m_contactBuffer;
|
||||
|
||||
b3OpenCLArray<b3RigidBodyCL>* m_bodyBufferGPU;
|
||||
b3OpenCLArray<b3InertiaCL>* m_inertiaBufferGPU;
|
||||
b3OpenCLArray<b3RigidBodyData>* m_bodyBufferGPU;
|
||||
b3OpenCLArray<b3InertiaData>* m_inertiaBufferGPU;
|
||||
b3OpenCLArray<b3Contact4>* m_pBufContactOutGPU;
|
||||
|
||||
b3OpenCLArray<b3Contact4>* 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<int>(ctx,q);
|
||||
m_data->m_bodyBufferGPU = new b3OpenCLArray<b3RigidBodyCL>(ctx,q);
|
||||
m_data->m_inertiaBufferGPU = new b3OpenCLArray<b3InertiaCL>(ctx,q);
|
||||
m_data->m_bodyBufferGPU = new b3OpenCLArray<b3RigidBodyData>(ctx,q);
|
||||
m_data->m_inertiaBufferGPU = new b3OpenCLArray<b3InertiaData>(ctx,q);
|
||||
m_data->m_pBufContactOutGPU = new b3OpenCLArray<b3Contact4>(ctx,q);
|
||||
|
||||
m_data->m_pBufContactOutGPUCopy = new b3OpenCLArray<b3Contact4>(ctx,q);
|
||||
@@ -293,7 +293,7 @@ struct b3ConstraintCfg
|
||||
|
||||
|
||||
|
||||
void b3GpuPgsContactSolver::solveContactConstraintBatchSizes( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf, const b3OpenCLArray<b3InertiaCL>* shapeBuf,
|
||||
void b3GpuPgsContactSolver::solveContactConstraintBatchSizes( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf,
|
||||
b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches,int numIterations, const b3AlignedObjectArray<int>* batchSizes)//const b3OpenCLArray<int>* gpuBatchSizes)
|
||||
{
|
||||
B3_PROFILE("solveContactConstraintBatchSizes");
|
||||
@@ -353,7 +353,7 @@ void b3GpuPgsContactSolver::solveContactConstraintBatchSizes( const b3OpenCLArr
|
||||
}
|
||||
}
|
||||
|
||||
void b3GpuPgsContactSolver::solveContactConstraint( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf, const b3OpenCLArray<b3InertiaCL>* shapeBuf,
|
||||
void b3GpuPgsContactSolver::solveContactConstraint( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf,
|
||||
b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches,int numIterations, const b3AlignedObjectArray<int>* batchSizes)//,const b3OpenCLArray<int>* 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<nContacts;gIdx++)
|
||||
{
|
||||
@@ -761,10 +761,10 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem
|
||||
csCfg.m_staticIdx = static0Index;
|
||||
|
||||
|
||||
b3OpenCLArray<b3RigidBodyCL>* bodyBuf = m_data->m_bodyBufferGPU;
|
||||
b3OpenCLArray<b3RigidBodyData>* bodyBuf = m_data->m_bodyBufferGPU;
|
||||
|
||||
void* additionalData = 0;//m_data->m_frictionCGPU;
|
||||
const b3OpenCLArray<b3InertiaCL>* shapeBuf = m_data->m_inertiaBufferGPU;
|
||||
const b3OpenCLArray<b3InertiaData>* shapeBuf = m_data->m_inertiaBufferGPU;
|
||||
b3OpenCLArray<b3GpuConstraint4>* contactConstraintOut = m_data->m_contactCGPU;
|
||||
int nContacts = nContactOut;
|
||||
|
||||
@@ -795,12 +795,12 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem
|
||||
|
||||
|
||||
|
||||
const b3OpenCLArray<b3RigidBodyCL>* bodyNative = bodyBuf;
|
||||
const b3OpenCLArray<b3RigidBodyData>* bodyNative = bodyBuf;
|
||||
|
||||
|
||||
{
|
||||
|
||||
//b3OpenCLArray<b3RigidBodyCL>* bodyNative = b3OpenCLArrayUtils::map<adl::TYPE_CL, true>( data->m_device, bodyBuf );
|
||||
//b3OpenCLArray<b3RigidBodyData>* bodyNative = b3OpenCLArrayUtils::map<adl::TYPE_CL, true>( data->m_device, bodyBuf );
|
||||
//b3OpenCLArray<b3Contact4>* contactNative = b3OpenCLArrayUtils::map<adl::TYPE_CL, true>( 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<b3Contact4> contactCPU;
|
||||
m_data->m_pBufContactOutGPU->copyToHost(contactCPU);
|
||||
b3AlignedObjectArray<b3RigidBodyCL> bodiesCPU;
|
||||
b3AlignedObjectArray<b3RigidBodyData> bodiesCPU;
|
||||
bodyBuf->copyToHost(bodiesCPU);
|
||||
float scale = 1.f/csCfg.m_batchCellSize;
|
||||
b3Int4 nSplit;
|
||||
|
||||
@@ -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<b3RigidBodyCL>* bodyBuf, const b3OpenCLArray<b3InertiaCL>* shapeBuf,
|
||||
void solveContactConstraintBatchSizes( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf,
|
||||
b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches, int numIterations, const b3AlignedObjectArray<int>* batchSizes);//const b3OpenCLArray<int>* gpuBatchSizes);
|
||||
|
||||
void solveContactConstraint( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf, const b3OpenCLArray<b3InertiaCL>* shapeBuf,
|
||||
void solveContactConstraint( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf,
|
||||
b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches, int numIterations, const b3AlignedObjectArray<int>* batchSizes);//const b3OpenCLArray<int>* gpuBatchSizes);
|
||||
|
||||
public:
|
||||
|
||||
@@ -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<b3RigidBodyCL> gpuBodies(m_data->m_context,m_data->m_queue,0,true);
|
||||
b3OpenCLArray<b3RigidBodyData> gpuBodies(m_data->m_context,m_data->m_queue,0,true);
|
||||
gpuBodies.setFromOpenCLBuffer(m_data->m_narrowphase->getBodiesGpu(),m_data->m_narrowphase->getNumRigidBodies());
|
||||
b3OpenCLArray<b3InertiaCL> gpuInertias(m_data->m_context,m_data->m_queue,0,true);
|
||||
b3OpenCLArray<b3InertiaData> gpuInertias(m_data->m_context,m_data->m_queue,0,true);
|
||||
gpuInertias.setFromOpenCLBuffer(m_data->m_narrowphase->getBodyInertiasGpu(),m_data->m_narrowphase->getNumRigidBodies());
|
||||
b3OpenCLArray<b3Contact4> 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<b3RigidBodyCL> hostBodies;
|
||||
b3AlignedObjectArray<b3RigidBodyData> hostBodies;
|
||||
gpuBodies.copyToHost(hostBodies);
|
||||
b3AlignedObjectArray<b3InertiaCL> hostInertias;
|
||||
b3AlignedObjectArray<b3InertiaData> 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<b3RigidBodyCL> hostBodies;
|
||||
b3AlignedObjectArray<b3InertiaCL> hostInertias;
|
||||
b3AlignedObjectArray<b3RigidBodyData> hostBodies;
|
||||
b3AlignedObjectArray<b3InertiaData> hostInertias;
|
||||
b3AlignedObjectArray<b3Contact4> hostContacts;
|
||||
|
||||
{
|
||||
@@ -399,9 +399,9 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
|
||||
}
|
||||
} else
|
||||
{
|
||||
b3AlignedObjectArray<b3RigidBodyCL> hostBodies;
|
||||
b3AlignedObjectArray<b3RigidBodyData> hostBodies;
|
||||
gpuBodies.copyToHost(hostBodies);
|
||||
b3AlignedObjectArray<b3InertiaCL> hostInertias;
|
||||
b3AlignedObjectArray<b3InertiaData> hostInertias;
|
||||
gpuInertias.copyToHost(hostInertias);
|
||||
b3AlignedObjectArray<b3Contact4> hostContacts;
|
||||
gpuContacts.copyToHost(hostContacts);
|
||||
|
||||
@@ -359,7 +359,7 @@ void solveContact(b3GpuConstraint4& cs,
|
||||
|
||||
struct SolveTask// : public ThreadPool::Task
|
||||
{
|
||||
SolveTask(b3AlignedObjectArray<b3RigidBodyCL>& bodies, b3AlignedObjectArray<b3InertiaCL>& shapes, b3AlignedObjectArray<b3GpuConstraint4>& constraints,
|
||||
SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies, b3AlignedObjectArray<b3InertiaData>& shapes, b3AlignedObjectArray<b3GpuConstraint4>& constraints,
|
||||
int start, int nConstraints,int maxNumBatches,b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx, b3AlignedObjectArray<int>* 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<b3RigidBodyCL>& m_bodies;
|
||||
b3AlignedObjectArray<b3InertiaCL>& m_shapes;
|
||||
b3AlignedObjectArray<b3RigidBodyData>& m_bodies;
|
||||
b3AlignedObjectArray<b3InertiaData>& m_shapes;
|
||||
b3AlignedObjectArray<b3GpuConstraint4>& m_constraints;
|
||||
b3AlignedObjectArray<int>* m_batchSizes;
|
||||
int m_cellIndex;
|
||||
@@ -492,7 +492,7 @@ struct SolveTask// : public ThreadPool::Task
|
||||
};
|
||||
|
||||
|
||||
void b3Solver::solveContactConstraintHost( b3OpenCLArray<b3RigidBodyCL>* bodyBuf, b3OpenCLArray<b3InertiaCL>* shapeBuf,
|
||||
void b3Solver::solveContactConstraintHost( b3OpenCLArray<b3RigidBodyData>* bodyBuf, b3OpenCLArray<b3InertiaData>* shapeBuf,
|
||||
b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches,b3AlignedObjectArray<int>* batchSizes)
|
||||
{
|
||||
|
||||
@@ -541,9 +541,9 @@ void b3Solver::solveContactConstraintHost( b3OpenCLArray<b3RigidBodyCL>* bodyBu
|
||||
}
|
||||
#endif
|
||||
|
||||
b3AlignedObjectArray<b3RigidBodyCL> bodyNative;
|
||||
b3AlignedObjectArray<b3RigidBodyData> bodyNative;
|
||||
bodyBuf->copyToHost(bodyNative);
|
||||
b3AlignedObjectArray<b3InertiaCL> shapeNative;
|
||||
b3AlignedObjectArray<b3InertiaData> shapeNative;
|
||||
shapeBuf->copyToHost(shapeNative);
|
||||
b3AlignedObjectArray<b3GpuConstraint4> constraintNative;
|
||||
constraint->copyToHost(constraintNative);
|
||||
@@ -674,8 +674,8 @@ void b3Solver::solveContactConstraintHost( b3OpenCLArray<b3RigidBodyCL>* bodyBu
|
||||
|
||||
}
|
||||
|
||||
void checkConstraintBatch(const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
|
||||
const b3OpenCLArray<b3InertiaCL>* shapeBuf,
|
||||
void checkConstraintBatch(const b3OpenCLArray<b3RigidBodyData>* bodyBuf,
|
||||
const b3OpenCLArray<b3InertiaData>* shapeBuf,
|
||||
b3OpenCLArray<b3GpuConstraint4>* constraint,
|
||||
b3OpenCLArray<unsigned int>* m_numConstraints,
|
||||
b3OpenCLArray<unsigned int>* m_offsets,
|
||||
@@ -751,7 +751,7 @@ void checkConstraintBatch(const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
|
||||
|
||||
static bool verify=false;
|
||||
|
||||
void b3Solver::solveContactConstraint( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf, const b3OpenCLArray<b3InertiaCL>* shapeBuf,
|
||||
void b3Solver::solveContactConstraint( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf,
|
||||
b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches)
|
||||
{
|
||||
|
||||
@@ -926,8 +926,8 @@ void b3Solver::solveContactConstraint( const b3OpenCLArray<b3RigidBodyCL>* body
|
||||
|
||||
}
|
||||
|
||||
void b3Solver::convertToConstraints( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
|
||||
const b3OpenCLArray<b3InertiaCL>* shapeBuf,
|
||||
void b3Solver::convertToConstraints( const b3OpenCLArray<b3RigidBodyData>* bodyBuf,
|
||||
const b3OpenCLArray<b3InertiaData>* shapeBuf,
|
||||
b3OpenCLArray<b3Contact4>* contactsIn, b3OpenCLArray<b3GpuConstraint4>* contactCOut, void* additionalData,
|
||||
int nContacts, const ConstraintCfg& cfg )
|
||||
{
|
||||
@@ -952,13 +952,13 @@ void b3Solver::convertToConstraints( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf
|
||||
|
||||
if (gConvertConstraintOnCpu)
|
||||
{
|
||||
b3AlignedObjectArray<b3RigidBodyCL> gBodies;
|
||||
b3AlignedObjectArray<b3RigidBodyData> gBodies;
|
||||
bodyBuf->copyToHost(gBodies);
|
||||
|
||||
b3AlignedObjectArray<b3Contact4> gContact;
|
||||
contactsIn->copyToHost(gContact);
|
||||
|
||||
b3AlignedObjectArray<b3InertiaCL> gShapes;
|
||||
b3AlignedObjectArray<b3InertiaData> gShapes;
|
||||
shapeBuf->copyToHost(gShapes);
|
||||
|
||||
b3AlignedObjectArray<b3GpuConstraint4> gConstraintOut;
|
||||
@@ -1021,7 +1021,7 @@ void b3Solver::convertToConstraints( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf
|
||||
}
|
||||
|
||||
/*
|
||||
void b3Solver::sortContacts( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
|
||||
void b3Solver::sortContacts( const b3OpenCLArray<b3RigidBodyData>* bodyBuf,
|
||||
b3OpenCLArray<b3Contact4>* contactsIn, void* additionalData,
|
||||
int nContacts, const b3Solver::ConstraintCfg& cfg )
|
||||
{
|
||||
|
||||
@@ -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<b3RigidBodyCL>* bodyBuf, const b3OpenCLArray<b3InertiaCL>* inertiaBuf,
|
||||
void solveContactConstraint( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* inertiaBuf,
|
||||
b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches);
|
||||
|
||||
void solveContactConstraintHost( b3OpenCLArray<b3RigidBodyCL>* bodyBuf, b3OpenCLArray<b3InertiaCL>* shapeBuf,
|
||||
void solveContactConstraintHost( b3OpenCLArray<b3RigidBodyData>* bodyBuf, b3OpenCLArray<b3InertiaData>* shapeBuf,
|
||||
b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches, b3AlignedObjectArray<int>* batchSizes);
|
||||
|
||||
|
||||
void convertToConstraints( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
|
||||
const b3OpenCLArray<b3InertiaCL>* shapeBuf,
|
||||
void convertToConstraints( const b3OpenCLArray<b3RigidBodyData>* bodyBuf,
|
||||
const b3OpenCLArray<b3InertiaData>* shapeBuf,
|
||||
b3OpenCLArray<b3Contact4>* contactsIn, b3OpenCLArray<b3GpuConstraint4>* contactCOut, void* additionalData,
|
||||
int nContacts, const ConstraintCfg& cfg );
|
||||
|
||||
|
||||
Reference in New Issue
Block a user