void DynamicsWorldComponent::addRigidBody( coca::AOutputAttribute<btRigidBody*>* source )
{
COCA_ASSERT( source );
btRigidBody* body = source->getValue();
_rigidBodies[source] = body;
addRigidBody( body );
}
btRigidBody* ForkLiftDemo::localCreateRigidBody(btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
{
btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
shape->calculateLocalInertia(mass,localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
#define USE_MOTIONSTATE 1
#ifdef USE_MOTIONSTATE
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo cInfo(mass,myMotionState,shape,localInertia);
btRigidBody* body = new btRigidBody(cInfo);
//body->setContactProcessingThreshold(m_defaultContactProcessingThreshold);
#else
btRigidBody* body = new btRigidBody(mass,0,shape,localInertia);
body->setWorldTransform(startTransform);
#endif//
m_dynamicsWorld->addRigidBody(body);
return body;
}
btRigidBody* Physics::addRigidBox(Ogre::Entity* entity, Ogre::SceneNode* node,
btScalar mass, btScalar rest, btVector3 localInertia, btVector3 origin, btQuaternion *rotation) {
Ogre::Vector3 s = entity->getBoundingBox().getHalfSize();
btCollisionShape *boxShape = new btBoxShape( btVector3(s[0],s[1],s[2]) );
addRigidBody(entity, node, boxShape, mass, rest, localInertia, origin, rotation);
};
void DynamicsWorldComponent::updateRigidBody( coca::AOutputAttribute<btRigidBody*>* source )
{
COCA_DEBUG_INFO( "updateRigidBody " << source );
COCA_ASSERT( source );
btRigidBody*& body = _rigidBodies[source];
removeRigidBody( body );
body = source->getValue();
addRigidBody( body );
}
void DynamicsWorldComponent::addRigidBodies()
{
if ( !_dynamicsWorld ) { return; }
RigidBodyMap::iterator it;
for ( it = _rigidBodies.begin(); it != _rigidBodies.end(); ++it )
{
addRigidBody( it->second );
}
}
void DynamicsWorld::fractureCallback()
{
#if (BT_BULLET_VERSION < 281)
m_activeConnections.resize(0);
int numManifolds = getDispatcher()->getNumManifolds();
for (int i = 0; i < numManifolds; ++i)
{
btPersistentManifold* manifold = getDispatcher()->getManifoldByIndexInternal(i);
if (!manifold->getNumContacts()) continue;
FractureBody* body = static_cast<FractureBody*>(manifold->getBody0());
if (body->getInternalType() & CO_FRACTURE_TYPE)
{
for (int k = 0; k < manifold->getNumContacts(); ++k)
{
btManifoldPoint& point = manifold->getContactPoint(k);
int con_id = body->getConnectionId(point.m_index0);
if (point.m_appliedImpulse > 1E-3 &&
body->applyImpulse(con_id, point.m_appliedImpulse))
{
m_activeConnections.push_back(ActiveCon(body, con_id));
}
}
}
body = static_cast<FractureBody*>(manifold->getBody1());
if (body->getInternalType() & CO_FRACTURE_TYPE)
{
for (int k = 0; k < manifold->getNumContacts(); ++k)
{
btManifoldPoint& point = manifold->getContactPoint(k);
int con_id = body->getConnectionId(point.m_index1);
if (point.m_appliedImpulse > 1E-3 &&
body->applyImpulse(con_id, point.m_appliedImpulse))
{
m_activeConnections.push_back(ActiveCon(body, con_id));
}
}
}
}
// Update active connections.
for (int i = 0; i < m_activeConnections.size(); ++i)
{
int con_id = m_activeConnections[i].id;
FractureBody* body = m_activeConnections[i].body;
btRigidBody* child = body->updateConnection(con_id);
if (child) addRigidBody(child);
}
#endif
}
b3GpuDynamicsWorld::b3GpuDynamicsWorld(class b3GpuSapBroadphase* bp,class b3GpuNarrowPhase* np, class b3GpuRigidBodyPipeline* rigidBodyPipeline)
:btDynamicsWorld(0,0,0),
m_gravity(0,-10,0),
m_cpuGpuSync(true),
m_bp(bp),
m_np(np),
m_rigidBodyPipeline(rigidBodyPipeline),
m_localTime(0.f),
m_staticBody(0)
{
btConvexHullShape* nullShape = new btConvexHullShape();
m_staticBody = new btRigidBody(0,0,nullShape);
addRigidBody(m_staticBody,0,0);
}
btRigidBody * btSimpleDynamicsWorld::localCreateRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape) {
// Create a rigid body for a body part and add it to the physics world
btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f); // Test if dynamic object
btVector3 localInertia(0,0,0); // Create inertia matrix
if (isDynamic) shape->calculateLocalInertia(mass,localInertia); // Calculate inertia matrix from primitive shape and mass
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); // Create default motion state
btRigidBody::btRigidBodyConstructionInfo cInfo(mass,myMotionState,shape,localInertia); // Create rigid body info structure
btRigidBody* body = new btRigidBody(cInfo); // Create new rigid body
//body->setContactProcessingThreshold(m_defaultContactProcessingThreshold); // Default Contact Threshold
addRigidBody(body); // Add body to the physics world
return body;// Return the body pointer
}
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;
}
btFractureBody* btFractureDynamicsWorld::addNewBody(const btTransform& oldTransform,btScalar* masses, btCompoundShape* oldCompound)
{
int i;
btTransform shift;
shift.setIdentity();
btVector3 localInertia;
btCompoundShape* newCompound = btFractureBody::shiftTransform(oldCompound,masses,shift,localInertia);
btScalar totalMass = 0;
for (i=0;i<newCompound->getNumChildShapes();i++)
totalMass += masses[i];
//newCompound->calculateLocalInertia(totalMass,localInertia);
btFractureBody* newBody = new btFractureBody(totalMass,0,newCompound,localInertia, masses,newCompound->getNumChildShapes(), this);
newBody->recomputeConnectivity(this);
newBody->setCollisionFlags(newBody->getCollisionFlags()|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
newBody->setWorldTransform(oldTransform*shift);
addRigidBody(newBody);
return newBody;
}
void BulletRigidBodyObject::load(MeshdataPtr retrievedMesh) {
mObjShape = computeCollisionShape(mID, mBBox, mTreatment, retrievedMesh);
assert(mObjShape != NULL);
addRigidBody();
}
void DynamicsWorld::fractureCallback()
{
m_activeConnections.resize(0);
int numManifolds = getDispatcher()->getNumManifolds();
for (int i = 0; i < numManifolds; ++i)
{
btPersistentManifold* manifold = getDispatcher()->getManifoldByIndexInternal(i);
if (!manifold->getNumContacts()) continue;
if (((btCollisionObject*)manifold->getBody0())->getInternalType() & CUSTOM_FRACTURE_TYPE)
{
FractureBody* body = (FractureBody*)manifold->getBody0();
btCompoundShape* shape = (btCompoundShape*)body->getCollisionShape();
for (int k = 0; k < manifold->getNumContacts(); ++k)
{
btManifoldPoint& point = manifold->getContactPoint(k);
int shape_id = point.m_index0;
btCollisionShape* child_shape = shape->getChildShape(shape_id);
int con_id = cast<int>(child_shape->getUserPointer());
if (con_id >= 0 && point.m_appliedImpulse > 1E-3)
{
btAssert(con_id < body->numConnections());
FractureBody::Connection & connection = body->m_connections[con_id];
if (connection.m_accImpulse < 1E-3)
{
m_activeConnections.push_back(ActiveCon(body, shape_id));
}
connection.m_accImpulse += point.m_appliedImpulse;
}
}
}
if (((btCollisionObject*)manifold->getBody1())->getInternalType() & CUSTOM_FRACTURE_TYPE)
{
FractureBody* body = (FractureBody*)manifold->getBody1();
btCompoundShape* shape = (btCompoundShape*)body->getCollisionShape();
for (int k = 0; k < manifold->getNumContacts(); ++k)
{
btManifoldPoint& point = manifold->getContactPoint(k);
int shape_id = point.m_index1;
btCollisionShape* child_shape = shape->getChildShape(shape_id);
int con_id = cast<int>(child_shape->getUserPointer());
if (con_id >= 0 && point.m_appliedImpulse > 1E-3)
{
btAssert(con_id < body->numConnections());
FractureBody::Connection & connection = body->m_connections[con_id];
if (connection.m_accImpulse < 1E-3)
{
m_activeConnections.push_back(ActiveCon(body, shape_id));
}
connection.m_accImpulse += point.m_appliedImpulse;
}
}
}
}
// Update active connections.
for (int i = 0; i < m_activeConnections.size(); ++i)
{
FractureBody* body = m_activeConnections[i].body;
int shape_id = m_activeConnections[i].id;
btRigidBody* child = body->updateConnection(shape_id);
if (child) addRigidBody(child);
}
}
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);
}
}
osg::Group * createWorld() {
setupPerformanceStatistics();
//getPhysics()->setUseFixedTimeSteps(false);
//getPhysics()->setMaxSubSteps(5);
getPhysics()->addPerformanceStatistics(this);
cefix::log::setInfoLevel(osg::ALWAYS);
getMainWindow()->getCamera()->setClearColor(osg::Vec4(0.2, 0.2, 0.2, 1.0));
// statistics
if (0) {
enablePerformanceStatistics(true);
get2DLayer()->addChild(getPerformanceStatisticsGroup());
}
setUseOptimizerFlag(false);
osg::Group* g = new osg::Group();
{
//btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),1);
cefixbt::StaticPlaneShape* groundShape = new cefixbt::StaticPlaneShape(osg::Vec3(0,0,1),0);
cefixbt::MotionState* groundMotionState = new cefixbt::MotionState( btTransform(btQuaternion(0,0,0,1),btVector3(-250,-250,-1)) );
btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
osg::ref_ptr<cefixbt::RigidBody> groundRigidBody = new cefixbt::RigidBody(groundRigidBodyCI);
groundRigidBody->setContactCallback(new GroundContactCallback());
addRigidBody(groundRigidBody);
osg::Geode* groundgeode = new osg::Geode();
groundgeode->addDrawable(new cefix::LineGridGeometry(osg::Vec3(500,500,0)));
groundMotionState->addChild(groundgeode);
g->addChild(groundMotionState);
}
for (unsigned int i = 0; i < 00; ++i) {
cefixbt::SphereShape* fallShape = new cefixbt::SphereShape(1);
cefixbt::MotionState* fallMotionState = new cefixbt::MotionState( btTransform(btQuaternion(0,0,0,1),btVector3(cefix::in_between(-5,5), cefix::in_between(-5,5), cefix::in_between(50,500))) );
btScalar mass = cefix::in_between(1,10);
btVector3 fallInertia(0,0,0);
fallShape->calculateLocalInertia(mass,fallInertia);
btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,fallMotionState,fallShape,fallInertia);
osg::ref_ptr<cefixbt::RigidBody> fallRigidBody = new cefixbt::RigidBody(fallRigidBodyCI);
addRigidBody(fallRigidBody);
osg::Geode* fallgeode = new osg::Geode();
osg::ShapeDrawable* drawable = new osg::ShapeDrawable(fallShape);
drawable->setColor(osg::Vec4(cefix::randomf(1.0),cefix::randomf(1.0),cefix::randomf(1.0),1.0));
fallgeode->addDrawable(drawable);
fallMotionState->addChild(fallgeode);
g->addChild(fallMotionState);
}
std::vector<osg::ref_ptr<cefixbt::RigidBody> > boxes;
for (unsigned int i = 0; i < 00; ++i) {
cefixbt::BoxShape* fallShape = new cefixbt::BoxShape(osg::Vec3(1,1,1));
cefixbt::MotionState* fallMotionState = new cefixbt::MotionState( btTransform(btQuaternion(0,0,0,1),btVector3(cefix::in_between(-5,5), cefix::in_between(-5,5), cefix::in_between(50,500))) );
btScalar mass = cefix::in_between(1,10);
btVector3 fallInertia(0,0,0);
fallShape->calculateLocalInertia(mass,fallInertia);
btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,fallMotionState,fallShape,fallInertia);
osg::ref_ptr<cefixbt::RigidBody> fallRigidBody = new cefixbt::RigidBody(fallRigidBodyCI);
addRigidBody(fallRigidBody);
osg::Geode* fallgeode = new osg::Geode();
osg::ShapeDrawable* drawable = new osg::ShapeDrawable(fallShape);
drawable->setColor(osg::Vec4(cefix::randomf(1.0),cefix::randomf(1.0),cefix::randomf(1.0),1.0));
fallgeode->addDrawable(drawable);
fallMotionState->addChild(fallgeode);
g->addChild(fallMotionState);
boxes.push_back(fallRigidBody);
}
if (1) {
osg::Node* node = osgDB::readNodeFile("w.obj");
osgUtil::Optimizer o;
o.optimize(node);
cefixbt::ConvexHullShape* fallShape = new cefixbt::ConvexHullShape(node, false);
//cefixbt::ConvexTriangleMeshShape* fallShape = new cefixbt::ConvexTriangleMeshShape(node, false);
osg::ref_ptr<cefixbt::RigidBody> last(NULL), current(NULL);
for (unsigned int i = 0; i < 10; ++i) {
cefixbt::MotionState* fallMotionState = new cefixbt::MotionState( btTransform(btQuaternion(0,0,0,1),btVector3(cefix::in_between(-50,50), cefix::in_between(-50,50), cefix::in_between(50,50))) );
btScalar mass = cefix::in_between(1,10);
btVector3 fallInertia(0,0,0);
fallShape->calculateLocalInertia(mass,fallInertia);
btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,fallMotionState,fallShape,fallInertia);
osg::ref_ptr<cefixbt::RigidBody> fallRigidBody = new cefixbt::RigidBody(fallRigidBodyCI);
fallRigidBody->setDamping(0.4, 0.4);
addRigidBody(fallRigidBody);
fallMotionState->addChild(node);
fallMotionState->setUserData(new cefixbt::RigidBodyDraggable(fallRigidBody, this));
g->addChild(fallMotionState);
last = current;
current = fallRigidBody;
_rigidBodies.push_back(current);
if (last && current) {
const int mode(0);
switch (mode) {
case -1:
break;
case 0:
{
cefixbt::Point2PointConstraint* constraint = new cefixbt::Point2PointConstraint(last, current, osg::Vec3(7, 7, 0), osg::Vec3(-7, 0 , 0));
addConstraint(constraint);
}
break;
case 1:
{
cefixbt::Generic6DofConstraint* constraint = new cefixbt::Generic6DofConstraint(
last,
current,
osg::Matrix::translate(osg::Vec3(5, 0, 0)),
osg::Matrix::translate(osg::Vec3(-15, 0 , 0)),
true
);
constraint->setLinearLowerLimit(btVector3(0.0f, 0.0f, 0.0f));
constraint->setLinearUpperLimit(btVector3(3.0f, 0.0f, 0.0f));
constraint->setAngularLowerLimit(btVector3(0,0,0));
constraint->setAngularUpperLimit(btVector3(0.2,0,0));
addConstraint(constraint);
}
break;
case 2:
{
cefixbt::Generic6DofSpringConstraint* constraint = new cefixbt::Generic6DofSpringConstraint(
last,
current,
osg::Matrix::translate(osg::Vec3(5, 0, 0)),
osg::Matrix::translate(osg::Vec3(-15, 0 , 0)),
true
);
constraint->setLinearLowerLimit(btVector3(-1.0f, -1.0f, -1.0f));
constraint->setLinearUpperLimit(btVector3(1.0f, 1.0f, 1.0f));
constraint->setAngularLowerLimit(btVector3(-0.5,0,0));
constraint->setAngularUpperLimit(btVector3(0.5,0.0,0));
for(unsigned int i=0; i < 6; ++i) {
constraint->enableSpring(i, true);
constraint->setStiffness(i, 0.01);
constraint->setDamping (i, 0.01);
}
addConstraint(constraint);
}
}
}
}
}
osg::Group* world = new osg::Group();
world->addChild(g);
_scene = g;
// debug-world
if (1) {
_debugNode = getDebugDrawNode(
/*cefixbt::DebugPhysicsDrawer::DBG_DrawAabb | */
cefixbt::DebugPhysicsDrawer::DBG_DrawConstraints |
cefixbt::DebugPhysicsDrawer::DBG_DrawConstraintLimits |
cefixbt::DebugPhysicsDrawer::DBG_DrawFeaturesText |
cefixbt::DebugPhysicsDrawer::DBG_DrawContactPoints |
cefixbt::DebugPhysicsDrawer::DBG_NoDeactivation |
cefixbt::DebugPhysicsDrawer::DBG_NoHelpText |
cefixbt::DebugPhysicsDrawer::DBG_DrawText |
cefixbt::DebugPhysicsDrawer::DBG_ProfileTimings |
cefixbt::DebugPhysicsDrawer::DBG_DrawWireframe
);
world->addChild(_debugNode);
_debugNode->setNodeMask(0x0);
}
return world;
}
int main_phys_viewer( int argc, char** argv )
{
osg::ArgumentParser arguments( &argc, argv );
const bool debugDisplay( arguments.find( "--debug" ) > 0 );
btDiscreteDynamicsWorld* bw = initPhysics();
osg::Group* root = new osg::Group;
osg::Group* launchHandlerAttachPoint = new osg::Group;
root->addChild( launchHandlerAttachPoint );
std::string model_name = "eisk";
auto obj = avCore::createObject(model_name, 10);
osg::ref_ptr< osg::Node > rootModel( obj->getOrCreateNode() );
if( !rootModel.valid() )
{
osg::notify( osg::FATAL ) << "mesh: Can't create mesh." << std::endl;
return( 1 );
}
btCompoundShape* cs_;
bool loaded = phys::loadBulletFile(cfg().path.data + "/areas/" + model_name + "/" + model_name + ".bullet", cs_);
if(loaded)
{
const btTransform ct0 = cs_->getChildTransform(0);
// cs.offset_ = from_bullet_vector3(ct0.getOrigin());
cs_->setLocalScaling( btVector3(0.013,0.013,0.013));
addRigidBody( bw, cs_ );
}
root->addChild( rootModel.get() );
DynamicCharacterController* m_character = new DynamicCharacterController ();
m_character->setup();
bw->addAction(m_character);
#if 0
// Add ground
const osg::Vec4 plane( 0., 0., 1., -100. );
root->addChild( osgbDynamics::generateGroundPlane( plane, bw ) );
#endif
osgbCollision::GLDebugDrawer* dbgDraw( NULL );
if( /*debugDisplay*/ true)
{
dbgDraw = new osgbCollision::GLDebugDrawer();
dbgDraw->setDebugMode( ~btIDebugDraw::DBG_DrawText );
bw->setDebugDrawer( dbgDraw );
root->addChild( dbgDraw->getSceneGraph() );
}
osgViewer::Viewer viewer( arguments );
viewer.apply(new osgViewer::SingleScreen(1));
viewer.addEventHandler( new osgViewer::StatsHandler() );
//viewer.setUpViewInWindow( 30, 30, 768, 480 );
viewer.setSceneData( root );
osgGA::TrackballManipulator* tb = new osgGA::TrackballManipulator;
// tb->setHomePosition( osg::Vec3( 0., -16., 6. ), osg::Vec3( 0., 0., 5. ), osg::Vec3( 0., 0., 1. ) );
viewer.setCameraManipulator( tb );
viewer.getCamera()->setClearColor( osg::Vec4( .5, .5, .5, 1. ) );
viewer.realize();
double prevSimTime = 0.;
while( !viewer.done() )
{
if( dbgDraw != NULL )
dbgDraw->BeginDraw();
const double currSimTime = viewer.getFrameStamp()->getSimulationTime();
bw->stepSimulation( currSimTime - prevSimTime/*0.0*//*0.1*/ );
prevSimTime = currSimTime;
if( dbgDraw != NULL )
{
bw->debugDrawWorld();
dbgDraw->EndDraw();
}
// worldInfo.m_sparsesdf.GarbageCollect();
viewer.frame();
}
return( 0 );
}
static void convertToBullet(void)
{
cleanupBullet();
m_config = new btDefaultCollisionConfiguration();
btHashedOverlappingPairCache* pairCache = new btHashedOverlappingPairCache();
//m_broadphase = new btDbvtBroadphase();
btLowLevelData* lowLevelData = new btLowLevelData;
lowLevelData->m_maxContacts = NUM_CONTACTS;//8024;
lowLevelData->m_contactIdPool = (int*) btAlignedAlloc(sizeof(int)*lowLevelData->m_maxContacts ,16);
lowLevelData->m_contacts = (PfxContactManifold*) btAlignedAlloc(sizeof(PfxContactManifold)*lowLevelData->m_maxContacts,16);
lowLevelData->m_maxPairs = lowLevelData->m_maxContacts;//??
lowLevelData->m_pairsBuff[0] = (PfxBroadphasePair*) btAlignedAlloc(sizeof(PfxBroadphasePair)*lowLevelData->m_maxPairs,16);
lowLevelData->m_pairsBuff[1] = (PfxBroadphasePair*) btAlignedAlloc(sizeof(PfxBroadphasePair)*lowLevelData->m_maxPairs,16);
#define USE_LL_BP
#ifdef USE_LL_BP
btLowLevelBroadphase* llbp = new btLowLevelBroadphase(lowLevelData,pairCache);
m_broadphase = llbp;
#else //USE_LL_BP
m_broadphase = new btDbvtBroadphase();
#endif //USE_LL_BP
//m_dispatcher = new btCollisionDispatcher(m_config);
m_dispatcher = new btLowLevelCollisionDispatcher(lowLevelData,m_config,NUM_CONTACTS);
//#ifdef USE_PARALLEL_SOLVER
// m_solver = new btSequentialImpulseConstraintSolver();
//#else
//sThreadSupport = createSolverThreadSupport(4);
//m_solver = new btLowLevelConstraintSolver(sThreadSupport);
m_solver = new btLowLevelConstraintSolver2(lowLevelData);
//#endif //USE_PARALLEL_SOLVER
//m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_config);
m_dynamicsWorld = new btBulletPhysicsEffectsWorld(lowLevelData, m_dispatcher,llbp,m_solver,m_config,0);
PEDebugDrawer* drawer = new PEDebugDrawer();
drawer->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawAabb);
m_dynamicsWorld->setDebugDrawer(drawer);
m_dynamicsWorld ->getSolverInfo().m_splitImpulse = true;
for(int i=0;i<physics_get_num_rigidbodies();i++) {
btRigidBody* body = 0;
btAlignedObjectArray<btCollisionShape*> shapes;
btAlignedObjectArray<btTransform> childTtransforms;
PfxRigidState &state = states[i];//physics_get_state(i);
state.setUserData(0);
const PfxCollidable &coll = physics_get_collidable(i);
PfxTransform3 rbT(state.getOrientation(), state.getPosition());
PfxShapeIterator itrShape(coll);
for(int j=0;j<coll.getNumShapes();j++,++itrShape) {
const PfxShape &shape = *itrShape;
PfxTransform3 offsetT = shape.getOffsetTransform();
PfxTransform3 worldT = rbT * offsetT;
btTransform childTransform;
childTransform.setIdentity();
childTransform.setOrigin(getBtVector3(offsetT.getTranslation()));
PfxQuat quat(offsetT.getUpper3x3());
childTransform.setBasis(btMatrix3x3(getBtQuat(quat)));
childTtransforms.push_back(childTransform);
switch(shape.getType()) {
case kPfxShapeSphere:
{
btSphereShape* sphere = new btSphereShape(shape.getSphere().m_radius);
shapes.push_back(sphere);
//render_sphere( worldT, Vectormath::Aos::Vector3(1,1,1),Vectormath::floatInVec(shape.getSphere().m_radius));
}
break;
case kPfxShapeBox:
{
btBoxShape* box = new btBoxShape(getBtVector3(shape.getBox().m_half));
shapes.push_back(box);
}
//render_box( worldT, Vectormath::Aos::Vector3(1,1,1), shape.getBox().m_half);
break;
case kPfxShapeCapsule:
shapes.push_back(new btCapsuleShapeX(shape.getCapsule().m_radius,2.f*shape.getCapsule().m_halfLen));
//render_capsule( worldT, Vectormath::Aos::Vector3(1,1,1), Vectormath::floatInVec(shape.getCapsule().m_radius), Vectormath::floatInVec(shape.getCapsule().m_halfLen));
break;
case kPfxShapeCylinder:
shapes.push_back(new btCylinderShapeX(btVector3(shape.getCylinder().m_halfLen,shape.getCylinder().m_radius,shape.getCylinder().m_radius)));
//render_cylinder( worldT, Vectormath::Aos::Vector3(1,1,1), Vectormath::floatInVec(shape.getCylinder().m_radius),Vectormath::floatInVec(shape.getCylinder().m_halfLen));
break;
case kPfxShapeConvexMesh:
{
const PfxConvexMesh* convex = shape.getConvexMesh();
const btScalar* vertices = (const btScalar*)&convex->m_verts[0];
btConvexHullShape* convexHull = new btConvexHullShape(vertices,convex->m_numVerts, sizeof(PfxVector3));
shapes.push_back(convexHull);
}
break;
case kPfxShapeLargeTriMesh:
{
const PfxLargeTriMesh* mesh = shape.getLargeTriMesh();
btTriangleIndexVertexArray* meshInterface = new btTriangleIndexVertexArray();
int i;
for (i= 0; i < mesh->m_numIslands;i++)
{
PfxTriMesh* island = &mesh->m_islands[i];
//mesh->m_islands
//mesh->m_numIslands
btIndexedMesh indexedMesh;
indexedMesh.m_indexType = PHY_UCHAR;
indexedMesh.m_numTriangles = island->m_numFacets;
indexedMesh.m_triangleIndexBase = &island->m_facets[0].m_vertIds[0];
indexedMesh.m_triangleIndexStride = sizeof(PfxFacet);
indexedMesh.m_vertexBase = (const unsigned char*) &island->m_verts[0];
indexedMesh.m_numVertices = island->m_numVerts;//unused
indexedMesh.m_vertexStride = sizeof(PfxVector3);
meshInterface->addIndexedMesh(indexedMesh,PHY_UCHAR);
}
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true);
shapes.push_back(trimesh);
}
break;
default:
{
printf("unknown\n");
}
break;
}
}
if(shapes.size()>0)
{
printf("shapes!\n");
btCollisionShape* colShape = 0;
if (shapes.size()==1 && childTtransforms[0].getOrigin().fuzzyZero())
//todo: also check orientation
{
colShape = shapes[0];
}
else
{
btCompoundShape* compound = new btCompoundShape();
for (int i=0;i<shapes.size();i++)
{
compound->addChildShape(childTtransforms[i],shapes[i]);
}
colShape = compound;
}
btTransform worldTransform;
worldTransform.setIdentity();
worldTransform.setOrigin(getBtVector3(rbT.getTranslation()));
PfxQuat quat(rbT.getUpper3x3());
worldTransform.setBasis(btMatrix3x3(getBtQuat(quat)));
btScalar mass = physics_get_body(i).getMass();
btRigidBody* body = addRigidBody(colShape,mass,worldTransform);
void* ptr = (void*)&state;
body->setUserPointer(ptr);
state.setUserData(body);
}
}
//very basic joint conversion (only limits of PFX_JOINT_SWINGTWIST joint)
for (int i=0;i<numJoints;i++)
{
PfxJoint& joint = joints[i];
switch (joint.m_type)
{
case kPfxJointSwingtwist:
{
//btConeTwistConstraint* coneTwist = new btConeTwistConstraint(rbA,rbB,frameA,frameB);
bool referenceA = true;
btRigidBody* bodyA = (btRigidBody*)states[joint.m_rigidBodyIdA].getUserData();
btRigidBody* bodyB = (btRigidBody*)states[joint.m_rigidBodyIdB].getUserData();
if (bodyA&&bodyB)
{
btTransform frameA,frameB;
frameA.setIdentity();
frameB.setIdentity();
frameA.setOrigin(getBtVector3(joint.m_anchorA));
frameB.setOrigin(getBtVector3(joint.m_anchorB));
bool referenceA = false;
btGeneric6DofConstraint* dof6 = new btGeneric6DofConstraint(*bodyA,*bodyB,frameA,frameB,referenceA);
for (int i=0;i<joint.m_numConstraints;i++)
{
switch (joint.m_constraints[i].m_lock)
{
case SCE_PFX_JOINT_LOCK_FIX:
{
dof6->setLimit(i,0,0);
break;
}
case SCE_PFX_JOINT_LOCK_FREE:
{
dof6->setLimit(i,1,0);
break;
}
case SCE_PFX_JOINT_LOCK_LIMIT:
{
//deal with the case where angular limits of Y-axis are free and/or beyond -PI/2 and PI/2
if ((i==4) && ((joint.m_constraints[i].m_movableLowerLimit<-SIMD_PI/2)||(joint.m_constraints[i].m_movableUpperLimit>SIMD_PI/2)))
{
printf("error with joint limits, forcing DOF to fixed\n");
dof6->setLimit(i,0,0);
} else
{
dof6->setLimit(i,joint.m_constraints[i].m_movableLowerLimit,joint.m_constraints[i].m_movableUpperLimit);
}
break;
}
default:
{
printf("unknown joint lock state\n");
}
}
}
m_dynamicsWorld->addConstraint(dof6,true);
} else
{
printf("error: missing bodies during joint conversion\n");
}
break;
};
case kPfxJointBall:
case kPfxJointHinge:
case kPfxJointSlider:
case kPfxJointFix:
case kPfxJointUniversal:
case kPfxJointAnimation:
default:
{
printf("unknown joint\n");
}
}
}
//create a large enough buffer. There is no method to pre-calculate the buffer size yet.
int maxSerializeBufferSize = 1024*1024*5;
btDefaultSerializer* serializer = new btDefaultSerializer(maxSerializeBufferSize);
m_dynamicsWorld->serialize(serializer);
FILE* file = fopen("testFile.bullet","wb");
fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1, file);
fclose(file);
}
void Context::addRigidBody( const RigidBodyRef &phyObj )
{
addRigidBody( phyObj->getRigidBody().get(), phyObj->mCollGroup, phyObj->mCollMask );
phyObj->setIsAddedToWorld( true );
}