From 7288313970916768227c531bc782c56a6cc9ceec Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 3 May 2015 10:01:30 -0700 Subject: [PATCH] converted FractureDemo --- examples/ExampleBrowser/CMakeLists.txt | 6 + examples/ExampleBrowser/ExampleEntries.cpp | 3 + examples/FractureDemo/FractureDemo.cpp | 400 ++++++++++ examples/FractureDemo/FractureDemo.h | 22 + examples/FractureDemo/btFractureBody.cpp | 139 ++++ examples/FractureDemo/btFractureBody.h | 78 ++ .../FractureDemo/btFractureDynamicsWorld.cpp | 688 ++++++++++++++++++ .../FractureDemo/btFractureDynamicsWorld.h | 51 ++ 8 files changed, 1387 insertions(+) create mode 100644 examples/FractureDemo/FractureDemo.cpp create mode 100644 examples/FractureDemo/FractureDemo.h create mode 100644 examples/FractureDemo/btFractureBody.cpp create mode 100644 examples/FractureDemo/btFractureBody.h create mode 100644 examples/FractureDemo/btFractureDynamicsWorld.cpp create mode 100644 examples/FractureDemo/btFractureDynamicsWorld.h diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 4cbd0c47d..1420d2822 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -23,6 +23,12 @@ SET(App_ExampleBrowser_SRCS ../GyroscopicDemo/GyroscopicSetup.h ../Planar2D/Planar2D.cpp ../Planar2D/Planar2D.h + ../FractureDemo/FractureDemo.cpp + ../FractureDemo/btFractureBody.cpp + ../FractureDemo/btFractureDynamicsWorld.cpp + ../FractureDemo/FractureDemo.h + ../FractureDemo/btFractureBody.h + ../FractureDemo/btFractureDynamicsWorld.h ../RenderingExamples/CoordinateSystemDemo.cpp ../RenderingExamples/CoordinateSystemDemo.h ../RenderingExamples/RaytracerSetup.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index f442619a2..21bb35521 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -28,6 +28,7 @@ #include "../Experiments/ImplicitCloth/ImplicitClothExample.h" #include "../Importers/ImportBullet/SerializeSetup.h" #include "../Raycast/RaytestDemo.h" +#include "../FractureDemo/FractureDemo.h" #ifdef B3_USE_CLEW @@ -172,6 +173,8 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Voronoi Fracture", "Automatically create a compound rigid body using voronoi tesselation. Individual parts are modeled as rigid bodies using a btConvexHullShape.", VoronoiFractureCreateFunc), + ExampleEntry(1,"Fracture demo", "Create a basic custom implementation to model fracturing objects, based on a btCompoundShape. It explicitly propagates the collision impulses and breaks the rigid body into multiple rigid bodies. Press F to toggle fracture and glue mode.", FractureDemoCreateFunc), + ExampleEntry(1,"Planar 2D","Show the use of 2D collision shapes and rigid body simulation. The collision shape is wrapped into a btConvex2dShape. The rigid bodies are restricted in a plane using the 'setAngularFactor' and 'setLinearFactor' API call.",Planar2DCreateFunc), ExampleEntry(1,"Implicit Cloth", "Cloth simulation using implicit integration, by Stan Melax. The cloth is only attached at the corners. Note the stability using a large time step even with high stiffness.", diff --git a/examples/FractureDemo/FractureDemo.cpp b/examples/FractureDemo/FractureDemo.cpp new file mode 100644 index 000000000..eed7a10d5 --- /dev/null +++ b/examples/FractureDemo/FractureDemo.cpp @@ -0,0 +1,400 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2011 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +///FractureDemo shows how to break objects. +///It assumes a btCompoundShaps (where the childshapes are the pre-fractured pieces) +///The btFractureBody is a class derived from btRigidBody, dealing with the collision impacts. +///Press the F key to toggle between fracture and glue mode +///This is preliminary work + + +#define CUBE_HALF_EXTENTS 1.f +#define EXTRA_HEIGHT 1.f +///scaling of the objects (0.1 = 20 centimeter boxes ) +#define SCALING 1. +#define START_POS_X -5 +#define START_POS_Y -5 +#define START_POS_Z -3 + +#include "FractureDemo.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" + + +#include //printf debugging + + +int sFrameNumber = 0; + +#include "btFractureBody.h" +#include "btFractureDynamicsWorld.h" + + +#include "LinearMath/btAlignedObjectArray.h" + +#include "../CommonInterfaces/CommonRigidBodyBase.h" + + +///FractureDemo shows basic breaking and glueing of objects +class FractureDemo : public CommonRigidBodyBase +{ + + +public: + + FractureDemo(struct GUIHelperInterface* helper) + :CommonRigidBodyBase(helper) + { + } + virtual ~FractureDemo() + { + } + void initPhysics(); + + void exitPhysics(); + + virtual void stepSimulation(float deltaTime) + { + CommonRigidBodyBase::stepSimulation(deltaTime); + + { + BT_PROFILE("recreate graphics"); + //@todo: make this graphics re-creation better + //right now: brute force remove all graphics objects, and re-create them every frame + m_guiHelper->getRenderInterface()->removeAllInstances(); + for (int i=0;igetNumCollisionObjects();i++) + { + btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[i]; + colObj->getCollisionShape()->setUserIndex(-1); + colObj->setUserIndex(-1); + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + } + } + + virtual bool keyboardCallback(int key, int state); + +}; + + + + +void FractureDemo::initPhysics() +{ + + m_guiHelper->setUpAxis(1); + + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + //m_collisionConfiguration->setConvexConvexMultipointIterations(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; + m_solver = sol; + + //m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); + + btFractureDynamicsWorld* fractureWorld = new btFractureDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); + m_dynamicsWorld = fractureWorld; + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + + //m_splitImpulse removes the penetration resolution from the applied impulse, otherwise objects might fracture due to deep penetrations. + m_dynamicsWorld->getSolverInfo().m_splitImpulse = true; + + { + ///create a few basic rigid bodies + btCollisionShape* groundShape = new btBoxShape(btVector3(50,1,50)); + /// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),0); + m_collisionShapes.push_back(groundShape); + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,0,0)); + createRigidBody(0.f,groundTransform,groundShape); + } + + { + ///create a few basic rigid bodies + btCollisionShape* shape = new btBoxShape(btVector3(1,1,1)); + m_collisionShapes.push_back(shape); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(5,2,0)); + createRigidBody(0.f,tr,shape); + } + + + + { + //create a few dynamic rigidbodies + // Re-using the same collision is better for memory usage and performance + + btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1)); + //btCollisionShape* colShape = new btCapsuleShape(SCALING*0.4,SCALING*1); + //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); + m_collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + btScalar mass(1.f); + + //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) + colShape->calculateLocalInertia(mass,localInertia); + + + int gNumObjects = 10; + + for (int i=0;isetLinearVelocity(btVector3(0,-10,0)); + + m_dynamicsWorld->addRigidBody(body); + + } + + } + + + + fractureWorld->stepSimulation(1./60.,0); + fractureWorld->glueCallback(); + + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +#if 0 +void FractureDemo::showMessage() +{ + if((getDebugMode() & btIDebugDraw::DBG_DrawText)) + { + setOrthographicProjection(); + glDisable(GL_LIGHTING); + glColor3f(0, 0, 0); + char buf[124]; + + int lineWidth=380; + int xStart = m_glutScreenWidth - lineWidth; + int yStart = 20; + + btFractureDynamicsWorld* world = (btFractureDynamicsWorld*)m_dynamicsWorld; + if (world->getFractureMode()) + { + sprintf(buf,"Fracture mode"); + } else + { + sprintf(buf,"Glue mode"); + } + GLDebugDrawString(xStart,yStart,buf); + sprintf(buf,"f to toggle fracture/glue mode"); + yStart+=20; + GLDebugDrawString(xStart,yStart,buf); + sprintf(buf,"space to restart, mouse to pick/shoot"); + yStart+=20; + GLDebugDrawString(xStart,yStart,buf); + + resetPerspectiveProjection(); + glEnable(GL_LIGHTING); + } + +} +#endif + +#if 0 +void FractureDemo::displayCallback(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + renderme(); + + showMessage(); + + //optional but useful: debug drawing to detect problems + if (m_dynamicsWorld) + m_dynamicsWorld->debugDrawWorld(); + + glFlush(); + swapBuffers(); +} +#endif + + +bool FractureDemo::keyboardCallback(int key, int state) +{ + + if (key=='f' && (state==0)) + { + btFractureDynamicsWorld* world = (btFractureDynamicsWorld*)m_dynamicsWorld; + world->setFractureMode(!world->getFractureMode()); + if (world->getFractureMode()) + { + b3Printf("Fracturing mode"); + } else + { + b3Printf("Gluing mode"); + + } + return true; + } + + return false; +} + + +#if 0 +void FractureDemo::keyboardUpCallback(unsigned char key, int x, int y) +{ + if (key=='f') + { + btFractureDynamicsWorld* world = (btFractureDynamicsWorld*)m_dynamicsWorld; + world->setFractureMode(!world->getFractureMode()); + } + + PlatformDemoApplication::keyboardUpCallback(key,x,y); + +} +#endif + +#if 0 +void FractureDemo::shootBox(const btVector3& destination) +{ + + if (m_dynamicsWorld) + { + btScalar mass = 1.f; + btTransform startTransform; + startTransform.setIdentity(); + btVector3 camPos = getCameraPosition(); + startTransform.setOrigin(camPos); + + setShootBoxShape (); + + btAssert((!m_shootBoxShape || m_shootBoxShape->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) + m_shootBoxShape->calculateLocalInertia(mass,localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + + btFractureBody* body = new btFractureBody(mass,0,m_shootBoxShape,localInertia,&mass,1,m_dynamicsWorld); + + body->setWorldTransform(startTransform); + + m_dynamicsWorld->addRigidBody(body); + + + body->setLinearFactor(btVector3(1,1,1)); + //body->setRestitution(1); + + btVector3 linVel(destination[0]-camPos[0],destination[1]-camPos[1],destination[2]-camPos[2]); + linVel.normalize(); + linVel*=m_ShootBoxInitialSpeed; + + body->getWorldTransform().setOrigin(camPos); + body->getWorldTransform().setRotation(btQuaternion(0,0,0,1)); + body->setLinearVelocity(linVel); + body->setAngularVelocity(btVector3(0,0,0)); + body->setCcdMotionThreshold(1.); + body->setCcdSweptSphereRadius(0.2f); + + } +} +#endif + + + + + + + +void FractureDemo::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()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject( obj ); + delete obj; + } + + //delete collision shapes + for (int j=0;jisCompound()) + { + btCompoundShape* compound = (btCompoundShape*)getCollisionShape(); + for (int i=0;igetNumChildShapes();i++) + { + for (int j=i+1;jgetNumChildShapes();j++) + { + + struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback + { + bool m_connected; + btScalar m_margin; + MyContactResultCallback() :m_connected(false),m_margin(0.05) + { + } + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) + { + if (cp.getDistance()<=m_margin) + m_connected = true; + return 1.f; + } + }; + + MyContactResultCallback result; + + btCollisionObject obA; + obA.setWorldTransform(compound->getChildTransform(i)); + obA.setCollisionShape(compound->getChildShape(i)); + btCollisionObject obB; + obB.setWorldTransform(compound->getChildTransform(j)); + obB.setCollisionShape(compound->getChildShape(j)); + world->contactPairTest(&obA,&obB,result); + if (result.m_connected) + { + btConnection tmp; + tmp.m_childIndex0 = i; + tmp.m_childIndex1 = j; + tmp.m_childShape0 = compound->getChildShape(i); + tmp.m_childShape1 = compound->getChildShape(j); + tmp.m_strength = 1.f;//?? + m_connections.push_back(tmp); + } + } + } + } + + +} + +btCompoundShape* btFractureBody::shiftTransformDistributeMass(btCompoundShape* boxCompound,btScalar mass,btTransform& shift) +{ + + btVector3 principalInertia; + + btScalar* masses = new btScalar[boxCompound->getNumChildShapes()]; + for (int j=0;jgetNumChildShapes();j++) + { + //evenly distribute mass + masses[j]=mass/boxCompound->getNumChildShapes(); + } + + return shiftTransform(boxCompound,masses,shift,principalInertia); + +} + + +btCompoundShape* btFractureBody::shiftTransform(btCompoundShape* boxCompound,btScalar* masses,btTransform& shift, btVector3& principalInertia) +{ + btTransform principal; + + boxCompound->calculatePrincipalAxisTransform(masses,principal,principalInertia); + + + ///create a new compound with world transform/center of mass properly aligned with the principal axis + + ///non-recursive compound shapes perform better + +#ifdef USE_RECURSIVE_COMPOUND + + btCompoundShape* newCompound = new btCompoundShape(); + newCompound->addChildShape(principal.inverse(),boxCompound); + newBoxCompound = newCompound; + //m_collisionShapes.push_back(newCompound); + + //btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); + //btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,newCompound,principalInertia); + +#else +#ifdef CHANGE_COMPOUND_INPLACE + newBoxCompound = boxCompound; + for (int i=0;igetNumChildShapes();i++) + { + btTransform newChildTransform = principal.inverse()*boxCompound->getChildTransform(i); + ///updateChildTransform is really slow, because it re-calculates the AABB each time. todo: add option to disable this update + boxCompound->updateChildTransform(i,newChildTransform); + } + bool isDynamic = (mass != 0.f); + btVector3 localInertia(0,0,0); + if (isDynamic) + boxCompound->calculateLocalInertia(mass,localInertia); + +#else + ///creation is faster using a new compound to store the shifted children + btCompoundShape* newBoxCompound = new btCompoundShape(); + for (int i=0;igetNumChildShapes();i++) + { + btTransform newChildTransform = principal.inverse()*boxCompound->getChildTransform(i); + ///updateChildTransform is really slow, because it re-calculates the AABB each time. todo: add option to disable this update + newBoxCompound->addChildShape(newChildTransform,boxCompound->getChildShape(i)); + } + + + +#endif + +#endif//USE_RECURSIVE_COMPOUND + + shift = principal; + return newBoxCompound; +} + + + + + + diff --git a/examples/FractureDemo/btFractureBody.h b/examples/FractureDemo/btFractureBody.h new file mode 100644 index 000000000..922db3c22 --- /dev/null +++ b/examples/FractureDemo/btFractureBody.h @@ -0,0 +1,78 @@ + +#ifndef BT_FRACTURE_BODY +#define BT_FRACTURE_BODY + +class btCollisionShape; +class btDynamicsWorld; +class btCollisionWorld; +class btCompoundShape; +class btManifoldPoint; + +#include "LinearMath/btAlignedObjectArray.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + +#define CUSTOM_FRACTURE_TYPE (btRigidBody::CO_USER_TYPE+1) + + +struct btConnection +{ + + btCollisionShape* m_childShape0; + btCollisionShape* m_childShape1; + int m_childIndex0; + int m_childIndex1; + btScalar m_strength; +}; + +class btFractureBody : public btRigidBody +{ + //connections +public: + + btDynamicsWorld* m_world; + btAlignedObjectArray m_masses; + btAlignedObjectArray m_connections; + + + + btFractureBody( const btRigidBodyConstructionInfo& constructionInfo, btDynamicsWorld* world) + :btRigidBody(constructionInfo), + m_world(world) + { + m_masses.push_back(constructionInfo.m_mass); + m_internalType=CUSTOM_FRACTURE_TYPE+CO_RIGID_BODY; + } + + + + ///btRigidBody constructor for backwards compatibility. + ///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo) + btFractureBody( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia, btScalar* masses, int numMasses, btDynamicsWorld* world) + :btRigidBody(mass,motionState,collisionShape,localInertia), + m_world(world) + { + + for (int i=0;igetNumManifolds(); + + ///first build the islands based on axis aligned bounding box overlap + + btUnionFind unionFind; + + int index = 0; + { + + int i; + for (i=0;iisStaticOrKinematicObject()) + { + 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;igetManifoldByIndexInternal(i); + if (!manifold->getNumContacts()) + continue; + + btScalar minDist = 1e30f; + for (int v=0;vgetNumContacts();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;aiisStaticOrKinematicObject()) + { + 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 removedObjects; + + ///iterate over all islands + for ( startIslandIndex=0;startIslandIndexgetInternalType()& 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 massArray; + + btAlignedObjectArray oldImpulses; + btAlignedObjectArray 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;cgetNumChildShapes();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;idxgetInvMass()) + 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;cgetNumChildShapes();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;igetCenterOfMassPosition(); + 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); + } + +} + + +struct btFracturePair +{ + btFractureBody* m_fracObj; + btAlignedObjectArray m_contactManifolds; +}; + + + +void btFractureDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +{ + // todo: after fracture we should run the solver again for better realism + // for example + // save all velocities and if one or more objects fracture: + // 1) revert all velocties + // 2) apply impulses for the fracture bodies at the contact locations + // 3)and run the constaint solver again + + btDiscreteDynamicsWorld::solveConstraints(solverInfo); + + fractureCallback(); +} + +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;igetNumChildShapes();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 btFractureDynamicsWorld::addRigidBody(btRigidBody* body) +{ + if (body->getInternalType() & CUSTOM_FRACTURE_TYPE) + { + btFractureBody* fbody = (btFractureBody*)body; + m_fractureBodies.push_back(fbody); + } + btDiscreteDynamicsWorld::addRigidBody(body); +} + +void btFractureDynamicsWorld::removeRigidBody(btRigidBody* body) +{ + if (body->getInternalType() & CUSTOM_FRACTURE_TYPE) + { + btFractureBody* fbody = (btFractureBody*)body; + btAlignedObjectArray tmpConstraints; + + for (int i=0;igetNumConstraintRefs();i++) + { + tmpConstraints.push_back(fbody->getConstraintRef(i)); + } + + //remove all constraints attached to this rigid body too + for (int i=0;igetCollisionShape()->isCompound()) + return; + + btCompoundShape* compound = (btCompoundShape*)fracObj->getCollisionShape(); + int numChildren = compound->getNumChildShapes(); + + if (numChildren<=1) + return; + + //compute connectivity + btUnionFind unionFind; + + btAlignedObjectArray tags; + tags.resize(numChildren); + int i, index = 0; + for ( i=0;im_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 removedObjects; + + int numIslands = 0; + + for ( startIslandIndex=0;startIslandIndex masses; + + int idx; + for (idx=startIslandIndex;idxgetChildShape(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? + + +} + +#include + + +void btFractureDynamicsWorld::fractureCallback( ) +{ + + btAlignedObjectArray sFracturePairs; + + if (!m_fracturingMode) + { + glueCallback(); + return; + } + + int numManifolds = getDispatcher()->getNumManifolds(); + + sFracturePairs.clear(); + + + for (int i=0;igetManifoldByIndexInternal(i); + if (!manifold->getNumContacts()) + continue; + + btScalar totalImpact = 0.f; + for (int p=0;pgetNumContacts();p++) + { + totalImpact += manifold->getContactPoint(p).m_appliedImpulse; + } + + +// printf("totalImpact=%f\n",totalImpact); + + static float maxImpact = 0; + if (totalImpact>maxImpact) + maxImpact = totalImpact; + + //some threshold otherwise resting contact would break objects after a while + if (totalImpact < 40.f) + continue; + + // printf("strong impact\n"); + + + //@todo: add better logic to decide what parts to fracture + //For example use the idea from the SIGGRAPH talk about the fracture in the movie 2012: + // + //Breaking thresholds can be stored as connectivity information between child shapes in the fracture object + // + //You can calculate some "impact value" by simulating all the individual child shapes + //as rigid bodies, without constraints, running it in a separate simulation world + //(or by running the constraint solver without actually modifying the dynamics world) + //Then measure some "impact value" using the offset and applied impulse for each child shape + //weaken the connections based on this "impact value" and only break + //if this impact value exceeds the breaking threshold. + //you can propagate the weakening and breaking of connections using the connectivity information + + int f0 = m_fractureBodies.findLinearSearch((btFractureBody*)manifold->getBody0()); + int f1 = m_fractureBodies.findLinearSearch((btFractureBody*)manifold->getBody1()); + + if (f0 == f1 == m_fractureBodies.size()) + continue; + + + if (f0getBody1(); + // btRigidBody* otherOb = btRigidBody::upcast(colOb); + // if (!otherOb->getInvMass()) + // continue; + + int pi=-1; + + for (int p=0;pgetBody0(); + btRigidBody* otherOb = btRigidBody::upcast(colOb); + // if (!otherOb->getInvMass()) + // continue; + + + int pi=-1; + + for (int p=0;pgetCollisionShape()->isCompound()) + { + btTransform tr; + tr.setIdentity(); + btCompoundShape* oldCompound = (btCompoundShape*)sFracturePairs[i].m_fracObj->getCollisionShape(); + if (oldCompound->getNumChildShapes()>1) + { + bool needsBreakingCheck = false; + + + //weaken/break the connections + + //@todo: propagate along the connection graph + for (int j=0;jgetNumContacts();k++) + { + btManifoldPoint& pt = manifold->getContactPoint(k); + if (manifold->getBody0()==sFracturePairs[i].m_fracObj) + { + for (int f=0;fm_connections.size();f++) + { + btConnection& connection = sFracturePairs[i].m_fracObj->m_connections[f]; + if ( (connection.m_childIndex0 == pt.m_index0) || + (connection.m_childIndex1 == pt.m_index0) + ) + { + connection.m_strength -= pt.m_appliedImpulse; + if (connection.m_strength<0) + { + //remove or set to zero + connection.m_strength=0.f; + needsBreakingCheck = true; + } + } + } + } else + { + for (int f=0;fm_connections.size();f++) + { + btConnection& connection = sFracturePairs[i].m_fracObj->m_connections[f]; + if ( (connection.m_childIndex0 == pt.m_index1) || + (connection.m_childIndex1 == pt.m_index1) + ) + { + connection.m_strength -= pt.m_appliedImpulse; + if (connection.m_strength<0) + { + //remove or set to zero + connection.m_strength=0.f; + needsBreakingCheck = true; + } + } + } + } + } + } + + if (needsBreakingCheck) + { + breakDisconnectedParts(sFracturePairs[i].m_fracObj); + } + } + + } + + } + } + + sFracturePairs.clear(); + +} + diff --git a/examples/FractureDemo/btFractureDynamicsWorld.h b/examples/FractureDemo/btFractureDynamicsWorld.h new file mode 100644 index 000000000..255487729 --- /dev/null +++ b/examples/FractureDemo/btFractureDynamicsWorld.h @@ -0,0 +1,51 @@ +#ifndef _BT_FRACTURE_DYNAMICS_WORLD_H +#define _BT_FRACTURE_DYNAMICS_WORLD_H + +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +#include "LinearMath/btAlignedObjectArray.h" + +class btFractureBody; +class btCompoundShape; +class btTransform; + + +///The btFractureDynamicsWorld class enabled basic glue and fracture of objects. +///If/once this implementation is stablized/tested we might merge it into btDiscreteDynamicsWorld and remove the class. +class btFractureDynamicsWorld : public btDiscreteDynamicsWorld +{ + btAlignedObjectArray m_fractureBodies; + + bool m_fracturingMode; + + btFractureBody* addNewBody(const btTransform& oldTransform,btScalar* masses, btCompoundShape* oldCompound); + + void breakDisconnectedParts( btFractureBody* fracObj); + +public: + + btFractureDynamicsWorld ( btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + + virtual void addRigidBody(btRigidBody* body); + + virtual void removeRigidBody(btRigidBody* body); + + void solveConstraints(btContactSolverInfo& solverInfo); + + ///either fracture or glue (!fracture) + void setFractureMode(bool fracture) + { + m_fracturingMode = fracture; + } + + bool getFractureMode() const { return m_fracturingMode;} + + ///normally those callbacks are called internally by the 'solveConstraints' + void glueCallback(); + + ///normally those callbacks are called internally by the 'solveConstraints' + void fractureCallback(); + +}; + +#endif //_BT_FRACTURE_DYNAMICS_WORLD_H +