dali-physics/third-party/bullet3/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp

   1 #include "b3CpuNarrowPhase.h"
   2 #include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
   3 #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
   4
   5 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
   6 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h"
   7
   8 struct b3CpuNarrowPhaseInternalData
   9 {
  10         b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU;
  11         b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
  12         b3AlignedObjectArray<b3ConvexUtility*> m_convexData;
  13         b3Config m_config;
  14
  15         b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
  16         b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
  17         b3AlignedObjectArray<b3Vector3> m_convexVertices;
  18         b3AlignedObjectArray<int> m_convexIndices;
  19         b3AlignedObjectArray<b3GpuFace> m_convexFaces;
  20
  21         b3AlignedObjectArray<b3Contact4Data> m_contacts;
  22
  23         int m_numAcceleratedShapes;
  24 };
  25
  26 const b3AlignedObjectArray<b3Contact4Data>& b3CpuNarrowPhase::getContacts() const
  27 {
  28         return m_data->m_contacts;
  29 }
  30
  31 b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex)
  32 {
  33         return m_data->m_collidablesCPU[collidableIndex];
  34 }
  35
  36 const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) const
  37 {
  38         return m_data->m_collidablesCPU[collidableIndex];
  39 }
  40
  41 b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
  42 {
  43         m_data = new b3CpuNarrowPhaseInternalData;
  44         m_data->m_config = config;
  45         m_data->m_numAcceleratedShapes = 0;
  46 }
  47
  48 b3CpuNarrowPhase::~b3CpuNarrowPhase()
  49 {
  50         delete m_data;
  51 }
  52
  53 void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies)
  54 {
  55         int nPairs = pairs.size();
  56         int numContacts = 0;
  57         int maxContactCapacity = m_data->m_config.m_maxContactCapacity;
  58         m_data->m_contacts.resize(maxContactCapacity);
  59
  60         for (int i = 0; i < nPairs; i++)
  61         {
  62                 int bodyIndexA = pairs[i].x;
  63                 int bodyIndexB = pairs[i].y;
  64                 int collidableIndexA = bodies[bodyIndexA].m_collidableIdx;
  65                 int collidableIndexB = bodies[bodyIndexB].m_collidableIdx;
  66
  67                 if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
  68                         m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
  69                 {
  70                         //                      computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
  71                         //                              &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
  72                 }
  73
  74                 if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
  75                         m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_SPHERE)
  76                 {
  77                         //                      computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
  78                         //                              &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
  79                         //printf("convex-sphere\n");
  80                 }
  81
  82                 if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
  83                         m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
  84                 {
  85                         //                      computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
  86                         //                      &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
  87                         //                      printf("convex-plane\n");
  88                 }
  89
  90                 if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
  91                         m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
  92                 {
  93                         //                      computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
  94                         //                      &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
  95                         //                      printf("plane-convex\n");
  96                 }
  97
  98                 if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
  99                         m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
 100                 {
 101                         //                      computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
 102                         //                      &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
 103                         //                      nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
 104                         //                      printf("convex-plane\n");
 105                 }
 106
 107                 if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
 108                         m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
 109                 {
 110                         //                      computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
 111                         //                      &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
 112                         //                      printf("convex-plane\n");
 113                 }
 114
 115                 if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
 116                         m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
 117                 {
 118                         //                      computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
 119                         //                      &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
 120                         //                      printf("plane-convex\n");
 121                 }
 122
 123                 if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
 124                         m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
 125                 {
 126                         //printf("pairs[i].z=%d\n",pairs[i].z);
 127                         //int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
 128                         //              m_data->m_collidablesCPU,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
 129                         int contactIndex = b3ContactConvexConvexSAT(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, bodies,
 130                                                                                                                 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);
 131
 132                         if (contactIndex >= 0)
 133                         {
 134                                 pairs[i].z = contactIndex;
 135                         }
 136                         //                      printf("plane-convex\n");
 137                 }
 138         }
 139
 140         m_data->m_contacts.resize(numContacts);
 141 }
 142
 143 int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
 144 {
 145         int collidableIndex = allocateCollidable();
 146         if (collidableIndex < 0)
 147                 return collidableIndex;
 148
 149         b3Collidable& col = m_data->m_collidablesCPU[collidableIndex];
 150         col.m_shapeType = SHAPE_CONVEX_HULL;
 151         col.m_shapeIndex = -1;
 152
 153         {
 154                 b3Vector3 localCenter = b3MakeVector3(0, 0, 0);
 155                 for (int i = 0; i < utilPtr->m_vertices.size(); i++)
 156                         localCenter += utilPtr->m_vertices[i];
 157                 localCenter *= (1.f / utilPtr->m_vertices.size());
 158                 utilPtr->m_localCenter = localCenter;
 159
 160                 col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr, col);
 161         }
 162
 163         if (col.m_shapeIndex >= 0)
 164         {
 165                 b3Aabb aabb;
 166
 167                 b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f);
 168                 b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f);
 169
 170                 for (int i = 0; i < utilPtr->m_vertices.size(); i++)
 171                 {
 172                         myAabbMin.setMin(utilPtr->m_vertices[i]);
 173                         myAabbMax.setMax(utilPtr->m_vertices[i]);
 174                 }
 175                 aabb.m_min[0] = myAabbMin[0];
 176                 aabb.m_min[1] = myAabbMin[1];
 177                 aabb.m_min[2] = myAabbMin[2];
 178                 aabb.m_minIndices[3] = 0;
 179
 180                 aabb.m_max[0] = myAabbMax[0];
 181                 aabb.m_max[1] = myAabbMax[1];
 182                 aabb.m_max[2] = myAabbMax[2];
 183                 aabb.m_signedMaxIndices[3] = 0;
 184
 185                 m_data->m_localShapeAABBCPU.push_back(aabb);
 186         }
 187
 188         return collidableIndex;
 189 }
 190
 191 int b3CpuNarrowPhase::allocateCollidable()
 192 {
 193         int curSize = m_data->m_collidablesCPU.size();
 194         if (curSize < m_data->m_config.m_maxConvexShapes)
 195         {
 196                 m_data->m_collidablesCPU.expand();
 197                 return curSize;
 198         }
 199         else
 200         {
 201                 b3Error("allocateCollidable out-of-range %d\n", m_data->m_config.m_maxConvexShapes);
 202         }
 203         return -1;
 204 }
 205
 206 int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
 207 {
 208         b3AlignedObjectArray<b3Vector3> verts;
 209
 210         unsigned char* vts = (unsigned char*)vertices;
 211         for (int i = 0; i < numVertices; i++)
 212         {
 213                 float* vertex = (float*)&vts[i * strideInBytes];
 214                 verts.push_back(b3MakeVector3(vertex[0] * scaling[0], vertex[1] * scaling[1], vertex[2] * scaling[2]));
 215         }
 216
 217         b3ConvexUtility* utilPtr = new b3ConvexUtility();
 218         bool merge = true;
 219         if (numVertices)
 220         {
 221                 utilPtr->initializePolyhedralFeatures(&verts[0], verts.size(), merge);
 222         }
 223
 224         int collidableIndex = registerConvexHullShape(utilPtr);
 225
 226         delete utilPtr;
 227         return collidableIndex;
 228 }
 229
 230 int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr, b3Collidable& col)
 231 {
 232         m_data->m_convexData.resize(m_data->m_numAcceleratedShapes + 1);
 233         m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1);
 234
 235         b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1);
 236         convex.mC = convexPtr->mC;
 237         convex.mE = convexPtr->mE;
 238         convex.m_extents = convexPtr->m_extents;
 239         convex.m_localCenter = convexPtr->m_localCenter;
 240         convex.m_radius = convexPtr->m_radius;
 241
 242         convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size();
 243         int edgeOffset = m_data->m_uniqueEdges.size();
 244         convex.m_uniqueEdgesOffset = edgeOffset;
 245
 246         m_data->m_uniqueEdges.resize(edgeOffset + convex.m_numUniqueEdges);
 247
 248         //convex data here
 249         int i;
 250         for (i = 0; i < convexPtr->m_uniqueEdges.size(); i++)
 251         {
 252                 m_data->m_uniqueEdges[edgeOffset + i] = convexPtr->m_uniqueEdges[i];
 253         }
 254
 255         int faceOffset = m_data->m_convexFaces.size();
 256         convex.m_faceOffset = faceOffset;
 257         convex.m_numFaces = convexPtr->m_faces.size();
 258
 259         m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces);
 260
 261         for (i = 0; i < convexPtr->m_faces.size(); i++)
 262         {
 263                 m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
 264                                                                                                                                                            convexPtr->m_faces[i].m_plane[1],
 265                                                                                                                                                            convexPtr->m_faces[i].m_plane[2],
 266                                                                                                                                                            convexPtr->m_faces[i].m_plane[3]);
 267
 268                 int indexOffset = m_data->m_convexIndices.size();
 269                 int numIndices = convexPtr->m_faces[i].m_indices.size();
 270                 m_data->m_convexFaces[convex.m_faceOffset + i].m_numIndices = numIndices;
 271                 m_data->m_convexFaces[convex.m_faceOffset + i].m_indexOffset = indexOffset;
 272                 m_data->m_convexIndices.resize(indexOffset + numIndices);
 273                 for (int p = 0; p < numIndices; p++)
 274                 {
 275                         m_data->m_convexIndices[indexOffset + p] = convexPtr->m_faces[i].m_indices[p];
 276                 }
 277         }
 278
 279         convex.m_numVertices = convexPtr->m_vertices.size();
 280         int vertexOffset = m_data->m_convexVertices.size();
 281         convex.m_vertexOffset = vertexOffset;
 282
 283         m_data->m_convexVertices.resize(vertexOffset + convex.m_numVertices);
 284         for (int i = 0; i < convexPtr->m_vertices.size(); i++)
 285         {
 286                 m_data->m_convexVertices[vertexOffset + i] = convexPtr->m_vertices[i];
 287         }
 288
 289         (m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
 290
 291         return m_data->m_numAcceleratedShapes++;
 292 }
 293
 294 const b3Aabb& b3CpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const
 295 {
 296         return m_data->m_localShapeAABBCPU[collidableIndex];
 297 }