833 lines
25 KiB
C++
833 lines
25 KiB
C++
#ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H
|
|
#define B3_FIND_CONCAVE_SEPARATING_AXIS_H
|
|
|
|
#define B3_TRIANGLE_NUM_CONVEX_FACES 5
|
|
|
|
|
|
#include "Bullet3Common/shared/b3Int4.h"
|
|
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
|
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
|
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
|
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
|
|
#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)
|
|
{
|
|
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++)
|
|
{
|
|
float dp = b3Dot(vertices[hull->m_vertexOffset+i],localDir);
|
|
if(dp < min[0])
|
|
min[0] = dp;
|
|
if(dp > max[0])
|
|
max[0] = dp;
|
|
}
|
|
if(min[0]>max[0])
|
|
{
|
|
float tmp = min[0];
|
|
min[0] = max[0];
|
|
max[0] = tmp;
|
|
}
|
|
min[0] += offset;
|
|
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)
|
|
{
|
|
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)
|
|
return false;
|
|
|
|
float d0 = Max0 - Min1;
|
|
float d1 = Max1 - Min0;
|
|
*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)
|
|
{
|
|
|
|
|
|
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;
|
|
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;
|
|
// Test normals from hullA
|
|
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;
|
|
curPlaneTests++;
|
|
float d;
|
|
if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d))
|
|
return false;
|
|
if(d<*dmin)
|
|
{
|
|
*dmin = d;
|
|
*sep = faceANormalWS;
|
|
}
|
|
}
|
|
}
|
|
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)
|
|
{
|
|
|
|
|
|
b3Float4 posA = posA1;
|
|
posA.w = 0.f;
|
|
b3Float4 posB = posB1;
|
|
posB.w = 0.f;
|
|
|
|
// int curPlaneTests=0;
|
|
|
|
int curEdgeEdge = 0;
|
|
// Test edges
|
|
static int maxEdgeTests = 0;
|
|
int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges;
|
|
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);
|
|
|
|
}
|
|
|
|
|
|
if (searchAllEdgeEdge)
|
|
{
|
|
for(int e0=0;e0<hullA->m_numUniqueEdges;e0++)
|
|
{
|
|
const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0];
|
|
b3Float4 edge0World = b3QuatRotate(ornA,edge0);
|
|
|
|
for(int e1=0;e1<hullB->m_numUniqueEdges;e1++)
|
|
{
|
|
const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1];
|
|
b3Float4 edge1World = b3QuatRotate(ornB,edge1);
|
|
|
|
|
|
b3Float4 crossje = b3Cross(edge0World,edge1World);
|
|
|
|
curEdgeEdge++;
|
|
if(!b3IsAlmostZero(crossje))
|
|
{
|
|
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;
|
|
|
|
float d0 = Max0 - Min1;
|
|
float d1 = Max1 - Min0;
|
|
dist = d0<d1 ? d0:d1;
|
|
result = true;
|
|
|
|
}
|
|
|
|
|
|
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)
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
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)
|
|
{
|
|
int numContactsOut = 0;
|
|
int numWorldVertsB1= 0;
|
|
|
|
|
|
int closestFaceB=-1;
|
|
float dmax = -FLT_MAX;
|
|
|
|
{
|
|
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 WorldNormal = b3QuatRotate(ornB, Normal);
|
|
float d = b3Dot(WorldNormal,separatingNormal);
|
|
if (d > dmax)
|
|
{
|
|
dmax = d;
|
|
closestFaceB = face;
|
|
}
|
|
}
|
|
}
|
|
|
|
{
|
|
const b3GpuFace_t polyB = facesB[hullB->m_faceOffset+closestFaceB];
|
|
const int numVertices = polyB.m_numIndices;
|
|
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);
|
|
}
|
|
}
|
|
|
|
int closestFaceA=-1;
|
|
{
|
|
float dmin = FLT_MAX;
|
|
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);
|
|
if (d < dmin)
|
|
{
|
|
dmin = d;
|
|
closestFaceA = face;
|
|
worldNormalsA1[pairIndex] = faceANormalWS;
|
|
}
|
|
}
|
|
}
|
|
|
|
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;
|
|
|
|
|
|
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
|
|
)
|
|
{
|
|
int i = pairIdx;
|
|
/* int i = get_global_id(0);
|
|
if (i>=numConcavePairs)
|
|
return;
|
|
int pairIdx = i;
|
|
*/
|
|
|
|
int bodyIndexA = concavePairs[i].x;
|
|
int bodyIndexB = concavePairs[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;
|
|
|
|
if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&&
|
|
collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
|
{
|
|
concavePairs[pairIdx].w = -1;
|
|
return;
|
|
}
|
|
|
|
hasSeparatingNormals[i] = 0;
|
|
|
|
// 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);
|
|
|
|
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);
|
|
|
|
b3Float4 verticesA[3];
|
|
for (int i=0;i<3;i++)
|
|
{
|
|
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);
|
|
|
|
}
|
|
|
|
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 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]);
|
|
|
|
|
|
convexPolyhedronA.m_faceOffset = 0;
|
|
|
|
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;
|
|
|
|
//front size of triangle
|
|
{
|
|
facesA[fidx].m_indexOffset=curUsedIndices;
|
|
indicesA[0] = 0;
|
|
indicesA[1] = 1;
|
|
indicesA[2] = 2;
|
|
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;
|
|
}
|
|
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_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;
|
|
}
|
|
fidx++;
|
|
|
|
bool addEdgePlanes = true;
|
|
if (addEdgePlanes)
|
|
{
|
|
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);
|
|
|
|
facesA[fidx].m_numIndices = 2;
|
|
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;
|
|
facesA[fidx].m_plane.w = c;
|
|
fidx++;
|
|
prevVertex = i;
|
|
}
|
|
}
|
|
convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES;
|
|
convexPolyhedronA.m_localCenter = localCenter*(1.f/3.f);
|
|
|
|
|
|
b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
|
|
posA.w = 0.f;
|
|
b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
|
|
posB.w = 0.f;
|
|
|
|
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
|
|
b3Quaternion ornB =rigidBodies[bodyIndexB].m_quat;
|
|
|
|
|
|
|
|
|
|
///////////////////
|
|
///compound shape support
|
|
|
|
if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
|
{
|
|
int compoundChild = concavePairs[pairIdx].w;
|
|
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);
|
|
posB = newPosB;
|
|
ornB = newOrnB;
|
|
shapeIndexB = collidables[childColIndexB].m_shapeIndex;
|
|
}
|
|
//////////////////
|
|
|
|
b3Float4 c0local = convexPolyhedronA.m_localCenter;
|
|
b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
|
|
b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter;
|
|
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);
|
|
hasSeparatingAxis = 4;
|
|
if (!sepA)
|
|
{
|
|
hasSeparatingAxis = 0;
|
|
} else
|
|
{
|
|
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
|
|
{
|
|
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
|
|
{
|
|
hasSeparatingAxis = 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (hasSeparatingAxis)
|
|
{
|
|
hasSeparatingNormals[i]=1;
|
|
sepAxis.w = dmin;
|
|
concaveSeparatingNormalsOut[pairIdx]=sepAxis;
|
|
|
|
//now compute clipping faces A and B, and world-space clipping vertices A and B...
|
|
|
|
float minDist = -1e30f;
|
|
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
|
|
{
|
|
//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
|
|
|