diff --git a/CMakeLists.txt b/CMakeLists.txt index 49143982f..7e48e120f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,6 +7,11 @@ SET(MSVC_INCREMENTAL_DEFAULT ON) PROJECT(BULLET_PHYSICS) SET(BULLET_VERSION 2.76) +IF(COMMAND cmake_policy) + cmake_policy(SET CMP0003 NEW) +ENDIF(COMMAND cmake_policy) + + IF (NOT CMAKE_BUILD_TYPE) # SET(CMAKE_BUILD_TYPE "Debug") SET(CMAKE_BUILD_TYPE "Release") @@ -157,6 +162,8 @@ IF (USE_GLUT AND MSVC) remove_definitions(-D_WINDOWS ) ENDIF() + + ELSE(WIN32) OPTION(USE_GLUT "Use Glut" ON) ENDIF(WIN32) diff --git a/Demos/Benchmarks/BenchmarkDemo.cpp b/Demos/Benchmarks/BenchmarkDemo.cpp index 194e38577..3b6dad02f 100644 --- a/Demos/Benchmarks/BenchmarkDemo.cpp +++ b/Demos/Benchmarks/BenchmarkDemo.cpp @@ -41,12 +41,40 @@ subject to the following restrictions: #ifdef USE_PARALLEL_DISPATCHER_BENCHMARK #include "BulletMultiThreaded/Win32ThreadSupport.h" #include "BulletMultiThreaded/SpuGatheringCollisionDispatcher.h" +#include "BulletMultiThreaded/btParallelConstraintSolver.h" + + + +btThreadSupportInterface* createSolverThreadSupport(int maxNumThreads) +{ +//#define SEQUENTIAL +#ifdef SEQUENTIAL + SequentialThreadSupport::SequentialThreadConstructionInfo tci("solverThreads",SolverThreadFunc,SolverlsMemoryFunc); + SequentialThreadSupport* threadSupport = new SequentialThreadSupport(tci); + threadSupport->startSPU(); +#else + +#ifdef _WIN32 + Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("solverThreads",SolverThreadFunc,SolverlsMemoryFunc,maxNumThreads); + Win32ThreadSupport* threadSupport = new Win32ThreadSupport(threadConstructionInfo); + threadSupport->startSPU(); +#elif defined (USE_PTHREADS) + PosixThreadSupport::ThreadConstructionInfo solverConstructionInfo("solver", SolverThreadFunc, + SolverlsMemoryFunc, maxNumThreads); + + PosixThreadSupport* threadSupport = new PosixThreadSupport(solverConstructionInfo); + +#else + SequentialThreadSupport::SequentialThreadConstructionInfo tci("solverThreads",SolverThreadFunc,SolverlsMemoryFunc); + SequentialThreadSupport* threadSupport = new SequentialThreadSupport(tci); + threadSupport->startSPU(); +#endif + #endif -#undef USE_PARALLEL_SOLVER_BENCHMARK -#ifdef USE_PARALLEL_SOLVER_BENCHMARK -#include "BulletMultiThreaded/btParallelConstraintSolver.h" + return threadSupport; +} #endif class btRaycastBar2 @@ -319,8 +347,10 @@ void BenchmarkDemo::initPhysics() ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) -#ifdef USE_PARALLEL_SOLVER_BENCHMARK - btConstraintSolver* sol = new btParallelConstraintSolver; +#ifdef USE_PARALLEL_DISPATCHER_BENCHMARK + + btThreadSupportInterface* thread = createSolverThreadSupport(4); + btConstraintSolver* sol = new btParallelConstraintSolver(thread); #else btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; #endif //USE_PARALLEL_DISPATCHER_BENCHMARK @@ -331,9 +361,13 @@ void BenchmarkDemo::initPhysics() btDiscreteDynamicsWorld* dynamicsWorld; m_dynamicsWorld = dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver,m_collisionConfiguration); +#ifdef USE_PARALLEL_DISPATCHER_BENCHMARK + dynamicsWorld->getSimulationIslandManager()->setSplitIslands(false); +#endif //USE_PARALLEL_DISPATCHER_BENCHMARK + ///the following 3 lines increase the performance dramatically, with a little bit of loss of quality m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_ENABLE_FRICTION_DIRECTION_CACHING; //don't recalculate friction values each frame - dynamicsWorld->getSolverInfo().m_numIterations = 4; //few solver iterations + dynamicsWorld->getSolverInfo().m_numIterations = 5; //few solver iterations m_defaultContactProcessingThreshold = 0.f;//used when creating bodies: body->setContactProcessingThreshold(...); @@ -494,8 +528,8 @@ void BenchmarkDemo::createWall(const btVector3& offsetPosition,int stackSize,con void BenchmarkDemo::createPyramid(const btVector3& offsetPosition,int stackSize,const btVector3& boxSize) { + btScalar space = 0.0001f; - btScalar space = 0.000f; btVector3 pos(0.0f, boxSize[1], 0.0f); btBoxShape* blockShape = new btBoxShape(btVector3(boxSize[0]-COLLISION_RADIUS,boxSize[1]-COLLISION_RADIUS,boxSize[2]-COLLISION_RADIUS)); @@ -507,9 +541,9 @@ void BenchmarkDemo::createPyramid(const btVector3& offsetPosition,int stackSize, blockShape->calculateLocalInertia(mass,localInertia); - btScalar diffX = boxSize[0]; - btScalar diffY = boxSize[1]; - btScalar diffZ = boxSize[2]; + btScalar diffX = boxSize[0]*1.02f; + btScalar diffY = boxSize[1]*1.02f; + btScalar diffZ = boxSize[2]*1.02f; btScalar offsetX = -stackSize * (diffX * 2.0f + space) * 0.5f; btScalar offsetZ = -stackSize * (diffZ * 2.0f + space) * 0.5f; diff --git a/Demos/CMakeLists.txt b/Demos/CMakeLists.txt index 979bc8180..44f6d029a 100644 --- a/Demos/CMakeLists.txt +++ b/Demos/CMakeLists.txt @@ -3,7 +3,7 @@ IF (USE_GLUT) IF(BUILD_CPU_DEMOS) SET(SharedDemoSubdirs - OpenGL AllBulletDemos ConvexDecompositionDemo Benchmarks HelloWorld + OpenGL AllBulletDemos ConvexDecompositionDemo HelloWorld CcdPhysicsDemo ConstraintDemo SliderConstraintDemo GenericJointDemo Raytracer RagdollDemo ForkLiftDemo BasicDemo Box2dDemo BspDemo MovingConcaveDemo VehicleDemo UserCollisionAlgorithm CharacterDemo SoftDemo HeightFieldFluidDemo @@ -19,6 +19,7 @@ ELSE() ENDIF() SUBDIRS( ${SharedDemoSubdirs} + Benchmarks ThreadingDemo MultiThreadedDemo VectorAdd_OpenCL diff --git a/Demos/ParticlesOpenCL/Apple/CMakeLists.txt b/Demos/ParticlesOpenCL/Apple/CMakeLists.txt index 629e52c9d..de9605343 100644 --- a/Demos/ParticlesOpenCL/Apple/CMakeLists.txt +++ b/Demos/ParticlesOpenCL/Apple/CMakeLists.txt @@ -64,8 +64,6 @@ IF (CMAKE_CL_64) COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/glut64.dll ${CMAKE_CURRENT_BINARY_DIR} ) ADD_CUSTOM_COMMAND( TARGET AppParticlesOCL_Apple POST_BUILD COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLEW64.DLL ${CMAKE_CURRENT_BINARY_DIR}) - ADD_CUSTOM_COMMAND( TARGET AppParticlesOCL_Apple POST_BUILD - COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/Demos/ParticlesOpenCL/ParticlesOCL.cl ${CMAKE_CURRENT_BINARY_DIR}) ENDIF() ELSE(CMAKE_CL_64) IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) @@ -73,12 +71,14 @@ ELSE(CMAKE_CL_64) COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}) ADD_CUSTOM_COMMAND( TARGET AppParticlesOCL_Apple POST_BUILD COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLEW32.DLL ${CMAKE_CURRENT_BINARY_DIR}) - ADD_CUSTOM_COMMAND( TARGET AppParticlesOCL_Apple POST_BUILD - COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/Demos/ParticlesOpenCL/ParticlesOCL.cl ${CMAKE_CURRENT_BINARY_DIR}) ENDIF() ENDIF(CMAKE_CL_64) ENDIF(WIN32) +ADD_CUSTOM_COMMAND( TARGET AppParticlesOCL_Apple POST_BUILD + COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/Demos/ParticlesOpenCL/ParticlesOCL.cl ${CMAKE_CURRENT_BINARY_DIR}) + + IF (UNIX) TARGET_LINK_LIBRARIES(AppParticlesOCL_Apple pthread) ENDIF(UNIX) diff --git a/Demos/ParticlesOpenCL/MiniCL/CMakeLists.txt b/Demos/ParticlesOpenCL/MiniCL/CMakeLists.txt index 600ded733..560795265 100644 --- a/Demos/ParticlesOpenCL/MiniCL/CMakeLists.txt +++ b/Demos/ParticlesOpenCL/MiniCL/CMakeLists.txt @@ -62,8 +62,6 @@ IF (CMAKE_CL_64) COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/glut64.dll ${CMAKE_CURRENT_BINARY_DIR} ) ADD_CUSTOM_COMMAND( TARGET AppParticlesOCL_Mini POST_BUILD COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLEW64.DLL ${CMAKE_CURRENT_BINARY_DIR}) - ADD_CUSTOM_COMMAND( TARGET AppParticlesOCL_Mini POST_BUILD - COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/Demos/ParticlesOpenCL/ParticlesOCL.cl ${CMAKE_CURRENT_BINARY_DIR}) ENDIF() ELSE(CMAKE_CL_64) IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) @@ -71,12 +69,14 @@ ELSE(CMAKE_CL_64) COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}) ADD_CUSTOM_COMMAND( TARGET AppParticlesOCL_Mini POST_BUILD COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLEW32.DLL ${CMAKE_CURRENT_BINARY_DIR}) - ADD_CUSTOM_COMMAND( TARGET AppParticlesOCL_Mini POST_BUILD - COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/Demos/ParticlesOpenCL/ParticlesOCL.cl ${CMAKE_CURRENT_BINARY_DIR}) ENDIF() ENDIF(CMAKE_CL_64) ENDIF(WIN32) +ADD_CUSTOM_COMMAND( TARGET AppParticlesOCL_Mini POST_BUILD + COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/Demos/ParticlesOpenCL/ParticlesOCL.cl ${CMAKE_CURRENT_BINARY_DIR}) + + IF (UNIX) TARGET_LINK_LIBRARIES(AppParticlesOCL_Mini pthread) ENDIF(UNIX) diff --git a/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index 93df73baa..ec26f12e8 100644 --- a/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -219,9 +219,13 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf for (k = 0; k < n; k++) { + btAssert(masses[k]>0); center += m_children[k].m_transform.getOrigin() * masses[k]; totalMass += masses[k]; } + + btAssert(totalMass>0); + center /= totalMass; principal.setOrigin(center); diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index e4919a820..527338ea1 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -539,7 +539,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra if (rb0) rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); if (rb1) - rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); + rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); } else { solverConstraint.m_appliedImpulse = 0.f; @@ -598,7 +598,7 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolver if (rb0) rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); if (rb1) - rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); + rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse); } else { frictionConstraint1.m_appliedImpulse = 0.f; @@ -614,7 +614,7 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolver if (rb0) rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); if (rb1) - rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); + rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse); } else { frictionConstraint2.m_appliedImpulse = 0.f; diff --git a/src/BulletMultiThreaded/CMakeLists.txt b/src/BulletMultiThreaded/CMakeLists.txt index 6a995d058..116e2aebf 100644 --- a/src/BulletMultiThreaded/CMakeLists.txt +++ b/src/BulletMultiThreaded/CMakeLists.txt @@ -5,6 +5,7 @@ INCLUDE_DIRECTORIES( ADD_LIBRARY(BulletMultiThreaded PlatformDefinitions.h + PpuAddressSpace.h SpuFakeDma.cpp SpuFakeDma.h SpuDoubleBuffer.h diff --git a/src/BulletMultiThreaded/PpuAddressSpace.h b/src/BulletMultiThreaded/PpuAddressSpace.h index 833b22bf9..be0a62116 100644 --- a/src/BulletMultiThreaded/PpuAddressSpace.h +++ b/src/BulletMultiThreaded/PpuAddressSpace.h @@ -1,3 +1,19 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2010 Erwin Coumans http://bulletphysics.org + +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 __PPU_ADDRESS_SPACE_H #define __PPU_ADDRESS_SPACE_H @@ -9,13 +25,13 @@ #endif //_WIN32 -#if defined(_WIN64) || defined(__LP64__) || defined(__x86_64__) -typedef unsigned __int64 ppu_address_t; +#if defined(_WIN64) + typedef unsigned __int64 ppu_address_t; +#elif defined(__LP64__) || defined(__x86_64__) + typedef uint64_t ppu_address_t; #else + typedef uint32_t ppu_address_t; +#endif //defined(_WIN64) -typedef uint32_t ppu_address_t; - -#endif - -#endif +#endif //__PPU_ADDRESS_SPACE_H diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp index dfab98028..e5e263c22 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp @@ -1216,7 +1216,7 @@ void processCollisionTask(void* userPtr, void* lsMemPtr) #endif ) { -#define USE_PE_BOX_BOX 1 +//#define USE_PE_BOX_BOX 1 #ifdef USE_PE_BOX_BOX { diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index 8ea6e9bb9..8edfd6873 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -1,8 +1,11 @@ INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src + ) +#SUBDIRS( Solvers ) + SET(BulletSoftBody_SRCS btSoftBody.cpp btSoftBodyConcaveCollisionAlgorithm.cpp @@ -11,6 +14,8 @@ SET(BulletSoftBody_SRCS btSoftRigidCollisionAlgorithm.cpp btSoftRigidDynamicsWorld.cpp btSoftSoftCollisionAlgorithm.cpp + btDefaultSoftBodySolver.cpp + ) SET(BulletSoftBody_HDRS @@ -22,6 +27,11 @@ SET(BulletSoftBody_HDRS btSoftRigidDynamicsWorld.h btSoftSoftCollisionAlgorithm.h btSparseSDF.h + + btSoftBodySolvers.h + btDefaultSoftBodySolver.h + + btSoftBodySolverVertexBuffer.h ) diff --git a/src/BulletSoftBody/btDefaultSoftBodySolver.cpp b/src/BulletSoftBody/btDefaultSoftBodySolver.cpp new file mode 100644 index 000000000..4c6d28532 --- /dev/null +++ b/src/BulletSoftBody/btDefaultSoftBodySolver.cpp @@ -0,0 +1,128 @@ +/* +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 "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" + +#include "btDefaultSoftBodySolver.h" +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" +#include "BulletSoftBody/btSoftBody.h" + + +btDefaultSoftBodySolver::btDefaultSoftBodySolver() +{ + // Initial we will clearly need to update solver constants + // For now this is global for the cloths linked with this solver - we should probably make this body specific + // for performance in future once we understand more clearly when constants need to be updated + m_updateSolverConstants = true; +} + +btDefaultSoftBodySolver::~btDefaultSoftBodySolver() +{ +} + + + + +void btDefaultSoftBodySolver::optimize( btAlignedObjectArray< btSoftBody * > &softBodies ) +{ + m_softBodySet.copyFromArray( softBodies ); +} + +void btDefaultSoftBodySolver::updateSoftBodies( ) +{ + for ( int i=0; i < m_softBodySet.size(); i++) + { + btSoftBody* psb=(btSoftBody*)m_softBodySet[i]; + psb->integrateMotion(); + } +} // updateSoftBodies + +bool btDefaultSoftBodySolver::checkInitialized() +{ + return true; +} + +void btDefaultSoftBodySolver::solveConstraints( float solverdt ) +{ + // Solve constraints for non-solver softbodies + for(int i=0; i < m_softBodySet.size(); ++i) + { + btSoftBody* psb = static_cast(m_softBodySet[i]); + psb->solveConstraints(); + } +} // btDefaultSoftBodySolver::solveConstraints + + +void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer ) +{ + // Currently only support CPU output buffers + // TODO: check for DX11 buffers. Take all offsets into the same DX11 buffer + // and use them together on a single kernel call if possible by setting up a + // per-cloth target buffer array for the copy kernel. + + if( vertexBuffer->getBufferType() == btVertexBufferDescriptor::CPU_BUFFER ) + { + const btAlignedObjectArray &clothVertices( softBody->m_nodes ); + int numVertices = clothVertices.size(); + + const btCPUVertexBufferDescriptor *cpuVertexBuffer = static_cast< btCPUVertexBufferDescriptor* >(vertexBuffer); + float *basePointer = cpuVertexBuffer->getBasePointer(); + + if( vertexBuffer->hasVertexPositions() ) + { + const int vertexOffset = cpuVertexBuffer->getVertexOffset(); + const int vertexStride = cpuVertexBuffer->getVertexStride(); + float *vertexPointer = basePointer + vertexOffset; + + for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) + { + btVector3 position = clothVertices[vertexIndex].m_x; + *(vertexPointer + 0) = position.getX(); + *(vertexPointer + 1) = position.getY(); + *(vertexPointer + 2) = position.getZ(); + vertexPointer += vertexStride; + } + } + if( vertexBuffer->hasNormals() ) + { + const int normalOffset = cpuVertexBuffer->getNormalOffset(); + const int normalStride = cpuVertexBuffer->getNormalStride(); + float *normalPointer = basePointer + normalOffset; + + for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) + { + btVector3 normal = clothVertices[vertexIndex].m_n; + *(normalPointer + 0) = normal.getX(); + *(normalPointer + 1) = normal.getY(); + *(normalPointer + 2) = normal.getZ(); + normalPointer += normalStride; + } + } + } +} // btDefaultSoftBodySolver::copySoftBodyToVertexBuffer + + +void btDefaultSoftBodySolver::predictMotion( float timeStep ) +{ + for ( int i=0; i < m_softBodySet.size(); ++i) + { + btSoftBody* psb = m_softBodySet[i]; + + psb->predictMotion(timeStep); + } +} + diff --git a/src/BulletSoftBody/btDefaultSoftBodySolver.h b/src/BulletSoftBody/btDefaultSoftBodySolver.h new file mode 100644 index 000000000..93c1ff0d3 --- /dev/null +++ b/src/BulletSoftBody/btDefaultSoftBodySolver.h @@ -0,0 +1,51 @@ +/* +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_BODY_DEFAULT_SOLVER_H +#define BT_SOFT_BODY_DEFAULT_SOLVER_H + + +#include "BulletSoftBody/btSoftBodySolvers.h" +#include "btSoftBodySolverVertexBuffer.h" + + +class btDefaultSoftBodySolver : public btSoftBodySolver +{ +protected: + /** Variable to define whether we need to update solver constants on the next iteration */ + bool m_updateSolverConstants; + + btAlignedObjectArray< btSoftBody * > m_softBodySet; + + +public: + btDefaultSoftBodySolver(); + + virtual ~btDefaultSoftBodySolver(); + + virtual bool checkInitialized(); + + virtual void updateSoftBodies( ); + + virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies ); + + virtual void solveConstraints( float solverdt ); + + virtual void predictMotion( float solverdt ); + + virtual void copySoftBodyToVertexBuffer( const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer ); +}; + +#endif // #ifndef BT_ACCELERATED_SOFT_BODY_CPU_SOLVER_H diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index f6b98ddf4..612f1df57 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -15,6 +15,7 @@ subject to the following restrictions: ///btSoftBody implementation by Nathanael Presson #include "btSoftBodyInternals.h" +#include "BulletSoftBody/btSoftBodySolvers.h" // btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m) @@ -88,6 +89,8 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btV updateBounds(); m_initialWorldTransform.setIdentity(); + + m_windVelocity = btVector3(0,0,0); } // @@ -606,6 +609,7 @@ void btSoftBody::rotate( const btQuaternion& rot) // void btSoftBody::scale(const btVector3& scl) { + const btScalar margin=getCollisionShape()->getMargin(); ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; @@ -1488,6 +1492,7 @@ void btSoftBody::setSolver(eSolverPresets::_ preset) // void btSoftBody::predictMotion(btScalar dt) { + int i,ni; /* Update */ @@ -1579,6 +1584,7 @@ void btSoftBody::predictMotion(btScalar dt) // void btSoftBody::solveConstraints() { + /* Apply clusters */ applyClusters(false); /* Prepare links */ @@ -1948,20 +1954,21 @@ bool btSoftBody::checkContact( btCollisionObject* colObj, btScalar margin, btSoftBody::sCti& cti) const { - btVector3 nrm; - btCollisionShape* shp=colObj->getCollisionShape(); - btRigidBody* tmpRigid = btRigidBody::upcast(colObj); - const btTransform& wtr=tmpRigid? tmpRigid->getInterpolationWorldTransform() : colObj->getWorldTransform(); - btScalar dst=m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), - shp, - nrm, - margin); + btVector3 nrm; + btCollisionShape *shp = colObj->getCollisionShape(); + btRigidBody *tmpRigid = btRigidBody::upcast(colObj); + const btTransform &wtr = tmpRigid ? tmpRigid->getInterpolationWorldTransform() : colObj->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(x), + shp, + nrm, + margin); if(dst<0) { - cti.m_colObj = colObj; - cti.m_normal = wtr.getBasis()*nrm; - cti.m_offset = -btDot( cti.m_normal, - x-cti.m_normal*dst); + cti.m_colObj = colObj; + cti.m_normal = wtr.getBasis()*nrm; + cti.m_offset = -btDot( cti.m_normal, x - cti.m_normal * dst ); return(true); } return(false); @@ -1970,6 +1977,7 @@ bool btSoftBody::checkContact( btCollisionObject* colObj, // void btSoftBody::updateNormals() { + const btVector3 zv(0,0,0); int i,ni; @@ -1998,29 +2006,42 @@ void btSoftBody::updateNormals() // void btSoftBody::updateBounds() { - if(m_ndbvt.m_root) + /*if( m_acceleratedSoftBody ) { - const btVector3& mins=m_ndbvt.m_root->volume.Mins(); - const btVector3& maxs=m_ndbvt.m_root->volume.Maxs(); - const btScalar csm=getCollisionShape()->getMargin(); - const btVector3 mrg=btVector3( csm, - csm, - csm)*1; // ??? to investigate... - m_bounds[0]=mins-mrg; - m_bounds[1]=maxs+mrg; - if(0!=getBroadphaseHandle()) - { - m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), - m_bounds[0], - m_bounds[1], - m_worldInfo->m_dispatcher); + // If we have an accelerated softbody we need to obtain the bounds correctly + // For now (slightly hackily) just have a very large AABB + // TODO: Write get bounds kernel + // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could + // probably do a test and exchange reasonably efficiently. + + m_bounds[0] = btVector3(-1000, -1000, -1000); + m_bounds[1] = btVector3(1000, 1000, 1000); + + } else {*/ + if(m_ndbvt.m_root) + { + const btVector3& mins=m_ndbvt.m_root->volume.Mins(); + const btVector3& maxs=m_ndbvt.m_root->volume.Maxs(); + const btScalar csm=getCollisionShape()->getMargin(); + const btVector3 mrg=btVector3( csm, + csm, + csm)*1; // ??? to investigate... + m_bounds[0]=mins-mrg; + m_bounds[1]=maxs+mrg; + if(0!=getBroadphaseHandle()) + { + m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); + } } - } - else - { - m_bounds[0]= - m_bounds[1]=btVector3(0,0,0); - } + else + { + m_bounds[0]= + m_bounds[1]=btVector3(0,0,0); + } + //} } @@ -2571,27 +2592,27 @@ void btSoftBody::applyForces() { BT_PROFILE("SoftBody applyForces"); - const btScalar dt=m_sst.sdt; - const btScalar kLF=m_cfg.kLF; - const btScalar kDG=m_cfg.kDG; - const btScalar kPR=m_cfg.kPR; - const btScalar kVC=m_cfg.kVC; - const bool as_lift=kLF>0; - const bool as_drag=kDG>0; - const bool as_pressure=kPR!=0; - const bool as_volume=kVC>0; - const bool as_aero= as_lift || - as_drag ; - const bool as_vaero= as_aero && - (m_cfg.aeromodel=btSoftBody::eAeroModel::F_TwoSided); - const bool use_medium= as_aero; - const bool use_volume= as_pressure || + const btScalar dt = m_sst.sdt; + const btScalar kLF = m_cfg.kLF; + const btScalar kDG = m_cfg.kDG; + const btScalar kPR = m_cfg.kPR; + const btScalar kVC = m_cfg.kVC; + const bool as_lift = kLF>0; + const bool as_drag = kDG>0; + const bool as_pressure = kPR!=0; + const bool as_volume = kVC>0; + const bool as_aero = as_lift || + as_drag ; + const bool as_vaero = as_aero && + (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided); + const bool as_faero = as_aero && + (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided); + const bool use_medium = as_aero; + const bool use_volume = as_pressure || as_volume ; - btScalar volume=0; - btScalar ivolumetp=0; - btScalar dvolumetv=0; + btScalar volume = 0; + btScalar ivolumetp = 0; + btScalar dvolumetv = 0; btSoftBody::sMedium medium; if(use_volume) { @@ -2609,36 +2630,41 @@ void btSoftBody::applyForces() { if(use_medium) { - EvaluateMedium(m_worldInfo,n.m_x,medium); + EvaluateMedium(m_worldInfo, n.m_x, medium); + medium.m_velocity = m_windVelocity; + medium.m_density = m_worldInfo->air_density; + /* Aerodynamics */ if(as_vaero) { - const btVector3 rel_v=n.m_v-medium.m_velocity; - const btScalar rel_v2=rel_v.length2(); + const btVector3 rel_v = n.m_v - medium.m_velocity; + const btScalar rel_v2 = rel_v.length2(); if(rel_v2>SIMD_EPSILON) { - btVector3 nrm=n.m_n; + btVector3 nrm = n.m_n; /* Setup normal */ switch(m_cfg.aeromodel) { case btSoftBody::eAeroModel::V_Point: - nrm=NormalizeAny(rel_v);break; + nrm = NormalizeAny(rel_v); + break; case btSoftBody::eAeroModel::V_TwoSided: - nrm*=(btScalar)(btDot(nrm,rel_v)<0?-1:+1);break; - default: + nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + break; + default: { } } - const btScalar dvn=btDot(rel_v,nrm); + const btScalar dvn = btDot(rel_v,nrm); /* Compute forces */ if(dvn>0) { btVector3 force(0,0,0); - const btScalar c0 = n.m_area*dvn*rel_v2/2; - const btScalar c1 = c0*medium.m_density; + const btScalar c0 = n.m_area * dvn * rel_v2/2; + const btScalar c1 = c0 * medium.m_density; force += nrm*(-c1*kLF); - force += rel_v.normalized()*(-c1*kDG); - ApplyClampedForce(n,force,dt); + force += rel_v.normalized() * (-c1 * kDG); + ApplyClampedForce(n, force, dt); } } } @@ -2716,26 +2742,27 @@ void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti) } // -void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti) +void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) { - const btScalar dt=psb->m_sst.sdt; - const btScalar mrg=psb->getCollisionShape()->getMargin(); + const btScalar dt = psb->m_sst.sdt; + const btScalar mrg = psb->getCollisionShape()->getMargin(); for(int i=0,ni=psb->m_rcontacts.size();im_rcontacts[i]; - const sCti& cti=c.m_cti; + const RContact& c = psb->m_rcontacts[i]; + const sCti& cti = c.m_cti; btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj); - const btVector3 va=tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); - const btVector3 vb=c.m_node->m_x-c.m_node->m_q; - const btVector3 vr=vb-va; - const btScalar dn=btDot(vr,cti.m_normal); + const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); + const btVector3 vb = c.m_node->m_x-c.m_node->m_q; + const btVector3 vr = vb-va; + const btScalar dn = btDot(vr, cti.m_normal); if(dn<=SIMD_EPSILON) { - const btScalar dp=btMin(btDot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg); - const btVector3 fv=vr-cti.m_normal*dn; - const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst); - c.m_node->m_x-=impulse*c.m_c2; + const btScalar dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg ); + const btVector3 fv = vr - (cti.m_normal * dn); + // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient + const btVector3 impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst ); + c.m_node->m_x -= impulse * c.m_c2; if (tmpRigid) tmpRigid->applyImpulse(impulse,c.m_c1); } @@ -2844,41 +2871,58 @@ btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) // void btSoftBody::defaultCollisionHandler(btCollisionObject* pco) { - switch(m_cfg.collisions&fCollision::RVSmask) +#if 0 + // If we have a solver, skip this work. + // It will have been done within the solver. + // TODO: In the case of the collision handler we need to ensure that we're + // updating the data structures correctly and in here we generate the + // collision lists to deal with in the solver + if( this->m_acceleratedSoftBody ) { - case fCollision::SDF_RS: + // We need to pass the collision data through to the solver collision engine + // Note that we only add the collision object here, we are not applying the collision or dealing with + // an impulse response. + m_acceleratedSoftBody->addCollisionObject(pco); + } else { +#endif + switch(m_cfg.collisions&fCollision::RVSmask) { - btSoftColliders::CollideSDF_RS docollide; - btRigidBody* prb1=btRigidBody::upcast(pco); - btTransform wtr=prb1 ? prb1->getInterpolationWorldTransform() : pco->getWorldTransform(); + case fCollision::SDF_RS: + { + btSoftColliders::CollideSDF_RS docollide; + btRigidBody* prb1=btRigidBody::upcast(pco); + btTransform wtr=prb1 ? prb1->getInterpolationWorldTransform() : pco->getWorldTransform(); - const btTransform ctr=pco->getWorldTransform(); - const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length(); - const btScalar basemargin=getCollisionShape()->getMargin(); - btVector3 mins; - btVector3 maxs; - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(), - mins, - maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(basemargin,basemargin,basemargin)); - docollide.psb = this; - docollide.m_colObj1 = pco; - docollide.m_rigidBody = prb1; + const btTransform ctr=pco->getWorldTransform(); + const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length(); + const btScalar basemargin=getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; + pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(), + mins, + maxs); + volume=btDbvtVolume::FromMM(mins,maxs); + volume.Expand(btVector3(basemargin,basemargin,basemargin)); + docollide.psb = this; + docollide.m_colObj1 = pco; + docollide.m_rigidBody = prb1; - docollide.dynmargin = basemargin+timemargin; - docollide.stamargin = basemargin; - m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide); + docollide.dynmargin = basemargin+timemargin; + docollide.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide); + } + break; + case fCollision::CL_RS: + { + btSoftColliders::CollideCL_RS collider; + collider.Process(this,pco); + } + break; } - break; - case fCollision::CL_RS: - { - btSoftColliders::CollideCL_RS collider; - collider.Process(this,pco); - } - break; +#if 0 } +#endif } // @@ -2929,3 +2973,16 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) } } } + + + +void btSoftBody::setWindVelocity( const btVector3 &velocity ) +{ + m_windVelocity = velocity; +} + + +const btVector3& btSoftBody::getWindVelocity() +{ + return m_windVelocity; +} diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index d69e835f1..4d975b7fb 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -30,6 +30,7 @@ subject to the following restrictions: class btBroadphaseInterface; class btDispatcher; + /* btSoftBodyWorldInfo */ struct btSoftBodyWorldInfo { @@ -589,7 +590,7 @@ public: }; // - // Typedef's + // Typedefs // typedef void (*psolver_t)(btSoftBody*,btScalar,btScalar); @@ -639,6 +640,7 @@ public: btTransform m_initialWorldTransform; + btVector3 m_windVelocity; // // Api // @@ -827,6 +829,24 @@ public: void defaultCollisionHandler(btCollisionObject* pco); void defaultCollisionHandler(btSoftBody* psb); + + + // + // Functionality to deal with new accelerated solvers. + // + + /** + * Set a wind velocity for interaction with the air. + */ + void setWindVelocity( const btVector3 &velocity ); + + + /** + * Return the wind velocity for interaction with the air. + */ + const btVector3& getWindVelocity(); + + // // Cast // diff --git a/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h b/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h new file mode 100644 index 000000000..c4733d640 --- /dev/null +++ b/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h @@ -0,0 +1,165 @@ +/* +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_BODY_SOLVER_VERTEX_BUFFER_H +#define BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H + + +class btVertexBufferDescriptor +{ +public: + enum BufferTypes + { + CPU_BUFFER, + DX11_BUFFER, + OPENGL_BUFFER + }; + +protected: + + bool m_hasVertexPositions; + bool m_hasNormals; + + int m_vertexOffset; + int m_vertexStride; + + int m_normalOffset; + int m_normalStride; + +public: + btVertexBufferDescriptor() + { + m_hasVertexPositions = false; + m_hasNormals = false; + m_vertexOffset = 0; + m_vertexStride = 0; + m_normalOffset = 0; + m_normalStride = 0; + } + + virtual ~btVertexBufferDescriptor() + { + + } + + virtual bool hasVertexPositions() const + { + return m_hasVertexPositions; + } + + virtual bool hasNormals() const + { + return m_hasNormals; + } + + /** + * Return the type of the vertex buffer descriptor. + */ + virtual BufferTypes getBufferType() const = 0; + + /** + * Return the vertex offset in floats from the base pointer. + */ + virtual int getVertexOffset() const + { + return m_vertexOffset; + } + + /** + * Return the vertex stride in number of floats between vertices. + */ + virtual int getVertexStride() const + { + return m_vertexStride; + } + + /** + * Return the vertex offset in floats from the base pointer. + */ + virtual int getNormalOffset() const + { + return m_normalOffset; + } + + /** + * Return the vertex stride in number of floats between vertices. + */ + virtual int getNormalStride() const + { + return m_normalStride; + } +}; + + +class btCPUVertexBufferDescriptor : public btVertexBufferDescriptor +{ +protected: + float *m_basePointer; + +public: + /** + * vertexBasePointer is pointer to beginning of the buffer. + * vertexOffset is the offset in floats to the first vertex. + * vertexStride is the stride in floats between vertices. + */ + btCPUVertexBufferDescriptor( float *basePointer, int vertexOffset, int vertexStride ) + { + m_basePointer = basePointer; + m_vertexOffset = vertexOffset; + m_vertexStride = vertexStride; + m_hasVertexPositions = true; + } + + /** + * vertexBasePointer is pointer to beginning of the buffer. + * vertexOffset is the offset in floats to the first vertex. + * vertexStride is the stride in floats between vertices. + */ + btCPUVertexBufferDescriptor( float *basePointer, int vertexOffset, int vertexStride, int normalOffset, int normalStride ) + { + m_basePointer = basePointer; + + m_vertexOffset = vertexOffset; + m_vertexStride = vertexStride; + m_hasVertexPositions = true; + + m_normalOffset = normalOffset; + m_normalStride = normalStride; + m_hasNormals = true; + } + + virtual ~btCPUVertexBufferDescriptor() + { + + } + + /** + * Return the type of the vertex buffer descriptor. + */ + virtual BufferTypes getBufferType() const + { + return CPU_BUFFER; + } + + /** + * Return the base pointer in memory to the first vertex. + */ + virtual float *getBasePointer() const + { + return m_basePointer; + } +}; + +#endif // #ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H diff --git a/src/BulletSoftBody/btSoftBodySolvers.h b/src/BulletSoftBody/btSoftBodySolvers.h new file mode 100644 index 000000000..24a742e49 --- /dev/null +++ b/src/BulletSoftBody/btSoftBodySolvers.h @@ -0,0 +1,143 @@ +/* +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_BODY_SOLVERS_H +#define BT_SOFT_BODY_SOLVERS_H + +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" + + +class btSoftBodyTriangleData; +class btSoftBodyLinkData; +class btSoftBodyVertexData; +class btVertexBufferDescriptor; +class btAcceleratedSoftBodyInterface; +class btCollisionObject; +class btSoftBody; + + +class btSoftBodySolver +{ + +protected: + int m_numberOfPositionIterations; + int m_numberOfVelocityIterations; + // Simulation timescale + float m_timeScale; + +public: + btSoftBodySolver() : + m_numberOfPositionIterations( 10 ), + m_timeScale( 1 ) + { + m_numberOfVelocityIterations = 0; + m_numberOfPositionIterations = 5; + } + + virtual ~btSoftBodySolver() + { + } + +#if 0 + /** Acceleration for all cloths in the solver. Can be used to efficiently apply gravity. */ + virtual void setPerClothAcceleration( int clothIdentifier, Vectormath::Aos::Vector3 acceleration ) = 0; + + /** A wind velocity applied normal to the cloth for all cloths in the solver. */ + virtual void setPerClothWindVelocity( int clothIdentifier, Vectormath::Aos::Vector3 windVelocity ) = 0; + + /** Set the density of the medium a given cloth is situated in. This could be air or possibly water. */ + virtual void setPerClothMediumDensity( int clothIdentifier, float mediumDensity ) = 0; + + /** A damping factor specific to each cloth applied for all cloths. */ + virtual void setPerClothDampingFactor( int clothIdentifier, float dampingFactor ) = 0; + + /** A damping factor specific to each cloth applied for all cloths. */ + virtual void setPerClothVelocityCorrectionCoefficient( int clothIdentifier, float velocityCorrectionCoefficient ) = 0; + + /** Lift parameter for wind action on cloth. */ + virtual void setPerClothLiftFactor( int clothIdentifier, float liftFactor ) = 0; + + /** Drag parameter for wind action on cloth. */ + virtual void setPerClothDragFactor( int clothIdentifier, float dragFactor ) = 0; + + /** + * Add a velocity to all soft bodies in the solver - useful for doing world-wide velocities such as a change due to gravity + * Only add a velocity to nodes with a non-zero inverse mass. + */ + virtual void addVelocity( Vectormath::Aos::Vector3 velocity ) = 0; +#endif + + + + /** Ensure that this solver is initialized. */ + virtual bool checkInitialized() = 0; + + /** Optimize soft bodies in this solver. */ + virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies ) = 0; + + /** Predict motion of soft bodies into next timestep */ + virtual void predictMotion( float solverdt ) = 0; + + /** Solve constraints for a set of soft bodies */ + virtual void solveConstraints( float solverdt ) = 0; + + /** Perform necessary per-step updates of soft bodies such as recomputing normals and bounding boxes */ + virtual void updateSoftBodies() = 0; + + /** Output current computed vertex data to the vertex buffers for all cloths in the solver. */ + virtual void copySoftBodyToVertexBuffer( const btSoftBody * const softBody, btVertexBufferDescriptor *vertexBuffer ) = 0; + + + + /** Set the number of velocity constraint solver iterations this solver uses. */ + virtual void setNumberOfPositionIterations( int iterations ) + { + m_numberOfPositionIterations = iterations; + } + + /** Get the number of velocity constraint solver iterations this solver uses. */ + virtual int getNumberOfPositionIterations() + { + return m_numberOfPositionIterations; + } + + /** Set the number of velocity constraint solver iterations this solver uses. */ + virtual void setNumberOfVelocityIterations( int iterations ) + { + m_numberOfVelocityIterations = iterations; + } + + /** Get the number of velocity constraint solver iterations this solver uses. */ + virtual int getNumberOfVelocityIterations() + { + return m_numberOfVelocityIterations; + } + + /** Return the timescale that the simulation is using */ + float getTimeScale() + { + return m_timeScale; + } + +#if 0 + /** + * Add a collision object to be used by the indicated softbody. + */ + virtual void addCollisionObjectForSoftBody( int clothIdentifier, btCollisionObject *collisionObject ) = 0; +#endif +}; + + +#endif // #ifndef BT_SOFT_BODY_SOLVERS_H diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index 9a3d040b5..0faf3a28c 100644 --- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -20,14 +20,28 @@ subject to the following restrictions: //softbody & helpers #include "btSoftBody.h" #include "btSoftBodyHelpers.h" +#include "btSoftBodySolvers.h" +#include "btDefaultSoftBodySolver.h" - - -btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) -:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) +btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld( + btDispatcher* dispatcher, + btBroadphaseInterface* pairCache, + btConstraintSolver* constraintSolver, + btCollisionConfiguration* collisionConfiguration, + btSoftBodySolver *softBodySolver ) : + btDiscreteDynamicsWorld(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; @@ -41,27 +55,34 @@ btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBr btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() { - + if (m_ownsSolver) + { + m_softBodySolver->~btSoftBodySolver(); + btAlignedFree(m_softBodySolver); + } } void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep); + btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep ); - for ( int i=0;ipredictMotion(timeStep); - } + m_softBodySolver->predictMotion( timeStep ); } -void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) +void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep ) { + if( !m_softBodySolver->checkInitialized() ) + { + btAssert( "Solver initialization failed\n" ); + } + + // Let the solver grab the soft bodies and if necessary optimize for it + m_softBodySolver->optimize( getSoftBodyArray() ); + btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); ///solve soft bodies constraints - solveSoftBodiesConstraints(); + solveSoftBodiesConstraints( timeStep ); //self collisions for ( int i=0;iupdateSoftBodies( ); + + // End solver-wise simulation step + // /////////////////////////////// } -void btSoftRigidDynamicsWorld::updateSoftBodies() -{ - BT_PROFILE("updateSoftBodies"); - - for ( int i=0;iintegrateMotion(); - } -} - -void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints() +void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep ) { BT_PROFILE("solveSoftConstraints"); @@ -95,11 +108,9 @@ void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints() btSoftBody::solveClusters(m_softBodies); } - for(int i=0;isolveConstraints(); - } + // Solve constraints solver-wise + m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() ); + } void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask) diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.h b/src/BulletSoftBody/btSoftRigidDynamicsWorld.h index f36a9b360..d2153f239 100644 --- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btSoftRigidDynamicsWorld.h @@ -21,6 +21,8 @@ subject to the following restrictions: typedef btAlignedObjectArray btSoftBodyArray; +class btSoftBodySolver; + class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld { @@ -30,6 +32,9 @@ class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld 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: @@ -37,14 +42,12 @@ protected: virtual void internalSingleStepSimulation( btScalar timeStep); - void updateSoftBodies(); - - void solveSoftBodiesConstraints(); + void solveSoftBodiesConstraints( btScalar timeStep ); public: - btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 ); virtual ~btSoftRigidDynamicsWorld(); @@ -92,6 +95,7 @@ public: const btTransform& colObjWorldTransform, RayResultCallback& resultCallback); + }; #endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H diff --git a/src/LinearMath/btAlignedObjectArray.h b/src/LinearMath/btAlignedObjectArray.h index 257317b21..d5716efc3 100644 --- a/src/LinearMath/btAlignedObjectArray.h +++ b/src/LinearMath/btAlignedObjectArray.h @@ -459,6 +459,13 @@ class btAlignedObjectArray m_capacity = capacity; } + void copyFromArray(const btAlignedObjectArray& otherArray) + { + int otherSize = otherArray.size(); + resize (otherSize); + otherArray.copy(0, otherSize, m_data); + } + }; #endif //BT_OBJECT_ARRAY__ diff --git a/src/MiniCL/cl_MiniCL_Defs.h b/src/MiniCL/cl_MiniCL_Defs.h index 285f48b64..ffdac1026 100644 --- a/src/MiniCL/cl_MiniCL_Defs.h +++ b/src/MiniCL/cl_MiniCL_Defs.h @@ -22,7 +22,7 @@ subject to the following restrictions: #define __kernel #define __global -#define __local static +#define __local #define get_global_id(a) __guid_arg #define get_local_id(a) ((__guid_arg) % gMiniCLNumOutstandingTasks) #define get_local_size(a) (gMiniCLNumOutstandingTasks)