diff --git a/CMakeLists.txt b/CMakeLists.txt index 41a4ba98d..acc474da3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,6 +27,7 @@ SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG") OPTION(USE_DOUBLE_PRECISION "Use double precision" OFF) OPTION(USE_GRAPHICAL_BENCHMARK "Use Graphical Benchmark" ON) OPTION(BUILD_SHARED_LIBS "Use shared libraries" OFF) +OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" OFF) OPTION(USE_MSVC_INCREMENTAL_LINKING "Use MSVC Incremental Linking" OFF) OPTION(USE_CUSTOM_VECTOR_MATH "Use custom vectormath library" OFF) @@ -146,6 +147,10 @@ ADD_DEFINITIONS( -DBT_USE_DOUBLE_PRECISION) SET( BULLET_DOUBLE_DEF "-DBT_USE_DOUBLE_PRECISION") ENDIF (USE_DOUBLE_PRECISION) +IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) +ADD_DEFINITIONS(-DUSE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) +ENDIF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) + IF(USE_GRAPHICAL_BENCHMARK) ADD_DEFINITIONS( -DUSE_GRAPHICAL_BENCHMARK) ENDIF (USE_GRAPHICAL_BENCHMARK) diff --git a/data/gripper/wsg50_one_motor_gripper_new.sdf b/data/gripper/wsg50_one_motor_gripper_new.sdf index a3a11f3b2..5bb80d6a6 100644 --- a/data/gripper/wsg50_one_motor_gripper_new.sdf +++ b/data/gripper/wsg50_one_motor_gripper_new.sdf @@ -2,7 +2,7 @@ - 0 0 0.26 3.14 0 0 + 0 0 0.7 3.14 0 0 diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 3310084fc..15a83af59 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -267,6 +267,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Gripper Grasp","Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc,eGRIPPER_GRASP), ExampleEntry(1,"Two Point Grasp","Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP), ExampleEntry(1,"One Motor Gripper Grasp","Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP), + ExampleEntry(1,"Grasp Soft Body","Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY), #ifdef ENABLE_LUA diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index b46706e65..37e6c06a0 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -217,7 +217,6 @@ public: if ((m_options & eONE_MOTOR_GRASP)!=0) { - { SliderParams slider("Vertical velocity",&sGripperVerticalVelocity); slider.m_minVal=-2; @@ -241,7 +240,6 @@ public: args.m_useMultiBody = true; m_robotSim.loadFile(args, results); } - { b3RobotSimLoadFileArgs args(""); args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf"; @@ -287,48 +285,6 @@ public: } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); - /* - b3JointInfo sliderJoint1; - sliderJoint1.m_parentFrame[0] = 0; - sliderJoint1.m_parentFrame[1] = 0; - sliderJoint1.m_parentFrame[2] = 0.06; - sliderJoint1.m_parentFrame[3] = 0; - sliderJoint1.m_parentFrame[4] = 0; - sliderJoint1.m_parentFrame[5] = 0; - sliderJoint1.m_parentFrame[6] = 1.0; - sliderJoint1.m_childFrame[0] = 0; - sliderJoint1.m_childFrame[1] = 0; - sliderJoint1.m_childFrame[2] = 0; - sliderJoint1.m_childFrame[3] = 0; - sliderJoint1.m_childFrame[4] = 0; - sliderJoint1.m_childFrame[5] = 0; - sliderJoint1.m_childFrame[6] = 1.0; - sliderJoint1.m_jointAxis[0] = 1.0; - sliderJoint1.m_jointAxis[1] = 0.0; - sliderJoint1.m_jointAxis[2] = 0.0; - sliderJoint1.m_jointType = ePrismaticType; - b3JointInfo sliderJoint2; - sliderJoint2.m_parentFrame[0] = 0; - sliderJoint2.m_parentFrame[1] = 0; - sliderJoint2.m_parentFrame[2] = 0.06; - sliderJoint2.m_parentFrame[3] = 0; - sliderJoint2.m_parentFrame[4] = 0; - sliderJoint2.m_parentFrame[5] = 0; - sliderJoint2.m_parentFrame[6] = 1.0; - sliderJoint2.m_childFrame[0] = 0; - sliderJoint2.m_childFrame[1] = 0; - sliderJoint2.m_childFrame[2] = 0; - sliderJoint2.m_childFrame[3] = 0; - sliderJoint2.m_childFrame[4] = 0; - sliderJoint2.m_childFrame[5] = 1.0; - sliderJoint2.m_childFrame[6] = 0; - sliderJoint2.m_jointAxis[0] = 1.0; - sliderJoint2.m_jointAxis[1] = 0.0; - sliderJoint2.m_jointAxis[2] = 0.0; - sliderJoint2.m_jointType = ePrismaticType; - m_robotSim.createJoint(1, 0, 1, 3, &sliderJoint1); - m_robotSim.createJoint(1, 0, 1, 6, &sliderJoint2); - */ b3JointInfo revoluteJoint1; revoluteJoint1.m_parentFrame[0] = -0.055; revoluteJoint1.m_parentFrame[1] = 0; @@ -371,6 +327,110 @@ public: m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2); } + if ((m_options & eGRASP_SOFT_BODY)!=0) + { + { + SliderParams slider("Vertical velocity",&sGripperVerticalVelocity); + slider.m_minVal=-2; + slider.m_maxVal=2; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { + SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity + ); + slider.m_minVal=-1; + slider.m_maxVal=1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + if (1) + { + b3RobotSimLoadFileArgs args(""); + args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf"; + args.m_fileType = B3_SDF_FILE; + args.m_useMultiBody = true; + b3RobotSimLoadFileResults results; + + if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + { + + m_gripperIndex = results.m_uniqueObjectIds[0]; + int numJoints = m_robotSim.getNumJoints(m_gripperIndex); + b3Printf("numJoints = %d",numJoints); + + for (int i=0;im_clientServerDirect) { m_data->m_clientServerDirect->renderScene(); } + m_data->m_physicsServer.renderScene(); } @@ -996,4 +998,10 @@ void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldP worldOrientation[2] = linkState.m_worldOrientation[2]; worldOrientation[3] = linkState.m_worldOrientation[3]; } +} + +void b3RobotSimAPI::loadBunny() +{ + b3SharedMemoryCommandHandle command = b3LoadBunnyCommandInit(m_data->m_physicsClient); + b3SubmitClientCommand(m_data->m_physicsClient, command); } \ No newline at end of file diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 798b892e0..882817384 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -164,6 +164,8 @@ public: void getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian); void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation); + + void loadBunny(); }; #endif //B3_ROBOT_SIM_API_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index beacb1eba..813457225 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -75,6 +75,20 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie return (b3SharedMemoryCommandHandle) command; } +b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_LOAD_BUNNY; + command->m_updateFlags = 0; + + return (b3SharedMemoryCommandHandle) command; +} + int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 7f60bc72c..2d2a4460c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -222,6 +222,8 @@ b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle phys b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient); void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags); void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags); + +b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient); #ifdef __cplusplus } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index ff8cbe1cd..613bc0234 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5,7 +5,6 @@ #include "../Importers/ImportURDFDemo/URDF2Bullet.h" #include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp" #include "TinyRendererVisualShapeConverter.h" -#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" @@ -27,6 +26,16 @@ #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "SharedMemoryCommands.h" +#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletSoftBody/btSoftBodySolvers.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btSoftMultiBodyDynamicsWorld.h" +#include "../SoftDemo/BunnyMesh.h" +#else +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#endif + //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet! btVector3 gLastPickPos(0, 0, 0); @@ -431,9 +440,17 @@ struct PhysicsServerCommandProcessorInternalData btCollisionDispatcher* m_dispatcher; btMultiBodyConstraintSolver* m_solver; btDefaultCollisionConfiguration* m_collisionConfiguration; - btMultiBodyDynamicsWorld* m_dynamicsWorld; + +#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + btSoftMultiBodyDynamicsWorld* m_dynamicsWorld; + btSoftBodySolver* m_softbodySolver; + btSoftBodyWorldInfo m_softBodyWorldInfo; +#else + btMultiBodyDynamicsWorld* m_dynamicsWorld; +#endif + SharedMemoryDebugDrawer* m_remoteDebugDrawer; - + btAlignedObjectArray m_cachedContactPoints; btAlignedObjectArray m_sdfRecentLoadedBodies; @@ -608,28 +625,35 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() { - ///collision configuration contains default setup for memory, collision setup - m_data->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_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration); - - m_data->m_broadphase = new btDbvtBroadphase(); - - m_data->m_solver = new btMultiBodyConstraintSolver; - - m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); - - //Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it - m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192); - - m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer(); - - - m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); - m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08; - + ///collision configuration contains default setup for memory, collision setup + //m_collisionConfiguration->setConvexConvexMultipointIterations(); +#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + m_data->m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); +#else + m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration(); +#endif + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration); + + m_data->m_broadphase = new btDbvtBroadphase(); + + m_data->m_solver = new btMultiBodyConstraintSolver; + +#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); +#else + m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); +#endif + + //Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it + m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192); + + m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer(); + + + m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); + m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08; + } void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies() @@ -1599,6 +1623,36 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm + break; + } + case CMD_LOAD_BUNNY: + { +#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + m_data->m_softBodyWorldInfo.air_density = (btScalar)1.2; + m_data->m_softBodyWorldInfo.water_density = 0; + m_data->m_softBodyWorldInfo.water_offset = 0; + m_data->m_softBodyWorldInfo.water_normal = btVector3(0,0,0); + m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10); + m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase; + m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize(); + + btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny, &gIndicesBunny[0][0], BUNNY_NUM_TRIANGLES); + + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_kLST = 1.0; + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2,pm); + psb->m_cfg.piterations = 2; + psb->m_cfg.kDF = 0.5; + psb->randomizeConstraints(); + psb->rotate(btQuaternion(0.70711,0,0,0.70711)); + psb->translate(btVector3(0,0,3.0)); + psb->scale(btVector3(0.1,0.1,0.1)); + psb->setTotalMass(1,true); + psb->getCollisionShape()->setMargin(0.01); + + m_data->m_dynamicsWorld->addSoftBody(psb); +#endif break; } case CMD_CREATE_SENSOR: @@ -2973,7 +3027,17 @@ void PhysicsServerCommandProcessor::renderScene() m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld); m_data->m_guiHelper->render(m_data->m_dynamicsWorld); } - +#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + for ( int i=0;im_dynamicsWorld->getSoftBodyArray().size();i++) + { + btSoftBody* psb=(btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i]; + if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + //btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb,m_data->m_dynamicsWorld->getDebugDrawer(),m_data->m_dynamicsWorld->getDrawFlags()); + } + } +#endif } void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags) diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index ea176fef7..c668e5fe8 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -7,6 +7,7 @@ enum EnumSharedMemoryClientCommand { CMD_LOAD_SDF, CMD_LOAD_URDF, + CMD_LOAD_BUNNY, CMD_SEND_BULLET_DATA_STREAM, CMD_CREATE_BOX_COLLISION_SHAPE, // CMD_DELETE_BOX_COLLISION_SHAPE, diff --git a/examples/SoftDemo/SoftDemo.cpp b/examples/SoftDemo/SoftDemo.cpp index 9f7f8144a..7a046f9c4 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(100,true); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); pdemo->m_cutting=true; @@ -2167,7 +2167,6 @@ void SoftDemo::initPhysics() m_collisionShapes.push_back(cylinderCompound); - m_dispatcher=0; ///register some softbody collision algorithms on top of the default btDefaultCollisionConfiguration 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/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index 1538eb037..d43df1c67 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -13,6 +13,7 @@ SET(BulletSoftBody_SRCS btSoftBodyRigidBodyCollisionConfiguration.cpp btSoftRigidCollisionAlgorithm.cpp btSoftRigidDynamicsWorld.cpp + btSoftMultiBodyDynamicsWorld.cpp btSoftSoftCollisionAlgorithm.cpp btDefaultSoftBodySolver.cpp @@ -26,6 +27,7 @@ SET(BulletSoftBody_HDRS btSoftBodyRigidBodyCollisionConfiguration.h btSoftRigidCollisionAlgorithm.h btSoftRigidDynamicsWorld.h + btSoftMultiBodyDynamicsWorld.h btSoftSoftCollisionAlgorithm.h btSparseSDF.h diff --git a/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp new file mode 100644 index 000000000..ffa243ebf --- /dev/null +++ b/src/BulletSoftBody/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/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h new file mode 100644 index 000000000..2d0423a44 --- /dev/null +++ b/src/BulletSoftBody/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 "BulletDynamics/Featherstone/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 diff --git a/test/InverseDynamics/CMakeLists.txt b/test/InverseDynamics/CMakeLists.txt index e31ea6e12..217655b09 100644 --- a/test/InverseDynamics/CMakeLists.txt +++ b/test/InverseDynamics/CMakeLists.txt @@ -13,7 +13,7 @@ INCLUDE_DIRECTORIES( ADD_DEFINITIONS(-D_VARIADIC_MAX=10) LINK_LIBRARIES( - BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest + BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest ) IF (NOT WIN32) LINK_LIBRARIES( pthread ) @@ -44,7 +44,7 @@ INCLUDE_DIRECTORIES( ADD_DEFINITIONS(-D_VARIADIC_MAX=10) LINK_LIBRARIES( -BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest +BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest ) IF (NOT WIN32) LINK_LIBRARIES( pthread ) @@ -110,7 +110,7 @@ INCLUDE_DIRECTORIES( ADD_DEFINITIONS(-D_VARIADIC_MAX=10) LINK_LIBRARIES( - BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest + BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest ) IF (NOT WIN32) LINK_LIBRARIES( pthread ) diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt index 496ca5076..8ece789cc 100644 --- a/test/SharedMemory/CMakeLists.txt +++ b/test/SharedMemory/CMakeLists.txt @@ -15,6 +15,9 @@ ADD_DEFINITIONS(-DPHYSICS_LOOP_BACK -DPHYSICS_SERVER_DIRECT -DENABLE_GTEST -D_VA LINK_LIBRARIES( BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest BussIK ) +IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) + LINK_LIBRARIES(BulletSoftBody) +ENDIF() IF (NOT WIN32) LINK_LIBRARIES( pthread )