diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 068bf1687..a0636381b 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -961,10 +961,12 @@ void b3RobotSimAPI::renderScene() } } + if (m_data->m_clientServerDirect) { m_data->m_clientServerDirect->renderScene(); } + m_data->m_physicsServer.renderScene(); } diff --git a/examples/SoftDemo/SoftDemo.cpp b/examples/SoftDemo/SoftDemo.cpp index 9f7f8144a..6b1611825 100644 --- a/examples/SoftDemo/SoftDemo.cpp +++ b/examples/SoftDemo/SoftDemo.cpp @@ -1017,7 +1017,7 @@ static void Init_Bunny(SoftDemo* pdemo) psb->m_cfg.kDF = 0.5; psb->randomizeConstraints(); psb->scale(btVector3(6,6,6)); - psb->setTotalMass(100,true); + psb->setTotalMass(1,true); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); pdemo->m_cutting=true; @@ -2097,6 +2097,7 @@ void SoftDemo::initPhysics() motorcontrol.maxtorque = 0; btCollisionShape* groundShape = 0; + if (0) { int i; int j; @@ -2151,11 +2152,12 @@ void SoftDemo::initPhysics() groundShape->setMargin(0.5); } - m_collisionShapes.push_back(groundShape); + //m_collisionShapes.push_back(groundShape); btCollisionShape* groundBox = new btBoxShape (btVector3(100,CUBE_HALF_EXTENTS,100)); m_collisionShapes.push_back(groundBox); + /* btCompoundShape* cylinderCompound = new btCompoundShape; btCollisionShape* cylinderShape = new btCylinderShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)); btTransform localTransform; @@ -2166,7 +2168,7 @@ void SoftDemo::initPhysics() cylinderCompound->addChildShape(localTransform,cylinderShape); m_collisionShapes.push_back(cylinderCompound); - + */ m_dispatcher=0; @@ -2260,7 +2262,7 @@ void SoftDemo::initPhysics() newOb->setCollisionShape(m_collisionShapes[0]); } else { - newOb->setCollisionShape(m_collisionShapes[1]); + newOb->setCollisionShape(m_collisionShapes[0]); } m_dynamicsWorld->addCollisionObject(newOb); diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index 656fc8433..7e31c13d1 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -29,6 +29,7 @@ SET(BulletDynamics_SRCS Featherstone/btMultiBody.cpp Featherstone/btMultiBodyConstraintSolver.cpp Featherstone/btMultiBodyDynamicsWorld.cpp + Featherstone/btSoftMultiBodyDynamicsWorld.cpp Featherstone/btMultiBodyJointLimitConstraint.cpp Featherstone/btMultiBodyConstraint.cpp Featherstone/btMultiBodyPoint2Point.cpp @@ -84,6 +85,7 @@ SET(Featherstone_HDRS Featherstone/btMultiBody.h Featherstone/btMultiBodyConstraintSolver.h Featherstone/btMultiBodyDynamicsWorld.h + Featherstone/btSoftMultiBodyDynamicsWorld.h Featherstone/btMultiBodyLink.h Featherstone/btMultiBodyLinkCollider.h Featherstone/btMultiBodySolverConstraint.h diff --git a/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 35dd1400f..4d65f5489 100644 --- a/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -34,7 +34,8 @@ enum btDynamicsWorldType BT_DISCRETE_DYNAMICS_WORLD=2, BT_CONTINUOUS_DYNAMICS_WORLD=3, BT_SOFT_RIGID_DYNAMICS_WORLD=4, - BT_GPU_DYNAMICS_WORLD=5 + BT_GPU_DYNAMICS_WORLD=5, + BT_SOFT_MULTIBODY_DYNAMICS_WORLD=6 }; ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. diff --git a/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.cpp new file mode 100644 index 000000000..ffa243ebf --- /dev/null +++ b/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.cpp @@ -0,0 +1,367 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 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. +*/ + + +#include "btSoftMultiBodyDynamicsWorld.h" +#include "LinearMath/btQuickprof.h" + +//softbody & helpers +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btSoftBodySolvers.h" +#include "BulletSoftBody/btDefaultSoftBodySolver.h" +#include "LinearMath/btSerializer.h" + + +btSoftMultiBodyDynamicsWorld::btSoftMultiBodyDynamicsWorld( + btDispatcher* dispatcher, + btBroadphaseInterface* pairCache, + btMultiBodyConstraintSolver* constraintSolver, + btCollisionConfiguration* collisionConfiguration, + btSoftBodySolver *softBodySolver ) : + btMultiBodyDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), + m_softBodySolver( softBodySolver ), + m_ownsSolver(false) +{ + if( !m_softBodySolver ) + { + void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16); + m_softBodySolver = new(ptr) btDefaultSoftBodySolver(); + m_ownsSolver = true; + } + + m_drawFlags = fDrawFlags::Std; + m_drawNodeTree = true; + m_drawFaceTree = false; + m_drawClusterTree = false; + m_sbi.m_broadphase = pairCache; + m_sbi.m_dispatcher = dispatcher; + m_sbi.m_sparsesdf.Initialize(); + m_sbi.m_sparsesdf.Reset(); + + m_sbi.air_density = (btScalar)1.2; + m_sbi.water_density = 0; + m_sbi.water_offset = 0; + m_sbi.water_normal = btVector3(0,0,0); + m_sbi.m_gravity.setValue(0,-10,0); + + m_sbi.m_sparsesdf.Initialize(); + + +} + +btSoftMultiBodyDynamicsWorld::~btSoftMultiBodyDynamicsWorld() +{ + if (m_ownsSolver) + { + m_softBodySolver->~btSoftBodySolver(); + btAlignedFree(m_softBodySolver); + } +} + +void btSoftMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep ); + { + BT_PROFILE("predictUnconstraintMotionSoftBody"); + m_softBodySolver->predictMotion( float(timeStep) ); + } +} + +void btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation( btScalar timeStep ) +{ + + // Let the solver grab the soft bodies and if necessary optimize for it + m_softBodySolver->optimize( getSoftBodyArray() ); + + if( !m_softBodySolver->checkInitialized() ) + { + btAssert( "Solver initialization failed\n" ); + } + + btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); + + ///solve soft bodies constraints + solveSoftBodiesConstraints( timeStep ); + + //self collisions + for ( int i=0;idefaultCollisionHandler(psb); + } + + ///update soft bodies + m_softBodySolver->updateSoftBodies( ); + + // End solver-wise simulation step + // /////////////////////////////// + +} + +void btSoftMultiBodyDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep ) +{ + BT_PROFILE("solveSoftConstraints"); + + if(m_softBodies.size()) + { + btSoftBody::solveClusters(m_softBodies); + } + + // Solve constraints solver-wise + m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() ); + +} + +void btSoftMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask) +{ + m_softBodies.push_back(body); + + // Set the soft body solver that will deal with this body + // to be the world's solver + body->setSoftBodySolver( m_softBodySolver ); + + btCollisionWorld::addCollisionObject(body, + collisionFilterGroup, + collisionFilterMask); + +} + +void btSoftMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body) +{ + m_softBodies.remove(body); + + btCollisionWorld::removeCollisionObject(body); +} + +void btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +{ + btSoftBody* body = btSoftBody::upcast(collisionObject); + if (body) + removeSoftBody(body); + else + btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); +} + +void btSoftMultiBodyDynamicsWorld::debugDrawWorld() +{ + btDiscreteDynamicsWorld::debugDrawWorld(); + + if (getDebugDrawer()) + { + int i; + for ( i=0;im_softBodies.size();i++) + { + btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; + if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); + btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags); + } + + if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + if(m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); + if(m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer); + if(m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer); + } + } + } +} + + + + +struct btSoftSingleRayCallback : public btBroadphaseRayCallback +{ + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; + + const btSoftMultiBodyDynamicsWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; + + btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftMultiBodyDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback) + :m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_world(world), + m_resultCallback(resultCallback) + { + m_rayFromTrans.setIdentity(); + m_rayFromTrans.setOrigin(m_rayFromWorld); + m_rayToTrans.setIdentity(); + m_rayToTrans.setOrigin(m_rayToWorld); + + btVector3 rayDir = (rayToWorld-rayFromWorld); + + rayDir.normalize (); + ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + m_signs[0] = m_rayDirectionInverse[0] < 0.0; + m_signs[1] = m_rayDirectionInverse[1] < 0.0; + m_signs[2] = m_rayDirectionInverse[2] < 0.0; + + m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld); + + } + + + + virtual bool process(const btBroadphaseProxy* proxy) + { + ///terminate further ray tests, once the closestHitFraction reached zero + if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) + return false; + + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + + //only perform raycast if filterMask matches + if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; +#if 0 +#ifdef RECALCULATE_AABB + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); +#else + //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); + const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; + const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; +#endif +#endif + //btScalar hitLambda = m_resultCallback.m_closestHitFraction; + //culling already done by broadphase + //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) + { + m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback); + } + } + return true; + } +}; + +void btSoftMultiBodyDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const +{ + BT_PROFILE("rayTest"); + /// use the broadphase to accelerate the search for objects, based on their aabb + /// and for each object with ray-aabb overlap, perform an exact ray test + btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); + +#ifndef USE_BRUTEFORCE_RAYBROADPHASE + m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); +#else + for (int i=0;igetNumCollisionObjects();i++) + { + rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE + +} + + +void btSoftMultiBodyDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) +{ + if (collisionShape->isSoftBody()) { + btSoftBody* softBody = btSoftBody::upcast(collisionObject); + if (softBody) { + btSoftBody::sRayCast softResult; + if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) + { + + if (softResult.fraction<= resultCallback.m_closestHitFraction) + { + + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = 0; + shapeInfo.m_triangleIndex = softResult.index; + // get the normal + btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); + btVector3 normal=-rayDir; + normal.normalize(); + + if (softResult.feature == btSoftBody::eFeature::Face) + { + normal = softBody->m_faces[softResult.index].m_normal; + if (normal.dot(rayDir) > 0) { + // normal always point toward origin of the ray + normal = -normal; + } + } + + btCollisionWorld::LocalRayResult rayResult + (collisionObject, + &shapeInfo, + normal, + softResult.fraction); + bool normalInWorldSpace = true; + resultCallback.addSingleResult(rayResult,normalInWorldSpace); + } + } + } + } + else { + btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback); + } +} + + +void btSoftMultiBodyDynamicsWorld::serializeSoftBodies(btSerializer* serializer) +{ + int i; + //serialize all collision objects + for (i=0;igetInternalType() & btCollisionObject::CO_SOFT_BODY) + { + int len = colObj->calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(len,1); + const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj); + } + } + +} + +void btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer) +{ + + serializer->startSerialization(); + + serializeDynamicsWorldInfo( serializer); + + serializeSoftBodies(serializer); + + serializeRigidBodies(serializer); + + serializeCollisionObjects(serializer); + + serializer->finishSerialization(); +} + + diff --git a/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h new file mode 100644 index 000000000..79ff913d6 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h @@ -0,0 +1,108 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 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. +*/ + +#ifndef BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H +#define BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H + +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +#include "btMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" + +typedef btAlignedObjectArray btSoftBodyArray; + +class btSoftBodySolver; + +class btSoftMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld +{ + + btSoftBodyArray m_softBodies; + int m_drawFlags; + bool m_drawNodeTree; + bool m_drawFaceTree; + bool m_drawClusterTree; + btSoftBodyWorldInfo m_sbi; + ///Solver classes that encapsulate multiple soft bodies for solving + btSoftBodySolver *m_softBodySolver; + bool m_ownsSolver; + +protected: + + virtual void predictUnconstraintMotion(btScalar timeStep); + + virtual void internalSingleStepSimulation( btScalar timeStep); + + void solveSoftBodiesConstraints( btScalar timeStep ); + + void serializeSoftBodies(btSerializer* serializer); + +public: + + btSoftMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 ); + + virtual ~btSoftMultiBodyDynamicsWorld(); + + virtual void debugDrawWorld(); + + void addSoftBody(btSoftBody* body,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); + + void removeSoftBody(btSoftBody* body); + + ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject + virtual void removeCollisionObject(btCollisionObject* collisionObject); + + int getDrawFlags() const { return(m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags=f; } + + btSoftBodyWorldInfo& getWorldInfo() + { + return m_sbi; + } + const btSoftBodyWorldInfo& getWorldInfo() const + { + return m_sbi; + } + + virtual btDynamicsWorldType getWorldType() const + { + return BT_SOFT_MULTIBODY_DYNAMICS_WORLD; + } + + btSoftBodyArray& getSoftBodyArray() + { + return m_softBodies; + } + + const btSoftBodyArray& getSoftBodyArray() const + { + return m_softBodies; + } + + + virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; + + /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. + /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. + /// This allows more customization. + static void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback); + + virtual void serialize(btSerializer* serializer); + +}; + +#endif //BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H