bool DynamicsWorldComponent::onRigidBodyEvent( const coca::AttributeEvent<btRigidBody*>& event )
{
switch ( event.getType() )
{
case coca::E_SOURCE_ATTACH_EVENT:
addRigidBody( event.getSource() );
break;
case coca::E_SOURCE_DETACH_EVENT:
removeRigidBody( event.getSource() );
break;
case coca::E_SOURCE_UPDATE_EVENT:
updateRigidBody( event.getSource() );
break;
case coca::E_STRING_UPDATE_EVENT:
default:
return false;
}
return true;
}
void PhysicEngine::addRigidBody(RigidBody* body, bool addToMap, RigidBody* raycastingBody)
{
if(!body && !raycastingBody)
return; // nothing to do
const std::string& name = (body ? body->mName : raycastingBody->mName);
if (body)
dynamicsWorld->addRigidBody(body,CollisionType_World,CollisionType_World|CollisionType_ActorInternal|CollisionType_ActorExternal);
if (raycastingBody)
dynamicsWorld->addRigidBody(raycastingBody,CollisionType_Raycasting,CollisionType_Raycasting|CollisionType_World);
if(addToMap){
removeRigidBody(name);
deleteRigidBody(name);
if (body)
mCollisionObjectMap[name] = body;
if (raycastingBody)
mRaycastingObjectMap[name] = raycastingBody;
}
}
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 DynamicsWorldComponent::removeRigidBody( coca::AOutputAttribute<btRigidBody*>* source )
{
COCA_ASSERT( source );
_rigidBodies.erase( source );
removeRigidBody( source->getValue() );
}
BulletRigidBodyObject::~BulletRigidBodyObject() {
removeRigidBody();
}
void BulletRigidBodyObject::unload() {
removeRigidBody();
}
void btFractureDynamicsWorld::breakDisconnectedParts( btFractureBody* fracObj)
{
if (!fracObj->getCollisionShape()->isCompound())
return;
btCompoundShape* compound = (btCompoundShape*)fracObj->getCollisionShape();
int numChildren = compound->getNumChildShapes();
if (numChildren<=1)
return;
//compute connectivity
btUnionFind unionFind;
btAlignedObjectArray<int> tags;
tags.resize(numChildren);
int i, index = 0;
for ( i=0;i<numChildren;i++)
{
#ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
tags[i] = index++;
#else
tags[i] = i;
index=i+1;
#endif
}
unionFind.reset(index);
int numElem = unionFind.getNumElements();
for (i=0;i<fracObj->m_connections.size();i++)
{
btConnection& connection = fracObj->m_connections[i];
if (connection.m_strength > 0.)
{
int tag0 = tags[connection.m_childIndex0];
int tag1 = tags[connection.m_childIndex1];
unionFind.unite(tag0, tag1);
}
}
numElem = unionFind.getNumElements();
index=0;
for (int ai=0;ai<numChildren;ai++)
{
int tag = unionFind.find(index);
tags[ai] = 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;
int numIslands = 0;
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 numShapes=0;
btCompoundShape* newCompound = new btCompoundShape();
btAlignedObjectArray<btScalar> masses;
int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++)
{
int i = unionFind.getElement(idx).m_sz;
// btCollisionShape* shape = compound->getChildShape(i);
newCompound->addChildShape(compound->getChildTransform(i),compound->getChildShape(i));
masses.push_back(fracObj->m_masses[i]);
numShapes++;
}
if (numShapes)
{
btFractureBody* newBody = addNewBody(fracObj->getWorldTransform(),&masses[0],newCompound);
newBody->setLinearVelocity(fracObj->getLinearVelocity());
newBody->setAngularVelocity(fracObj->getAngularVelocity());
numIslands++;
}
}
removeRigidBody(fracObj);//should it also be removed from the array?
}
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);
}
}
void Context::removeRigidBody( const RigidBodyRef &phyObj )
{
removeRigidBody( phyObj->getRigidBody().get() );
}
