summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h')
-rw-r--r--thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h935
1 files changed, 450 insertions, 485 deletions
diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
index 89993f3565..983554eb2e 100644
--- a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
+++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
@@ -3,7 +3,6 @@
#define B3_TRIANGLE_NUM_CONVEX_FACES 5
-
#include "Bullet3Common/shared/b3Int4.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
@@ -12,25 +11,24 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
-
-inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn,
-const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
+inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn,
+ const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
{
min[0] = FLT_MAX;
max[0] = -FLT_MAX;
int numVerts = hull->m_numVertices;
- const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),*dir);
- float offset = b3Dot(pos,*dir);
- for(int i=0;i<numVerts;i++)
+ const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn), *dir);
+ float offset = b3Dot(pos, *dir);
+ for (int i = 0; i < numVerts; i++)
{
- float dp = b3Dot(vertices[hull->m_vertexOffset+i],localDir);
- if(dp < min[0])
+ float dp = b3Dot(vertices[hull->m_vertexOffset + i], localDir);
+ if (dp < min[0])
min[0] = dp;
- if(dp > max[0])
+ if (dp > max[0])
max[0] = dp;
}
- if(min[0]>max[0])
+ if (min[0] > max[0])
{
float tmp = min[0];
min[0] = max[0];
@@ -40,53 +38,49 @@ const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
max[0] += offset;
}
-
-inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
- b3Float4ConstArg posA,b3QuatConstArg ornA,
- b3Float4ConstArg posB,b3QuatConstArg ornB,
- b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB,float* depth)
+inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
+ b3Float4ConstArg posA, b3QuatConstArg ornA,
+ b3Float4ConstArg posB, b3QuatConstArg ornB,
+ b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB, float* depth)
{
- float Min0,Max0;
- float Min1,Max1;
- b3Project(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0);
- b3Project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1);
+ float Min0, Max0;
+ float Min1, Max1;
+ b3Project(hullA, posA, ornA, sep_axis, verticesA, &Min0, &Max0);
+ b3Project(hullB, posB, ornB, sep_axis, verticesB, &Min1, &Max1);
- if(Max0<Min1 || Max1<Min0)
+ if (Max0 < Min1 || Max1 < Min0)
return false;
float d0 = Max0 - Min1;
float d1 = Max1 - Min0;
- *depth = d0<d1 ? d0:d1;
+ *depth = d0 < d1 ? d0 : d1;
return true;
}
-
-bool b3FindSeparatingAxis( const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
- b3Float4ConstArg posA1,
- b3QuatConstArg ornA,
- b3Float4ConstArg posB1,
- b3QuatConstArg ornB,
- b3Float4ConstArg DeltaC2,
-
- const b3Float4* verticesA,
- const b3Float4* uniqueEdgesA,
- const b3GpuFace* facesA,
- const int* indicesA,
-
- __global const b3Float4* verticesB,
- __global const b3Float4* uniqueEdgesB,
- __global const b3GpuFace* facesB,
- __global const int* indicesB,
- b3Float4* sep,
- float* dmin)
+bool b3FindSeparatingAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
+ b3Float4ConstArg posA1,
+ b3QuatConstArg ornA,
+ b3Float4ConstArg posB1,
+ b3QuatConstArg ornB,
+ b3Float4ConstArg DeltaC2,
+
+ const b3Float4* verticesA,
+ const b3Float4* uniqueEdgesA,
+ const b3GpuFace* facesA,
+ const int* indicesA,
+
+ __global const b3Float4* verticesB,
+ __global const b3Float4* uniqueEdgesB,
+ __global const b3GpuFace* facesB,
+ __global const int* indicesB,
+ b3Float4* sep,
+ float* dmin)
{
-
-
b3Float4 posA = posA1;
posA.w = 0.f;
b3Float4 posB = posB1;
posB.w = 0.f;
-/*
+ /*
static int maxFaceVertex = 0;
int curFaceVertexAB = hullA->m_numFaces*hullB->m_numVertices;
@@ -102,300 +96,289 @@ bool b3FindSeparatingAxis( const b3ConvexPolyhedronData* hullA, __global const b
}
*/
- int curPlaneTests=0;
+ int curPlaneTests = 0;
{
int numFacesA = hullA->m_numFaces;
// Test normals from hullA
- for(int i=0;i<numFacesA;i++)
+ for (int i = 0; i < numFacesA; i++)
{
- const b3Float4 normal = facesA[hullA->m_faceOffset+i].m_plane;
- b3Float4 faceANormalWS = b3QuatRotate(ornA,normal);
- if (b3Dot(DeltaC2,faceANormalWS)<0)
- faceANormalWS*=-1.f;
+ const b3Float4 normal = facesA[hullA->m_faceOffset + i].m_plane;
+ b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
+ if (b3Dot(DeltaC2, faceANormalWS) < 0)
+ faceANormalWS *= -1.f;
curPlaneTests++;
float d;
- if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d))
+ if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, &faceANormalWS, verticesA, verticesB, &d))
return false;
- if(d<*dmin)
+ if (d < *dmin)
{
*dmin = d;
*sep = faceANormalWS;
}
}
}
- if((b3Dot(-DeltaC2,*sep))>0.0f)
+ if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
{
*sep = -(*sep);
}
return true;
}
-
-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,
- b3Float4ConstArg posA1,
- b3QuatConstArg ornA,
- b3Float4ConstArg posB1,
- b3QuatConstArg ornB,
- b3Float4ConstArg DeltaC2,
- const b3Float4* verticesA,
- const b3Float4* uniqueEdgesA,
- const b3GpuFace* facesA,
- const int* indicesA,
- __global const b3Float4* verticesB,
- __global const b3Float4* uniqueEdgesB,
- __global const b3GpuFace* facesB,
- __global const int* indicesB,
- b3Float4* sep,
- float* dmin,
- bool searchAllEdgeEdge)
+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,
+ b3Float4ConstArg posA1,
+ b3QuatConstArg ornA,
+ b3Float4ConstArg posB1,
+ b3QuatConstArg ornB,
+ b3Float4ConstArg DeltaC2,
+ const b3Float4* verticesA,
+ const b3Float4* uniqueEdgesA,
+ const b3GpuFace* facesA,
+ const int* indicesA,
+ __global const b3Float4* verticesB,
+ __global const b3Float4* uniqueEdgesB,
+ __global const b3GpuFace* facesB,
+ __global const int* indicesB,
+ b3Float4* sep,
+ float* dmin,
+ bool searchAllEdgeEdge)
{
-
-
b3Float4 posA = posA1;
posA.w = 0.f;
b3Float4 posB = posB1;
posB.w = 0.f;
-// int curPlaneTests=0;
+ // int curPlaneTests=0;
int curEdgeEdge = 0;
// Test edges
static int maxEdgeTests = 0;
int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges;
- if (curEdgeTests >maxEdgeTests )
+ if (curEdgeTests > maxEdgeTests)
{
- 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);
-
+ 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);
}
-
if (searchAllEdgeEdge)
{
- for(int e0=0;e0<hullA->m_numUniqueEdges;e0++)
+ for (int e0 = 0; e0 < hullA->m_numUniqueEdges; e0++)
{
- const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0];
- b3Float4 edge0World = b3QuatRotate(ornA,edge0);
+ const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset + e0];
+ b3Float4 edge0World = b3QuatRotate(ornA, edge0);
- for(int e1=0;e1<hullB->m_numUniqueEdges;e1++)
+ for (int e1 = 0; e1 < hullB->m_numUniqueEdges; e1++)
{
- const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1];
- b3Float4 edge1World = b3QuatRotate(ornB,edge1);
-
+ const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset + e1];
+ b3Float4 edge1World = b3QuatRotate(ornB, edge1);
- b3Float4 crossje = b3Cross(edge0World,edge1World);
+ b3Float4 crossje = b3Cross(edge0World, edge1World);
curEdgeEdge++;
- if(!b3IsAlmostZero(crossje))
+ if (!b3IsAlmostZero(crossje))
{
crossje = b3Normalized(crossje);
- if (b3Dot(DeltaC2,crossje)<0)
+ 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)
+ 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;
+ dist = d0 < d1 ? d0 : d1;
result = true;
-
}
-
- if(dist<*dmin)
+ if (dist < *dmin)
{
*dmin = dist;
*sep = crossje;
}
}
}
-
}
- } else
+ }
+ else
{
- int numDirections = sizeof(unitSphere162)/sizeof(b3Vector3);
+ int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
//printf("numDirections =%d\n",numDirections );
-
- for(int i=0;i<numDirections;i++)
+ for (int i = 0; i < numDirections; i++)
{
b3Float4 crossje = unitSphere162[i];
{
@@ -404,23 +387,21 @@ bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global
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)
+ 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;
+ dist = d0 < d1 ? d0 : d1;
result = true;
-
}
-
- if(dist<*dmin)
+ if (dist < *dmin)
{
*dmin = dist;
*sep = crossje;
@@ -428,50 +409,45 @@ bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global
}
}
}
-
}
-
- if((b3Dot(-DeltaC2,*sep))>0.0f)
+ if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
{
*sep = -(*sep);
}
return true;
}
-
-
-inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
- __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
- b3Float4ConstArg posA, b3QuatConstArg ornA,b3Float4ConstArg posB, b3QuatConstArg ornB,
- __global b3Float4* worldVertsA1,
- __global b3Float4* worldNormalsA1,
- __global b3Float4* worldVertsB1,
- int capacityWorldVerts,
- const float minDist, float maxDist,
- __global const b3Float4* verticesA,
- __global const b3GpuFace_t* facesA,
- __global const int* indicesA,
- __global const b3Float4* verticesB,
- __global const b3GpuFace_t* facesB,
- __global const int* indicesB,
-
- __global b3Int4* clippingFaces, int pairIndex)
+inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
+ __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
+ b3Float4ConstArg posA, b3QuatConstArg ornA, b3Float4ConstArg posB, b3QuatConstArg ornB,
+ __global b3Float4* worldVertsA1,
+ __global b3Float4* worldNormalsA1,
+ __global b3Float4* worldVertsB1,
+ int capacityWorldVerts,
+ const float minDist, float maxDist,
+ __global const b3Float4* verticesA,
+ __global const b3GpuFace_t* facesA,
+ __global const int* indicesA,
+ __global const b3Float4* verticesB,
+ __global const b3GpuFace_t* facesB,
+ __global const int* indicesB,
+
+ __global b3Int4* clippingFaces, int pairIndex)
{
int numContactsOut = 0;
- int numWorldVertsB1= 0;
-
-
- int closestFaceB=-1;
+ int numWorldVertsB1 = 0;
+
+ int closestFaceB = -1;
float dmax = -FLT_MAX;
-
+
{
- for(int face=0;face<hullB->m_numFaces;face++)
+ for (int face = 0; face < hullB->m_numFaces; face++)
{
- const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset+face].m_plane.x,
- facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f);
+ const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset + face].m_plane.x,
+ facesB[hullB->m_faceOffset + face].m_plane.y, facesB[hullB->m_faceOffset + face].m_plane.z, 0.f);
const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
- float d = b3Dot(WorldNormal,separatingNormal);
+ float d = b3Dot(WorldNormal, separatingNormal);
if (d > dmax)
{
dmax = d;
@@ -479,81 +455,76 @@ inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
}
}
}
-
+
{
- const b3GpuFace_t polyB = facesB[hullB->m_faceOffset+closestFaceB];
+ const b3GpuFace_t polyB = facesB[hullB->m_faceOffset + closestFaceB];
const int numVertices = polyB.m_numIndices;
- for(int e0=0;e0<numVertices;e0++)
+ for (int e0 = 0; e0 < numVertices; e0++)
{
- const b3Float4 b = verticesB[hullB->m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];
- worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = b3TransformPoint(b,posB,ornB);
+ const b3Float4 b = verticesB[hullB->m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
+ worldVertsB1[pairIndex * capacityWorldVerts + numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
}
}
-
- int closestFaceA=-1;
+
+ int closestFaceA = -1;
{
float dmin = FLT_MAX;
- for(int face=0;face<hullA->m_numFaces;face++)
+ for (int face = 0; face < hullA->m_numFaces; face++)
{
const b3Float4 Normal = b3MakeFloat4(
- facesA[hullA->m_faceOffset+face].m_plane.x,
- facesA[hullA->m_faceOffset+face].m_plane.y,
- facesA[hullA->m_faceOffset+face].m_plane.z,
- 0.f);
- const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal);
-
- float d = b3Dot(faceANormalWS,separatingNormal);
+ facesA[hullA->m_faceOffset + face].m_plane.x,
+ facesA[hullA->m_faceOffset + face].m_plane.y,
+ facesA[hullA->m_faceOffset + face].m_plane.z,
+ 0.f);
+ const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
+
+ float d = b3Dot(faceANormalWS, separatingNormal);
if (d < dmin)
{
dmin = d;
closestFaceA = face;
- worldNormalsA1[pairIndex] = faceANormalWS;
+ worldNormalsA1[pairIndex] = faceANormalWS;
}
}
}
-
- int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices;
- for(int e0=0;e0<numVerticesA;e0++)
+
+ int numVerticesA = facesA[hullA->m_faceOffset + closestFaceA].m_numIndices;
+ for (int e0 = 0; e0 < numVerticesA; e0++)
{
- const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]];
- worldVertsA1[pairIndex*capacityWorldVerts+e0] = b3TransformPoint(a, posA,ornA);
- }
-
- clippingFaces[pairIndex].x = closestFaceA;
- clippingFaces[pairIndex].y = closestFaceB;
- clippingFaces[pairIndex].z = numVerticesA;
- clippingFaces[pairIndex].w = numWorldVertsB1;
-
-
+ const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[facesA[hullA->m_faceOffset + closestFaceA].m_indexOffset + e0]];
+ worldVertsA1[pairIndex * capacityWorldVerts + e0] = b3TransformPoint(a, posA, ornA);
+ }
+
+ clippingFaces[pairIndex].x = closestFaceA;
+ clippingFaces[pairIndex].y = closestFaceB;
+ clippingFaces[pairIndex].z = numVerticesA;
+ clippingFaces[pairIndex].w = numWorldVertsB1;
+
return numContactsOut;
}
-
-
-
-__kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs,
- __global const b3RigidBodyData* rigidBodies,
- __global const b3Collidable* collidables,
- __global const b3ConvexPolyhedronData* convexShapes,
- __global const b3Float4* vertices,
- __global const b3Float4* uniqueEdges,
- __global const b3GpuFace* faces,
- __global const int* indices,
- __global const b3GpuChildShape* gpuChildShapes,
- __global b3Aabb* aabbs,
- __global b3Float4* concaveSeparatingNormalsOut,
- __global b3Int4* clippingFacesOut,
- __global b3Vector3* worldVertsA1Out,
- __global b3Vector3* worldNormalsA1Out,
- __global b3Vector3* worldVertsB1Out,
- __global int* hasSeparatingNormals,
- int vertexFaceCapacity,
- int numConcavePairs,
- int pairIdx
- )
+__kernel void b3FindConcaveSeparatingAxisKernel(__global b3Int4* concavePairs,
+ __global const b3RigidBodyData* rigidBodies,
+ __global const b3Collidable* collidables,
+ __global const b3ConvexPolyhedronData* convexShapes,
+ __global const b3Float4* vertices,
+ __global const b3Float4* uniqueEdges,
+ __global const b3GpuFace* faces,
+ __global const int* indices,
+ __global const b3GpuChildShape* gpuChildShapes,
+ __global b3Aabb* aabbs,
+ __global b3Float4* concaveSeparatingNormalsOut,
+ __global b3Int4* clippingFacesOut,
+ __global b3Vector3* worldVertsA1Out,
+ __global b3Vector3* worldNormalsA1Out,
+ __global b3Vector3* worldVertsB1Out,
+ __global int* hasSeparatingNormals,
+ int vertexFaceCapacity,
+ int numConcavePairs,
+ int pairIdx)
{
int i = pairIdx;
-/* int i = get_global_id(0);
+ /* int i = get_global_id(0);
if (i>=numConcavePairs)
return;
int pairIdx = i;
@@ -568,8 +539,8 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
- if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&&
- collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS)
+ if (collidables[collidableIndexB].m_shapeType != SHAPE_CONVEX_HULL &&
+ collidables[collidableIndexB].m_shapeType != SHAPE_COMPOUND_OF_CONVEX_HULLS)
{
concavePairs[pairIdx].w = -1;
return;
@@ -577,121 +548,119 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
hasSeparatingNormals[i] = 0;
-// int numFacesA = convexShapes[shapeIndexA].m_numFaces;
+ // int numFacesA = convexShapes[shapeIndexA].m_numFaces;
int numActualConcaveConvexTests = 0;
-
+
int f = concavePairs[i].z;
-
+
bool overlap = false;
-
+
b3ConvexPolyhedronData convexPolyhedronA;
//add 3 vertices of the triangle
convexPolyhedronA.m_numVertices = 3;
convexPolyhedronA.m_vertexOffset = 0;
- b3Float4 localCenter = b3MakeFloat4(0.f,0.f,0.f,0.f);
+ b3Float4 localCenter = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
- b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];
+ b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset + f];
b3Aabb triAabb;
- triAabb.m_minVec = b3MakeFloat4(1e30f,1e30f,1e30f,0.f);
- triAabb.m_maxVec = b3MakeFloat4(-1e30f,-1e30f,-1e30f,0.f);
-
+ triAabb.m_minVec = b3MakeFloat4(1e30f, 1e30f, 1e30f, 0.f);
+ triAabb.m_maxVec = b3MakeFloat4(-1e30f, -1e30f, -1e30f, 0.f);
+
b3Float4 verticesA[3];
- for (int i=0;i<3;i++)
+ for (int i = 0; i < 3; i++)
{
- int index = indices[face.m_indexOffset+i];
- b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];
+ int index = indices[face.m_indexOffset + i];
+ b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset + index];
verticesA[i] = vert;
localCenter += vert;
-
- triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec,vert);
- triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec,vert);
+ triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec, vert);
+ triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec, vert);
}
overlap = true;
overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap;
overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap;
overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap;
-
+
if (overlap)
{
float dmin = FLT_MAX;
- int hasSeparatingAxis=5;
- b3Float4 sepAxis=b3MakeFloat4(1,2,3,4);
+ int hasSeparatingAxis = 5;
+ b3Float4 sepAxis = b3MakeFloat4(1, 2, 3, 4);
- // int localCC=0;
+ // int localCC=0;
numActualConcaveConvexTests++;
//a triangle has 3 unique edges
convexPolyhedronA.m_numUniqueEdges = 3;
convexPolyhedronA.m_uniqueEdgesOffset = 0;
b3Float4 uniqueEdgesA[3];
-
- uniqueEdgesA[0] = (verticesA[1]-verticesA[0]);
- uniqueEdgesA[1] = (verticesA[2]-verticesA[1]);
- uniqueEdgesA[2] = (verticesA[0]-verticesA[2]);
+ uniqueEdgesA[0] = (verticesA[1] - verticesA[0]);
+ uniqueEdgesA[1] = (verticesA[2] - verticesA[1]);
+ uniqueEdgesA[2] = (verticesA[0] - verticesA[2]);
convexPolyhedronA.m_faceOffset = 0;
-
- b3Float4 normal = b3MakeFloat4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
-
+
+ b3Float4 normal = b3MakeFloat4(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
+
b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES];
- int indicesA[3+3+2+2+2];
- int curUsedIndices=0;
- int fidx=0;
+ int indicesA[3 + 3 + 2 + 2 + 2];
+ int curUsedIndices = 0;
+ int fidx = 0;
//front size of triangle
{
- facesA[fidx].m_indexOffset=curUsedIndices;
+ facesA[fidx].m_indexOffset = curUsedIndices;
indicesA[0] = 0;
indicesA[1] = 1;
indicesA[2] = 2;
- curUsedIndices+=3;
+ curUsedIndices += 3;
float c = face.m_plane.w;
facesA[fidx].m_plane.x = normal.x;
facesA[fidx].m_plane.y = normal.y;
facesA[fidx].m_plane.z = normal.z;
facesA[fidx].m_plane.w = c;
- facesA[fidx].m_numIndices=3;
+ facesA[fidx].m_numIndices = 3;
}
fidx++;
//back size of triangle
{
- facesA[fidx].m_indexOffset=curUsedIndices;
- indicesA[3]=2;
- indicesA[4]=1;
- indicesA[5]=0;
- curUsedIndices+=3;
- float c = b3Dot(normal,verticesA[0]);
- // float c1 = -face.m_plane.w;
+ facesA[fidx].m_indexOffset = curUsedIndices;
+ indicesA[3] = 2;
+ indicesA[4] = 1;
+ indicesA[5] = 0;
+ curUsedIndices += 3;
+ float c = b3Dot(normal, verticesA[0]);
+ // float c1 = -face.m_plane.w;
facesA[fidx].m_plane.x = -normal.x;
facesA[fidx].m_plane.y = -normal.y;
facesA[fidx].m_plane.z = -normal.z;
facesA[fidx].m_plane.w = c;
- facesA[fidx].m_numIndices=3;
+ facesA[fidx].m_numIndices = 3;
}
fidx++;
bool addEdgePlanes = true;
if (addEdgePlanes)
{
- int numVertices=3;
- int prevVertex = numVertices-1;
- for (int i=0;i<numVertices;i++)
+ int numVertices = 3;
+ int prevVertex = numVertices - 1;
+ for (int i = 0; i < numVertices; i++)
{
b3Float4 v0 = verticesA[i];
b3Float4 v1 = verticesA[prevVertex];
-
- b3Float4 edgeNormal = b3Normalized(b3Cross(normal,v1-v0));
- float c = -b3Dot(edgeNormal,v0);
+
+ b3Float4 edgeNormal = b3Normalized(b3Cross(normal, v1 - v0));
+ float c = -b3Dot(edgeNormal, v0);
facesA[fidx].m_numIndices = 2;
- facesA[fidx].m_indexOffset=curUsedIndices;
- indicesA[curUsedIndices++]=i;
- indicesA[curUsedIndices++]=prevVertex;
-
+ facesA[fidx].m_indexOffset = curUsedIndices;
+ indicesA[curUsedIndices++] = i;
+ indicesA[curUsedIndices++] = prevVertex;
+
facesA[fidx].m_plane.x = edgeNormal.x;
facesA[fidx].m_plane.y = edgeNormal.y;
facesA[fidx].m_plane.z = edgeNormal.z;
@@ -701,8 +670,7 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
}
}
convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES;
- convexPolyhedronA.m_localCenter = localCenter*(1.f/3.f);
-
+ convexPolyhedronA.m_localCenter = localCenter * (1.f / 3.f);
b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
posA.w = 0.f;
@@ -710,23 +678,20 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
posB.w = 0.f;
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
- b3Quaternion ornB =rigidBodies[bodyIndexB].m_quat;
-
-
-
+ b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
///////////////////
///compound shape support
- if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
+ if (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
{
int compoundChild = concavePairs[pairIdx].w;
- int childShapeIndexB = compoundChild;//collidables[collidableIndexB].m_shapeIndex+compoundChild;
+ int childShapeIndexB = compoundChild; //collidables[collidableIndexB].m_shapeIndex+compoundChild;
int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
- b3Float4 newPosB = b3TransformPoint(childPosB,posB,ornB);
- b3Quaternion newOrnB = b3QuatMul(ornB,childOrnB);
+ b3Float4 newPosB = b3TransformPoint(childPosB, posB, ornB);
+ b3Quaternion newOrnB = b3QuatMul(ornB, childOrnB);
posB = newPosB;
ornB = newOrnB;
shapeIndexB = collidables[childColIndexB].m_shapeIndex;
@@ -736,59 +701,61 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
b3Float4 c0local = convexPolyhedronA.m_localCenter;
b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter;
- b3Float4 c1 = b3TransformPoint(c1local,posB,ornB);
+ b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
const b3Float4 DeltaC2 = c0 - c1;
-
- bool sepA = b3FindSeparatingAxis( &convexPolyhedronA, &convexShapes[shapeIndexB],
- posA,ornA,
- posB,ornB,
- DeltaC2,
- verticesA,uniqueEdgesA,facesA,indicesA,
- vertices,uniqueEdges,faces,indices,
- &sepAxis,&dmin);
+ bool sepA = b3FindSeparatingAxis(&convexPolyhedronA, &convexShapes[shapeIndexB],
+ posA, ornA,
+ posB, ornB,
+ DeltaC2,
+ verticesA, uniqueEdgesA, facesA, indicesA,
+ vertices, uniqueEdges, faces, indices,
+ &sepAxis, &dmin);
hasSeparatingAxis = 4;
if (!sepA)
{
hasSeparatingAxis = 0;
- } else
+ }
+ else
{
- bool sepB = b3FindSeparatingAxis( &convexShapes[shapeIndexB],&convexPolyhedronA,
- posB,ornB,
- posA,ornA,
- DeltaC2,
- vertices,uniqueEdges,faces,indices,
- verticesA,uniqueEdgesA,facesA,indicesA,
- &sepAxis,&dmin);
+ bool sepB = b3FindSeparatingAxis(&convexShapes[shapeIndexB], &convexPolyhedronA,
+ posB, ornB,
+ posA, ornA,
+ DeltaC2,
+ vertices, uniqueEdges, faces, indices,
+ verticesA, uniqueEdgesA, facesA, indicesA,
+ &sepAxis, &dmin);
if (!sepB)
{
hasSeparatingAxis = 0;
- } else
+ }
+ else
{
- bool sepEE = b3FindSeparatingAxisEdgeEdge( &convexPolyhedronA, &convexShapes[shapeIndexB],
- posA,ornA,
- posB,ornB,
- DeltaC2,
- verticesA,uniqueEdgesA,facesA,indicesA,
- vertices,uniqueEdges,faces,indices,
- &sepAxis,&dmin,true);
-
+ bool sepEE = b3FindSeparatingAxisEdgeEdge(&convexPolyhedronA, &convexShapes[shapeIndexB],
+ posA, ornA,
+ posB, ornB,
+ DeltaC2,
+ verticesA, uniqueEdgesA, facesA, indicesA,
+ vertices, uniqueEdges, faces, indices,
+ &sepAxis, &dmin, true);
+
if (!sepEE)
{
hasSeparatingAxis = 0;
- } else
+ }
+ else
{
hasSeparatingAxis = 1;
}
}
- }
-
+ }
+
if (hasSeparatingAxis)
{
- hasSeparatingNormals[i]=1;
+ hasSeparatingNormals[i] = 1;
sepAxis.w = dmin;
- concaveSeparatingNormalsOut[pairIdx]=sepAxis;
+ concaveSeparatingNormalsOut[pairIdx] = sepAxis;
//now compute clipping faces A and B, and world-space clipping vertices A and B...
@@ -796,37 +763,35 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
float maxDist = 0.02f;
b3FindClippingFaces(sepAxis,
- &convexPolyhedronA,
- &convexShapes[shapeIndexB],
- posA,ornA,
- posB,ornB,
- worldVertsA1Out,
- worldNormalsA1Out,
- worldVertsB1Out,
- vertexFaceCapacity,
- minDist, maxDist,
- verticesA,
- facesA,
- indicesA,
-
- vertices,
- faces,
- indices,
- clippingFacesOut, pairIdx);
-
- } else
- {
+ &convexPolyhedronA,
+ &convexShapes[shapeIndexB],
+ posA, ornA,
+ posB, ornB,
+ worldVertsA1Out,
+ worldNormalsA1Out,
+ worldVertsB1Out,
+ vertexFaceCapacity,
+ minDist, maxDist,
+ verticesA,
+ facesA,
+ indicesA,
+
+ vertices,
+ faces,
+ indices,
+ clippingFacesOut, pairIdx);
+ }
+ else
+ {
//mark this pair as in-active
concavePairs[pairIdx].w = -1;
}
}
else
- {
+ {
//mark this pair as in-active
concavePairs[pairIdx].w = -1;
}
}
-
-#endif //B3_FIND_CONCAVE_SEPARATING_AXIS_H
-
+#endif //B3_FIND_CONCAVE_SEPARATING_AXIS_H
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-11 23:43:34 +0000