dali-physics/third-party/bullet3/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp

   1 #include "b3GpuNarrowPhase.h"
   2
   3 #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
   4 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
   5 #include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h"
   6 #include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h"
   7 #include <string.h>
   8 #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
   9 #include "Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.h"
  10 #include "Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h"
  11 #include "Bullet3Geometry/b3AabbUtil.h"
  12 #include "Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h"
  13
  14 #include "b3GpuNarrowPhaseInternalData.h"
  15 #include "Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h"
  16 #include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
  17
  18 b3GpuNarrowPhase::b3GpuNarrowPhase(cl_context ctx, cl_device_id device, cl_command_queue queue, const b3Config& config)
  19         : m_data(0), m_planeBodyIndex(-1), m_static0Index(-1), m_context(ctx), m_device(device), m_queue(queue)
  20 {
  21         m_data = new b3GpuNarrowPhaseInternalData();
  22         m_data->m_currentContactBuffer = 0;
  23
  24         memset(m_data, 0, sizeof(b3GpuNarrowPhaseInternalData));
  25
  26         m_data->m_config = config;
  27
  28         m_data->m_gpuSatCollision = new GpuSatCollision(ctx, device, queue);
  29
  30         m_data->m_triangleConvexPairs = new b3OpenCLArray<b3Int4>(m_context, m_queue, config.m_maxTriConvexPairCapacity);
  31
  32         //m_data->m_convexPairsOutGPU = new b3OpenCLArray<b3Int2>(ctx,queue,config.m_maxBroadphasePairs,false);
  33         //m_data->m_planePairs = new b3OpenCLArray<b3Int2>(ctx,queue,config.m_maxBroadphasePairs,false);
  34
  35         m_data->m_pBufContactOutCPU = new b3AlignedObjectArray<b3Contact4>();
  36         m_data->m_pBufContactOutCPU->resize(config.m_maxBroadphasePairs);
  37         m_data->m_bodyBufferCPU = new b3AlignedObjectArray<b3RigidBodyData>();
  38         m_data->m_bodyBufferCPU->resize(config.m_maxConvexBodies);
  39
  40         m_data->m_inertiaBufferCPU = new b3AlignedObjectArray<b3InertiaData>();
  41         m_data->m_inertiaBufferCPU->resize(config.m_maxConvexBodies);
  42
  43         m_data->m_pBufContactBuffersGPU[0] = new b3OpenCLArray<b3Contact4>(ctx, queue, config.m_maxContactCapacity, true);
  44         m_data->m_pBufContactBuffersGPU[1] = new b3OpenCLArray<b3Contact4>(ctx, queue, config.m_maxContactCapacity, true);
  45
  46         m_data->m_inertiaBufferGPU = new b3OpenCLArray<b3InertiaData>(ctx, queue, config.m_maxConvexBodies, false);
  47         m_data->m_collidablesGPU = new b3OpenCLArray<b3Collidable>(ctx, queue, config.m_maxConvexShapes);
  48         m_data->m_collidablesCPU.reserve(config.m_maxConvexShapes);
  49
  50         m_data->m_localShapeAABBCPU = new b3AlignedObjectArray<b3SapAabb>;
  51         m_data->m_localShapeAABBGPU = new b3OpenCLArray<b3SapAabb>(ctx, queue, config.m_maxConvexShapes);
  52
  53         //m_data->m_solverDataGPU = adl::Solver<adl::TYPE_CL>::allocate(ctx,queue, config.m_maxBroadphasePairs,false);
  54         m_data->m_bodyBufferGPU = new b3OpenCLArray<b3RigidBodyData>(ctx, queue, config.m_maxConvexBodies, false);
  55
  56         m_data->m_convexFacesGPU = new b3OpenCLArray<b3GpuFace>(ctx, queue, config.m_maxConvexShapes * config.m_maxFacesPerShape, false);
  57         m_data->m_convexFaces.reserve(config.m_maxConvexShapes * config.m_maxFacesPerShape);
  58
  59         m_data->m_gpuChildShapes = new b3OpenCLArray<b3GpuChildShape>(ctx, queue, config.m_maxCompoundChildShapes, false);
  60
  61         m_data->m_convexPolyhedraGPU = new b3OpenCLArray<b3ConvexPolyhedronData>(ctx, queue, config.m_maxConvexShapes, false);
  62         m_data->m_convexPolyhedra.reserve(config.m_maxConvexShapes);
  63
  64         m_data->m_uniqueEdgesGPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexUniqueEdges, true);
  65         m_data->m_uniqueEdges.reserve(config.m_maxConvexUniqueEdges);
  66
  67         m_data->m_convexVerticesGPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexVertices, true);
  68         m_data->m_convexVertices.reserve(config.m_maxConvexVertices);
  69
  70         m_data->m_convexIndicesGPU = new b3OpenCLArray<int>(ctx, queue, config.m_maxConvexIndices, true);
  71         m_data->m_convexIndices.reserve(config.m_maxConvexIndices);
  72
  73         m_data->m_worldVertsB1GPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexBodies * config.m_maxVerticesPerFace);
  74         m_data->m_clippingFacesOutGPU = new b3OpenCLArray<b3Int4>(ctx, queue, config.m_maxConvexBodies);
  75         m_data->m_worldNormalsAGPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexBodies);
  76         m_data->m_worldVertsA1GPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexBodies * config.m_maxVerticesPerFace);
  77         m_data->m_worldVertsB2GPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexBodies * config.m_maxVerticesPerFace);
  78
  79         m_data->m_convexData = new b3AlignedObjectArray<b3ConvexUtility*>();
  80
  81         m_data->m_convexData->resize(config.m_maxConvexShapes);
  82         m_data->m_convexPolyhedra.resize(config.m_maxConvexShapes);
  83
  84         m_data->m_numAcceleratedShapes = 0;
  85         m_data->m_numAcceleratedRigidBodies = 0;
  86
  87         m_data->m_subTreesGPU = new b3OpenCLArray<b3BvhSubtreeInfo>(this->m_context, this->m_queue);
  88         m_data->m_treeNodesGPU = new b3OpenCLArray<b3QuantizedBvhNode>(this->m_context, this->m_queue);
  89         m_data->m_bvhInfoGPU = new b3OpenCLArray<b3BvhInfo>(this->m_context, this->m_queue);
  90
  91         //m_data->m_contactCGPU = new b3OpenCLArray<Constraint4>(ctx,queue,config.m_maxBroadphasePairs,false);
  92         //m_data->m_frictionCGPU = new b3OpenCLArray<adl::Solver<adl::TYPE_CL>::allocateFrictionConstraint( m_data->m_deviceCL, config.m_maxBroadphasePairs);
  93 }
  94
  95 b3GpuNarrowPhase::~b3GpuNarrowPhase()
  96 {
  97         delete m_data->m_gpuSatCollision;
  98
  99         delete m_data->m_triangleConvexPairs;
 100         //delete m_data->m_convexPairsOutGPU;
 101         //delete m_data->m_planePairs;
 102         delete m_data->m_pBufContactOutCPU;
 103         delete m_data->m_bodyBufferCPU;
 104         delete m_data->m_inertiaBufferCPU;
 105         delete m_data->m_pBufContactBuffersGPU[0];
 106         delete m_data->m_pBufContactBuffersGPU[1];
 107
 108         delete m_data->m_inertiaBufferGPU;
 109         delete m_data->m_collidablesGPU;
 110         delete m_data->m_localShapeAABBCPU;
 111         delete m_data->m_localShapeAABBGPU;
 112         delete m_data->m_bodyBufferGPU;
 113         delete m_data->m_convexFacesGPU;
 114         delete m_data->m_gpuChildShapes;
 115         delete m_data->m_convexPolyhedraGPU;
 116         delete m_data->m_uniqueEdgesGPU;
 117         delete m_data->m_convexVerticesGPU;
 118         delete m_data->m_convexIndicesGPU;
 119         delete m_data->m_worldVertsB1GPU;
 120         delete m_data->m_clippingFacesOutGPU;
 121         delete m_data->m_worldNormalsAGPU;
 122         delete m_data->m_worldVertsA1GPU;
 123         delete m_data->m_worldVertsB2GPU;
 124
 125         delete m_data->m_bvhInfoGPU;
 126
 127         for (int i = 0; i < m_data->m_bvhData.size(); i++)
 128         {
 129                 delete m_data->m_bvhData[i];
 130         }
 131         for (int i = 0; i < m_data->m_meshInterfaces.size(); i++)
 132         {
 133                 delete m_data->m_meshInterfaces[i];
 134         }
 135         m_data->m_meshInterfaces.clear();
 136         m_data->m_bvhData.clear();
 137         delete m_data->m_treeNodesGPU;
 138         delete m_data->m_subTreesGPU;
 139
 140         delete m_data->m_convexData;
 141         delete m_data;
 142 }
 143
 144 int b3GpuNarrowPhase::allocateCollidable()
 145 {
 146         int curSize = m_data->m_collidablesCPU.size();
 147         if (curSize < m_data->m_config.m_maxConvexShapes)
 148         {
 149                 m_data->m_collidablesCPU.expand();
 150                 return curSize;
 151         }
 152         else
 153         {
 154                 b3Error("allocateCollidable out-of-range %d\n", m_data->m_config.m_maxConvexShapes);
 155         }
 156         return -1;
 157 }
 158
 159 int b3GpuNarrowPhase::registerSphereShape(float radius)
 160 {
 161         int collidableIndex = allocateCollidable();
 162         if (collidableIndex < 0)
 163                 return collidableIndex;
 164
 165         b3Collidable& col = getCollidableCpu(collidableIndex);
 166         col.m_shapeType = SHAPE_SPHERE;
 167         col.m_shapeIndex = 0;
 168         col.m_radius = radius;
 169
 170         if (col.m_shapeIndex >= 0)
 171         {
 172                 b3SapAabb aabb;
 173                 b3Vector3 myAabbMin = b3MakeVector3(-radius, -radius, -radius);
 174                 b3Vector3 myAabbMax = b3MakeVector3(radius, radius, radius);
 175
 176                 aabb.m_min[0] = myAabbMin[0];  //s_convexHeightField->m_aabb.m_min.x;
 177                 aabb.m_min[1] = myAabbMin[1];  //s_convexHeightField->m_aabb.m_min.y;
 178                 aabb.m_min[2] = myAabbMin[2];  //s_convexHeightField->m_aabb.m_min.z;
 179                 aabb.m_minIndices[3] = 0;
 180
 181                 aabb.m_max[0] = myAabbMax[0];  //s_convexHeightField->m_aabb.m_max.x;
 182                 aabb.m_max[1] = myAabbMax[1];  //s_convexHeightField->m_aabb.m_max.y;
 183                 aabb.m_max[2] = myAabbMax[2];  //s_convexHeightField->m_aabb.m_max.z;
 184                 aabb.m_signedMaxIndices[3] = 0;
 185
 186                 m_data->m_localShapeAABBCPU->push_back(aabb);
 187                 //              m_data->m_localShapeAABBGPU->push_back(aabb);
 188                 clFinish(m_queue);
 189         }
 190
 191         return collidableIndex;
 192 }
 193
 194 int b3GpuNarrowPhase::registerFace(const b3Vector3& faceNormal, float faceConstant)
 195 {
 196         int faceOffset = m_data->m_convexFaces.size();
 197         b3GpuFace& face = m_data->m_convexFaces.expand();
 198         face.m_plane = b3MakeVector3(faceNormal.x, faceNormal.y, faceNormal.z, faceConstant);
 199         return faceOffset;
 200 }
 201
 202 int b3GpuNarrowPhase::registerPlaneShape(const b3Vector3& planeNormal, float planeConstant)
 203 {
 204         int collidableIndex = allocateCollidable();
 205         if (collidableIndex < 0)
 206                 return collidableIndex;
 207
 208         b3Collidable& col = getCollidableCpu(collidableIndex);
 209         col.m_shapeType = SHAPE_PLANE;
 210         col.m_shapeIndex = registerFace(planeNormal, planeConstant);
 211         col.m_radius = planeConstant;
 212
 213         if (col.m_shapeIndex >= 0)
 214         {
 215                 b3SapAabb aabb;
 216                 aabb.m_min[0] = -1e30f;
 217                 aabb.m_min[1] = -1e30f;
 218                 aabb.m_min[2] = -1e30f;
 219                 aabb.m_minIndices[3] = 0;
 220
 221                 aabb.m_max[0] = 1e30f;
 222                 aabb.m_max[1] = 1e30f;
 223                 aabb.m_max[2] = 1e30f;
 224                 aabb.m_signedMaxIndices[3] = 0;
 225
 226                 m_data->m_localShapeAABBCPU->push_back(aabb);
 227                 //              m_data->m_localShapeAABBGPU->push_back(aabb);
 228                 clFinish(m_queue);
 229         }
 230
 231         return collidableIndex;
 232 }
 233
 234 int b3GpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr, b3Collidable& col)
 235 {
 236         m_data->m_convexData->resize(m_data->m_numAcceleratedShapes + 1);
 237         m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1);
 238
 239         b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1);
 240         convex.mC = convexPtr->mC;
 241         convex.mE = convexPtr->mE;
 242         convex.m_extents = convexPtr->m_extents;
 243         convex.m_localCenter = convexPtr->m_localCenter;
 244         convex.m_radius = convexPtr->m_radius;
 245
 246         convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size();
 247         int edgeOffset = m_data->m_uniqueEdges.size();
 248         convex.m_uniqueEdgesOffset = edgeOffset;
 249
 250         m_data->m_uniqueEdges.resize(edgeOffset + convex.m_numUniqueEdges);
 251
 252         //convex data here
 253         int i;
 254         for (i = 0; i < convexPtr->m_uniqueEdges.size(); i++)
 255         {
 256                 m_data->m_uniqueEdges[edgeOffset + i] = convexPtr->m_uniqueEdges[i];
 257         }
 258
 259         int faceOffset = m_data->m_convexFaces.size();
 260         convex.m_faceOffset = faceOffset;
 261         convex.m_numFaces = convexPtr->m_faces.size();
 262
 263         m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces);
 264
 265         for (i = 0; i < convexPtr->m_faces.size(); i++)
 266         {
 267                 m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
 268                                                                                                                                                            convexPtr->m_faces[i].m_plane[1],
 269                                                                                                                                                            convexPtr->m_faces[i].m_plane[2],
 270                                                                                                                                                            convexPtr->m_faces[i].m_plane[3]);
 271
 272                 int indexOffset = m_data->m_convexIndices.size();
 273                 int numIndices = convexPtr->m_faces[i].m_indices.size();
 274                 m_data->m_convexFaces[convex.m_faceOffset + i].m_numIndices = numIndices;
 275                 m_data->m_convexFaces[convex.m_faceOffset + i].m_indexOffset = indexOffset;
 276                 m_data->m_convexIndices.resize(indexOffset + numIndices);
 277                 for (int p = 0; p < numIndices; p++)
 278                 {
 279                         m_data->m_convexIndices[indexOffset + p] = convexPtr->m_faces[i].m_indices[p];
 280                 }
 281         }
 282
 283         convex.m_numVertices = convexPtr->m_vertices.size();
 284         int vertexOffset = m_data->m_convexVertices.size();
 285         convex.m_vertexOffset = vertexOffset;
 286
 287         m_data->m_convexVertices.resize(vertexOffset + convex.m_numVertices);
 288         for (int i = 0; i < convexPtr->m_vertices.size(); i++)
 289         {
 290                 m_data->m_convexVertices[vertexOffset + i] = convexPtr->m_vertices[i];
 291         }
 292
 293         (*m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
 294
 295         return m_data->m_numAcceleratedShapes++;
 296 }
 297
 298 int b3GpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
 299 {
 300         b3AlignedObjectArray<b3Vector3> verts;
 301
 302         unsigned char* vts = (unsigned char*)vertices;
 303         for (int i = 0; i < numVertices; i++)
 304         {
 305                 float* vertex = (float*)&vts[i * strideInBytes];
 306                 verts.push_back(b3MakeVector3(vertex[0] * scaling[0], vertex[1] * scaling[1], vertex[2] * scaling[2]));
 307         }
 308
 309         b3ConvexUtility* utilPtr = new b3ConvexUtility();
 310         bool merge = true;
 311         if (numVertices)
 312         {
 313                 utilPtr->initializePolyhedralFeatures(&verts[0], verts.size(), merge);
 314         }
 315
 316         int collidableIndex = registerConvexHullShape(utilPtr);
 317         delete utilPtr;
 318         return collidableIndex;
 319 }
 320
 321 int b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
 322 {
 323         int collidableIndex = allocateCollidable();
 324         if (collidableIndex < 0)
 325                 return collidableIndex;
 326
 327         b3Collidable& col = getCollidableCpu(collidableIndex);
 328         col.m_shapeType = SHAPE_CONVEX_HULL;
 329         col.m_shapeIndex = -1;
 330
 331         {
 332                 b3Vector3 localCenter = b3MakeVector3(0, 0, 0);
 333                 for (int i = 0; i < utilPtr->m_vertices.size(); i++)
 334                         localCenter += utilPtr->m_vertices[i];
 335                 localCenter *= (1.f / utilPtr->m_vertices.size());
 336                 utilPtr->m_localCenter = localCenter;
 337
 338                 col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr, col);
 339         }
 340
 341         if (col.m_shapeIndex >= 0)
 342         {
 343                 b3SapAabb aabb;
 344
 345                 b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f);
 346                 b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f);
 347
 348                 for (int i = 0; i < utilPtr->m_vertices.size(); i++)
 349                 {
 350                         myAabbMin.setMin(utilPtr->m_vertices[i]);
 351                         myAabbMax.setMax(utilPtr->m_vertices[i]);
 352                 }
 353                 aabb.m_min[0] = myAabbMin[0];
 354                 aabb.m_min[1] = myAabbMin[1];
 355                 aabb.m_min[2] = myAabbMin[2];
 356                 aabb.m_minIndices[3] = 0;
 357
 358                 aabb.m_max[0] = myAabbMax[0];
 359                 aabb.m_max[1] = myAabbMax[1];
 360                 aabb.m_max[2] = myAabbMax[2];
 361                 aabb.m_signedMaxIndices[3] = 0;
 362
 363                 m_data->m_localShapeAABBCPU->push_back(aabb);
 364                 //              m_data->m_localShapeAABBGPU->push_back(aabb);
 365         }
 366
 367         return collidableIndex;
 368 }
 369
 370 int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes)
 371 {
 372         int collidableIndex = allocateCollidable();
 373         if (collidableIndex < 0)
 374                 return collidableIndex;
 375
 376         b3Collidable& col = getCollidableCpu(collidableIndex);
 377         col.m_shapeType = SHAPE_COMPOUND_OF_CONVEX_HULLS;
 378         col.m_shapeIndex = m_data->m_cpuChildShapes.size();
 379         col.m_compoundBvhIndex = m_data->m_bvhInfoCPU.size();
 380
 381         {
 382                 b3Assert(col.m_shapeIndex + childShapes->size() < m_data->m_config.m_maxCompoundChildShapes);
 383                 for (int i = 0; i < childShapes->size(); i++)
 384                 {
 385                         m_data->m_cpuChildShapes.push_back(childShapes->at(i));
 386                 }
 387         }
 388
 389         col.m_numChildShapes = childShapes->size();
 390
 391         b3SapAabb aabbLocalSpace;
 392         b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f);
 393         b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f);
 394
 395         b3AlignedObjectArray<b3Aabb> childLocalAabbs;
 396         childLocalAabbs.resize(childShapes->size());
 397
 398         //compute local AABB of the compound of all children
 399         for (int i = 0; i < childShapes->size(); i++)
 400         {
 401                 int childColIndex = childShapes->at(i).m_shapeIndex;
 402                 //b3Collidable& childCol = getCollidableCpu(childColIndex);
 403                 b3SapAabb aabbLoc = m_data->m_localShapeAABBCPU->at(childColIndex);
 404
 405                 b3Vector3 childLocalAabbMin = b3MakeVector3(aabbLoc.m_min[0], aabbLoc.m_min[1], aabbLoc.m_min[2]);
 406                 b3Vector3 childLocalAabbMax = b3MakeVector3(aabbLoc.m_max[0], aabbLoc.m_max[1], aabbLoc.m_max[2]);
 407                 b3Vector3 aMin, aMax;
 408                 b3Scalar margin(0.f);
 409                 b3Transform childTr;
 410                 childTr.setIdentity();
 411
 412                 childTr.setOrigin(childShapes->at(i).m_childPosition);
 413                 childTr.setRotation(b3Quaternion(childShapes->at(i).m_childOrientation));
 414                 b3TransformAabb(childLocalAabbMin, childLocalAabbMax, margin, childTr, aMin, aMax);
 415                 myAabbMin.setMin(aMin);
 416                 myAabbMax.setMax(aMax);
 417                 childLocalAabbs[i].m_min[0] = aMin[0];
 418                 childLocalAabbs[i].m_min[1] = aMin[1];
 419                 childLocalAabbs[i].m_min[2] = aMin[2];
 420                 childLocalAabbs[i].m_min[3] = 0;
 421                 childLocalAabbs[i].m_max[0] = aMax[0];
 422                 childLocalAabbs[i].m_max[1] = aMax[1];
 423                 childLocalAabbs[i].m_max[2] = aMax[2];
 424                 childLocalAabbs[i].m_max[3] = 0;
 425         }
 426
 427         aabbLocalSpace.m_min[0] = myAabbMin[0];  //s_convexHeightField->m_aabb.m_min.x;
 428         aabbLocalSpace.m_min[1] = myAabbMin[1];  //s_convexHeightField->m_aabb.m_min.y;
 429         aabbLocalSpace.m_min[2] = myAabbMin[2];  //s_convexHeightField->m_aabb.m_min.z;
 430         aabbLocalSpace.m_minIndices[3] = 0;
 431
 432         aabbLocalSpace.m_max[0] = myAabbMax[0];  //s_convexHeightField->m_aabb.m_max.x;
 433         aabbLocalSpace.m_max[1] = myAabbMax[1];  //s_convexHeightField->m_aabb.m_max.y;
 434         aabbLocalSpace.m_max[2] = myAabbMax[2];  //s_convexHeightField->m_aabb.m_max.z;
 435         aabbLocalSpace.m_signedMaxIndices[3] = 0;
 436
 437         m_data->m_localShapeAABBCPU->push_back(aabbLocalSpace);
 438
 439         b3QuantizedBvh* bvh = new b3QuantizedBvh;
 440         bvh->setQuantizationValues(myAabbMin, myAabbMax);
 441         QuantizedNodeArray& nodes = bvh->getLeafNodeArray();
 442         int numNodes = childShapes->size();
 443
 444         for (int i = 0; i < numNodes; i++)
 445         {
 446                 b3QuantizedBvhNode node;
 447                 b3Vector3 aabbMin, aabbMax;
 448                 aabbMin = (b3Vector3&)childLocalAabbs[i].m_min;
 449                 aabbMax = (b3Vector3&)childLocalAabbs[i].m_max;
 450
 451                 bvh->quantize(&node.m_quantizedAabbMin[0], aabbMin, 0);
 452                 bvh->quantize(&node.m_quantizedAabbMax[0], aabbMax, 1);
 453                 int partId = 0;
 454                 node.m_escapeIndexOrTriangleIndex = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | i;
 455                 nodes.push_back(node);
 456         }
 457         bvh->buildInternal();
 458
 459         int numSubTrees = bvh->getSubtreeInfoArray().size();
 460
 461         //void  setQuantizationValues(const b3Vector3& bvhAabbMin,const b3Vector3& bvhAabbMax,b3Scalar quantizationMargin=b3Scalar(1.0));
 462         //QuantizedNodeArray&   getLeafNodeArray() {                    return  m_quantizedLeafNodes;   }
 463         ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized
 464         //void  buildInternal();
 465
 466         b3BvhInfo bvhInfo;
 467
 468         bvhInfo.m_aabbMin = bvh->m_bvhAabbMin;
 469         bvhInfo.m_aabbMax = bvh->m_bvhAabbMax;
 470         bvhInfo.m_quantization = bvh->m_bvhQuantization;
 471         bvhInfo.m_numNodes = numNodes;
 472         bvhInfo.m_numSubTrees = numSubTrees;
 473         bvhInfo.m_nodeOffset = m_data->m_treeNodesCPU.size();
 474         bvhInfo.m_subTreeOffset = m_data->m_subTreesCPU.size();
 475
 476         int numNewNodes = bvh->getQuantizedNodeArray().size();
 477
 478         for (int i = 0; i < numNewNodes - 1; i++)
 479         {
 480                 if (bvh->getQuantizedNodeArray()[i].isLeafNode())
 481                 {
 482                         int orgIndex = bvh->getQuantizedNodeArray()[i].getTriangleIndex();
 483
 484                         b3Vector3 nodeMinVec = bvh->unQuantize(bvh->getQuantizedNodeArray()[i].m_quantizedAabbMin);
 485                         b3Vector3 nodeMaxVec = bvh->unQuantize(bvh->getQuantizedNodeArray()[i].m_quantizedAabbMax);
 486
 487                         for (int c = 0; c < 3; c++)
 488                         {
 489                                 if (childLocalAabbs[orgIndex].m_min[c] < nodeMinVec[c])
 490                                 {
 491                                         printf("min org (%f) and new (%f) ? at i:%d,c:%d\n", childLocalAabbs[i].m_min[c], nodeMinVec[c], i, c);
 492                                 }
 493                                 if (childLocalAabbs[orgIndex].m_max[c] > nodeMaxVec[c])
 494                                 {
 495                                         printf("max org (%f) and new (%f) ? at i:%d,c:%d\n", childLocalAabbs[i].m_max[c], nodeMaxVec[c], i, c);
 496                                 }
 497                         }
 498                 }
 499         }
 500
 501         m_data->m_bvhInfoCPU.push_back(bvhInfo);
 502
 503         int numNewSubtrees = bvh->getSubtreeInfoArray().size();
 504         m_data->m_subTreesCPU.reserve(m_data->m_subTreesCPU.size() + numNewSubtrees);
 505         for (int i = 0; i < numNewSubtrees; i++)
 506         {
 507                 m_data->m_subTreesCPU.push_back(bvh->getSubtreeInfoArray()[i]);
 508         }
 509         int numNewTreeNodes = bvh->getQuantizedNodeArray().size();
 510
 511         for (int i = 0; i < numNewTreeNodes; i++)
 512         {
 513                 m_data->m_treeNodesCPU.push_back(bvh->getQuantizedNodeArray()[i]);
 514         }
 515
 516         //      m_data->m_localShapeAABBGPU->push_back(aabbWS);
 517         clFinish(m_queue);
 518         return collidableIndex;
 519 }
 520
 521 int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, const float* scaling1)
 522 {
 523         b3Vector3 scaling = b3MakeVector3(scaling1[0], scaling1[1], scaling1[2]);
 524
 525         int collidableIndex = allocateCollidable();
 526         if (collidableIndex < 0)
 527                 return collidableIndex;
 528
 529         b3Collidable& col = getCollidableCpu(collidableIndex);
 530
 531         col.m_shapeType = SHAPE_CONCAVE_TRIMESH;
 532         col.m_shapeIndex = registerConcaveMeshShape(vertices, indices, col, scaling);
 533         col.m_bvhIndex = m_data->m_bvhInfoCPU.size();
 534
 535         b3SapAabb aabb;
 536         b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f);
 537         b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f);
 538
 539         for (int i = 0; i < vertices->size(); i++)
 540         {
 541                 b3Vector3 vtx(vertices->at(i) * scaling);
 542                 myAabbMin.setMin(vtx);
 543                 myAabbMax.setMax(vtx);
 544         }
 545         aabb.m_min[0] = myAabbMin[0];
 546         aabb.m_min[1] = myAabbMin[1];
 547         aabb.m_min[2] = myAabbMin[2];
 548         aabb.m_minIndices[3] = 0;
 549
 550         aabb.m_max[0] = myAabbMax[0];
 551         aabb.m_max[1] = myAabbMax[1];
 552         aabb.m_max[2] = myAabbMax[2];
 553         aabb.m_signedMaxIndices[3] = 0;
 554
 555         m_data->m_localShapeAABBCPU->push_back(aabb);
 556         //      m_data->m_localShapeAABBGPU->push_back(aabb);
 557
 558         b3OptimizedBvh* bvh = new b3OptimizedBvh();
 559         //void b3OptimizedBvh::build(b3StridingMeshInterface* triangles, bool useQuantizedAabbCompression, const b3Vector3& bvhAabbMin, const b3Vector3& bvhAabbMax)
 560
 561         bool useQuantizedAabbCompression = true;
 562         b3TriangleIndexVertexArray* meshInterface = new b3TriangleIndexVertexArray();
 563         m_data->m_meshInterfaces.push_back(meshInterface);
 564         b3IndexedMesh mesh;
 565         mesh.m_numTriangles = indices->size() / 3;
 566         mesh.m_numVertices = vertices->size();
 567         mesh.m_vertexBase = (const unsigned char*)&vertices->at(0).x;
 568         mesh.m_vertexStride = sizeof(b3Vector3);
 569         mesh.m_triangleIndexStride = 3 * sizeof(int);  // or sizeof(int)
 570         mesh.m_triangleIndexBase = (const unsigned char*)&indices->at(0);
 571
 572         meshInterface->addIndexedMesh(mesh);
 573         bvh->build(meshInterface, useQuantizedAabbCompression, (b3Vector3&)aabb.m_min, (b3Vector3&)aabb.m_max);
 574         m_data->m_bvhData.push_back(bvh);
 575         int numNodes = bvh->getQuantizedNodeArray().size();
 576         //b3OpenCLArray<b3QuantizedBvhNode>*    treeNodesGPU = new b3OpenCLArray<b3QuantizedBvhNode>(this->m_context,this->m_queue,numNodes);
 577         int numSubTrees = bvh->getSubtreeInfoArray().size();
 578
 579         b3BvhInfo bvhInfo;
 580
 581         bvhInfo.m_aabbMin = bvh->m_bvhAabbMin;
 582         bvhInfo.m_aabbMax = bvh->m_bvhAabbMax;
 583         bvhInfo.m_quantization = bvh->m_bvhQuantization;
 584         bvhInfo.m_numNodes = numNodes;
 585         bvhInfo.m_numSubTrees = numSubTrees;
 586         bvhInfo.m_nodeOffset = m_data->m_treeNodesCPU.size();
 587         bvhInfo.m_subTreeOffset = m_data->m_subTreesCPU.size();
 588
 589         m_data->m_bvhInfoCPU.push_back(bvhInfo);
 590
 591         int numNewSubtrees = bvh->getSubtreeInfoArray().size();
 592         m_data->m_subTreesCPU.reserve(m_data->m_subTreesCPU.size() + numNewSubtrees);
 593         for (int i = 0; i < numNewSubtrees; i++)
 594         {
 595                 m_data->m_subTreesCPU.push_back(bvh->getSubtreeInfoArray()[i]);
 596         }
 597         int numNewTreeNodes = bvh->getQuantizedNodeArray().size();
 598
 599         for (int i = 0; i < numNewTreeNodes; i++)
 600         {
 601                 m_data->m_treeNodesCPU.push_back(bvh->getQuantizedNodeArray()[i]);
 602         }
 603
 604         return collidableIndex;
 605 }
 606
 607 int b3GpuNarrowPhase::registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling1)
 608 {
 609         b3Vector3 scaling = b3MakeVector3(scaling1[0], scaling1[1], scaling1[2]);
 610
 611         m_data->m_convexData->resize(m_data->m_numAcceleratedShapes + 1);
 612         m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1);
 613
 614         b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1);
 615         convex.mC = b3MakeVector3(0, 0, 0);
 616         convex.mE = b3MakeVector3(0, 0, 0);
 617         convex.m_extents = b3MakeVector3(0, 0, 0);
 618         convex.m_localCenter = b3MakeVector3(0, 0, 0);
 619         convex.m_radius = 0.f;
 620
 621         convex.m_numUniqueEdges = 0;
 622         int edgeOffset = m_data->m_uniqueEdges.size();
 623         convex.m_uniqueEdgesOffset = edgeOffset;
 624
 625         int faceOffset = m_data->m_convexFaces.size();
 626         convex.m_faceOffset = faceOffset;
 627
 628         convex.m_numFaces = indices->size() / 3;
 629         m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces);
 630         m_data->m_convexIndices.reserve(convex.m_numFaces * 3);
 631         for (int i = 0; i < convex.m_numFaces; i++)
 632         {
 633                 if (i % 256 == 0)
 634                 {
 635                         //printf("i=%d out of %d", i,convex.m_numFaces);
 636                 }
 637                 b3Vector3 vert0(vertices->at(indices->at(i * 3)) * scaling);
 638                 b3Vector3 vert1(vertices->at(indices->at(i * 3 + 1)) * scaling);
 639                 b3Vector3 vert2(vertices->at(indices->at(i * 3 + 2)) * scaling);
 640
 641                 b3Vector3 normal = ((vert1 - vert0).cross(vert2 - vert0)).normalize();
 642                 b3Scalar c = -(normal.dot(vert0));
 643
 644                 m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector4(normal.x, normal.y, normal.z, c);
 645                 int indexOffset = m_data->m_convexIndices.size();
 646                 int numIndices = 3;
 647                 m_data->m_convexFaces[convex.m_faceOffset + i].m_numIndices = numIndices;
 648                 m_data->m_convexFaces[convex.m_faceOffset + i].m_indexOffset = indexOffset;
 649                 m_data->m_convexIndices.resize(indexOffset + numIndices);
 650                 for (int p = 0; p < numIndices; p++)
 651                 {
 652                         int vi = indices->at(i * 3 + p);
 653                         m_data->m_convexIndices[indexOffset + p] = vi;  //convexPtr->m_faces[i].m_indices[p];
 654                 }
 655         }
 656
 657         convex.m_numVertices = vertices->size();
 658         int vertexOffset = m_data->m_convexVertices.size();
 659         convex.m_vertexOffset = vertexOffset;
 660         m_data->m_convexVertices.resize(vertexOffset + convex.m_numVertices);
 661         for (int i = 0; i < vertices->size(); i++)
 662         {
 663                 m_data->m_convexVertices[vertexOffset + i] = vertices->at(i) * scaling;
 664         }
 665
 666         (*m_data->m_convexData)[m_data->m_numAcceleratedShapes] = 0;
 667
 668         return m_data->m_numAcceleratedShapes++;
 669 }
 670
 671 cl_mem b3GpuNarrowPhase::getBodiesGpu()
 672 {
 673         return (cl_mem)m_data->m_bodyBufferGPU->getBufferCL();
 674 }
 675
 676 const struct b3RigidBodyData* b3GpuNarrowPhase::getBodiesCpu() const
 677 {
 678         return &m_data->m_bodyBufferCPU->at(0);
 679 };
 680
 681 int b3GpuNarrowPhase::getNumBodiesGpu() const
 682 {
 683         return m_data->m_bodyBufferGPU->size();
 684 }
 685
 686 cl_mem b3GpuNarrowPhase::getBodyInertiasGpu()
 687 {
 688         return (cl_mem)m_data->m_inertiaBufferGPU->getBufferCL();
 689 }
 690
 691 int b3GpuNarrowPhase::getNumBodyInertiasGpu() const
 692 {
 693         return m_data->m_inertiaBufferGPU->size();
 694 }
 695
 696 b3Collidable& b3GpuNarrowPhase::getCollidableCpu(int collidableIndex)
 697 {
 698         return m_data->m_collidablesCPU[collidableIndex];
 699 }
 700
 701 const b3Collidable& b3GpuNarrowPhase::getCollidableCpu(int collidableIndex) const
 702 {
 703         return m_data->m_collidablesCPU[collidableIndex];
 704 }
 705
 706 cl_mem b3GpuNarrowPhase::getCollidablesGpu()
 707 {
 708         return m_data->m_collidablesGPU->getBufferCL();
 709 }
 710
 711 const struct b3Collidable* b3GpuNarrowPhase::getCollidablesCpu() const
 712 {
 713         if (m_data->m_collidablesCPU.size())
 714                 return &m_data->m_collidablesCPU[0];
 715         return 0;
 716 }
 717
 718 const struct b3SapAabb* b3GpuNarrowPhase::getLocalSpaceAabbsCpu() const
 719 {
 720         if (m_data->m_localShapeAABBCPU->size())
 721         {
 722                 return &m_data->m_localShapeAABBCPU->at(0);
 723         }
 724         return 0;
 725 }
 726
 727 cl_mem b3GpuNarrowPhase::getAabbLocalSpaceBufferGpu()
 728 {
 729         return m_data->m_localShapeAABBGPU->getBufferCL();
 730 }
 731 int b3GpuNarrowPhase::getNumCollidablesGpu() const
 732 {
 733         return m_data->m_collidablesGPU->size();
 734 }
 735
 736 int b3GpuNarrowPhase::getNumContactsGpu() const
 737 {
 738         return m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->size();
 739 }
 740 cl_mem b3GpuNarrowPhase::getContactsGpu()
 741 {
 742         return m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->getBufferCL();
 743 }
 744
 745 const b3Contact4* b3GpuNarrowPhase::getContactsCPU() const
 746 {
 747         m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->copyToHost(*m_data->m_pBufContactOutCPU);
 748         return &m_data->m_pBufContactOutCPU->at(0);
 749 }
 750
 751 void b3GpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects)
 752 {
 753         cl_mem aabbsLocalSpace = m_data->m_localShapeAABBGPU->getBufferCL();
 754
 755         int nContactOut = 0;
 756
 757         //swap buffer
 758         m_data->m_currentContactBuffer = 1 - m_data->m_currentContactBuffer;
 759
 760         //int curSize = m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->size();
 761
 762         int maxTriConvexPairCapacity = m_data->m_config.m_maxTriConvexPairCapacity;
 763         int numTriConvexPairsOut = 0;
 764
 765         b3OpenCLArray<b3Int4> broadphasePairsGPU(m_context, m_queue);
 766         broadphasePairsGPU.setFromOpenCLBuffer(broadphasePairs, numBroadphasePairs);
 767
 768         b3OpenCLArray<b3Aabb> clAabbArrayWorldSpace(this->m_context, this->m_queue);
 769         clAabbArrayWorldSpace.setFromOpenCLBuffer(aabbsWorldSpace, numObjects);
 770
 771         b3OpenCLArray<b3Aabb> clAabbArrayLocalSpace(this->m_context, this->m_queue);
 772         clAabbArrayLocalSpace.setFromOpenCLBuffer(aabbsLocalSpace, numObjects);
 773
 774         m_data->m_gpuSatCollision->computeConvexConvexContactsGPUSAT(
 775                 &broadphasePairsGPU, numBroadphasePairs,
 776                 m_data->m_bodyBufferGPU,
 777                 m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer],
 778                 nContactOut,
 779                 m_data->m_pBufContactBuffersGPU[1 - m_data->m_currentContactBuffer],
 780                 m_data->m_config.m_maxContactCapacity,
 781                 m_data->m_config.m_compoundPairCapacity,
 782                 *m_data->m_convexPolyhedraGPU,
 783                 *m_data->m_convexVerticesGPU,
 784                 *m_data->m_uniqueEdgesGPU,
 785                 *m_data->m_convexFacesGPU,
 786                 *m_data->m_convexIndicesGPU,
 787                 *m_data->m_collidablesGPU,
 788                 *m_data->m_gpuChildShapes,
 789                 clAabbArrayWorldSpace,
 790                 clAabbArrayLocalSpace,
 791                 *m_data->m_worldVertsB1GPU,
 792                 *m_data->m_clippingFacesOutGPU,
 793                 *m_data->m_worldNormalsAGPU,
 794                 *m_data->m_worldVertsA1GPU,
 795                 *m_data->m_worldVertsB2GPU,
 796                 m_data->m_bvhData,
 797                 m_data->m_treeNodesGPU,
 798                 m_data->m_subTreesGPU,
 799                 m_data->m_bvhInfoGPU,
 800                 numObjects,
 801                 maxTriConvexPairCapacity,
 802                 *m_data->m_triangleConvexPairs,
 803                 numTriConvexPairsOut);
 804
 805         /*b3AlignedObjectArray<b3Int4> broadphasePairsCPU;
 806         broadphasePairsGPU.copyToHost(broadphasePairsCPU);
 807         printf("checking pairs\n");
 808         */
 809 }
 810
 811 const b3SapAabb& b3GpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const
 812 {
 813         return m_data->m_localShapeAABBCPU->at(collidableIndex);
 814 }
 815
 816 int b3GpuNarrowPhase::registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMinPtr, const float* aabbMaxPtr, bool writeToGpu)
 817 {
 818         b3Vector3 aabbMin = b3MakeVector3(aabbMinPtr[0], aabbMinPtr[1], aabbMinPtr[2]);
 819         b3Vector3 aabbMax = b3MakeVector3(aabbMaxPtr[0], aabbMaxPtr[1], aabbMaxPtr[2]);
 820
 821         if (m_data->m_numAcceleratedRigidBodies >= (m_data->m_config.m_maxConvexBodies))
 822         {
 823                 b3Error("registerRigidBody: exceeding the number of rigid bodies, %d > %d \n", m_data->m_numAcceleratedRigidBodies, m_data->m_config.m_maxConvexBodies);
 824                 return -1;
 825         }
 826
 827         m_data->m_bodyBufferCPU->resize(m_data->m_numAcceleratedRigidBodies + 1);
 828
 829         b3RigidBodyData& body = m_data->m_bodyBufferCPU->at(m_data->m_numAcceleratedRigidBodies);
 830
 831         float friction = 1.f;
 832         float restitution = 0.f;
 833
 834         body.m_frictionCoeff = friction;
 835         body.m_restituitionCoeff = restitution;
 836         body.m_angVel = b3MakeVector3(0, 0, 0);
 837         body.m_linVel = b3MakeVector3(0, 0, 0);  //.setZero();
 838         body.m_pos = b3MakeVector3(position[0], position[1], position[2]);
 839         body.m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]);
 840         body.m_collidableIdx = collidableIndex;
 841         if (collidableIndex >= 0)
 842         {
 843                 //              body.m_shapeType = m_data->m_collidablesCPU.at(collidableIndex).m_shapeType;
 844         }
 845         else
 846         {
 847                 //      body.m_shapeType = CollisionShape::SHAPE_PLANE;
 848                 m_planeBodyIndex = m_data->m_numAcceleratedRigidBodies;
 849         }
 850         //body.m_shapeType = shapeType;
 851
 852         body.m_invMass = mass ? 1.f / mass : 0.f;
 853
 854         if (writeToGpu)
 855         {
 856                 m_data->m_bodyBufferGPU->copyFromHostPointer(&body, 1, m_data->m_numAcceleratedRigidBodies);
 857         }
 858
 859         b3InertiaData& shapeInfo = m_data->m_inertiaBufferCPU->at(m_data->m_numAcceleratedRigidBodies);
 860
 861         if (mass == 0.f)
 862         {
 863                 if (m_data->m_numAcceleratedRigidBodies == 0)
 864                         m_static0Index = 0;
 865
 866                 shapeInfo.m_initInvInertia.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
 867                 shapeInfo.m_invInertiaWorld.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
 868         }
 869         else
 870         {
 871                 b3Assert(body.m_collidableIdx >= 0);
 872
 873                 //approximate using the aabb of the shape
 874
 875                 //Aabb aabb = (*m_data->m_shapePointers)[shapeIndex]->m_aabb;
 876                 b3Vector3 halfExtents = (aabbMax - aabbMin);  //*0.5f;//fake larger inertia makes demos more stable ;-)
 877
 878                 b3Vector3 localInertia;
 879
 880                 float lx = 2.f * halfExtents[0];
 881                 float ly = 2.f * halfExtents[1];
 882                 float lz = 2.f * halfExtents[2];
 883
 884                 localInertia.setValue((mass / 12.0f) * (ly * ly + lz * lz),
 885                                                           (mass / 12.0f) * (lx * lx + lz * lz),
 886                                                           (mass / 12.0f) * (lx * lx + ly * ly));
 887
 888                 b3Vector3 invLocalInertia;
 889                 invLocalInertia[0] = 1.f / localInertia[0];
 890                 invLocalInertia[1] = 1.f / localInertia[1];
 891                 invLocalInertia[2] = 1.f / localInertia[2];
 892                 invLocalInertia[3] = 0.f;
 893
 894                 shapeInfo.m_initInvInertia.setValue(
 895                         invLocalInertia[0], 0, 0,
 896                         0, invLocalInertia[1], 0,
 897                         0, 0, invLocalInertia[2]);
 898
 899                 b3Matrix3x3 m(body.m_quat);
 900
 901                 shapeInfo.m_invInertiaWorld = m.scaled(invLocalInertia) * m.transpose();
 902         }
 903
 904         if (writeToGpu)
 905                 m_data->m_inertiaBufferGPU->copyFromHostPointer(&shapeInfo, 1, m_data->m_numAcceleratedRigidBodies);
 906
 907         return m_data->m_numAcceleratedRigidBodies++;
 908 }
 909
 910 int b3GpuNarrowPhase::getNumRigidBodies() const
 911 {
 912         return m_data->m_numAcceleratedRigidBodies;
 913 }
 914
 915 void b3GpuNarrowPhase::writeAllBodiesToGpu()
 916 {
 917         if (m_data->m_localShapeAABBCPU->size())
 918         {
 919                 m_data->m_localShapeAABBGPU->copyFromHost(*m_data->m_localShapeAABBCPU);
 920         }
 921
 922         m_data->m_gpuChildShapes->copyFromHost(m_data->m_cpuChildShapes);
 923         m_data->m_convexFacesGPU->copyFromHost(m_data->m_convexFaces);
 924         m_data->m_convexPolyhedraGPU->copyFromHost(m_data->m_convexPolyhedra);
 925         m_data->m_uniqueEdgesGPU->copyFromHost(m_data->m_uniqueEdges);
 926         m_data->m_convexVerticesGPU->copyFromHost(m_data->m_convexVertices);
 927         m_data->m_convexIndicesGPU->copyFromHost(m_data->m_convexIndices);
 928         m_data->m_bvhInfoGPU->copyFromHost(m_data->m_bvhInfoCPU);
 929         m_data->m_treeNodesGPU->copyFromHost(m_data->m_treeNodesCPU);
 930         m_data->m_subTreesGPU->copyFromHost(m_data->m_subTreesCPU);
 931
 932         m_data->m_bodyBufferGPU->resize(m_data->m_numAcceleratedRigidBodies);
 933         m_data->m_inertiaBufferGPU->resize(m_data->m_numAcceleratedRigidBodies);
 934
 935         if (m_data->m_numAcceleratedRigidBodies)
 936         {
 937                 m_data->m_bodyBufferGPU->copyFromHostPointer(&m_data->m_bodyBufferCPU->at(0), m_data->m_numAcceleratedRigidBodies);
 938                 m_data->m_inertiaBufferGPU->copyFromHostPointer(&m_data->m_inertiaBufferCPU->at(0), m_data->m_numAcceleratedRigidBodies);
 939         }
 940         if (m_data->m_collidablesCPU.size())
 941         {
 942                 m_data->m_collidablesGPU->copyFromHost(m_data->m_collidablesCPU);
 943         }
 944 }
 945
 946 void b3GpuNarrowPhase::reset()
 947 {
 948         m_data->m_numAcceleratedShapes = 0;
 949         m_data->m_numAcceleratedRigidBodies = 0;
 950         this->m_static0Index = -1;
 951         m_data->m_uniqueEdges.resize(0);
 952         m_data->m_convexVertices.resize(0);
 953         m_data->m_convexPolyhedra.resize(0);
 954         m_data->m_convexIndices.resize(0);
 955         m_data->m_cpuChildShapes.resize(0);
 956         m_data->m_convexFaces.resize(0);
 957         m_data->m_collidablesCPU.resize(0);
 958         m_data->m_localShapeAABBCPU->resize(0);
 959         m_data->m_bvhData.resize(0);
 960         m_data->m_treeNodesCPU.resize(0);
 961         m_data->m_subTreesCPU.resize(0);
 962         m_data->m_bvhInfoCPU.resize(0);
 963 }
 964
 965 void b3GpuNarrowPhase::readbackAllBodiesToCpu()
 966 {
 967         m_data->m_bodyBufferGPU->copyToHostPointer(&m_data->m_bodyBufferCPU->at(0), m_data->m_numAcceleratedRigidBodies);
 968 }
 969
 970 void b3GpuNarrowPhase::setObjectTransformCpu(float* position, float* orientation, int bodyIndex)
 971 {
 972         if (bodyIndex >= 0 && bodyIndex < m_data->m_bodyBufferCPU->size())
 973         {
 974                 m_data->m_bodyBufferCPU->at(bodyIndex).m_pos = b3MakeVector3(position[0], position[1], position[2]);
 975                 m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]);
 976         }
 977         else
 978         {
 979                 b3Warning("setObjectVelocityCpu out of range.\n");
 980         }
 981 }
 982 void b3GpuNarrowPhase::setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex)
 983 {
 984         if (bodyIndex >= 0 && bodyIndex < m_data->m_bodyBufferCPU->size())
 985         {
 986                 m_data->m_bodyBufferCPU->at(bodyIndex).m_linVel = b3MakeVector3(linVel[0], linVel[1], linVel[2]);
 987                 m_data->m_bodyBufferCPU->at(bodyIndex).m_angVel = b3MakeVector3(angVel[0], angVel[1], angVel[2]);
 988         }
 989         else
 990         {
 991                 b3Warning("setObjectVelocityCpu out of range.\n");
 992         }
 993 }
 994
 995 bool b3GpuNarrowPhase::getObjectTransformFromCpu(float* position, float* orientation, int bodyIndex) const
 996 {
 997         if (bodyIndex >= 0 && bodyIndex < m_data->m_bodyBufferCPU->size())
 998         {
 999                 position[0] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.x;
1000                 position[1] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.y;
1001                 position[2] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.z;
1002                 position[3] = 1.f;  //or 1
1003
1004                 orientation[0] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.x;
1005                 orientation[1] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.y;
1006                 orientation[2] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.z;
1007                 orientation[3] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.w;
1008                 return true;
1009         }
1010
1011         b3Warning("getObjectTransformFromCpu out of range.\n");
1012         return false;
1013 }