diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp')
| -rw-r--r-- | thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp | 732 |
1 files changed, 319 insertions, 413 deletions
diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp index 698fa15f96..2e4f6c1572 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp @@ -1,6 +1,5 @@ #include "b3GpuNarrowPhase.h" - #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" #include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h" @@ -16,107 +15,87 @@ #include "Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h" #include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h" - - - b3GpuNarrowPhase::b3GpuNarrowPhase(cl_context ctx, cl_device_id device, cl_command_queue queue, const b3Config& config) -:m_data(0) ,m_planeBodyIndex(-1),m_static0Index(-1), -m_context(ctx), -m_device(device), -m_queue(queue) + : m_data(0), m_planeBodyIndex(-1), m_static0Index(-1), m_context(ctx), m_device(device), m_queue(queue) { - m_data = new b3GpuNarrowPhaseInternalData(); m_data->m_currentContactBuffer = 0; - memset(m_data,0,sizeof(b3GpuNarrowPhaseInternalData)); - + memset(m_data, 0, sizeof(b3GpuNarrowPhaseInternalData)); m_data->m_config = config; - - m_data->m_gpuSatCollision = new GpuSatCollision(ctx,device,queue); - - - m_data->m_triangleConvexPairs = new b3OpenCLArray<b3Int4>(m_context,m_queue, config.m_maxTriConvexPairCapacity); + m_data->m_gpuSatCollision = new GpuSatCollision(ctx, device, queue); + + m_data->m_triangleConvexPairs = new b3OpenCLArray<b3Int4>(m_context, m_queue, config.m_maxTriConvexPairCapacity); //m_data->m_convexPairsOutGPU = new b3OpenCLArray<b3Int2>(ctx,queue,config.m_maxBroadphasePairs,false); //m_data->m_planePairs = new b3OpenCLArray<b3Int2>(ctx,queue,config.m_maxBroadphasePairs,false); - + m_data->m_pBufContactOutCPU = new b3AlignedObjectArray<b3Contact4>(); m_data->m_pBufContactOutCPU->resize(config.m_maxBroadphasePairs); m_data->m_bodyBufferCPU = new b3AlignedObjectArray<b3RigidBodyData>(); m_data->m_bodyBufferCPU->resize(config.m_maxConvexBodies); - + m_data->m_inertiaBufferCPU = new b3AlignedObjectArray<b3InertiaData>(); m_data->m_inertiaBufferCPU->resize(config.m_maxConvexBodies); - - m_data->m_pBufContactBuffersGPU[0] = new b3OpenCLArray<b3Contact4>(ctx,queue, config.m_maxContactCapacity,true); - m_data->m_pBufContactBuffersGPU[1] = new b3OpenCLArray<b3Contact4>(ctx,queue, config.m_maxContactCapacity,true); - - m_data->m_inertiaBufferGPU = new b3OpenCLArray<b3InertiaData>(ctx,queue,config.m_maxConvexBodies,false); - m_data->m_collidablesGPU = new b3OpenCLArray<b3Collidable>(ctx,queue,config.m_maxConvexShapes); + + m_data->m_pBufContactBuffersGPU[0] = new b3OpenCLArray<b3Contact4>(ctx, queue, config.m_maxContactCapacity, true); + m_data->m_pBufContactBuffersGPU[1] = new b3OpenCLArray<b3Contact4>(ctx, queue, config.m_maxContactCapacity, true); + + m_data->m_inertiaBufferGPU = new b3OpenCLArray<b3InertiaData>(ctx, queue, config.m_maxConvexBodies, false); + m_data->m_collidablesGPU = new b3OpenCLArray<b3Collidable>(ctx, queue, config.m_maxConvexShapes); m_data->m_collidablesCPU.reserve(config.m_maxConvexShapes); m_data->m_localShapeAABBCPU = new b3AlignedObjectArray<b3SapAabb>; - m_data->m_localShapeAABBGPU = new b3OpenCLArray<b3SapAabb>(ctx,queue,config.m_maxConvexShapes); - - + m_data->m_localShapeAABBGPU = new b3OpenCLArray<b3SapAabb>(ctx, queue, config.m_maxConvexShapes); + //m_data->m_solverDataGPU = adl::Solver<adl::TYPE_CL>::allocate(ctx,queue, config.m_maxBroadphasePairs,false); - m_data->m_bodyBufferGPU = new b3OpenCLArray<b3RigidBodyData>(ctx,queue, config.m_maxConvexBodies,false); + m_data->m_bodyBufferGPU = new b3OpenCLArray<b3RigidBodyData>(ctx, queue, config.m_maxConvexBodies, false); - m_data->m_convexFacesGPU = new b3OpenCLArray<b3GpuFace>(ctx,queue,config.m_maxConvexShapes*config.m_maxFacesPerShape,false); - m_data->m_convexFaces.reserve(config.m_maxConvexShapes*config.m_maxFacesPerShape); + m_data->m_convexFacesGPU = new b3OpenCLArray<b3GpuFace>(ctx, queue, config.m_maxConvexShapes * config.m_maxFacesPerShape, false); + m_data->m_convexFaces.reserve(config.m_maxConvexShapes * config.m_maxFacesPerShape); - m_data->m_gpuChildShapes = new b3OpenCLArray<b3GpuChildShape>(ctx,queue,config.m_maxCompoundChildShapes,false); - - m_data->m_convexPolyhedraGPU = new b3OpenCLArray<b3ConvexPolyhedronData>(ctx,queue,config.m_maxConvexShapes,false); + m_data->m_gpuChildShapes = new b3OpenCLArray<b3GpuChildShape>(ctx, queue, config.m_maxCompoundChildShapes, false); + + m_data->m_convexPolyhedraGPU = new b3OpenCLArray<b3ConvexPolyhedronData>(ctx, queue, config.m_maxConvexShapes, false); m_data->m_convexPolyhedra.reserve(config.m_maxConvexShapes); - m_data->m_uniqueEdgesGPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexUniqueEdges,true); + m_data->m_uniqueEdgesGPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexUniqueEdges, true); m_data->m_uniqueEdges.reserve(config.m_maxConvexUniqueEdges); - - - m_data->m_convexVerticesGPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexVertices,true); + m_data->m_convexVerticesGPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexVertices, true); m_data->m_convexVertices.reserve(config.m_maxConvexVertices); - m_data->m_convexIndicesGPU = new b3OpenCLArray<int>(ctx,queue,config.m_maxConvexIndices,true); - m_data->m_convexIndices.reserve(config.m_maxConvexIndices); - - m_data->m_worldVertsB1GPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexBodies*config.m_maxVerticesPerFace); - m_data->m_clippingFacesOutGPU = new b3OpenCLArray<b3Int4>(ctx,queue,config.m_maxConvexBodies); - m_data->m_worldNormalsAGPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexBodies); - m_data->m_worldVertsA1GPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexBodies*config.m_maxVerticesPerFace); - m_data->m_worldVertsB2GPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexBodies*config.m_maxVerticesPerFace); - - + m_data->m_convexIndicesGPU = new b3OpenCLArray<int>(ctx, queue, config.m_maxConvexIndices, true); + m_data->m_convexIndices.reserve(config.m_maxConvexIndices); + + m_data->m_worldVertsB1GPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexBodies * config.m_maxVerticesPerFace); + m_data->m_clippingFacesOutGPU = new b3OpenCLArray<b3Int4>(ctx, queue, config.m_maxConvexBodies); + m_data->m_worldNormalsAGPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexBodies); + m_data->m_worldVertsA1GPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexBodies * config.m_maxVerticesPerFace); + m_data->m_worldVertsB2GPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexBodies * config.m_maxVerticesPerFace); - m_data->m_convexData = new b3AlignedObjectArray<b3ConvexUtility* >(); + m_data->m_convexData = new b3AlignedObjectArray<b3ConvexUtility*>(); m_data->m_convexData->resize(config.m_maxConvexShapes); m_data->m_convexPolyhedra.resize(config.m_maxConvexShapes); - + m_data->m_numAcceleratedShapes = 0; m_data->m_numAcceleratedRigidBodies = 0; - - - m_data->m_subTreesGPU = new b3OpenCLArray<b3BvhSubtreeInfo>(this->m_context,this->m_queue); - m_data->m_treeNodesGPU = new b3OpenCLArray<b3QuantizedBvhNode>(this->m_context,this->m_queue); - m_data->m_bvhInfoGPU = new b3OpenCLArray<b3BvhInfo>(this->m_context,this->m_queue); + + m_data->m_subTreesGPU = new b3OpenCLArray<b3BvhSubtreeInfo>(this->m_context, this->m_queue); + m_data->m_treeNodesGPU = new b3OpenCLArray<b3QuantizedBvhNode>(this->m_context, this->m_queue); + m_data->m_bvhInfoGPU = new b3OpenCLArray<b3BvhInfo>(this->m_context, this->m_queue); //m_data->m_contactCGPU = new b3OpenCLArray<Constraint4>(ctx,queue,config.m_maxBroadphasePairs,false); //m_data->m_frictionCGPU = new b3OpenCLArray<adl::Solver<adl::TYPE_CL>::allocateFrictionConstraint( m_data->m_deviceCL, config.m_maxBroadphasePairs); - - - } - b3GpuNarrowPhase::~b3GpuNarrowPhase() { delete m_data->m_gpuSatCollision; - + delete m_data->m_triangleConvexPairs; //delete m_data->m_convexPairsOutGPU; //delete m_data->m_planePairs; @@ -126,7 +105,6 @@ b3GpuNarrowPhase::~b3GpuNarrowPhase() delete m_data->m_pBufContactBuffersGPU[0]; delete m_data->m_pBufContactBuffersGPU[1]; - delete m_data->m_inertiaBufferGPU; delete m_data->m_collidablesGPU; delete m_data->m_localShapeAABBCPU; @@ -139,18 +117,18 @@ b3GpuNarrowPhase::~b3GpuNarrowPhase() delete m_data->m_convexVerticesGPU; delete m_data->m_convexIndicesGPU; delete m_data->m_worldVertsB1GPU; - delete m_data->m_clippingFacesOutGPU; - delete m_data->m_worldNormalsAGPU; + delete m_data->m_clippingFacesOutGPU; + delete m_data->m_worldNormalsAGPU; delete m_data->m_worldVertsA1GPU; - delete m_data->m_worldVertsB2GPU; - + delete m_data->m_worldVertsB2GPU; + delete m_data->m_bvhInfoGPU; - for (int i=0;i<m_data->m_bvhData.size();i++) + for (int i = 0; i < m_data->m_bvhData.size(); i++) { delete m_data->m_bvhData[i]; } - for (int i=0;i<m_data->m_meshInterfaces.size();i++) + for (int i = 0; i < m_data->m_meshInterfaces.size(); i++) { delete m_data->m_meshInterfaces[i]; } @@ -159,198 +137,180 @@ b3GpuNarrowPhase::~b3GpuNarrowPhase() delete m_data->m_treeNodesGPU; delete m_data->m_subTreesGPU; - - delete m_data->m_convexData; + delete m_data->m_convexData; delete m_data; } - -int b3GpuNarrowPhase::allocateCollidable() +int b3GpuNarrowPhase::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 b3GpuNarrowPhase::registerSphereShape(float radius) +int b3GpuNarrowPhase::registerSphereShape(float radius) { int collidableIndex = allocateCollidable(); - if (collidableIndex<0) + if (collidableIndex < 0) return collidableIndex; - b3Collidable& col = getCollidableCpu(collidableIndex); col.m_shapeType = SHAPE_SPHERE; col.m_shapeIndex = 0; col.m_radius = radius; - - if (col.m_shapeIndex>=0) + + if (col.m_shapeIndex >= 0) { b3SapAabb aabb; - b3Vector3 myAabbMin=b3MakeVector3(-radius,-radius,-radius); - b3Vector3 myAabbMax=b3MakeVector3(radius,radius,radius); + b3Vector3 myAabbMin = b3MakeVector3(-radius, -radius, -radius); + b3Vector3 myAabbMax = b3MakeVector3(radius, radius, radius); - aabb.m_min[0] = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x; - aabb.m_min[1] = myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y; - aabb.m_min[2] = myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z; + aabb.m_min[0] = myAabbMin[0]; //s_convexHeightField->m_aabb.m_min.x; + aabb.m_min[1] = myAabbMin[1]; //s_convexHeightField->m_aabb.m_min.y; + aabb.m_min[2] = myAabbMin[2]; //s_convexHeightField->m_aabb.m_min.z; aabb.m_minIndices[3] = 0; - aabb.m_max[0] = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x; - aabb.m_max[1] = myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y; - aabb.m_max[2] = myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z; + aabb.m_max[0] = myAabbMax[0]; //s_convexHeightField->m_aabb.m_max.x; + aabb.m_max[1] = myAabbMax[1]; //s_convexHeightField->m_aabb.m_max.y; + aabb.m_max[2] = myAabbMax[2]; //s_convexHeightField->m_aabb.m_max.z; aabb.m_signedMaxIndices[3] = 0; m_data->m_localShapeAABBCPU->push_back(aabb); -// m_data->m_localShapeAABBGPU->push_back(aabb); + // m_data->m_localShapeAABBGPU->push_back(aabb); clFinish(m_queue); } - + return collidableIndex; } - int b3GpuNarrowPhase::registerFace(const b3Vector3& faceNormal, float faceConstant) { int faceOffset = m_data->m_convexFaces.size(); b3GpuFace& face = m_data->m_convexFaces.expand(); - face.m_plane = b3MakeVector3(faceNormal.x,faceNormal.y,faceNormal.z,faceConstant); + face.m_plane = b3MakeVector3(faceNormal.x, faceNormal.y, faceNormal.z, faceConstant); return faceOffset; } -int b3GpuNarrowPhase::registerPlaneShape(const b3Vector3& planeNormal, float planeConstant) +int b3GpuNarrowPhase::registerPlaneShape(const b3Vector3& planeNormal, float planeConstant) { int collidableIndex = allocateCollidable(); - if (collidableIndex<0) + if (collidableIndex < 0) return collidableIndex; - b3Collidable& col = getCollidableCpu(collidableIndex); col.m_shapeType = SHAPE_PLANE; - col.m_shapeIndex = registerFace(planeNormal,planeConstant); + col.m_shapeIndex = registerFace(planeNormal, planeConstant); col.m_radius = planeConstant; - - if (col.m_shapeIndex>=0) + + if (col.m_shapeIndex >= 0) { b3SapAabb aabb; aabb.m_min[0] = -1e30f; aabb.m_min[1] = -1e30f; aabb.m_min[2] = -1e30f; aabb.m_minIndices[3] = 0; - + aabb.m_max[0] = 1e30f; aabb.m_max[1] = 1e30f; aabb.m_max[2] = 1e30f; aabb.m_signedMaxIndices[3] = 0; m_data->m_localShapeAABBCPU->push_back(aabb); -// m_data->m_localShapeAABBGPU->push_back(aabb); + // m_data->m_localShapeAABBGPU->push_back(aabb); clFinish(m_queue); } - + return collidableIndex; } - -int b3GpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col) +int b3GpuNarrowPhase::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++; } - -int b3GpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling) +int b3GpuNarrowPhase::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); @@ -358,35 +318,34 @@ int b3GpuNarrowPhase::registerConvexHullShape(const float* vertices, int stride return collidableIndex; } -int b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr) +int b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr) { int collidableIndex = allocateCollidable(); - if (collidableIndex<0) + if (collidableIndex < 0) return collidableIndex; b3Collidable& col = getCollidableCpu(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) { b3SapAabb 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]); @@ -402,18 +361,16 @@ int b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr) aabb.m_signedMaxIndices[3] = 0; m_data->m_localShapeAABBCPU->push_back(aabb); -// m_data->m_localShapeAABBGPU->push_back(aabb); + // m_data->m_localShapeAABBGPU->push_back(aabb); } - - return collidableIndex; + return collidableIndex; } -int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes) +int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes) { - int collidableIndex = allocateCollidable(); - if (collidableIndex<0) + if (collidableIndex < 0) return collidableIndex; b3Collidable& col = getCollidableCpu(collidableIndex); @@ -422,44 +379,41 @@ int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShap col.m_compoundBvhIndex = m_data->m_bvhInfoCPU.size(); { - b3Assert(col.m_shapeIndex+childShapes->size()<m_data->m_config.m_maxCompoundChildShapes); - for (int i=0;i<childShapes->size();i++) + b3Assert(col.m_shapeIndex + childShapes->size() < m_data->m_config.m_maxCompoundChildShapes); + for (int i = 0; i < childShapes->size(); i++) { m_data->m_cpuChildShapes.push_back(childShapes->at(i)); } } - - col.m_numChildShapes = childShapes->size(); - - + b3SapAabb aabbLocalSpace; - b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f); - b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f); - + b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f); + b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f); + b3AlignedObjectArray<b3Aabb> childLocalAabbs; childLocalAabbs.resize(childShapes->size()); //compute local AABB of the compound of all children - for (int i=0;i<childShapes->size();i++) + for (int i = 0; i < childShapes->size(); i++) { int childColIndex = childShapes->at(i).m_shapeIndex; //b3Collidable& childCol = getCollidableCpu(childColIndex); - b3SapAabb aabbLoc =m_data->m_localShapeAABBCPU->at(childColIndex); + b3SapAabb aabbLoc = m_data->m_localShapeAABBCPU->at(childColIndex); - b3Vector3 childLocalAabbMin=b3MakeVector3(aabbLoc.m_min[0],aabbLoc.m_min[1],aabbLoc.m_min[2]); - b3Vector3 childLocalAabbMax=b3MakeVector3(aabbLoc.m_max[0],aabbLoc.m_max[1],aabbLoc.m_max[2]); - b3Vector3 aMin,aMax; + b3Vector3 childLocalAabbMin = b3MakeVector3(aabbLoc.m_min[0], aabbLoc.m_min[1], aabbLoc.m_min[2]); + b3Vector3 childLocalAabbMax = b3MakeVector3(aabbLoc.m_max[0], aabbLoc.m_max[1], aabbLoc.m_max[2]); + b3Vector3 aMin, aMax; b3Scalar margin(0.f); b3Transform childTr; childTr.setIdentity(); childTr.setOrigin(childShapes->at(i).m_childPosition); childTr.setRotation(b3Quaternion(childShapes->at(i).m_childOrientation)); - b3TransformAabb(childLocalAabbMin,childLocalAabbMax,margin,childTr,aMin,aMax); + b3TransformAabb(childLocalAabbMin, childLocalAabbMax, margin, childTr, aMin, aMax); myAabbMin.setMin(aMin); - myAabbMax.setMax(aMax); + myAabbMax.setMax(aMax); childLocalAabbs[i].m_min[0] = aMin[0]; childLocalAabbs[i].m_min[1] = aMin[1]; childLocalAabbs[i].m_min[2] = aMin[2]; @@ -469,36 +423,35 @@ int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShap childLocalAabbs[i].m_max[2] = aMax[2]; childLocalAabbs[i].m_max[3] = 0; } - - aabbLocalSpace.m_min[0] = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x; - aabbLocalSpace.m_min[1]= myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y; - aabbLocalSpace.m_min[2]= myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z; + + aabbLocalSpace.m_min[0] = myAabbMin[0]; //s_convexHeightField->m_aabb.m_min.x; + aabbLocalSpace.m_min[1] = myAabbMin[1]; //s_convexHeightField->m_aabb.m_min.y; + aabbLocalSpace.m_min[2] = myAabbMin[2]; //s_convexHeightField->m_aabb.m_min.z; aabbLocalSpace.m_minIndices[3] = 0; - - aabbLocalSpace.m_max[0] = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x; - aabbLocalSpace.m_max[1]= myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y; - aabbLocalSpace.m_max[2]= myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z; + + aabbLocalSpace.m_max[0] = myAabbMax[0]; //s_convexHeightField->m_aabb.m_max.x; + aabbLocalSpace.m_max[1] = myAabbMax[1]; //s_convexHeightField->m_aabb.m_max.y; + aabbLocalSpace.m_max[2] = myAabbMax[2]; //s_convexHeightField->m_aabb.m_max.z; aabbLocalSpace.m_signedMaxIndices[3] = 0; - - m_data->m_localShapeAABBCPU->push_back(aabbLocalSpace); + m_data->m_localShapeAABBCPU->push_back(aabbLocalSpace); b3QuantizedBvh* bvh = new b3QuantizedBvh; - bvh->setQuantizationValues(myAabbMin,myAabbMax); - QuantizedNodeArray& nodes = bvh->getLeafNodeArray(); + bvh->setQuantizationValues(myAabbMin, myAabbMax); + QuantizedNodeArray& nodes = bvh->getLeafNodeArray(); int numNodes = childShapes->size(); - for (int i=0;i<numNodes;i++) + for (int i = 0; i < numNodes; i++) { b3QuantizedBvhNode node; - b3Vector3 aabbMin,aabbMax; - aabbMin = (b3Vector3&) childLocalAabbs[i].m_min; - aabbMax = (b3Vector3&) childLocalAabbs[i].m_max; + b3Vector3 aabbMin, aabbMax; + aabbMin = (b3Vector3&)childLocalAabbs[i].m_min; + aabbMax = (b3Vector3&)childLocalAabbs[i].m_max; - bvh->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); - bvh->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); + bvh->quantize(&node.m_quantizedAabbMin[0], aabbMin, 0); + bvh->quantize(&node.m_quantizedAabbMax[0], aabbMax, 1); int partId = 0; - node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i; + node.m_escapeIndexOrTriangleIndex = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | i; nodes.push_back(node); } bvh->buildInternal(); @@ -511,7 +464,7 @@ int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShap //void buildInternal(); b3BvhInfo bvhInfo; - + bvhInfo.m_aabbMin = bvh->m_bvhAabbMin; bvhInfo.m_aabbMax = bvh->m_bvhAabbMax; bvhInfo.m_quantization = bvh->m_bvhQuantization; @@ -520,80 +473,72 @@ int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShap bvhInfo.m_nodeOffset = m_data->m_treeNodesCPU.size(); bvhInfo.m_subTreeOffset = m_data->m_subTreesCPU.size(); - int numNewNodes = bvh->getQuantizedNodeArray().size(); + int numNewNodes = bvh->getQuantizedNodeArray().size(); - for (int i=0;i<numNewNodes-1;i++) + for (int i = 0; i < numNewNodes - 1; i++) { - if (bvh->getQuantizedNodeArray()[i].isLeafNode()) { int orgIndex = bvh->getQuantizedNodeArray()[i].getTriangleIndex(); b3Vector3 nodeMinVec = bvh->unQuantize(bvh->getQuantizedNodeArray()[i].m_quantizedAabbMin); b3Vector3 nodeMaxVec = bvh->unQuantize(bvh->getQuantizedNodeArray()[i].m_quantizedAabbMax); - - for (int c=0;c<3;c++) + + for (int c = 0; c < 3; c++) { if (childLocalAabbs[orgIndex].m_min[c] < nodeMinVec[c]) { - printf("min org (%f) and new (%f) ? at i:%d,c:%d\n",childLocalAabbs[i].m_min[c],nodeMinVec[c],i,c); + printf("min org (%f) and new (%f) ? at i:%d,c:%d\n", childLocalAabbs[i].m_min[c], nodeMinVec[c], i, c); } if (childLocalAabbs[orgIndex].m_max[c] > nodeMaxVec[c]) { - printf("max org (%f) and new (%f) ? at i:%d,c:%d\n",childLocalAabbs[i].m_max[c],nodeMaxVec[c],i,c); + printf("max org (%f) and new (%f) ? at i:%d,c:%d\n", childLocalAabbs[i].m_max[c], nodeMaxVec[c], i, c); } - } } - } m_data->m_bvhInfoCPU.push_back(bvhInfo); int numNewSubtrees = bvh->getSubtreeInfoArray().size(); - m_data->m_subTreesCPU.reserve(m_data->m_subTreesCPU.size()+numNewSubtrees); - for (int i=0;i<numNewSubtrees;i++) + m_data->m_subTreesCPU.reserve(m_data->m_subTreesCPU.size() + numNewSubtrees); + for (int i = 0; i < numNewSubtrees; i++) { m_data->m_subTreesCPU.push_back(bvh->getSubtreeInfoArray()[i]); } int numNewTreeNodes = bvh->getQuantizedNodeArray().size(); - for (int i=0;i<numNewTreeNodes;i++) + for (int i = 0; i < numNewTreeNodes; i++) { m_data->m_treeNodesCPU.push_back(bvh->getQuantizedNodeArray()[i]); } -// m_data->m_localShapeAABBGPU->push_back(aabbWS); + // m_data->m_localShapeAABBGPU->push_back(aabbWS); clFinish(m_queue); return collidableIndex; - } - -int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,const float* scaling1) +int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, const float* scaling1) { - - - b3Vector3 scaling=b3MakeVector3(scaling1[0],scaling1[1],scaling1[2]); + b3Vector3 scaling = b3MakeVector3(scaling1[0], scaling1[1], scaling1[2]); int collidableIndex = allocateCollidable(); - if (collidableIndex<0) + if (collidableIndex < 0) return collidableIndex; b3Collidable& col = getCollidableCpu(collidableIndex); - + col.m_shapeType = SHAPE_CONCAVE_TRIMESH; - col.m_shapeIndex = registerConcaveMeshShape(vertices,indices,col,scaling); + col.m_shapeIndex = registerConcaveMeshShape(vertices, indices, col, scaling); col.m_bvhIndex = m_data->m_bvhInfoCPU.size(); - b3SapAabb aabb; - b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f); - b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f); + b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f); + b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f); - for (int i=0;i<vertices->size();i++) + for (int i = 0; i < vertices->size(); i++) { - b3Vector3 vtx(vertices->at(i)*scaling); + b3Vector3 vtx(vertices->at(i) * scaling); myAabbMin.setMin(vtx); myAabbMax.setMax(vtx); } @@ -603,27 +548,27 @@ int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vert aabb.m_minIndices[3] = 0; aabb.m_max[0] = myAabbMax[0]; - aabb.m_max[1]= myAabbMax[1]; - aabb.m_max[2]= myAabbMax[2]; - aabb.m_signedMaxIndices[3]= 0; + aabb.m_max[1] = myAabbMax[1]; + aabb.m_max[2] = myAabbMax[2]; + aabb.m_signedMaxIndices[3] = 0; m_data->m_localShapeAABBCPU->push_back(aabb); -// m_data->m_localShapeAABBGPU->push_back(aabb); + // m_data->m_localShapeAABBGPU->push_back(aabb); b3OptimizedBvh* bvh = new b3OptimizedBvh(); //void b3OptimizedBvh::build(b3StridingMeshInterface* triangles, bool useQuantizedAabbCompression, const b3Vector3& bvhAabbMin, const b3Vector3& bvhAabbMax) - + bool useQuantizedAabbCompression = true; - b3TriangleIndexVertexArray* meshInterface=new b3TriangleIndexVertexArray(); + b3TriangleIndexVertexArray* meshInterface = new b3TriangleIndexVertexArray(); m_data->m_meshInterfaces.push_back(meshInterface); b3IndexedMesh mesh; - mesh.m_numTriangles = indices->size()/3; + mesh.m_numTriangles = indices->size() / 3; mesh.m_numVertices = vertices->size(); - mesh.m_vertexBase = (const unsigned char *)&vertices->at(0).x; + mesh.m_vertexBase = (const unsigned char*)&vertices->at(0).x; mesh.m_vertexStride = sizeof(b3Vector3); - mesh.m_triangleIndexStride = 3 * sizeof(int);// or sizeof(int) - mesh.m_triangleIndexBase = (const unsigned char *)&indices->at(0); - + mesh.m_triangleIndexStride = 3 * sizeof(int); // or sizeof(int) + mesh.m_triangleIndexBase = (const unsigned char*)&indices->at(0); + meshInterface->addIndexedMesh(mesh); bvh->build(meshInterface, useQuantizedAabbCompression, (b3Vector3&)aabb.m_min, (b3Vector3&)aabb.m_max); m_data->m_bvhData.push_back(bvh); @@ -632,7 +577,7 @@ int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vert int numSubTrees = bvh->getSubtreeInfoArray().size(); b3BvhInfo bvhInfo; - + bvhInfo.m_aabbMin = bvh->m_bvhAabbMin; bvhInfo.m_aabbMax = bvh->m_bvhAabbMax; bvhInfo.m_quantization = bvh->m_bvhQuantization; @@ -643,97 +588,87 @@ int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vert m_data->m_bvhInfoCPU.push_back(bvhInfo); - int numNewSubtrees = bvh->getSubtreeInfoArray().size(); - m_data->m_subTreesCPU.reserve(m_data->m_subTreesCPU.size()+numNewSubtrees); - for (int i=0;i<numNewSubtrees;i++) + m_data->m_subTreesCPU.reserve(m_data->m_subTreesCPU.size() + numNewSubtrees); + for (int i = 0; i < numNewSubtrees; i++) { m_data->m_subTreesCPU.push_back(bvh->getSubtreeInfoArray()[i]); } int numNewTreeNodes = bvh->getQuantizedNodeArray().size(); - for (int i=0;i<numNewTreeNodes;i++) + for (int i = 0; i < numNewTreeNodes; i++) { m_data->m_treeNodesCPU.push_back(bvh->getQuantizedNodeArray()[i]); } - - - return collidableIndex; } -int b3GpuNarrowPhase::registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,b3Collidable& col, const float* scaling1) +int b3GpuNarrowPhase::registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling1) { + b3Vector3 scaling = b3MakeVector3(scaling1[0], scaling1[1], scaling1[2]); + m_data->m_convexData->resize(m_data->m_numAcceleratedShapes + 1); + m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1); - b3Vector3 scaling=b3MakeVector3(scaling1[0],scaling1[1],scaling1[2]); - - 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); - convex.mC = b3MakeVector3(0,0,0); - convex.mE = b3MakeVector3(0,0,0); - convex.m_extents= b3MakeVector3(0,0,0); - convex.m_localCenter = b3MakeVector3(0,0,0); + b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1); + convex.mC = b3MakeVector3(0, 0, 0); + convex.mE = b3MakeVector3(0, 0, 0); + convex.m_extents = b3MakeVector3(0, 0, 0); + convex.m_localCenter = b3MakeVector3(0, 0, 0); convex.m_radius = 0.f; - + convex.m_numUniqueEdges = 0; int edgeOffset = m_data->m_uniqueEdges.size(); convex.m_uniqueEdgesOffset = edgeOffset; - + int faceOffset = m_data->m_convexFaces.size(); convex.m_faceOffset = faceOffset; - - convex.m_numFaces = indices->size()/3; - m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces); - m_data->m_convexIndices.reserve(convex.m_numFaces*3); - for (int i=0;i<convex.m_numFaces;i++) + + convex.m_numFaces = indices->size() / 3; + m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces); + m_data->m_convexIndices.reserve(convex.m_numFaces * 3); + for (int i = 0; i < convex.m_numFaces; i++) { - if (i%256==0) + if (i % 256 == 0) { //printf("i=%d out of %d", i,convex.m_numFaces); } - b3Vector3 vert0(vertices->at(indices->at(i*3))*scaling); - b3Vector3 vert1(vertices->at(indices->at(i*3+1))*scaling); - b3Vector3 vert2(vertices->at(indices->at(i*3+2))*scaling); + b3Vector3 vert0(vertices->at(indices->at(i * 3)) * scaling); + b3Vector3 vert1(vertices->at(indices->at(i * 3 + 1)) * scaling); + b3Vector3 vert2(vertices->at(indices->at(i * 3 + 2)) * scaling); - b3Vector3 normal = ((vert1-vert0).cross(vert2-vert0)).normalize(); + b3Vector3 normal = ((vert1 - vert0).cross(vert2 - vert0)).normalize(); b3Scalar c = -(normal.dot(vert0)); - m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector4(normal.x,normal.y,normal.z,c); + m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector4(normal.x, normal.y, normal.z, c); int indexOffset = m_data->m_convexIndices.size(); int numIndices = 3; - 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++) { - int vi = indices->at(i*3+p); - m_data->m_convexIndices[indexOffset+p] = vi;//convexPtr->m_faces[i].m_indices[p]; + int vi = indices->at(i * 3 + p); + m_data->m_convexIndices[indexOffset + p] = vi; //convexPtr->m_faces[i].m_indices[p]; } } - + convex.m_numVertices = 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<vertices->size();i++) + convex.m_vertexOffset = vertexOffset; + m_data->m_convexVertices.resize(vertexOffset + convex.m_numVertices); + for (int i = 0; i < vertices->size(); i++) { - m_data->m_convexVertices[vertexOffset+i] = vertices->at(i)*scaling; + m_data->m_convexVertices[vertexOffset + i] = vertices->at(i) * scaling; } (*m_data->m_convexData)[m_data->m_numAcceleratedShapes] = 0; - - + return m_data->m_numAcceleratedShapes++; } - - -cl_mem b3GpuNarrowPhase::getBodiesGpu() +cl_mem b3GpuNarrowPhase::getBodiesGpu() { return (cl_mem)m_data->m_bodyBufferGPU->getBufferCL(); } @@ -743,25 +678,21 @@ const struct b3RigidBodyData* b3GpuNarrowPhase::getBodiesCpu() const return &m_data->m_bodyBufferCPU->at(0); }; - - - -int b3GpuNarrowPhase::getNumBodiesGpu() const +int b3GpuNarrowPhase::getNumBodiesGpu() const { return m_data->m_bodyBufferGPU->size(); } -cl_mem b3GpuNarrowPhase::getBodyInertiasGpu() +cl_mem b3GpuNarrowPhase::getBodyInertiasGpu() { return (cl_mem)m_data->m_inertiaBufferGPU->getBufferCL(); } -int b3GpuNarrowPhase::getNumBodyInertiasGpu() const +int b3GpuNarrowPhase::getNumBodyInertiasGpu() const { return m_data->m_inertiaBufferGPU->size(); } - b3Collidable& b3GpuNarrowPhase::getCollidableCpu(int collidableIndex) { return m_data->m_collidablesCPU[collidableIndex]; @@ -789,25 +720,20 @@ const struct b3SapAabb* b3GpuNarrowPhase::getLocalSpaceAabbsCpu() const if (m_data->m_localShapeAABBCPU->size()) { return &m_data->m_localShapeAABBCPU->at(0); - } + } return 0; } - -cl_mem b3GpuNarrowPhase::getAabbLocalSpaceBufferGpu() +cl_mem b3GpuNarrowPhase::getAabbLocalSpaceBufferGpu() { return m_data->m_localShapeAABBGPU->getBufferCL(); } -int b3GpuNarrowPhase::getNumCollidablesGpu() const +int b3GpuNarrowPhase::getNumCollidablesGpu() const { return m_data->m_collidablesGPU->size(); } - - - - -int b3GpuNarrowPhase::getNumContactsGpu() const +int b3GpuNarrowPhase::getNumContactsGpu() const { return m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->size(); } @@ -824,37 +750,33 @@ const b3Contact4* b3GpuNarrowPhase::getContactsCPU() const void b3GpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects) { - cl_mem aabbsLocalSpace = m_data->m_localShapeAABBGPU->getBufferCL(); int nContactOut = 0; //swap buffer - m_data->m_currentContactBuffer=1-m_data->m_currentContactBuffer; + m_data->m_currentContactBuffer = 1 - m_data->m_currentContactBuffer; //int curSize = m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->size(); int maxTriConvexPairCapacity = m_data->m_config.m_maxTriConvexPairCapacity; - int numTriConvexPairsOut=0; - - b3OpenCLArray<b3Int4> broadphasePairsGPU(m_context,m_queue); - broadphasePairsGPU.setFromOpenCLBuffer(broadphasePairs,numBroadphasePairs); - - + int numTriConvexPairsOut = 0; + b3OpenCLArray<b3Int4> broadphasePairsGPU(m_context, m_queue); + broadphasePairsGPU.setFromOpenCLBuffer(broadphasePairs, numBroadphasePairs); - b3OpenCLArray<b3Aabb> clAabbArrayWorldSpace(this->m_context,this->m_queue); - clAabbArrayWorldSpace.setFromOpenCLBuffer(aabbsWorldSpace,numObjects); + b3OpenCLArray<b3Aabb> clAabbArrayWorldSpace(this->m_context, this->m_queue); + clAabbArrayWorldSpace.setFromOpenCLBuffer(aabbsWorldSpace, numObjects); - b3OpenCLArray<b3Aabb> clAabbArrayLocalSpace(this->m_context,this->m_queue); - clAabbArrayLocalSpace.setFromOpenCLBuffer(aabbsLocalSpace,numObjects); + b3OpenCLArray<b3Aabb> clAabbArrayLocalSpace(this->m_context, this->m_queue); + clAabbArrayLocalSpace.setFromOpenCLBuffer(aabbsLocalSpace, numObjects); m_data->m_gpuSatCollision->computeConvexConvexContactsGPUSAT( &broadphasePairsGPU, numBroadphasePairs, m_data->m_bodyBufferGPU, m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer], nContactOut, - m_data->m_pBufContactBuffersGPU[1-m_data->m_currentContactBuffer], + m_data->m_pBufContactBuffersGPU[1 - m_data->m_currentContactBuffer], m_data->m_config.m_maxContactCapacity, m_data->m_config.m_compoundPairCapacity, *m_data->m_convexPolyhedraGPU, @@ -878,8 +800,7 @@ void b3GpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphase numObjects, maxTriConvexPairCapacity, *m_data->m_triangleConvexPairs, - numTriConvexPairsOut - ); + numTriConvexPairsOut); /*b3AlignedObjectArray<b3Int4> broadphasePairsCPU; broadphasePairsGPU.copyToHost(broadphasePairsCPU); @@ -892,105 +813,97 @@ const b3SapAabb& b3GpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const return m_data->m_localShapeAABBCPU->at(collidableIndex); } - - - - -int b3GpuNarrowPhase::registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation , const float* aabbMinPtr, const float* aabbMaxPtr,bool writeToGpu) +int b3GpuNarrowPhase::registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMinPtr, const float* aabbMaxPtr, bool writeToGpu) { - b3Vector3 aabbMin=b3MakeVector3(aabbMinPtr[0],aabbMinPtr[1],aabbMinPtr[2]); - b3Vector3 aabbMax=b3MakeVector3(aabbMaxPtr[0],aabbMaxPtr[1],aabbMaxPtr[2]); - + b3Vector3 aabbMin = b3MakeVector3(aabbMinPtr[0], aabbMinPtr[1], aabbMinPtr[2]); + b3Vector3 aabbMax = b3MakeVector3(aabbMaxPtr[0], aabbMaxPtr[1], aabbMaxPtr[2]); if (m_data->m_numAcceleratedRigidBodies >= (m_data->m_config.m_maxConvexBodies)) { - b3Error("registerRigidBody: exceeding the number of rigid bodies, %d > %d \n",m_data->m_numAcceleratedRigidBodies,m_data->m_config.m_maxConvexBodies); + b3Error("registerRigidBody: exceeding the number of rigid bodies, %d > %d \n", m_data->m_numAcceleratedRigidBodies, m_data->m_config.m_maxConvexBodies); return -1; } - - m_data->m_bodyBufferCPU->resize(m_data->m_numAcceleratedRigidBodies+1); - + + m_data->m_bodyBufferCPU->resize(m_data->m_numAcceleratedRigidBodies + 1); + b3RigidBodyData& body = m_data->m_bodyBufferCPU->at(m_data->m_numAcceleratedRigidBodies); - + float friction = 1.f; float restitution = 0.f; - + body.m_frictionCoeff = friction; body.m_restituitionCoeff = restitution; - body.m_angVel = b3MakeVector3(0,0,0); - body.m_linVel=b3MakeVector3(0,0,0);//.setZero(); - body.m_pos =b3MakeVector3(position[0],position[1],position[2]); - body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]); + body.m_angVel = b3MakeVector3(0, 0, 0); + body.m_linVel = b3MakeVector3(0, 0, 0); //.setZero(); + body.m_pos = b3MakeVector3(position[0], position[1], position[2]); + body.m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]); body.m_collidableIdx = collidableIndex; - if (collidableIndex>=0) + if (collidableIndex >= 0) { -// body.m_shapeType = m_data->m_collidablesCPU.at(collidableIndex).m_shapeType; - } else + // body.m_shapeType = m_data->m_collidablesCPU.at(collidableIndex).m_shapeType; + } + else { - // body.m_shapeType = CollisionShape::SHAPE_PLANE; + // body.m_shapeType = CollisionShape::SHAPE_PLANE; m_planeBodyIndex = m_data->m_numAcceleratedRigidBodies; } //body.m_shapeType = shapeType; - - - body.m_invMass = mass? 1.f/mass : 0.f; - + + body.m_invMass = mass ? 1.f / mass : 0.f; + if (writeToGpu) { - m_data->m_bodyBufferGPU->copyFromHostPointer(&body,1,m_data->m_numAcceleratedRigidBodies); + m_data->m_bodyBufferGPU->copyFromHostPointer(&body, 1, m_data->m_numAcceleratedRigidBodies); } - + b3InertiaData& shapeInfo = m_data->m_inertiaBufferCPU->at(m_data->m_numAcceleratedRigidBodies); - - if (mass==0.f) + + if (mass == 0.f) { - if (m_data->m_numAcceleratedRigidBodies==0) + if (m_data->m_numAcceleratedRigidBodies == 0) m_static0Index = 0; - - shapeInfo.m_initInvInertia.setValue(0,0,0,0,0,0,0,0,0); - shapeInfo.m_invInertiaWorld.setValue(0,0,0,0,0,0,0,0,0); - } else + + shapeInfo.m_initInvInertia.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0); + shapeInfo.m_invInertiaWorld.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0); + } + else { - - b3Assert(body.m_collidableIdx>=0); - + b3Assert(body.m_collidableIdx >= 0); + //approximate using the aabb of the shape - + //Aabb aabb = (*m_data->m_shapePointers)[shapeIndex]->m_aabb; - b3Vector3 halfExtents = (aabbMax-aabbMin);//*0.5f;//fake larger inertia makes demos more stable ;-) - + b3Vector3 halfExtents = (aabbMax - aabbMin); //*0.5f;//fake larger inertia makes demos more stable ;-) + b3Vector3 localInertia; - - float lx=2.f*halfExtents[0]; - float ly=2.f*halfExtents[1]; - float lz=2.f*halfExtents[2]; - - localInertia.setValue( (mass/12.0f) * (ly*ly + lz*lz), - (mass/12.0f) * (lx*lx + lz*lz), - (mass/12.0f) * (lx*lx + ly*ly)); - + + float lx = 2.f * halfExtents[0]; + float ly = 2.f * halfExtents[1]; + float lz = 2.f * halfExtents[2]; + + localInertia.setValue((mass / 12.0f) * (ly * ly + lz * lz), + (mass / 12.0f) * (lx * lx + lz * lz), + (mass / 12.0f) * (lx * lx + ly * ly)); + b3Vector3 invLocalInertia; - invLocalInertia[0] = 1.f/localInertia[0]; - invLocalInertia[1] = 1.f/localInertia[1]; - invLocalInertia[2] = 1.f/localInertia[2]; + invLocalInertia[0] = 1.f / localInertia[0]; + invLocalInertia[1] = 1.f / localInertia[1]; + invLocalInertia[2] = 1.f / localInertia[2]; invLocalInertia[3] = 0.f; - + shapeInfo.m_initInvInertia.setValue( - invLocalInertia[0], 0, 0, - 0, invLocalInertia[1], 0, - 0, 0, invLocalInertia[2]); + invLocalInertia[0], 0, 0, + 0, invLocalInertia[1], 0, + 0, 0, invLocalInertia[2]); - b3Matrix3x3 m (body.m_quat); + b3Matrix3x3 m(body.m_quat); shapeInfo.m_invInertiaWorld = m.scaled(invLocalInertia) * m.transpose(); - } - + if (writeToGpu) - m_data->m_inertiaBufferGPU->copyFromHostPointer(&shapeInfo,1,m_data->m_numAcceleratedRigidBodies); - - - + m_data->m_inertiaBufferGPU->copyFromHostPointer(&shapeInfo, 1, m_data->m_numAcceleratedRigidBodies); + return m_data->m_numAcceleratedRigidBodies++; } @@ -999,15 +912,13 @@ int b3GpuNarrowPhase::getNumRigidBodies() const return m_data->m_numAcceleratedRigidBodies; } -void b3GpuNarrowPhase::writeAllBodiesToGpu() +void b3GpuNarrowPhase::writeAllBodiesToGpu() { - if (m_data->m_localShapeAABBCPU->size()) { m_data->m_localShapeAABBGPU->copyFromHost(*m_data->m_localShapeAABBCPU); } - - + m_data->m_gpuChildShapes->copyFromHost(m_data->m_cpuChildShapes); m_data->m_convexFacesGPU->copyFromHost(m_data->m_convexFaces); m_data->m_convexPolyhedraGPU->copyFromHost(m_data->m_convexPolyhedra); @@ -1018,25 +929,21 @@ void b3GpuNarrowPhase::writeAllBodiesToGpu() m_data->m_treeNodesGPU->copyFromHost(m_data->m_treeNodesCPU); m_data->m_subTreesGPU->copyFromHost(m_data->m_subTreesCPU); - m_data->m_bodyBufferGPU->resize(m_data->m_numAcceleratedRigidBodies); m_data->m_inertiaBufferGPU->resize(m_data->m_numAcceleratedRigidBodies); - + if (m_data->m_numAcceleratedRigidBodies) { - m_data->m_bodyBufferGPU->copyFromHostPointer(&m_data->m_bodyBufferCPU->at(0),m_data->m_numAcceleratedRigidBodies); - m_data->m_inertiaBufferGPU->copyFromHostPointer(&m_data->m_inertiaBufferCPU->at(0),m_data->m_numAcceleratedRigidBodies); + m_data->m_bodyBufferGPU->copyFromHostPointer(&m_data->m_bodyBufferCPU->at(0), m_data->m_numAcceleratedRigidBodies); + m_data->m_inertiaBufferGPU->copyFromHostPointer(&m_data->m_inertiaBufferCPU->at(0), m_data->m_numAcceleratedRigidBodies); } - if (m_data->m_collidablesCPU.size()) + if (m_data->m_collidablesCPU.size()) { m_data->m_collidablesGPU->copyFromHost(m_data->m_collidablesCPU); } - - } - -void b3GpuNarrowPhase::reset() +void b3GpuNarrowPhase::reset() { m_data->m_numAcceleratedShapes = 0; m_data->m_numAcceleratedRigidBodies = 0; @@ -1053,21 +960,19 @@ void b3GpuNarrowPhase::reset() m_data->m_treeNodesCPU.resize(0); m_data->m_subTreesCPU.resize(0); m_data->m_bvhInfoCPU.resize(0); - } - -void b3GpuNarrowPhase::readbackAllBodiesToCpu() +void b3GpuNarrowPhase::readbackAllBodiesToCpu() { - m_data->m_bodyBufferGPU->copyToHostPointer(&m_data->m_bodyBufferCPU->at(0),m_data->m_numAcceleratedRigidBodies); + m_data->m_bodyBufferGPU->copyToHostPointer(&m_data->m_bodyBufferCPU->at(0), m_data->m_numAcceleratedRigidBodies); } -void b3GpuNarrowPhase::setObjectTransformCpu(float* position, float* orientation , int bodyIndex) +void b3GpuNarrowPhase::setObjectTransformCpu(float* position, float* orientation, int bodyIndex) { - if (bodyIndex>=0 && bodyIndex<m_data->m_bodyBufferCPU->size()) + if (bodyIndex >= 0 && bodyIndex < m_data->m_bodyBufferCPU->size()) { - m_data->m_bodyBufferCPU->at(bodyIndex).m_pos=b3MakeVector3(position[0],position[1],position[2]); - m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]); + m_data->m_bodyBufferCPU->at(bodyIndex).m_pos = b3MakeVector3(position[0], position[1], position[2]); + m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]); } else { @@ -1076,24 +981,25 @@ void b3GpuNarrowPhase::setObjectTransformCpu(float* position, float* orientation } void b3GpuNarrowPhase::setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex) { - if (bodyIndex>=0 && bodyIndex<m_data->m_bodyBufferCPU->size()) + if (bodyIndex >= 0 && bodyIndex < m_data->m_bodyBufferCPU->size()) { - m_data->m_bodyBufferCPU->at(bodyIndex).m_linVel=b3MakeVector3(linVel[0],linVel[1],linVel[2]); - m_data->m_bodyBufferCPU->at(bodyIndex).m_angVel=b3MakeVector3(angVel[0],angVel[1],angVel[2]); - } else + m_data->m_bodyBufferCPU->at(bodyIndex).m_linVel = b3MakeVector3(linVel[0], linVel[1], linVel[2]); + m_data->m_bodyBufferCPU->at(bodyIndex).m_angVel = b3MakeVector3(angVel[0], angVel[1], angVel[2]); + } + else { b3Warning("setObjectVelocityCpu out of range.\n"); } } -bool b3GpuNarrowPhase::getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const +bool b3GpuNarrowPhase::getObjectTransformFromCpu(float* position, float* orientation, int bodyIndex) const { - if (bodyIndex>=0 && bodyIndex<m_data->m_bodyBufferCPU->size()) + if (bodyIndex >= 0 && bodyIndex < m_data->m_bodyBufferCPU->size()) { position[0] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.x; position[1] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.y; position[2] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.z; - position[3] = 1.f;//or 1 + position[3] = 1.f; //or 1 orientation[0] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.x; orientation[1] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.y; |