move parts of collision pipeline to shared header files (work-in-progress)
This commit is contained in:
erwin coumans
@@ -0,0 +1,20 @@
|
||||
|
||||
#ifndef B3_BVH_SUBTREE_INFO_DATA_H
|
||||
#define B3_BVH_SUBTREE_INFO_DATA_H
|
||||
|
||||
typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t;
|
||||
|
||||
struct b3BvhSubtreeInfoData
|
||||
{
|
||||
//12 bytes
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
//4 bytes, points to the root of the subtree
|
||||
int m_rootNodeIndex;
|
||||
//4 bytes
|
||||
int m_subtreeSize;
|
||||
int m_padding[3];
|
||||
};
|
||||
|
||||
#endif //B3_BVH_SUBTREE_INFO_DATA_H
|
||||
|
||||
@@ -0,0 +1,126 @@
|
||||
|
||||
|
||||
#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"
|
||||
|
||||
|
||||
|
||||
// work-in-progress
|
||||
void b3BvhTraversal( __global const b3Int4* pairs,
|
||||
__global const b3RigidBodyData* rigidBodies,
|
||||
__global const b3Collidable* collidables,
|
||||
__global b3Aabb* aabbs,
|
||||
__global b3Int4* concavePairsOut,
|
||||
__global volatile int* numConcavePairsOut,
|
||||
__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
|
||||
__global const b3QuantizedBvhNode* quantizedNodesRoot,
|
||||
__global const b3BvhInfo* bvhInfos,
|
||||
int numPairs,
|
||||
int maxNumConcavePairsCapacity,
|
||||
int id)
|
||||
{
|
||||
|
||||
int bodyIndexA = pairs[id].x;
|
||||
int bodyIndexB = pairs[id].y;
|
||||
int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
|
||||
int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
|
||||
|
||||
//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_CONCAVE_TRIMESH)
|
||||
return;
|
||||
|
||||
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
|
||||
|
||||
if (shapeTypeB!=SHAPE_CONVEX_HULL &&
|
||||
shapeTypeB!=SHAPE_SPHERE &&
|
||||
shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS
|
||||
)
|
||||
return;
|
||||
|
||||
b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
|
||||
|
||||
b3Float4 bvhAabbMin = bvhInfo.m_aabbMin;
|
||||
b3Float4 bvhAabbMax = bvhInfo.m_aabbMax;
|
||||
b3Float4 bvhQuantization = bvhInfo.m_quantization;
|
||||
int numSubtreeHeaders = bvhInfo.m_numSubTrees;
|
||||
__global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
|
||||
__global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
|
||||
|
||||
|
||||
unsigned short int quantizedQueryAabbMin[3];
|
||||
unsigned short int quantizedQueryAabbMax[3];
|
||||
b3QuantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_minVec,false,bvhAabbMin, bvhAabbMax,bvhQuantization);
|
||||
b3QuantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_maxVec,true ,bvhAabbMin, bvhAabbMax,bvhQuantization);
|
||||
|
||||
for (int i=0;i<numSubtreeHeaders;i++)
|
||||
{
|
||||
b3BvhSubtreeInfoData subtree = subtreeHeaders[i];
|
||||
|
||||
int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
|
||||
if (overlap != 0)
|
||||
{
|
||||
int startNodeIndex = subtree.m_rootNodeIndex;
|
||||
int endNodeIndex = subtree.m_rootNodeIndex+subtree.m_subtreeSize;
|
||||
int curIndex = startNodeIndex;
|
||||
int escapeIndex;
|
||||
int isLeafNode;
|
||||
int aabbOverlap;
|
||||
while (curIndex < endNodeIndex)
|
||||
{
|
||||
b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex];
|
||||
aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode.m_quantizedAabbMin,rootNode.m_quantizedAabbMax);
|
||||
isLeafNode = b3IsLeaf(&rootNode);
|
||||
if (aabbOverlap)
|
||||
{
|
||||
if (isLeafNode)
|
||||
{
|
||||
int triangleIndex = b3GetTriangleIndex(&rootNode);
|
||||
if (shapeTypeB==SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
|
||||
int pairIdx = b3AtomicAdd (numConcavePairsOut,numChildrenB);
|
||||
for (int b=0;b<numChildrenB;b++)
|
||||
{
|
||||
if ((pairIdx+b)<maxNumConcavePairsCapacity)
|
||||
{
|
||||
int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+b;
|
||||
b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,childShapeIndexB);
|
||||
concavePairsOut[pairIdx+b] = newPair;
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
int pairIdx = b3AtomicInc(numConcavePairsOut);
|
||||
if (pairIdx<maxNumConcavePairsCapacity)
|
||||
{
|
||||
b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,0);
|
||||
concavePairsOut[pairIdx] = newPair;
|
||||
}
|
||||
}
|
||||
}
|
||||
curIndex++;
|
||||
} else
|
||||
{
|
||||
if (isLeafNode)
|
||||
{
|
||||
curIndex++;
|
||||
} else
|
||||
{
|
||||
escapeIndex = b3GetEscapeIndex(&rootNode);
|
||||
curIndex += escapeIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,474 @@
|
||||
#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;
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
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)
|
||||
{
|
||||
|
||||
|
||||
b3Float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = posB1;
|
||||
posB.w = 0.f;
|
||||
|
||||
int curPlaneTests=0;
|
||||
|
||||
int curEdgeEdge = 0;
|
||||
// Test edges
|
||||
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)
|
||||
result = false;
|
||||
|
||||
float d0 = Max0 - Min1;
|
||||
float d1 = Max1 - Min0;
|
||||
dist = d0<d1 ? d0:d1;
|
||||
result = true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
if(dist<*dmin)
|
||||
{
|
||||
*dmin = dist;
|
||||
*sep = crossje;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
if((b3Dot(-DeltaC2,*sep))>0.0f)
|
||||
{
|
||||
*sep = -(*sep);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// work-in-progress
|
||||
__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,
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
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];
|
||||
b3Float4 triMinAabb, triMaxAabb;
|
||||
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);
|
||||
|
||||
if (!sepEE)
|
||||
{
|
||||
hasSeparatingAxis = 0;
|
||||
} else
|
||||
{
|
||||
hasSeparatingAxis = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (hasSeparatingAxis)
|
||||
{
|
||||
sepAxis.w = dmin;
|
||||
concaveSeparatingNormalsOut[pairIdx]=sepAxis;
|
||||
} 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
|
||||
|
||||
@@ -0,0 +1,90 @@
|
||||
|
||||
|
||||
#ifndef B3_QUANTIZED_BVH_NODE_H
|
||||
#define B3_QUANTIZED_BVH_NODE_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
|
||||
#define B3_MAX_NUM_PARTS_IN_BITS 10
|
||||
|
||||
///b3QuantizedBvhNodeData is a compressed aabb node, 16 bytes.
|
||||
///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
|
||||
typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t;
|
||||
|
||||
struct b3QuantizedBvhNodeData
|
||||
{
|
||||
//12 bytes
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
//4 bytes
|
||||
int m_escapeIndexOrTriangleIndex;
|
||||
};
|
||||
|
||||
inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
unsigned int x=0;
|
||||
unsigned int y = (~(x&0))<<(31-B3_MAX_NUM_PARTS_IN_BITS);
|
||||
// Get only the lower bits where the triangle index is stored
|
||||
return (rootNode->m_escapeIndexOrTriangleIndex&~(y));
|
||||
}
|
||||
|
||||
inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
//skipindex is negative (internal node), triangleindex >=0 (leafnode)
|
||||
return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;
|
||||
}
|
||||
|
||||
inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
return -rootNode->m_escapeIndexOrTriangleIndex;
|
||||
}
|
||||
|
||||
inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2,int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
|
||||
{
|
||||
b3Float4 clampedPoint = b3MaxFloat4(point2,bvhAabbMin);
|
||||
clampedPoint = b3MinFloat4 (clampedPoint, bvhAabbMax);
|
||||
|
||||
b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
|
||||
if (isMax)
|
||||
{
|
||||
out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1));
|
||||
out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1));
|
||||
out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1));
|
||||
} else
|
||||
{
|
||||
out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe));
|
||||
out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe));
|
||||
out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
|
||||
const unsigned short int* aabbMin1,
|
||||
const unsigned short int* aabbMax1,
|
||||
const unsigned short int* aabbMin2,
|
||||
const unsigned short int* aabbMax2)
|
||||
{
|
||||
//int overlap = 1;
|
||||
if (aabbMin1[0] > aabbMax2[0])
|
||||
return 0;
|
||||
if (aabbMax1[0] < aabbMin2[0])
|
||||
return 0;
|
||||
if (aabbMin1[1] > aabbMax2[1])
|
||||
return 0;
|
||||
if (aabbMax1[1] < aabbMin2[1])
|
||||
return 0;
|
||||
if (aabbMin1[2] > aabbMax2[2])
|
||||
return 0;
|
||||
if (aabbMax1[2] < aabbMin2[2])
|
||||
return 0;
|
||||
return 1;
|
||||
//overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap;
|
||||
//overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap;
|
||||
//overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap;
|
||||
//return overlap;
|
||||
}
|
||||
|
||||
|
||||
#endif //B3_QUANTIZED_BVH_NODE_H
|
||||
Reference in New Issue
Block a user