summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp')
-rw-r--r--thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp190
1 files changed, 82 insertions, 108 deletions
diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp
index c3134b2c65..e0b2161100 100644
--- a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp
+++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp
@@ -5,15 +5,13 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h"
-
struct b3CpuNarrowPhaseInternalData
{
b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU;
- b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
+ b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
b3AlignedObjectArray<b3ConvexUtility*> m_convexData;
b3Config m_config;
-
b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
b3AlignedObjectArray<b3Vector3> m_convexVertices;
@@ -22,10 +20,9 @@ struct b3CpuNarrowPhaseInternalData
b3AlignedObjectArray<b3Contact4Data> m_contacts;
- int m_numAcceleratedShapes;
+ int m_numAcceleratedShapes;
};
-
const b3AlignedObjectArray<b3Contact4Data>& b3CpuNarrowPhase::getContacts() const
{
return m_data->m_contacts;
@@ -41,7 +38,6 @@ const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) cons
return m_data->m_collidablesCPU[collidableIndex];
}
-
b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
{
m_data = new b3CpuNarrowPhaseInternalData;
@@ -61,7 +57,7 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
int maxContactCapacity = m_data->m_config.m_maxContactCapacity;
m_data->m_contacts.resize(maxContactCapacity);
- for (int i=0;i<nPairs;i++)
+ for (int i = 0; i < nPairs; i++)
{
int bodyIndexA = pairs[i].x;
int bodyIndexB = pairs[i].y;
@@ -71,64 +67,57 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
{
-// computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
-// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+ // computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
+ // &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_SPHERE)
{
-// computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
-// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+ // computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+ // &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
//printf("convex-sphere\n");
-
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
{
-// computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
-// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-// printf("convex-plane\n");
-
+ // computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+ // &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+ // printf("convex-plane\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
{
-// computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
-// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-// printf("plane-convex\n");
-
+ // computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
+ // &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+ // printf("plane-convex\n");
}
- if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
+ if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
{
-// computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
-// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
-// nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
-// printf("convex-plane\n");
-
+ // computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+ // &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
+ // nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
+ // printf("convex-plane\n");
}
-
- if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
+ if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
{
-// computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
-// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-// printf("convex-plane\n");
-
+ // computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+ // &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+ // printf("convex-plane\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
{
-// computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
-// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-// printf("plane-convex\n");
-
+ // computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
+ // &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+ // printf("plane-convex\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
@@ -137,54 +126,48 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
//printf("pairs[i].z=%d\n",pairs[i].z);
//int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
// m_data->m_collidablesCPU,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
- int contactIndex = b3ContactConvexConvexSAT(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
- m_data->m_collidablesCPU,m_data->m_convexPolyhedra,m_data->m_convexVertices,m_data->m_uniqueEdges,m_data->m_convexIndices,m_data->m_convexFaces,m_data->m_contacts,numContacts,maxContactCapacity);
-
+ int contactIndex = b3ContactConvexConvexSAT(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, bodies,
+ m_data->m_collidablesCPU, m_data->m_convexPolyhedra, m_data->m_convexVertices, m_data->m_uniqueEdges, m_data->m_convexIndices, m_data->m_convexFaces, m_data->m_contacts, numContacts, maxContactCapacity);
- if (contactIndex>=0)
+ if (contactIndex >= 0)
{
pairs[i].z = contactIndex;
}
-// printf("plane-convex\n");
-
+ // printf("plane-convex\n");
}
-
-
}
m_data->m_contacts.resize(numContacts);
}
-int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
+int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
{
int collidableIndex = allocateCollidable();
- if (collidableIndex<0)
+ if (collidableIndex < 0)
return collidableIndex;
-
b3Collidable& col = m_data->m_collidablesCPU[collidableIndex];
col.m_shapeType = SHAPE_CONVEX_HULL;
col.m_shapeIndex = -1;
-
-
+
{
- b3Vector3 localCenter=b3MakeVector3(0,0,0);
- for (int i=0;i<utilPtr->m_vertices.size();i++)
- localCenter+=utilPtr->m_vertices[i];
- localCenter*= (1.f/utilPtr->m_vertices.size());
+ b3Vector3 localCenter = b3MakeVector3(0, 0, 0);
+ for (int i = 0; i < utilPtr->m_vertices.size(); i++)
+ localCenter += utilPtr->m_vertices[i];
+ localCenter *= (1.f / utilPtr->m_vertices.size());
utilPtr->m_localCenter = localCenter;
- col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr,col);
+ col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr, col);
}
- if (col.m_shapeIndex>=0)
+ if (col.m_shapeIndex >= 0)
{
b3Aabb aabb;
-
- b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f);
- b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f);
- for (int i=0;i<utilPtr->m_vertices.size();i++)
+ b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f);
+ b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f);
+
+ for (int i = 0; i < utilPtr->m_vertices.size(); i++)
{
myAabbMin.setMin(utilPtr->m_vertices[i]);
myAabbMax.setMax(utilPtr->m_vertices[i]);
@@ -200,44 +183,42 @@ int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
aabb.m_signedMaxIndices[3] = 0;
m_data->m_localShapeAABBCPU.push_back(aabb);
-
}
-
+
return collidableIndex;
}
-int b3CpuNarrowPhase::allocateCollidable()
+int b3CpuNarrowPhase::allocateCollidable()
{
int curSize = m_data->m_collidablesCPU.size();
- if (curSize<m_data->m_config.m_maxConvexShapes)
+ if (curSize < m_data->m_config.m_maxConvexShapes)
{
m_data->m_collidablesCPU.expand();
return curSize;
}
else
{
- b3Error("allocateCollidable out-of-range %d\n",m_data->m_config.m_maxConvexShapes);
+ b3Error("allocateCollidable out-of-range %d\n", m_data->m_config.m_maxConvexShapes);
}
return -1;
-
}
-int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
+int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
{
b3AlignedObjectArray<b3Vector3> verts;
- unsigned char* vts = (unsigned char*) vertices;
- for (int i=0;i<numVertices;i++)
+ unsigned char* vts = (unsigned char*)vertices;
+ for (int i = 0; i < numVertices; i++)
{
- float* vertex = (float*) &vts[i*strideInBytes];
- verts.push_back(b3MakeVector3(vertex[0]*scaling[0],vertex[1]*scaling[1],vertex[2]*scaling[2]));
+ float* vertex = (float*)&vts[i * strideInBytes];
+ verts.push_back(b3MakeVector3(vertex[0] * scaling[0], vertex[1] * scaling[1], vertex[2] * scaling[2]));
}
b3ConvexUtility* utilPtr = new b3ConvexUtility();
bool merge = true;
if (numVertices)
{
- utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge);
+ utilPtr->initializePolyhedralFeatures(&verts[0], verts.size(), merge);
}
int collidableIndex = registerConvexHullShape(utilPtr);
@@ -246,74 +227,67 @@ int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideI
return collidableIndex;
}
-
-int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col)
+int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr, b3Collidable& col)
{
+ m_data->m_convexData.resize(m_data->m_numAcceleratedShapes + 1);
+ m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1);
- m_data->m_convexData.resize(m_data->m_numAcceleratedShapes+1);
- m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1);
-
-
- b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1);
+ b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1);
convex.mC = convexPtr->mC;
convex.mE = convexPtr->mE;
- convex.m_extents= convexPtr->m_extents;
+ convex.m_extents = convexPtr->m_extents;
convex.m_localCenter = convexPtr->m_localCenter;
convex.m_radius = convexPtr->m_radius;
-
+
convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size();
int edgeOffset = m_data->m_uniqueEdges.size();
convex.m_uniqueEdgesOffset = edgeOffset;
-
- m_data->m_uniqueEdges.resize(edgeOffset+convex.m_numUniqueEdges);
-
+
+ m_data->m_uniqueEdges.resize(edgeOffset + convex.m_numUniqueEdges);
+
//convex data here
int i;
- for ( i=0;i<convexPtr->m_uniqueEdges.size();i++)
+ for (i = 0; i < convexPtr->m_uniqueEdges.size(); i++)
{
- m_data->m_uniqueEdges[edgeOffset+i] = convexPtr->m_uniqueEdges[i];
+ m_data->m_uniqueEdges[edgeOffset + i] = convexPtr->m_uniqueEdges[i];
}
-
+
int faceOffset = m_data->m_convexFaces.size();
convex.m_faceOffset = faceOffset;
convex.m_numFaces = convexPtr->m_faces.size();
- m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces);
-
+ m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces);
- for (i=0;i<convexPtr->m_faces.size();i++)
+ for (i = 0; i < convexPtr->m_faces.size(); i++)
{
- m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
- convexPtr->m_faces[i].m_plane[1],
- convexPtr->m_faces[i].m_plane[2],
- convexPtr->m_faces[i].m_plane[3]);
+ m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
+ convexPtr->m_faces[i].m_plane[1],
+ convexPtr->m_faces[i].m_plane[2],
+ convexPtr->m_faces[i].m_plane[3]);
-
int indexOffset = m_data->m_convexIndices.size();
int numIndices = convexPtr->m_faces[i].m_indices.size();
- m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices;
- m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset;
- m_data->m_convexIndices.resize(indexOffset+numIndices);
- for (int p=0;p<numIndices;p++)
+ m_data->m_convexFaces[convex.m_faceOffset + i].m_numIndices = numIndices;
+ m_data->m_convexFaces[convex.m_faceOffset + i].m_indexOffset = indexOffset;
+ m_data->m_convexIndices.resize(indexOffset + numIndices);
+ for (int p = 0; p < numIndices; p++)
{
- m_data->m_convexIndices[indexOffset+p] = convexPtr->m_faces[i].m_indices[p];
+ m_data->m_convexIndices[indexOffset + p] = convexPtr->m_faces[i].m_indices[p];
}
}
-
+
convex.m_numVertices = convexPtr->m_vertices.size();
int vertexOffset = m_data->m_convexVertices.size();
- convex.m_vertexOffset =vertexOffset;
-
- m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices);
- for (int i=0;i<convexPtr->m_vertices.size();i++)
+ convex.m_vertexOffset = vertexOffset;
+
+ m_data->m_convexVertices.resize(vertexOffset + convex.m_numVertices);
+ for (int i = 0; i < convexPtr->m_vertices.size(); i++)
{
- m_data->m_convexVertices[vertexOffset+i] = convexPtr->m_vertices[i];
+ m_data->m_convexVertices[vertexOffset + i] = convexPtr->m_vertices[i];
}
(m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
-
-
-
+
return m_data->m_numAcceleratedShapes++;
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-21 14:31:05 +0000