void btParticlesDynamicsWorld::getShapeData()
{
int numObjects = getNumCollisionObjects();
btCollisionObjectArray& collisionObjects = getCollisionObjectArray();
for(int i = 0; i < numObjects; i++)
{
btCollisionObject* colObj = collisionObjects[i];
btCollisionShape* pShape = colObj->getCollisionShape();
int shapeType = pShape->getShapeType();
if(shapeType == SPHERE_SHAPE_PROXYTYPE)
{
btSphereShape* pSph = (btSphereShape*)pShape;
btScalar sphRad = pSph->getRadius();
if(!i)
{
m_particleRad = sphRad;
}
else
{
btAssert(m_particleRad == sphRad);
}
}
else
{
btAssert(0);
}
}
printf("Total number of particles : %d\n", m_numParticles);
}
int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep, btScalar fixedSubSteps)
{
(void)fixedTimeStep;
(void)maxSubSteps;
(void)fixedSubSteps;
///apply gravity, predict motion
predictUnconstraintMotion(timeStep);
btDispatcherInfo& dispatchInfo = getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = getDebugDrawer();
///perform collision detection
performDiscreteCollisionDetection();
///solve contact constraints
int numManifolds = m_dispatcher1->getNumManifolds();
if (numManifolds)
{
btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
btContactSolverInfo infoGlobal;
infoGlobal.m_timeStep = timeStep;
m_constraintSolver->prepareSolve(0, numManifolds);
m_constraintSolver->solveGroup(&getCollisionObjectArray()[0], getNumCollisionObjects(), manifoldPtr, numManifolds,0,0, infoGlobal, m_debugDrawer, m_dispatcher1);
m_constraintSolver->allSolved(infoGlobal, m_debugDrawer);
}
///integrate transforms
integrateTransforms(timeStep);
updateAabbs();
synchronizeMotionStates();
clearForces();
return 1;
}
void btBulletPhysicsEffectsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
BT_PROFILE("solveConstraints");
btCollisionDispatcher* disp = (btCollisionDispatcher*) getDispatcher();
int numBodies = getNumCollisionObjects();
btPersistentManifold** manifolds = disp->getInternalManifoldPointer();
int numManifolds = disp->getNumManifolds();
if ((getNumCollisionObjects()>0) && (numManifolds + m_constraints.size()>0))
{
btCollisionObject** bodies = numBodies ? &getCollisionObjectArray()[0] : 0;
btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
getConstraintSolver()->solveGroup( bodies,numBodies, disp->getInternalManifoldPointer(),numManifolds, constraints, m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,disp);
}
}
void ForkLiftDemo::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
while (body->getNumConstraintRefs())
{
btTypedConstraint* constraint = body->getConstraintRef(0);
m_dynamicsWorld->removeConstraint(constraint);
delete constraint;
}
delete body->getMotionState();
m_dynamicsWorld->removeRigidBody(body);
} else
{
m_dynamicsWorld->removeCollisionObject( obj );
}
delete obj;
}
//delete collision shapes
for (int j=0;j<m_collisionShapes.size();j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_indexVertexArrays;
delete m_vertices;
//delete dynamics world
delete m_dynamicsWorld;
m_dynamicsWorld=0;
delete m_vehicleRayCaster;
m_vehicleRayCaster = 0;
delete m_vehicle;
m_vehicle=0;
delete m_wheelShape;
m_wheelShape=0;
//delete solver
delete m_constraintSolver;
m_constraintSolver=0;
//delete broadphase
delete m_overlappingPairCache;
m_overlappingPairCache=0;
//delete dispatcher
delete m_dispatcher;
m_dispatcher=0;
delete m_collisionConfiguration;
m_collisionConfiguration=0;
}
void btFractureDynamicsWorld::glueCallback()
{
int numManifolds = getDispatcher()->getNumManifolds();
///first build the islands based on axis aligned bounding box overlap
btUnionFind unionFind;
int index = 0;
{
int i;
for (i=0;i<getCollisionObjectArray().size(); i++)
{
btCollisionObject* collisionObject= getCollisionObjectArray()[i];
// btRigidBody* body = btRigidBody::upcast(collisionObject);
//Adding filtering here
#ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
if (!collisionObject->isStaticOrKinematicObject())
{
collisionObject->setIslandTag(index++);
} else
{
collisionObject->setIslandTag(-1);
}
#else
collisionObject->setIslandTag(i);
index=i+1;
#endif
}
}
unionFind.reset(index);
int numElem = unionFind.getNumElements();
for (int i=0;i<numManifolds;i++)
{
btPersistentManifold* manifold = getDispatcher()->getManifoldByIndexInternal(i);
if (!manifold->getNumContacts())
continue;
btScalar minDist = 1e30f;
for (int v=0;v<manifold->getNumContacts();v++)
{
minDist = btMin(minDist,manifold->getContactPoint(v).getDistance());
}
if (minDist>0.)
continue;
btCollisionObject* colObj0 = (btCollisionObject*)manifold->getBody0();
btCollisionObject* colObj1 = (btCollisionObject*)manifold->getBody1();
int tag0 = (colObj0)->getIslandTag();
int tag1 = (colObj1)->getIslandTag();
//btRigidBody* body0 = btRigidBody::upcast(colObj0);
//btRigidBody* body1 = btRigidBody::upcast(colObj1);
if (!colObj0->isStaticOrKinematicObject() && !colObj1->isStaticOrKinematicObject())
{
unionFind.unite(tag0, tag1);
}
}
numElem = unionFind.getNumElements();
index=0;
for (int ai=0;ai<getCollisionObjectArray().size();ai++)
{
btCollisionObject* collisionObject= getCollisionObjectArray()[ai];
if (!collisionObject->isStaticOrKinematicObject())
{
int tag = unionFind.find(index);
collisionObject->setIslandTag( tag);
//Set the correct object offset in Collision Object Array
#if STATIC_SIMULATION_ISLAND_OPTIMIZATION
unionFind.getElement(index).m_sz = ai;
#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
index++;
}
}
unionFind.sortIslands();
int endIslandIndex=1;
int startIslandIndex;
btAlignedObjectArray<btCollisionObject*> removedObjects;
///iterate over all islands
for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
{
int islandId = unionFind.getElement(startIslandIndex).m_id;
for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (unionFind.getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
{
}
int fractureObjectIndex = -1;
int numObjects=0;
int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++)
{
int i = unionFind.getElement(idx).m_sz;
btCollisionObject* colObj0 = getCollisionObjectArray()[i];
if (colObj0->getInternalType()& CUSTOM_FRACTURE_TYPE)
{
fractureObjectIndex = i;
}
btRigidBody* otherObject = btRigidBody::upcast(colObj0);
if (!otherObject || !otherObject->getInvMass())
continue;
numObjects++;
}
///Then for each island that contains at least two objects and one fracture object
if (fractureObjectIndex>=0 && numObjects>1)
{
btFractureBody* fracObj = (btFractureBody*)getCollisionObjectArray()[fractureObjectIndex];
///glueing objects means creating a new compound and removing the old objects
///delay the removal of old objects to avoid array indexing problems
removedObjects.push_back(fracObj);
m_fractureBodies.remove(fracObj);
btAlignedObjectArray<btScalar> massArray;
btAlignedObjectArray<btVector3> oldImpulses;
btAlignedObjectArray<btVector3> oldCenterOfMassesWS;
oldImpulses.push_back(fracObj->getLinearVelocity()/1./fracObj->getInvMass());
oldCenterOfMassesWS.push_back(fracObj->getCenterOfMassPosition());
btScalar totalMass = 0.f;
btCompoundShape* compound = new btCompoundShape();
if (fracObj->getCollisionShape()->isCompound())
{
btTransform tr;
tr.setIdentity();
btCompoundShape* oldCompound = (btCompoundShape*)fracObj->getCollisionShape();
for (int c=0;c<oldCompound->getNumChildShapes();c++)
{
compound->addChildShape(oldCompound->getChildTransform(c),oldCompound->getChildShape(c));
massArray.push_back(fracObj->m_masses[c]);
totalMass+=fracObj->m_masses[c];
}
} else
{
btTransform tr;
tr.setIdentity();
compound->addChildShape(tr,fracObj->getCollisionShape());
massArray.push_back(fracObj->m_masses[0]);
totalMass+=fracObj->m_masses[0];
}
for (idx=startIslandIndex;idx<endIslandIndex;idx++)
{
int i = unionFind.getElement(idx).m_sz;
if (i==fractureObjectIndex)
continue;
btCollisionObject* otherCollider = getCollisionObjectArray()[i];
btRigidBody* otherObject = btRigidBody::upcast(otherCollider);
//don't glue/merge with static objects right now, otherwise everything gets stuck to the ground
///todo: expose this as a callback
if (!otherObject || !otherObject->getInvMass())
continue;
oldImpulses.push_back(otherObject->getLinearVelocity()*(1.f/otherObject->getInvMass()));
oldCenterOfMassesWS.push_back(otherObject->getCenterOfMassPosition());
removedObjects.push_back(otherObject);
m_fractureBodies.remove((btFractureBody*)otherObject);
btScalar curMass = 1.f/otherObject->getInvMass();
if (otherObject->getCollisionShape()->isCompound())
{
btTransform tr;
btCompoundShape* oldCompound = (btCompoundShape*)otherObject->getCollisionShape();
for (int c=0;c<oldCompound->getNumChildShapes();c++)
{
tr = fracObj->getWorldTransform().inverseTimes(otherObject->getWorldTransform()*oldCompound->getChildTransform(c));
compound->addChildShape(tr,oldCompound->getChildShape(c));
massArray.push_back(curMass/(btScalar)oldCompound->getNumChildShapes());
}
} else
{
btTransform tr;
tr = fracObj->getWorldTransform().inverseTimes(otherObject->getWorldTransform());
compound->addChildShape(tr,otherObject->getCollisionShape());
massArray.push_back(curMass);
}
totalMass+=curMass;
}
btTransform shift;
shift.setIdentity();
btCompoundShape* newCompound = btFractureBody::shiftTransformDistributeMass(compound,totalMass,shift);
int numChildren = newCompound->getNumChildShapes();
btAssert(numChildren == massArray.size());
btVector3 localInertia;
newCompound->calculateLocalInertia(totalMass,localInertia);
btFractureBody* newBody = new btFractureBody(totalMass,0,newCompound,localInertia, &massArray[0], numChildren,this);
newBody->recomputeConnectivity(this);
newBody->setWorldTransform(fracObj->getWorldTransform()*shift);
//now the linear/angular velocity is still zero, apply the impulses
for (int i=0;i<oldImpulses.size();i++)
{
btVector3 rel_pos = oldCenterOfMassesWS[i]-newBody->getCenterOfMassPosition();
const btVector3& imp = oldImpulses[i];
newBody->applyImpulse(imp, rel_pos);
}
addRigidBody(newBody);
}
}
//remove the objects from the world at the very end,
//otherwise the island tags would not match the world collision object array indices anymore
while (removedObjects.size())
{
btCollisionObject* otherCollider = removedObjects[removedObjects.size()-1];
removedObjects.pop_back();
btRigidBody* otherObject = btRigidBody::upcast(otherCollider);
if (!otherObject || !otherObject->getInvMass())
continue;
removeRigidBody(otherObject);
}
}