int b3GpuNarrowPhase::registerSphereShape(float radius)
{
int collidableIndex = allocateCollidable();
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)
{
b3SapAabb aabb;
b3Vector3 myAabbMin(-radius,-radius,-radius);
b3Vector3 myAabbMax(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_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_signedMaxIndices[3] = 0;
m_data->m_localShapeAABBCPU->push_back(aabb);
// m_data->m_localShapeAABBGPU->push_back(aabb);
clFinish(m_queue);
}
return collidableIndex;
}
int b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
{
int collidableIndex = allocateCollidable();
if (collidableIndex<0)
return collidableIndex;
b3Collidable& col = getCollidableCpu(collidableIndex);
col.m_shapeType = SHAPE_CONVEX_HULL;
col.m_shapeIndex = -1;
{
b3Vector3 localCenter(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 = registerConvexHullShape(utilPtr,col);
}
if (col.m_shapeIndex>=0)
{
b3SapAabb aabb;
b3Vector3 myAabbMin(1e30f,1e30f,1e30f);
b3Vector3 myAabbMax(-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]);
}
aabb.m_min[0] = myAabbMin[0];
aabb.m_min[1] = myAabbMin[1];
aabb.m_min[2] = myAabbMin[2];
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;
m_data->m_localShapeAABBCPU->push_back(aabb);
// m_data->m_localShapeAABBGPU->push_back(aabb);
}
return collidableIndex;
}
int CLPhysicsDemo::registerConcaveMesh(btAlignedObjectArray<btVector3>* vertices, btAlignedObjectArray<int>* indices,const float* scaling1)
{
btVector3 scaling(scaling1[0],scaling1[1],scaling1[2]);
int collidableIndex = m_narrowphaseAndSolver->allocateCollidable();
btCollidable& col = m_narrowphaseAndSolver->getCollidableCpu(collidableIndex);
col.m_shapeType = CollisionShape::SHAPE_CONCAVE_TRIMESH;
col.m_shapeIndex = m_narrowphaseAndSolver->registerConcaveMeshShape(vertices,indices,col,scaling);
btAABBHost aabbMin, aabbMax;
btVector3 myAabbMin(1e30f,1e30f,1e30f);
btVector3 myAabbMax(-1e30f,-1e30f,-1e30f);
for (int i=0;i<vertices->size();i++)
{
btVector3 vtx(vertices->at(i)*scaling);
myAabbMin.setMin(vtx);
myAabbMax.setMax(vtx);
}
aabbMin.fx = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x;
aabbMin.fy = myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y;
aabbMin.fz= myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z;
aabbMin.uw = 0;
aabbMax.fx = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x;
aabbMax.fy = myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y;
aabbMax.fz= myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z;
aabbMax.uw = 0;
m_data->m_localShapeAABBCPU->push_back(aabbMin);
m_data->m_localShapeAABBGPU->push_back(aabbMin);
m_data->m_localShapeAABBCPU->push_back(aabbMax);
m_data->m_localShapeAABBGPU->push_back(aabbMax);
clFinish(g_cqCommandQue);
return collidableIndex;
}
int CLPhysicsDemo::registerConcaveMesh(objLoader* obj,const float* scaling)
{
int collidableIndex = narrowphaseAndSolver->allocateCollidable();
btCollidable& col = narrowphaseAndSolver->getCollidableCpu(collidableIndex);
col.m_shapeType = CollisionShape::SHAPE_CONCAVE_TRIMESH;
col.m_shapeIndex = narrowphaseAndSolver->registerConcaveMeshShape(obj,col,scaling);
btAABBHost aabbMin, aabbMax;
btVector3 myAabbMin(1e30f,1e30f,1e30f);
btVector3 myAabbMax(-1e30f,-1e30f,-1e30f);
for (int i=0;i<obj->vertexCount;i++)
{
btVector3 vtx(obj->vertexList[i]->e[0]*scaling[0],obj->vertexList[i]->e[1]*scaling[1],obj->vertexList[i]->e[2]*scaling[2]);
myAabbMin.setMin(vtx);
myAabbMax.setMax(vtx);
}
aabbMin.fx = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x;
aabbMin.fy = myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y;
aabbMin.fz= myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z;
aabbMin.uw = 0;
aabbMax.fx = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x;
aabbMax.fy = myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y;
aabbMax.fz= myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z;
aabbMax.uw = 0;
m_data->m_localShapeAABBCPU->push_back(aabbMin);
m_data->m_localShapeAABBGPU->push_back(aabbMin);
m_data->m_localShapeAABBCPU->push_back(aabbMax);
m_data->m_localShapeAABBGPU->push_back(aabbMax);
clFinish(g_cqCommandQue);
return collidableIndex;
}
int CLPhysicsDemo::registerSphereShape(float radius)
{
int collidableIndex = m_narrowphaseAndSolver->allocateCollidable();
btCollidable& col = m_narrowphaseAndSolver->getCollidableCpu(collidableIndex);
col.m_shapeType = CollisionShape::SHAPE_SPHERE;
col.m_shapeIndex = 0;
col.m_radius = radius;
if (col.m_shapeIndex>=0)
{
btAABBHost aabbMin, aabbMax;
btVector3 myAabbMin(-radius,-radius,-radius);
btVector3 myAabbMax(radius,radius,radius);
aabbMin.fx = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x;
aabbMin.fy = myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y;
aabbMin.fz= myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z;
aabbMin.uw = 0;
aabbMax.fx = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x;
aabbMax.fy = myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y;
aabbMax.fz= myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z;
aabbMax.uw = 0;
m_data->m_localShapeAABBCPU->push_back(aabbMin);
m_data->m_localShapeAABBGPU->push_back(aabbMin);
m_data->m_localShapeAABBCPU->push_back(aabbMax);
m_data->m_localShapeAABBGPU->push_back(aabbMax);
//m_data->m_localShapeAABB->copyFromHostPointer(&aabbMin,1,shapeIndex*2);
//m_data->m_localShapeAABB->copyFromHostPointer(&aabbMax,1,shapeIndex*2+1);
clFinish(g_cqCommandQue);
}
return collidableIndex;
}
int CLPhysicsDemo::registerConvexShape(btConvexUtility* utilPtr , bool noHeightField)
{
int collidableIndex = narrowphaseAndSolver->allocateCollidable();
btCollidable& col = narrowphaseAndSolver->getCollidableCpu(collidableIndex);
col.m_shapeType = CollisionShape::SHAPE_CONVEX_HULL;
col.m_shapeIndex = -1;
int numFaces= utilPtr->m_faces.size();
float4* eqn = new float4[numFaces];
for (int i=0;i<numFaces;i++)
{
eqn[i].x = utilPtr->m_faces[i].m_plane[0];
eqn[i].y = utilPtr->m_faces[i].m_plane[1];
eqn[i].z = utilPtr->m_faces[i].m_plane[2];
eqn[i].w = utilPtr->m_faces[i].m_plane[3];
}
printf("numFaces = %d\n", numFaces);
if (noHeightField)
{
s_convexHeightField = 0;
} else
{
s_convexHeightField = new ConvexHeightField(eqn,numFaces);
}
if (narrowphaseAndSolver)
{
btVector3 localCenter(0,0,0);
for (int i=0;i<utilPtr->m_vertices.size();i++)
localCenter+=utilPtr->m_vertices[i];
localCenter*= (1./utilPtr->m_vertices.size());
utilPtr->m_localCenter = localCenter;
if (useConvexHeightfield)
col.m_shapeIndex = narrowphaseAndSolver->registerConvexHeightfield(s_convexHeightField,col);
else
col.m_shapeIndex = narrowphaseAndSolver->registerConvexHullShape(utilPtr,col);
}
if (col.m_shapeIndex>=0)
{
btAABBHost aabbMin, aabbMax;
btVector3 myAabbMin(1e30f,1e30f,1e30f);
btVector3 myAabbMax(-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]);
}
aabbMin.fx = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x;
aabbMin.fy = myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y;
aabbMin.fz= myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z;
aabbMin.uw = 0;
aabbMax.fx = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x;
aabbMax.fy = myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y;
aabbMax.fz= myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z;
aabbMax.uw = 0;
m_data->m_localShapeAABBCPU->push_back(aabbMin);
m_data->m_localShapeAABBGPU->push_back(aabbMin);
m_data->m_localShapeAABBCPU->push_back(aabbMax);
m_data->m_localShapeAABBGPU->push_back(aabbMax);
//m_data->m_localShapeAABB->copyFromHostPointer(&aabbMin,1,shapeIndex*2);
//m_data->m_localShapeAABB->copyFromHostPointer(&aabbMax,1,shapeIndex*2+1);
clFinish(g_cqCommandQue);
}
delete[] eqn;
return collidableIndex;
}
int CLPhysicsDemo::registerCompoundShape(btAlignedObjectArray<btGpuChildShape>* childShapes)
{
int collidableIndex = m_narrowphaseAndSolver->allocateCollidable();
btCollidable& col = m_narrowphaseAndSolver->getCollidableCpu(collidableIndex);
col.m_shapeType = CollisionShape::SHAPE_COMPOUND_OF_CONVEX_HULLS;
col.m_shapeIndex = m_narrowphaseAndSolver->registerCompoundShape(childShapes);
col.m_numChildShapes = childShapes->size();
btAABBHost aabbMin, aabbMax;
btVector3 myAabbMin(1e30f,1e30f,1e30f);
btVector3 myAabbMax(-1e30f,-1e30f,-1e30f);
//compute local AABB of the compound of all children
for (int i=0;i<childShapes->size();i++)
{
int childColIndex = childShapes->at(i).m_shapeIndex;
btCollidable& childCol = m_narrowphaseAndSolver->getCollidableCpu(childColIndex);
btAABBHost aabbMinLoc =m_data->m_localShapeAABBCPU->at(childColIndex*2);
btAABBHost aabbMaxLoc =m_data->m_localShapeAABBCPU->at(childColIndex*2+1);
btVector3 childLocalAabbMin(aabbMinLoc.fx,aabbMinLoc.fy,aabbMinLoc.fz);
btVector3 childLocalAabbMax(aabbMaxLoc.fx,aabbMaxLoc.fy,aabbMaxLoc.fz);
btVector3 aMin,aMax;
btScalar margin(0.f);
btTransform childTr;
childTr.setIdentity();
childTr.setOrigin(btVector3(childShapes->at(i).m_childPosition[0],
childShapes->at(i).m_childPosition[1],
childShapes->at(i).m_childPosition[2]));
childTr.setRotation(btQuaternion(childShapes->at(i).m_childOrientation[0],
childShapes->at(i).m_childOrientation[1],
childShapes->at(i).m_childOrientation[2],
childShapes->at(i).m_childOrientation[3]));
btTransformAabb(childLocalAabbMin,childLocalAabbMax,margin,childTr,aMin,aMax);
myAabbMin.setMin(aMin);
myAabbMax.setMax(aMax);
}
aabbMin.fx = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x;
aabbMin.fy = myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y;
aabbMin.fz= myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z;
aabbMin.uw = 0;
aabbMax.fx = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x;
aabbMax.fy = myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y;
aabbMax.fz= myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z;
aabbMax.uw = 0;
m_data->m_localShapeAABBCPU->push_back(aabbMin);
m_data->m_localShapeAABBGPU->push_back(aabbMin);
m_data->m_localShapeAABBCPU->push_back(aabbMax);
m_data->m_localShapeAABBGPU->push_back(aabbMax);
clFinish(g_cqCommandQue);
return collidableIndex;
}
int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,const float* scaling1)
{
b3Vector3 scaling(scaling1[0],scaling1[1],scaling1[2]);
int collidableIndex = allocateCollidable();
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_bvhIndex = m_data->m_bvhInfoCPU.size();
b3SapAabb aabb;
b3Vector3 myAabbMin(1e30f,1e30f,1e30f);
b3Vector3 myAabbMax(-1e30f,-1e30f,-1e30f);
for (int i=0;i<vertices->size();i++)
{
b3Vector3 vtx(vertices->at(i)*scaling);
myAabbMin.setMin(vtx);
myAabbMax.setMax(vtx);
}
aabb.m_min[0] = myAabbMin[0];
aabb.m_min[1] = myAabbMin[1];
aabb.m_min[2] = myAabbMin[2];
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;
m_data->m_localShapeAABBCPU->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();
b3IndexedMesh mesh;
mesh.m_numTriangles = indices->size()/3;
mesh.m_numVertices = vertices->size();
mesh.m_vertexBase = (const unsigned char *)&vertices->at(0).getX();
mesh.m_vertexStride = sizeof(b3Vector3);
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);
int numNodes = bvh->getQuantizedNodeArray().size();
//b3OpenCLArray<b3QuantizedBvhNode>* treeNodesGPU = new b3OpenCLArray<b3QuantizedBvhNode>(this->m_context,this->m_queue,numNodes);
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;
bvhInfo.m_numNodes = numNodes;
bvhInfo.m_numSubTrees = numSubTrees;
bvhInfo.m_nodeOffset = m_data->m_treeNodesCPU.size();
bvhInfo.m_subTreeOffset = m_data->m_subTreesCPU.size();
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.push_back(bvh->getSubtreeInfoArray()[i]);
}
int numNewTreeNodes = bvh->getQuantizedNodeArray().size();
for (int i=0;i<numNewTreeNodes;i++)
{
m_data->m_treeNodesCPU.push_back(bvh->getQuantizedNodeArray()[i]);
}
return collidableIndex;
}
int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes)
{
int collidableIndex = allocateCollidable();
if (collidableIndex<0)
return collidableIndex;
b3Collidable& col = getCollidableCpu(collidableIndex);
col.m_shapeType = SHAPE_COMPOUND_OF_CONVEX_HULLS;
col.m_shapeIndex = m_data->m_cpuChildShapes.size();
{
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 aabbWS;
b3Vector3 myAabbMin(1e30f,1e30f,1e30f);
b3Vector3 myAabbMax(-1e30f,-1e30f,-1e30f);
//compute local AABB of the compound of all children
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);
b3Vector3 childLocalAabbMin(aabbLoc.m_min[0],aabbLoc.m_min[1],aabbLoc.m_min[2]);
b3Vector3 childLocalAabbMax(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(b3Vector3(childShapes->at(i).m_childPosition[0],
childShapes->at(i).m_childPosition[1],
childShapes->at(i).m_childPosition[2]));
childTr.setRotation(b3Quaternion(childShapes->at(i).m_childOrientation[0],
childShapes->at(i).m_childOrientation[1],
childShapes->at(i).m_childOrientation[2],
childShapes->at(i).m_childOrientation[3]));
b3TransformAabb(childLocalAabbMin,childLocalAabbMax,margin,childTr,aMin,aMax);
myAabbMin.setMin(aMin);
myAabbMax.setMax(aMax);
}
aabbWS.m_min[0] = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x;
aabbWS.m_min[1]= myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y;
aabbWS.m_min[2]= myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z;
aabbWS.m_minIndices[3] = 0;
aabbWS.m_max[0] = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x;
aabbWS.m_max[1]= myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y;
aabbWS.m_max[2]= myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z;
aabbWS.m_signedMaxIndices[3] = 0;
m_data->m_localShapeAABBCPU->push_back(aabbWS);
// m_data->m_localShapeAABBGPU->push_back(aabbWS);
clFinish(m_queue);
return collidableIndex;
}
