int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
{
b3RigidBodyData body;
int bodyIndex = m_data->m_rigidBodies.size();
body.m_invMass = mass ? 1.f/mass : 0.f;
body.m_angVel.setValue(0,0,0);
body.m_collidableIdx = collidableIndex;
body.m_frictionCoeff = 0.3f;
body.m_linVel.setValue(0,0,0);
body.m_pos.setValue(position[0],position[1],position[2]);
body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
body.m_restituitionCoeff = 0.f;
m_data->m_rigidBodies.push_back(body);
if (collidableIndex>=0)
{
b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex);
b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
b3Scalar margin = 0.01f;
b3Transform t;
t.setIdentity();
t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec);
m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1);
// b3Vector3 aabbMin,aabbMax;
// m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
} else
{
b3Error("registerPhysicsInstance using invalid collidableIndex\n");
}
return bodyIndex;
}
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;
}
