int CLPhysicsDemo::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userIndex)
{
btVector3 aabbMin(0,0,0),aabbMax(0,0,0);
if (collidableIndex>=0)
{
btAABBHost hostLocalAabbMin = m_data->m_localShapeAABBCPU->at(collidableIndex*2);
btAABBHost hostLocalAabbMax = m_data->m_localShapeAABBCPU->at(collidableIndex*2+1);
btVector3 localAabbMin(hostLocalAabbMin.fx,hostLocalAabbMin.fy,hostLocalAabbMin.fz);
btVector3 localAabbMax(hostLocalAabbMax.fx,hostLocalAabbMax.fy,hostLocalAabbMax.fz);
btScalar margin = 0.01f;
btTransform t;
t.setIdentity();
t.setOrigin(btVector3(position[0],position[1],position[2]));
t.setRotation(btQuaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
btTransformAabb(localAabbMin,localAabbMax, margin,t,aabbMin,aabbMax);
//(position[0],position[0],position[0]);
//aabbMin -= btVector3(400.f,410.f,400.f);
//aabbMax += btVector3(400.f,410.f,400.f);
//btBroadphaseProxy* proxy = m_data->m_Broadphase->createProxy(aabbMin,aabbMax,collisionShapeIndex,userPointer,1,1,0,0);//m_dispatcher);
if (useSapGpuBroadphase)
m_data->m_BroadphaseSap->createProxy(aabbMin,aabbMax,userIndex,1,1);//m_dispatcher);
else
{
void* userPtr = (void*)userIndex;
m_data->m_BroadphaseGrid->createProxy(aabbMin,aabbMax,collidableIndex,userPtr ,1,1);//m_dispatcher);
}
}
bool writeToGpu = false;
int bodyIndex = -1;
m_data->m_linVelHost.push_back(btVector3(0,0,0));
m_data->m_angVelHost.push_back(btVector3(0,0,0));
m_data->m_bodyTimesHost.push_back(0.f);
if (narrowphaseAndSolver)
{
//bodyIndex = narrowphaseAndSolver->registerRigidBody(collisionShapeIndex,CollisionShape::SHAPE_CONVEX_HEIGHT_FIELD,mass,position,orientation,&aabbMin.getX(),&aabbMax.getX(),writeToGpu);
bodyIndex = narrowphaseAndSolver->registerRigidBody(collidableIndex,mass,position,orientation,&aabbMin.getX(),&aabbMax.getX(),writeToGpu);
}
if (mass>0.f)
m_numDynamicPhysicsInstances++;
m_numPhysicsInstances++;
return bodyIndex;
}
static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b, const btTransform& xform)
{
btVector3 newmin,newmax;
btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
return Intersect(a,newb);
}
void Process(const btDbvtNode* leaf)
{
int index = leaf->dataAsInt;
btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
btCollisionShape* childShape = compoundShape->getChildShape(index);
if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
{
btVector3 worldAabbMin,worldAabbMax;
btTransform orgTrans = m_compoundColObj->getWorldTransform();
btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
}
ProcessChildShape(childShape,index);
}
void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
m_dispatchInfoPtr = &dispatchInfo;
m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
m_resultOut = resultOut;
btVector3 aabbWorldSpaceMin,aabbWorldSpaceMax;
m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5);
btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5);
btTransform softTransform;
softTransform.setIdentity();
softTransform.setOrigin(softBodyCenter);
btTransform convexInTriangleSpace;
convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * softTransform;
btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax);
}
void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{
btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
}
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;
}
void btRayShape::getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const {
#define MARGIN_BROADPHASE 0.1
btVector3 localAabbMin(0, 0, 0);
btVector3 localAabbMax(m_shapeAxis * m_length);
btTransformAabb(localAabbMin, localAabbMax, MARGIN_BROADPHASE, t, aabbMin, aabbMax);
}
