From d4698cb3d5ffdc73366314b675956488e082a5e9 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Sun, 30 Mar 2008 23:08:06 +0000 Subject: [PATCH] Added SoftBody demo, contribution by Nathanael Presson. Will integrate into Bullet broadphase. Added very basic drawTriangle for btIDebugDraw, useful for basic softbody visualization. Added btGjkEpa2, contribution by Nathanael Presson. Improved version of EPA penetration depth computation, more suitable for multi-core/SPU (less memory usage). Note: btGjkEpa2 is not enabled by default currently. --- Demos/CMakeLists.txt | 2 +- Demos/Jamfile | 1 + Demos/OpenGL/GLDebugDrawer.cpp | 2 +- Demos/SoftDemo/CMakeLists.txt | 62 + Demos/SoftDemo/Jamfile | 3 + Demos/SoftDemo/SoftDemo.cpp | 1286 +++++++++++++++++ Demos/SoftDemo/SoftDemo.h | 116 ++ Demos/SoftDemo/main.cpp | 37 + src/BulletCollision/CMakeLists.txt | 2 + .../NarrowPhaseCollision/btGjkEpa2.cpp | 891 ++++++++++++ .../NarrowPhaseCollision/btGjkEpa2.h | 39 + src/BulletDynamics/SoftBody/btSoftBody.cpp | 1189 +++++++++++++++ src/BulletDynamics/SoftBody/btSoftBody.h | 356 +++++ .../SoftBody/btSoftBodyHelpers.cpp | 429 ++++++ src/BulletDynamics/SoftBody/btSparseSDF.h | 313 ++++ src/LinearMath/btIDebugDraw.h | 11 + 16 files changed, 4737 insertions(+), 2 deletions(-) create mode 100644 Demos/SoftDemo/CMakeLists.txt create mode 100644 Demos/SoftDemo/Jamfile create mode 100644 Demos/SoftDemo/SoftDemo.cpp create mode 100644 Demos/SoftDemo/SoftDemo.h create mode 100644 Demos/SoftDemo/main.cpp create mode 100644 src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp create mode 100644 src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h create mode 100644 src/BulletDynamics/SoftBody/btSoftBody.cpp create mode 100644 src/BulletDynamics/SoftBody/btSoftBody.h create mode 100644 src/BulletDynamics/SoftBody/btSoftBodyHelpers.cpp create mode 100644 src/BulletDynamics/SoftBody/btSparseSDF.h diff --git a/Demos/CMakeLists.txt b/Demos/CMakeLists.txt index faa2614fb..a271c17c8 100644 --- a/Demos/CMakeLists.txt +++ b/Demos/CMakeLists.txt @@ -1,2 +1,2 @@ -SUBDIRS( OpenGL AllBulletDemos ConvexDecompositionDemo Benchmarks HelloWorld MultiThreadedDemo CcdPhysicsDemo ConstraintDemo GenericJointDemo RagdollDemo BasicDemo BspDemo MovingConcaveDemo VehicleDemo ColladaDemo UserCollisionAlgorithm CharacterDemo ) +SUBDIRS( OpenGL AllBulletDemos ConvexDecompositionDemo Benchmarks HelloWorld MultiThreadedDemo CcdPhysicsDemo ConstraintDemo GenericJointDemo RagdollDemo BasicDemo BspDemo MovingConcaveDemo VehicleDemo ColladaDemo UserCollisionAlgorithm CharacterDemo SoftBodyDemo ) diff --git a/Demos/Jamfile b/Demos/Jamfile index 4111b5d3d..84b8eccf7 100644 --- a/Demos/Jamfile +++ b/Demos/Jamfile @@ -82,6 +82,7 @@ SubInclude TOP Demos ConcaveDemo ; SubInclude TOP Demos ConstraintDemo ; SubInclude TOP Demos RagdollDemo ; SubInclude TOP Demos GenericJointDemo ; +SubInclude TOP Demos SoftBodyDemo ; SubInclude TOP Demos ContinuousConvexCollision ; SubInclude TOP Demos GjkConvexCastDemo ; SubInclude TOP Demos Raytracer ; diff --git a/Demos/OpenGL/GLDebugDrawer.cpp b/Demos/OpenGL/GLDebugDrawer.cpp index 3995a6e75..7e939ba95 100644 --- a/Demos/OpenGL/GLDebugDrawer.cpp +++ b/Demos/OpenGL/GLDebugDrawer.cpp @@ -24,7 +24,7 @@ GLDebugDrawer::GLDebugDrawer() } void GLDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& color) { - if (m_debugMode > 0) +// if (m_debugMode > 0) { glBegin(GL_LINES); glColor3f(color.getX(), color.getY(), color.getZ()); diff --git a/Demos/SoftDemo/CMakeLists.txt b/Demos/SoftDemo/CMakeLists.txt new file mode 100644 index 000000000..9f911f05e --- /dev/null +++ b/Demos/SoftDemo/CMakeLists.txt @@ -0,0 +1,62 @@ +# This is basically the overall name of the project in Visual Studio this is the name of the Solution File + + +# For every executable you have with a main method you should have an add_executable line below. +# For every add executable line you should list every .cpp and .h file you have associated with that executable. + + +# This is the variable for Windows. I use this to define the root of my directory structure. +SET(GLUT_ROOT ${BULLET_PHYSICS_SOURCE_DIR}/Glut) + +# You shouldn't have to modify anything below this line +######################################################## + + +# This is the shortcut to finding GLU, GLUT and OpenGL if they are properly installed on your system +# This should be the case. +INCLUDE (${CMAKE_ROOT}/Modules/FindGLU.cmake) +INCLUDE (${CMAKE_ROOT}/Modules/FindGLUT.cmake) +INCLUDE (${CMAKE_ROOT}/Modules/FindOpenGL.cmake) + + +IF (WIN32) + # This is the Windows code for which Opengl, and Glut are not properly installed + # since I can't install them I must cheat and copy libraries around + INCLUDE_DIRECTORIES(${GLUT_ROOT}) + # LINK_DIRECTORIES(${GLUT_ROOT}\\lib) + IF (${GLUT_glut_LIBRARY} MATCHES "GLUT_glut_LIBRARY-NOTFOUND") + SET(GLUT_glut_LIBRARY ${BULLET_PHYSICS_SOURCE_DIR}/Glut/glut32.lib) + # LINK_LIBRARIES(${GLUT_ROOT}\\lib\\glut32 ${OPENGL_gl_LIBRARY} ${OPENGL_glU_LIBRARY}) + # TARGET_LINK_LIBRARIES(table ${GLUT_ROOT}\\lib\\glut32) +# +# ADD_CUSTOM_COMMAND(TARGET table POST_BUILD COMMAND copy ${GLUT_ROOT}\\lib\\glut32.dll ${GLUT_ROOT}\\bin\\vs2005\\Debug +# COMMAND copy ${GLUT_ROOT}\\lib\\glut32.dll ${GLUT_ROOT}\\bin\\vs2003\\Debug +# COMMAND copy ${GLUT_ROOT}\\lib\\glut32.dll ${GLUT_ROOT}\\bin\\vs6\\Debug) + ELSE (${GLUT_glut_LIBRARY} MATCHES "GLUT_glut_LIBRARY-NOTFOUND") +# LINK_LIBRARIES(${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glU_LIBRARY}) +# TARGET_LINK_LIBRARIES(table ${GLUT_glut_LIBRARY}) + ENDIF(${GLUT_glut_LIBRARY} MATCHES "GLUT_glut_LIBRARY-NOTFOUND") +# TARGET_LINK_LIBRARIES(table ${OPENGL_gl_LIBRARY}) +# TARGET_LINK_LIBRARIES(table ${OPENGL_glu_LIBRARY}) +ELSE (WIN32) + # This is the lines for linux. This should always work if everything is installed and working fine. +# SET(CMAKE_BUILD_TYPE Debug) +# SET(CMAKE_CXX_FLAGS_DEBUG "-g") + INCLUDE_DIRECTORIES(/usr/include /usr/local/include ${GLUT_INCLUDE_DIR}) +# TARGET_LINK_LIBRARIES(table ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glU_LIBRARY}) +# TARGET_LINK_LIBRARIES(checker ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glU_LIBRARY}) +ENDIF (WIN32) + +INCLUDE_DIRECTORIES( +${BULLET_PHYSICS_SOURCE_DIR}/src ${BULLET_PHYSICS_SOURCE_DIR}/Demos/OpenGL } +) + +LINK_LIBRARIES( +LibOpenGLSupport LibConvexHull LibBulletDynamics LibBulletCollision LibLinearMath ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} +) + +ADD_EXECUTABLE(SoftBodyDemo + main.cpp + SoftDemo.cpp +) + diff --git a/Demos/SoftDemo/Jamfile b/Demos/SoftDemo/Jamfile new file mode 100644 index 000000000..ec446deca --- /dev/null +++ b/Demos/SoftDemo/Jamfile @@ -0,0 +1,3 @@ +SubDir TOP Demos SoftBodyDemo ; + +BulletDemo SoftBodyDemo : [ Wildcard *.h *.cpp ] ; diff --git a/Demos/SoftDemo/SoftDemo.cpp b/Demos/SoftDemo/SoftDemo.cpp new file mode 100644 index 000000000..3d8234c69 --- /dev/null +++ b/Demos/SoftDemo/SoftDemo.cpp @@ -0,0 +1,1286 @@ +/* +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. +*/ + +///btSoftBody implementation by Nathanael Presson + +#define DO_WALL 1 + +#include "btBulletDynamicsCommon.h" +#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" +#include "LinearMath/btQuickprof.h" +#include "LinearMath/btIDebugDraw.h" +#include "BMF_Api.h" +#include "demos\gimpacttestdemo\bunnymesh.h" +#include "demos\gimpacttestdemo\torusmesh.h" +#include //printf debugging + +static float gCollisionMargin = 0.05f/*0.05f*/; +#include "SoftDemo.h" +#include "GL_ShapeDrawer.h" + +#include "GlutStuff.h" + +btTransform comOffset; +btVector3 comOffsetVec(0,2,0); +extern float eye[3]; +extern int glutScreenWidth; +extern int glutScreenHeight; + +const int maxProxies = 32766; +const int maxOverlap = 65535; + +bool createConstraint = true;//false; +#ifdef CENTER_OF_MASS_SHIFT +bool useCompound = true; +#else +bool useCompound = false; +#endif + +#ifdef _DEBUG +const int gNumObjects = 1; +#else +const int gNumObjects = 1;//try this in release mode: 3000. never go above 16384, unless you increate maxNumObjects value in DemoApplication.cp +#endif + +const int maxNumObjects = 32760; +int shapeIndex[maxNumObjects]; +#define CUBE_HALF_EXTENTS 1.5 +#define EXTRA_HEIGHT -10.f + +// +void SoftDemo::createStack( btCollisionShape* boxShape, float halfCubeSize, int size, float zPos ) +{ + btTransform trans; + trans.setIdentity(); + + for(int i=0; im_frictionSolverType = USER_CONTACT_SOLVER_TYPE1; +#endif //USER_DEFINED_FRICTION_MODEL + + } + } +} + + +//////////////////////////////////// + + + +//by default, Bullet will use its own nearcallback, but you can override it using dispatcher->setNearCallback() +void customNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, btDispatcherInfo& dispatchInfo) +{ + btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; + + if (dispatcher.needsCollision(colObj0,colObj1)) + { + //dispatcher will keep algorithms persistent in the collision pair + if (!collisionPair.m_algorithm) + { + collisionPair.m_algorithm = dispatcher.findAlgorithm(colObj0,colObj1); + } + + if (collisionPair.m_algorithm) + { + btManifoldResult contactPointResult(colObj0,colObj1); + + if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) + { + //discrete collision detection query + collisionPair.m_algorithm->processCollision(colObj0,colObj1,dispatchInfo,&contactPointResult); + } else + { + //continuous collision detection query, time of impact (toi) + float toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; + + } + } + } + +} + +// +// ISoftBody implementation +// + +// +void SoftDemo::SoftBodyImpl::Attach(btSoftBody*) +{} + +// +void SoftDemo::SoftBodyImpl::Detach(btSoftBody*) +{} + +// +void SoftDemo::SoftBodyImpl::StartCollide(const btVector3&,const btVector3&) +{} + +// +bool SoftDemo::SoftBodyImpl::CheckContactPrecise(const btVector3& x, + btSoftBody::ISoftBody::sCti& cti) +{ +btScalar maxdepth=0; +btGjkEpaSolver2::sResults res; +btDynamicsWorld* pdw=pdemo->m_dynamicsWorld; +btCollisionObjectArray& coa=pdw->getCollisionObjectArray(); +for(int i=0,ni=coa.size();igetCollisionShape(); + if(shp->isConvex()) + { + btConvexShape* csh=static_cast(shp); + const btTransform& wtr=prb->getWorldTransform(); + const btScalar dst=btGjkEpaSolver2::SignedDistance(x,0.1,csh,wtr,res); + if(dstm_dynamicsWorld; +btCollisionObjectArray& coa=pdw->getCollisionObjectArray(); +for(int i=0,ni=coa.size();igetCollisionShape(); + btConvexShape* csh=static_cast(shp); + const btTransform& wtr=prb->getWorldTransform(); + btScalar dst=pdemo->m_sparsesdf.Evaluate(wtr.invXform(x),csh,nrm); + nrm=wtr.getBasis()*nrm; + btVector3 wit=x-nrm*dst; + if(dst0) + { + const btScalar depth=-(dot(x,water_normal)+water_offset); + if(depth>0) + { + medium.m_density = water_density; + medium.m_pressure = depth * + water_density * + pdemo->m_dynamicsWorld->getGravity().length(); + } + } +} + + + +extern int gNumManifold; +extern int gOverlappingPairs; +extern int gTotalContactPoints; + +void SoftDemo::clientMoveAndDisplay() +{ + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + +#ifdef USE_KINEMATIC_GROUND + //btQuaternion kinRotation(btVector3(0,0,1),0.); + btVector3 kinTranslation(-0.01,0,0); + //kinematic object + btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[0]; + //is this a rigidbody with a motionstate? then use the motionstate to update positions! + if (btRigidBody::upcast(colObj) && btRigidBody::upcast(colObj)->getMotionState()) + { + btTransform newTrans; + btRigidBody::upcast(colObj)->getMotionState()->getWorldTransform(newTrans); + newTrans.getOrigin()+=kinTranslation; + btRigidBody::upcast(colObj)->getMotionState()->setWorldTransform(newTrans); + } else + { + m_dynamicsWorld->getCollisionObjectArray()[0]->getWorldTransform().getOrigin() += kinTranslation; + } + +#endif //USE_KINEMATIC_GROUND + + + float dt = getDeltaTimeMicroseconds() * 0.000001f; + +// printf("dt = %f: ",dt); + + + if (m_dynamicsWorld) + { +#define FIXED_STEP 0 +#ifdef FIXED_STEP + m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0); + +#else + //during idle mode, just run 1 simulation step maximum + int maxSimSubSteps = m_idle ? 1 : 1; + if (m_idle) + dt = 1.0/420.f; + + int numSimSteps = 0; + numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps); + +#ifdef VERBOSE_TIMESTEPPING_CONSOLEOUTPUT + if (!numSimSteps) + printf("Interpolated transforms\n"); + else + { + if (numSimSteps > maxSimSubSteps) + { + //detect dropping frames + printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps); + } else + { + printf("Simulated (%i) steps\n",numSimSteps); + } + } +#endif //VERBOSE_TIMESTEPPING_CONSOLEOUTPUT + +#endif + + /* soft bodies */ + for(int ib=0;ibAddVelocity(m_dynamicsWorld->getGravity()*dt); + psb->Step(dt); + } + m_sparsesdf.GarbageCollect(); + + //optional but useful: debug drawing + + m_dynamicsWorld->debugDrawWorld(); + } + +#ifdef USE_QUICKPROF + btProfiler::beginBlock("render"); +#endif //USE_QUICKPROF + + renderme(); + + //render the graphics objects, with center of mass shift + + updateCamera(); + + + +#ifdef USE_QUICKPROF + btProfiler::endBlock("render"); +#endif + glFlush(); + //some additional debugging info +#ifdef PRINT_CONTACT_STATISTICS + printf("num manifolds: %i\n",gNumManifold); + printf("num gOverlappingPairs: %i\n",gOverlappingPairs); + printf("num gTotalContactPoints : %i\n",gTotalContactPoints ); +#endif //PRINT_CONTACT_STATISTICS + + gTotalContactPoints = 0; + glutSwapBuffers(); + +} + + + +void SoftDemo::displayCallback(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + + renderme(); + + glFlush(); + glutSwapBuffers(); +} + + + + + +///User-defined friction model, the most simple friction model available: no friction +float myFrictionModel( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo ) +{ + //don't do any friction + return 0.f; +} + +// +// Random +// + +static inline btScalar UnitRand() +{ +return(rand()/(btScalar)RAND_MAX); +} + +static inline btScalar SignedUnitRand() +{ +return(UnitRand()*2-1); +} + +static inline btVector3 Vector3Rand() +{ +const btVector3 p=btVector3(SignedUnitRand(),SignedUnitRand(),SignedUnitRand()); +return(p.normalized()); +} + +// +// Rb rain +// +static void Ctor_RbUpStack(SoftDemo* pdemo,int count) +{ +float mass=10; +btCollisionShape* shape[]={ new btSphereShape(1.5), + new btBoxShape(btVector3(1,1,1)), + new btCylinderShapeX(btVector3(4,1,1))}; +static const int nshapes=sizeof(shape)/sizeof(shape[0]); +for(int i=0;ilocalCreateRigidBody(mass,startTransform,shape[i%nshapes]); + } +} + +// +// Big ball +// +static void Ctor_BigBall(SoftDemo* pdemo,btScalar mass=10) +{ +btTransform startTransform; +startTransform.setIdentity(); +startTransform.setOrigin(btVector3(0,13,0)); +btRigidBody* body=pdemo->localCreateRigidBody(mass,startTransform,new btSphereShape(3)); +} + +// +// Big plate +// +static void Ctor_BigPlate(SoftDemo* pdemo,btScalar mass=15) +{ +btTransform startTransform; +startTransform.setIdentity(); +startTransform.setOrigin(btVector3(0,4,0.5)); +btRigidBody* body=pdemo->localCreateRigidBody(mass,startTransform,new btBoxShape(btVector3(5,1,5))); +body->setFriction(1); +} + +// +// Linear stair +// +static void Ctor_LinearStair(SoftDemo* pdemo,const btVector3& org,const btVector3& sizes,btScalar angle,int count) +{ +btBoxShape* shape=new btBoxShape(sizes); +for(int i=0;ilocalCreateRigidBody(0,startTransform,shape); + body->setFriction(1); + } +} + +// +// Softbox +// +static btSoftBody* Ctor_SoftBox(SoftDemo* pdemo,const btVector3& p,const btVector3& s) +{ +const btVector3 h=s*0.5; +const btVector3 c[]={ p+h*btVector3(-1,-1,-1), + p+h*btVector3(+1,-1,-1), + p+h*btVector3(-1,+1,-1), + p+h*btVector3(+1,+1,-1), + p+h*btVector3(-1,-1,+1), + p+h*btVector3(+1,-1,+1), + p+h*btVector3(-1,+1,+1), + p+h*btVector3(+1,+1,+1)}; +btSoftBody* psb=CreateFromConvexHull(&pdemo->m_softbodyimpl,c,8); +psb->GenerateBendingConstraints(2,1); +pdemo->m_softbodies.push_back(psb); +return(psb); +} + +// +// SoftBoulder +// +static btSoftBody* Ctor_SoftBoulder(SoftDemo* pdemo,const btVector3& p,const btVector3& s,int np,int id) +{ +btAlignedObjectArray pts; +if(id) srand(id); +for(int i=0;im_softbodyimpl,&pts[0],pts.size()); +psb->GenerateBendingConstraints(2,1); +pdemo->m_softbodies.push_back(psb); +return(psb); +} + +//#define TRACEDEMO { pdemo->demoname=__FUNCTION__+5;printf("Launching demo: " __FUNCTION__ "\r\n"); } + +// +// Basic ropes +// +static void Init_Ropes(SoftDemo* pdemo) +{ +//TRACEDEMO +const int n=15; +for(int i=0;im_softbodyimpl, + btVector3(-10,0,i*0.25), + btVector3(10,0,i*0.25), + 16, + 1+2); + psb->m_cfg.iterations = 4; + psb->m_cfg.kLST = 0.1+(i/(btScalar)(n-1))*0.9; + psb->SetTotalMass(20); + pdemo->m_softbodies.push_back(psb); + } +} + +// +// Rope attach +// +static void Init_RopeAttach(SoftDemo* pdemo) +{ +//TRACEDEMO +struct Functors + { + static btSoftBody* CtorRope(SoftDemo* pdemo,const btVector3& p) + { + btSoftBody* psb=CreateRope(&pdemo->m_softbodyimpl, + p,p+btVector3(10,0,0),8,1); + psb->m_cfg.kDF = 0; + psb->SetTotalMass(50); + pdemo->m_softbodies.push_back(psb); + return(psb); + } + }; +btTransform startTransform; +startTransform.setIdentity(); +startTransform.setOrigin(btVector3(12,8,0)); +btRigidBody* body=pdemo->localCreateRigidBody(50,startTransform,new btBoxShape(btVector3(2,6,2))); +btSoftBody* psb0=Functors::CtorRope(pdemo,btVector3(0,8,-1)); +btSoftBody* psb1=Functors::CtorRope(pdemo,btVector3(0,8,+1)); +psb0->AppendAnchor(psb0->m_nodes.size()-1,body); +psb1->AppendAnchor(psb1->m_nodes.size()-1,body); +} + +// +// Cloth attach +// +static void Init_ClothAttach(SoftDemo* pdemo) +{ +//TRACEDEMO +const btScalar s=4; +const btScalar h=6; +const int r=9; +btSoftBody* psb=CreatePatch(&pdemo->m_softbodyimpl, + btVector3(-s,h,-s), + btVector3(+s,h,-s), + btVector3(-s,h,+s), + btVector3(+s,h,+s),r,r,4+8,true); +pdemo->m_softbodies.push_back(psb); +btTransform startTransform; +startTransform.setIdentity(); +startTransform.setOrigin(btVector3(0,h,-(s+3.5))); +btRigidBody* body=pdemo->localCreateRigidBody(20,startTransform,new btBoxShape(btVector3(s,1,3))); +psb->AppendAnchor(0,body); +psb->AppendAnchor(r-1,body); +} + +// +// Impact +// +static void Init_Impact(SoftDemo* pdemo) +{ +//TRACEDEMO +btSoftBody* psb=CreateRope( &pdemo->m_softbodyimpl, + btVector3(0,0,0), + btVector3(0,-1,0), + 0, + 1); +pdemo->m_softbodies.push_back(psb); +btTransform startTransform; +startTransform.setIdentity(); +startTransform.setOrigin(btVector3(0,20,0)); +btRigidBody* body=pdemo->localCreateRigidBody(10,startTransform,new btBoxShape(btVector3(2,2,2))); +} + +// +// Aerodynamic forces, 50x1g flyers +// +static void Init_Aero(SoftDemo* pdemo) +{ +//TRACEDEMO +const btScalar s=2; +const btScalar h=10; +const int segments=6; +const int count=50; +for(int i=0;im_softbodyimpl, + btVector3(-s,h,-s), + btVector3(+s,h,-s), + btVector3(-s,h,+s), + btVector3(+s,h,+s), + segments,segments, + 0,true); + psb->GenerateBendingConstraints(2,1); + psb->m_cfg.kLF = 0.004; + psb->m_cfg.kDG = 0.0003; + psb->m_cfg.aeromodel = btSoftBody::eAeroModel::V_TwoSided; + btTransform trs; + btQuaternion rot; + btVector3 ra=Vector3Rand()*0.1; + btVector3 rp=Vector3Rand()*15+btVector3(0,20,80); + rot.setEuler(SIMD_PI/8+ra.x(),-SIMD_PI/7+ra.y(),ra.z()); + trs.setIdentity(); + trs.setOrigin(rp); + trs.setRotation(rot); + psb->Transform(trs); + psb->SetTotalMass(0.1); + psb->AddForce(btVector3(0,2,0),0); + pdemo->m_softbodies.push_back(psb); + } +pdemo->m_autocam=true; +} + +// +// Friction +// +static void Init_Friction(SoftDemo* pdemo) +{ +//TRACEDEMO +const btScalar bs=2; +const btScalar ts=bs+bs/4; +for(int i=0,ni=20;im_cfg.kDF = 0.1 * ((i+1)/(btScalar)ni); + psb->AddVelocity(btVector3(0,0,-10)); + } +} + +// +// Pressure +// +static void Init_Pressure(SoftDemo* pdemo) +{ +//TRACEDEMO +btSoftBody* psb=CreateEllipsoid(&pdemo->m_softbodyimpl, + btVector3(35,25,0), + btVector3(1,1,1)*3, + 512); +psb->m_cfg.kLST = 0.1; +psb->m_cfg.kDF = 1; +psb->m_cfg.kPR = 2500; +psb->SetTotalMass(30,true); +pdemo->m_softbodies.push_back(psb); +Ctor_BigPlate(pdemo); +Ctor_LinearStair(pdemo,btVector3(0,0,0),btVector3(2,1,5),0,10); +pdemo->m_autocam=true; +} + +// +// Volume conservation +// +static void Init_Volume(SoftDemo* pdemo) +{ +//TRACEDEMO +btSoftBody* psb=CreateEllipsoid(&pdemo->m_softbodyimpl, + btVector3(35,25,0), + btVector3(1,1,1)*3, + 512); +psb->m_cfg.kLST = 0.45; +psb->m_cfg.kVC = 20; +psb->SetTotalMass(50,true); +psb->SetPose(true,false); +pdemo->m_softbodies.push_back(psb); +Ctor_BigPlate(pdemo); +Ctor_LinearStair(pdemo,btVector3(0,0,0),btVector3(2,1,5),0,10); +pdemo->m_autocam=true; +} + +// +// Stick+Bending+Rb's +// +static void Init_Sticks(SoftDemo* pdemo) +{ +//TRACEDEMO +const int n=16; +const int sg=4; +const btScalar sz=5; +const btScalar hg=4; +const btScalar in=1/(btScalar)(n-1); +for(int y=0;ym_softbodyimpl, + org, + org+btVector3(hg*0.001,hg,0), + sg, + 1); + psb->m_cfg.iterations = 1; + psb->m_cfg.kDP = 0.005; + psb->m_cfg.kCHR = 0.1; + for(int i=0;i<3;++i) + { + psb->GenerateBendingConstraints(2+i,1); + } + psb->SetMass(1,0); + psb->SetTotalMass(0.01); + pdemo->m_softbodies.push_back(psb); + } + } +Ctor_BigBall(pdemo); +} + +// +// 100kg cloth locked at corners, 10 falling 10kg rb's. +// +static void Init_Cloth(SoftDemo* pdemo) +{ +//TRACEDEMO +const btScalar s=8; +btSoftBody* psb=CreatePatch(&pdemo->m_softbodyimpl, + btVector3(-s,0,-s), + btVector3(+s,0,-s), + btVector3(-s,0,+s), + btVector3(+s,0,+s), + 31,31, + 1+2+4+8,true); +psb->GenerateBendingConstraints(2,1); +psb->m_cfg.kLST = 0.4; +psb->SetTotalMass(150); +pdemo->m_softbodies.push_back(psb); +Ctor_RbUpStack(pdemo,10); +} + +// +// 100kg Stanford's bunny +// +static void Init_Bunny(SoftDemo* pdemo) +{ +//TRACEDEMO +btSoftBody* psb=CreateFromTriMesh( &pdemo->m_softbodyimpl, + gVerticesBunny, + &gIndicesBunny[0][0], + BUNNY_NUM_TRIANGLES); +psb->GenerateBendingConstraints(2,0.5); +psb->m_cfg.iterations = 2; +psb->m_cfg.kDF = 0.5; +psb->RandomizeConstraints(); +psb->Scale(btVector3(6,6,6)); +psb->SetTotalMass(100,true); +pdemo->m_softbodies.push_back(psb); +} + +// +// 100kg Stanford's bunny with pose matching +// +static void Init_BunnyMatch(SoftDemo* pdemo) +{ +//TRACEDEMO +btSoftBody* psb=CreateFromTriMesh( &pdemo->m_softbodyimpl, + gVerticesBunny, + &gIndicesBunny[0][0], + BUNNY_NUM_TRIANGLES); +psb->GenerateBendingConstraints(2,0.5); +psb->m_cfg.kDF = 0.5; +psb->m_cfg.kLST = 0.1; +psb->m_cfg.kMT = 0.01; +psb->RandomizeConstraints(); +psb->Scale(btVector3(6,6,6)); +psb->SetTotalMass(100,true); +psb->SetPose(true,true); +pdemo->m_softbodies.push_back(psb); +} + +// +// 50Kg Torus +// +static void Init_Torus(SoftDemo* pdemo) +{ +//TRACEDEMO +btSoftBody* psb=CreateFromTriMesh( &pdemo->m_softbodyimpl, + gVertices, + &gIndices[0][0], + NUM_TRIANGLES); +psb->GenerateBendingConstraints(2,1); +psb->m_cfg.iterations=2; +psb->RandomizeConstraints(); +btMatrix3x3 m; +m.setEulerZYX(SIMD_PI/2,0,0); +psb->Transform(btTransform(m,btVector3(0,4,0))); +psb->Scale(btVector3(2,2,2)); +psb->SetTotalMass(50,true); +pdemo->m_softbodies.push_back(psb); +} + +// +// 50Kg Torus with pose matching +// +static void Init_TorusMatch(SoftDemo* pdemo) +{ +//TRACEDEMO +btSoftBody* psb=CreateFromTriMesh( &pdemo->m_softbodyimpl, + gVertices, + &gIndices[0][0], + NUM_TRIANGLES); +psb->GenerateBendingConstraints(2,1); +psb->m_cfg.kLST=0.1; +psb->m_cfg.kMT=0.05; +psb->RandomizeConstraints(); +btMatrix3x3 m; +m.setEulerZYX(SIMD_PI/2,0,0); +psb->Transform(btTransform(m,btVector3(0,4,0))); +psb->Scale(btVector3(2,2,2)); +psb->SetTotalMass(50,true); +psb->SetPose(true,true); +pdemo->m_softbodies.push_back(psb); +} + +static unsigned current_demo=0; + +void SoftDemo::clientResetScene() +{ +DemoApplication::clientResetScene(); +/* Clean up */ +for(int i=m_dynamicsWorld->getNumCollisionObjects()-1;i>0;i--) + { + btCollisionObject* obj=m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body=btRigidBody::upcast(obj); + if(body&&body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } +for(int i=0;igetDebugDrawer(); +/* Bodies */ +btVector3 ps(0,0,0); +int nps=0; +for(int ib=0;ibm_nodes.size(); + for(int i=0;im_nodes.size();++i) + { + ps+=psb->m_nodes[i].m_x; + } + DrawFrame(psb,idraw); + Draw(psb,idraw,fDrawFlags::Std); + } +ps/=nps; +if(m_autocam) + m_cameraTargetPosition+=(ps-m_cameraTargetPosition)*0.01; + else + m_cameraTargetPosition=btVector3(0,0,0); +/* Water level */ +static const btVector3 axis[]={btVector3(1,0,0), + btVector3(0,1,0), + btVector3(0,0,1)}; +if(m_softbodyimpl.water_density>0) + { + const btVector3 c= btVector3((btScalar)0.25,(btScalar)0.25,1); + const btScalar a= (btScalar)0.5; + const btVector3 n= m_softbodyimpl.water_normal; + const btVector3 o= -n*m_softbodyimpl.water_offset; + const btVector3 x= cross(n,axis[n.minAxis()]).normalized(); + const btVector3 y= cross(x,n).normalized(); + const btScalar s= 25; + idraw->drawTriangle(o-x*s-y*s,o+x*s-y*s,o+x*s+y*s,c,a); + idraw->drawTriangle(o-x*s-y*s,o+x*s+y*s,o-x*s+y*s,c,a); + } +} + +void SoftDemo::keyboardCallback(unsigned char key, int x, int y) +{ +switch(key) + { + case ']': ++current_demo;clientResetScene();break; + case '[': --current_demo;clientResetScene();break; + case ';': m_autocam=!m_autocam;break; + default: DemoApplication::keyboardCallback(key,x,y); + } +} + + +void SoftDemo::initPhysics() +{ +#ifdef USE_PARALLEL_DISPATCHER +#ifdef WIN32 + m_threadSupportSolver = 0; + m_threadSupportCollision = 0; +#endif // +#endif + +//#define USE_GROUND_PLANE 1 +#ifdef USE_GROUND_PLANE + m_collisionShapes.push_back(new btStaticPlaneShape(btVector3(0,1,0),0.5)); +#else + + ///Please don't make the box sizes larger then 1000: the collision detection will be inaccurate. + ///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=346 + m_collisionShapes.push_back(new btBoxShape (btVector3(200,CUBE_HALF_EXTENTS,200))); + //m_collisionShapes.push_back(new btCylinderShapeZ (btVector3(5,5,5))); + //m_collisionShapes.push_back(new btSphereShape(5)); +#endif + +#ifdef DO_BENCHMARK_PYRAMIDS + m_collisionShapes.push_back(new btBoxShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS))); +#else + m_collisionShapes.push_back(new btCylinderShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS))); +#endif + + + +#ifdef DO_BENCHMARK_PYRAMIDS + setCameraDistance(32.5f); +#endif + +#ifdef DO_BENCHMARK_PYRAMIDS + m_azi = 90.f; +#endif //DO_BENCHMARK_PYRAMIDS + + m_dispatcher=0; + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + +#ifdef USE_PARALLEL_DISPATCHER +int maxNumOutstandingTasks = 4; + +#ifdef USE_WIN32_THREADING + + m_threadSupportCollision = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo( + "collision", + processCollisionTask, + createCollisionLocalStoreMemory, + maxNumOutstandingTasks)); +#else + +#ifdef USE_LIBSPE2 + + spe_program_handle_t * program_handle; +#ifndef USE_CESOF + program_handle = spe_image_open ("./spuCollision.elf"); + if (program_handle == NULL) + { + perror( "SPU OPEN IMAGE ERROR\n"); + } + else + { + printf( "IMAGE OPENED\n"); + } +#else + extern spe_program_handle_t spu_program; + program_handle = &spu_program; +#endif + SpuLibspe2Support* threadSupportCollision = new SpuLibspe2Support( program_handle, maxNumOutstandingTasks); +#endif //USE_LIBSPE2 + +///Playstation 3 SPU (SPURS) version is available through PS3 Devnet +/// For Unix/Mac someone could implement a pthreads version of btThreadSupportInterface? +///you can hook it up to your custom task scheduler by deriving from btThreadSupportInterface +#endif + + + m_dispatcher = new SpuGatheringCollisionDispatcher(m_threadSupportCollision,maxNumOutstandingTasks,m_collisionConfiguration); +// m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); +#else + + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); +#endif //USE_PARALLEL_DISPATCHER + +#ifdef USE_CUSTOM_NEAR_CALLBACK + //this is optional + m_dispatcher->setNearCallback(customNearCallback); +#endif + + btVector3 worldAabbMin(-1000,-1000,-1000); + btVector3 worldAabbMax(1000,1000,1000); + + m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies); +/// For large worlds or over 16384 objects, use the bt32BitAxisSweep3 broadphase +// m_broadphase = new bt32BitAxisSweep3(worldAabbMin,worldAabbMax,maxProxies); +/// When trying to debug broadphase issues, try to use the btSimpleBroadphase +// m_broadphase = new btSimpleBroadphase; + + //box-box is in Extras/AlternativeCollisionAlgorithms:it requires inclusion of those files +#ifdef REGISTER_BOX_BOX + m_dispatcher->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE,BOX_SHAPE_PROXYTYPE,new BoxBoxCollisionAlgorithm::CreateFunc); +#endif //REGISTER_BOX_BOX + +#ifdef COMPARE_WITH_QUICKSTEP + m_solver = new OdeConstraintSolver(); +#else + + +#ifdef USE_PARALLEL_SOLVER + + m_threadSupportSolver = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo( + "solver", + processSolverTask, + createSolverLocalStoreMemory, + maxNumOutstandingTasks)); + + m_solver = new btParallelSequentialImpulseSolver(m_threadSupportSolver,maxNumOutstandingTasks); +#else + btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver(); + + m_solver = solver; + //default solverMode is SOLVER_RANDMIZE_ORDER. Warmstarting seems not to improve convergence, see + //solver->setSolverMode(0);//btSequentialImpulseConstraintSolver::SOLVER_USE_WARMSTARTING | btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER); + +#endif //USE_PARALLEL_SOLVER + +#endif + +#ifdef USER_DEFINED_FRICTION_MODEL + //user defined friction model is not supported in 'cache friendly' solver yet, so switch to old solver + m_solver->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER); +#endif //USER_DEFINED_FRICTION_MODEL + + btDiscreteDynamicsWorld* world = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); + m_dynamicsWorld = world; + +#ifdef DO_BENCHMARK_PYRAMIDS + world->getSolverInfo().m_numIterations = 4; +#endif //DO_BENCHMARK_PYRAMIDS + + m_dynamicsWorld->getDispatchInfo().m_enableSPU = true; + m_dynamicsWorld->setGravity(btVector3(0,-10,0)); + + + +#ifdef USER_DEFINED_FRICTION_MODEL + { + //m_solver->setContactSolverFunc(ContactSolverFunc func,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE); + m_solver->SetFrictionSolverFunc(myFrictionModel,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE); + m_solver->SetFrictionSolverFunc(myFrictionModel,DEFAULT_CONTACT_SOLVER_TYPE,USER_CONTACT_SOLVER_TYPE1); + m_solver->SetFrictionSolverFunc(myFrictionModel,USER_CONTACT_SOLVER_TYPE1,USER_CONTACT_SOLVER_TYPE1); + //m_physicsEnvironmentPtr->setNumIterations(2); + } +#endif //USER_DEFINED_FRICTION_MODEL + + int i; + + btTransform tr; + tr.setIdentity(); + + + for (i=0;i0) + { + shapeIndex[i] = 1;//sphere + } + else + shapeIndex[i] = 0; + } + + if (useCompound) + { + btCompoundShape* compoundShape = new btCompoundShape(); + btCollisionShape* oldShape = m_collisionShapes[1]; + m_collisionShapes[1] = compoundShape; + btVector3 sphereOffset(0,0,2); + + comOffset.setIdentity(); + +#ifdef CENTER_OF_MASS_SHIFT + comOffset.setOrigin(comOffsetVec); + compoundShape->addChildShape(comOffset,oldShape); + +#else + compoundShape->addChildShape(tr,oldShape); + tr.setOrigin(sphereOffset); + compoundShape->addChildShape(tr,new btSphereShape(0.9)); +#endif + } + +#ifdef DO_WALL + + for (i=0;isetMargin(gCollisionMargin); + + bool isDyna = i>0; + + btTransform trans; + trans.setIdentity(); + + if (i>0) + { + //stack them + int colsize = 10; + int row = (i*CUBE_HALF_EXTENTS*2)/(colsize*2*CUBE_HALF_EXTENTS); + int row2 = row; + int col = (i)%(colsize)-colsize/2; + + + if (col>3) + { + col=11; + row2 |=1; + } + + btVector3 pos(col*2*CUBE_HALF_EXTENTS + (row2%2)*CUBE_HALF_EXTENTS, + row*2*CUBE_HALF_EXTENTS+CUBE_HALF_EXTENTS+EXTRA_HEIGHT,0); + + trans.setOrigin(pos); + } else + { + trans.setOrigin(btVector3(0,EXTRA_HEIGHT-CUBE_HALF_EXTENTS,0)); + /*btQuaternion q; + q.setRotation(btVector3(1,0,0).normalized(),SIMD_PI/4); + trans.setRotation(q);*/ + } + + float mass = 1.f; + + if (!isDyna) + mass = 0.f; + btRigidBody* body = localCreateRigidBody(mass,trans,shape); +#ifdef USE_KINEMATIC_GROUND + if (mass == 0.f) + { + body->setCollisionFlags( body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); + body->setActivationState(DISABLE_DEACTIVATION); + } +#endif //USE_KINEMATIC_GROUND + + + // Only do CCD if motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS + body->setCcdSquareMotionThreshold( CUBE_HALF_EXTENTS ); + + //Experimental: better estimation of CCD Time of Impact: + body->setCcdSweptSphereRadius( 0.2*CUBE_HALF_EXTENTS ); + +#ifdef USER_DEFINED_FRICTION_MODEL + ///Advanced use: override the friction solver + body->m_frictionSolverType = USER_CONTACT_SOLVER_TYPE1; +#endif //USER_DEFINED_FRICTION_MODEL + + } +#endif + + +#ifdef DO_BENCHMARK_PYRAMIDS + btTransform trans; + trans.setIdentity(); + + btScalar halfExtents = CUBE_HALF_EXTENTS; + + trans.setOrigin(btVector3(0,-halfExtents,0)); + + + + localCreateRigidBody(0.f,trans,m_collisionShapes[shapeIndex[0]]); + + int numWalls = 15; + int wallHeight = 15; + float wallDistance = 3; + + + for (int i=0;isetLinearVelocity(btVector3(0,0,-10)); +#endif +#endif //DO_BENCHMARK_PYRAMIDS +// clientResetScene(); + +m_softbodyimpl.pdemo=this; +m_sparsesdf.Initialize(); +clientResetScene(); +} + + + + + + +void SoftDemo::exitPhysics() +{ + + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject( obj ); + delete obj; + } + + //delete collision shapes + for (int j=0;j m_softbodies; + btSparseSdf<3> m_sparsesdf; + bool m_autocam; + + + //keep the collision shapes, for deletion/cleanup + btAlignedObjectArray m_collisionShapes; + + btBroadphaseInterface* m_broadphase; + + btCollisionDispatcher* m_dispatcher; + +#ifdef USE_PARALLEL_DISPATCHER +#ifdef WIN32 + class Win32ThreadSupport* m_threadSupportCollision; + class Win32ThreadSupport* m_threadSupportSolver; +#endif +#endif + + btConstraintSolver* m_solver; + + btCollisionAlgorithmCreateFunc* m_boxBoxCF; + + btDefaultCollisionConfiguration* m_collisionConfiguration; + + + public: + + void initPhysics(); + + void exitPhysics(); + + virtual ~SoftDemo() + { + exitPhysics(); + } + + virtual void clientMoveAndDisplay(); + + virtual void displayCallback(); + + void createStack( btCollisionShape* boxShape, float halfCubeSize, int size, float zPos ); + + static DemoApplication* Create() + { + SoftDemo* demo = new SoftDemo; + demo->myinit(); + demo->initPhysics(); + return demo; + } + + // + void clientResetScene(); + void renderme(); + void keyboardCallback(unsigned char key, int x, int y); + +}; + +#endif //CCD_PHYSICS_DEMO_H + diff --git a/Demos/SoftDemo/main.cpp b/Demos/SoftDemo/main.cpp new file mode 100644 index 000000000..0ee80ab5b --- /dev/null +++ b/Demos/SoftDemo/main.cpp @@ -0,0 +1,37 @@ +/* +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 "SoftDemo.h" +#include "GlutStuff.h" +#include "GLDebugDrawer.h" +#include "btBulletDynamicsCommon.h" + +GLDebugDrawer gDebugDrawer; + +int main(int argc,char** argv) +{ + + SoftDemo* softDemo = new SoftDemo(); + + softDemo->initPhysics(); + softDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); + + + glutmain(argc, argv,640,480,"Bullet Physics Demo. http://bullet.sf.net",softDemo); + + delete softDemo; + return 0; + +} diff --git a/src/BulletCollision/CMakeLists.txt b/src/BulletCollision/CMakeLists.txt index a8ada5837..4d77d1e24 100644 --- a/src/BulletCollision/CMakeLists.txt +++ b/src/BulletCollision/CMakeLists.txt @@ -111,6 +111,8 @@ ADD_LIBRARY(LibBulletCollision NarrowPhaseCollision/btContinuousConvexCollision.h NarrowPhaseCollision/btGjkEpa.cpp NarrowPhaseCollision/btGjkEpa.h + NarrowPhaseCollision/btGjkEpa2.cpp + NarrowPhaseCollision/btGjkEpa2.h NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h NarrowPhaseCollision/btConvexCast.cpp diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp new file mode 100644 index 000000000..3e9057cd8 --- /dev/null +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -0,0 +1,891 @@ +#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "btGjkEpa2.h" + +#if defined(DEBUG) || defined (_DEBUG) +#include //for debug printf +#ifdef __SPU__ +#include +#define printf spu_printf +#endif //__SPU__ +#endif + +namespace gjkepa2_impl +{ + +// Config + + /* GJK */ +#define GJK_MAX_ITERATIONS 128 +#define GJK_ACCURARY ((btScalar)0.0001) +#define GJK_MIN_DISTANCE ((btScalar)0.0001) +#define GJK_DUPLICATED_EPS ((btScalar)0.0001) +#define GJK_SIMPLEX2_EPS ((btScalar)0.0) +#define GJK_SIMPLEX3_EPS ((btScalar)0.0) +#define GJK_SIMPLEX4_EPS ((btScalar)0.0) + + /* EPA */ +#define EPA_MAX_VERTICES 64 +#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) +#define EPA_MAX_ITERATIONS 255 +#define EPA_ACCURACY ((btScalar)0.0001) +#define EPA_FALLBACK (10*EPA_ACCURACY) +#define EPA_PLANE_EPS ((btScalar)0.00001) +#define EPA_INSIDE_EPS ((btScalar)0.01) + + +// Shorthands +typedef unsigned int U; +typedef unsigned char U1; + +// MinkowskiDiff +struct MinkowskiDiff + { + const btConvexShape* m_shapes[2]; + btMatrix3x3 m_toshape1; + btTransform m_toshape0; + btVector3 (btConvexShape::*Ls)(const btVector3&) const; + void EnableMargin(bool enable) + { + if(enable) + Ls=&btConvexShape::localGetSupportingVertex; + else + Ls=&btConvexShape::localGetSupportingVertexWithoutMargin; + } + inline btVector3 Support0(const btVector3& d) const + { + return(((m_shapes[0])->*(Ls))(d)); + } + inline btVector3 Support1(const btVector3& d) const + { + return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); + } + inline btVector3 Support(const btVector3& d) const + { + return(Support0(d)-Support1(-d)); + } + btVector3 Support(const btVector3& d,U index) const + { + if(index) + return(Support1(d)); + else + return(Support0(d)); + } + }; + +typedef MinkowskiDiff tShape; + +// GJK +struct GJK +{ +/* Types */ +struct sSV + { + btVector3 d,w; + }; +struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; +struct eStatus { enum _ { + Valid, + Inside, + Failed };}; +/* Fields */ +tShape m_shape; +btVector3 m_ray; +btScalar m_distance; +sSimplex m_simplices[2]; +sSV m_store[4]; +sSV* m_free[4]; +U m_nfree; +U m_current; +sSimplex* m_simplex; +eStatus::_ m_status; +/* Methods */ + GJK() + { + Initialize(); + } +void Initialize() + { + m_ray = btVector3(0,0,0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; + } +eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) + { + U iterations=0; + btScalar sqdist=0; + btScalar alpha=0; + btVector3 lastw[4]; + U clastw=0; + /* Initialize solver */ + m_free[0] = &m_store[0]; + m_free[1] = &m_store[1]; + m_free[2] = &m_store[2]; + m_free[3] = &m_store[3]; + m_nfree = 4; + m_current = 0; + m_status = eStatus::Valid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const btScalar sqrl= m_ray.length2(); + appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do { + const U next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const btScalar rl=m_ray.length(); + if(rlw; + bool found=false; + for(U i=0;i<4;++i) + { + if((w-lastw[i]).length2()w, + cs.c[1]->w, + weights,mask);break; + case 3: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights,mask);break; + case 4: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights,mask);break; + } + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = btVector3(0,0,0); + m_current = next; + for(U i=0,ni=cs.rank;iw*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) m_status=eStatus::Inside; + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)rank) + { + case 1: + { + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + appendvertice(*m_simplex, axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + if(btFabs(dot(axis,d))>0) + { + const btVector3 p=cross(d,axis); + appendvertice(*m_simplex, p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, + m_simplex->c[2]->w-m_simplex->c[0]->w); + const btScalar l=n.length(); + if(l>0) + { + appendvertice(*m_simplex,n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 4: + { + if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, + m_simplex->c[1]->w-m_simplex->c[3]->w, + m_simplex->c[2]->w-m_simplex->c[3]->w))>0) + return(true); + } + break; + } + return(false); + } +/* Internals */ +void getsupport(const btVector3& d,sSV& sv) const + { + sv.d = d/d.length(); + sv.w = m_shape.Support(sv.d); + } +void removevertice(sSimplex& simplex) + { + m_free[m_nfree++]=simplex.c[--simplex.rank]; + } +void appendvertice(sSimplex& simplex,const btVector3& v) + { + simplex.p[simplex.rank]=0; + simplex.c[simplex.rank]=m_free[--m_nfree]; + getsupport(v,*simplex.c[simplex.rank++]); + } +static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) + { + return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- + a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ + a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); + } +static btScalar projectorigin( const btVector3& a, + const btVector3& b, + btScalar* w,U& m) + { + const btVector3 d=b-a; + const btScalar l=d.length2(); + if(l>GJK_SIMPLEX2_EPS) + { + const btScalar t(l>0?-dot(a,d)/l:0); + if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } + else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } + else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } + } + return(-1); + } +static btScalar projectorigin( const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar* w,U& m) + { + static const U imd3[]={1,2,0}; + const btVector3* vt[]={&a,&b,&c}; + const btVector3 dl[]={a-b,b-c,c-a}; + const btVector3 n=cross(dl[0],dl[1]); + const btScalar l=n.length2(); + if(l>GJK_SIMPLEX3_EPS) + { + btScalar mindist=-1; + btScalar subw[2]; + U subm; + for(U i=0;i<3;++i) + { + if(dot(*vt[i],cross(dl[i],n))>0) + { + const U j=imd3[i]; + const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); + if((mindist<0)||(subdGJK_SIMPLEX4_EPS)) + { + btScalar mindist=-1; + btScalar subw[3]; + U subm; + for(U i=0;i<3;++i) + { + const U j=imd3[i]; + const btScalar s=vl*dot(d,cross(dl[i],dl[j])); + if(s>0) + { + const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); + if((mindist<0)||(subd1)&&gjk.EncloseOrigin()) + { + /* Clean up */ + while(m_hull.root) + { + sFace* f(m_hull.root); + remove(m_hull,f); + append(m_stock,f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, + simplex.c[1]->w-simplex.c[3]->w, + simplex.c[2]->w-simplex.c[3]->w)<0) + { + btSwap(simplex.c[0],simplex.c[1]); + btSwap(simplex.p[0],simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), + newface(simplex.c[1],simplex.c[0],simplex.c[3],true), + newface(simplex.c[2],simplex.c[1],simplex.c[3],true), + newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; + if(m_hull.count==4) + { + sFace* best=findbest(); + sFace outer=*best; + U pass=0; + U iterations=0; + bind(tetra[0],0,tetra[1],0); + bind(tetra[0],1,tetra[2],0); + bind(tetra[0],2,tetra[3],0); + bind(tetra[1],1,tetra[3],2); + bind(tetra[1],2,tetra[2],1); + bind(tetra[2],2,tetra[3],1); + m_status=eStatus::Valid; + for(;iterationspass = (U1)(++pass); + gjk.getsupport(best->n,*w); + const btScalar wdist=dot(best->n,w->w)-best->d; + if(wdist>EPA_ACCURACY) + { + for(U j=0;(j<3)&&valid;++j) + { + valid&=expand( pass,w, + best->f[j],best->e[j], + horizon); + } + if(valid&&(horizon.nf>=3)) + { + bind(horizon.cf,1,horizon.ff,2); + remove(m_hull,best); + append(m_stock,best); + best=findbest(); + if(best->p>=outer.p) outer=*best; + } else { m_status=eStatus::InvalidHull;break; } + } else { m_status=eStatus::AccuraryReached;break; } + } else { m_status=eStatus::OutOfVertices;break; } + } + const btVector3 projection=outer.n*outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = cross( outer.c[1]->w-projection, + outer.c[2]->w-projection).length(); + m_result.p[1] = cross( outer.c[2]->w-projection, + outer.c[0]->w-projection).length(); + m_result.p[2] = cross( outer.c[0]->w-projection, + outer.c[1]->w-projection).length(); + const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return(m_status); + } + } + /* Fallback */ + m_status = eStatus::FallBack; + m_normal = -guess; + const btScalar nl=m_normal.length(); + if(nl>0) + m_normal = m_normal/nl; + else + m_normal = btVector3(1,0,0); + m_depth = 0; + m_result.rank=1; + m_result.c[0]=simplex.c[0]; + m_result.p[0]=1; + return(m_status); + } +sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) + { + if(m_stock.root) + { + sFace* face=m_stock.root; + remove(m_stock,face); + append(m_hull,face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = cross(b->w-a->w,c->w-a->w); + const btScalar l=face->n.length(); + const bool v=l>EPA_ACCURACY; + face->p = btMin(btMin( + dot(a->w,cross(face->n,a->w-b->w)), + dot(b->w,cross(face->n,b->w-c->w))), + dot(c->w,cross(face->n,c->w-a->w))) / + (v?l:1); + face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; + if(v) + { + face->d = dot(a->w,face->n)/l; + face->n /= l; + if(forced||(face->d>=-EPA_PLANE_EPS)) + { + return(face); + } else m_status=eStatus::NonConvex; + } else m_status=eStatus::Degenerated; + remove(m_hull,face); + append(m_stock,face); + return(0); + } + m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; + return(0); + } +sFace* findbest() + { + sFace* minf=m_hull.root; + btScalar mind=minf->d*minf->d; + btScalar maxp=minf->p; + for(sFace* f=minf->l[1];f;f=f->l[1]) + { + const btScalar sqd=f->d*f->d; + if((f->p>=maxp)&&(sqdp; + } + } + return(minf); + } +bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) + { + static const U i1m3[]={1,2,0}; + static const U i2m3[]={2,0,1}; + if(f->pass!=pass) + { + const U e1=i1m3[e]; + if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + { + sFace* nf=newface(f->c[e1],f->c[e],w,false); + if(nf) + { + bind(nf,0,f,e); + if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; + horizon.cf=nf; + ++horizon.nf; + return(true); + } + } + else + { + const U e2=i2m3[e]; + f->pass = (U1)pass; + if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& + expand(pass,w,f->f[e2],f->e[e2],horizon)) + { + remove(m_hull,f); + append(m_stock,f); + return(true); + } + } + } + return(false); + } +static inline void bind(sFace* fa,U ea,sFace* fb,U eb) + { + fa->e[ea]=(U1)eb;fa->f[ea]=fb; + fb->e[eb]=(U1)ea;fb->f[eb]=fa; + } +static inline void append(sList& list,sFace* face) + { + face->l[0] = 0; + face->l[1] = list.root; + if(list.root) list.root->l[0]=face; + list.root = face; + ++list.count; + } +static inline void remove(sList& list,sFace* face) + { + if(face->l[1]) face->l[1]->l[0]=face->l[0]; + if(face->l[0]) face->l[0]->l[1]=face->l[1]; + if(face==list.root) list.root=face->l[1]; + --list.count; + } +}; + +// +static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + btGjkEpaSolver2::sResults& results, + tShape& shape, + bool withmargins) +{ +/* Results */ +results.witnesses[0] = +results.witnesses[1] = btVector3(0,0,0); +results.status = btGjkEpaSolver2::sResults::Separated; +/* Shape */ +shape.m_shapes[0] = shape0; +shape.m_shapes[1] = shape1; +shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); +shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); +shape.EnableMargin(withmargins); +} + +} + +// +// Api +// + +using namespace gjkepa2_impl; + +// +int btGjkEpaSolver2::StackSizeRequirement() +{ +return(sizeof(GJK)+sizeof(EPA)); +} + +// +btScalar btGjkEpaSolver2::Distance( const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + sResults& results) +{ +tShape shape; +Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); +GJK gjk; +GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); +if(gjk_status==GJK::eStatus::Valid) + { + btVector3 w0=btVector3(0,0,0); + btVector3 w1=btVector3(0,0,0); + for(U i=0;irank;++i) + { + const btScalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*w1; + return((w0-w1).length()); + } + else + { + results.status = gjk_status==GJK::eStatus::Inside? + sResults::Penetrating : + sResults::GJK_Failed ; + return(-1); + } +} + +// +btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, + btScalar margin, + const btConvexShape* shape0, + const btTransform& wtrs0, + sResults& results) +{ +tShape shape; +btSphereShape shape1(margin); +btTransform wtrs1(btQuaternion(0,0,0,1),position); +Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); +GJK gjk; +GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); +if(gjk_status==GJK::eStatus::Valid) + { + btVector3 w0=btVector3(0,0,0); + btVector3 w1=btVector3(0,0,0); + for(U i=0;irank;++i) + { + const btScalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*w1; + const btVector3 delta= results.witnesses[1]- + results.witnesses[0]; + const btScalar margin= shape0->getMargin()+ + shape1.getMargin(); + const btScalar length= delta.length(); + results.normal = delta/length; + results.witnesses[0] += results.normal*margin; + return(length-margin); + } + else + { + if(gjk_status==GJK::eStatus::Inside) + { + if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) + { + const btVector3 delta= results.witnesses[0]- + results.witnesses[1]; + const btScalar length= delta.length(); + results.normal = delta/length; + return(-length); + } + } + } +return(SIMD_INFINITY); +} + +// +bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) +{ +tShape shape; +Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,true); +GJK gjk; +GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); +switch(gjk_status) + { + case GJK::eStatus::Inside: + { + EPA epa; + EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); + if(epa_status!=EPA::eStatus::Failed) + { + btVector3 w0=btVector3(0,0,0); + for(U i=0;id,0)*epa.m_result.p[i]; + } + results.status = sResults::Penetrating; + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); + return(true); + } else results.status=sResults::EPA_Failed; + } + break; + case GJK::eStatus::Failed: + results.status=sResults::GJK_Failed; + break; + } +return(false); +} + +/* Symbols cleanup */ + +#undef GJK_MAX_ITERATIONS +#undef GJK_ACCURARY +#undef GJK_MIN_DISTANCE +#undef GJK_DUPLICATED_EPS +#undef GJK_SIMPLEX2_EPS +#undef GJK_SIMPLEX3_EPS +#undef GJK_SIMPLEX4_EPS + +#undef EPA_MAX_VERTICES +#undef EPA_MAX_FACES +#undef EPA_MAX_ITERATIONS +#undef EPA_ACCURACY +#undef EPA_FALLBACK +#undef EPA_PLANE_EPS +#undef EPA_INSIDE_EPS diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h new file mode 100644 index 000000000..0706342b1 --- /dev/null +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h @@ -0,0 +1,39 @@ +#ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ +#define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ +#include "BulletCollision/CollisionShapes/btConvexShape.h" + +///btGjkEpaSolver contributed under zlib by Nathanael Presson +struct btGjkEpaSolver2 +{ +struct sResults + { + enum eStatus + { + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed, /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } status; + btVector3 witnesses[2]; + btVector3 normal; + }; + +static int StackSizeRequirement(); + +static btScalar Distance( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + sResults& results); + +static btScalar SignedDistance( const btVector3& position, + btScalar margin, + const btConvexShape* shape, + const btTransform& wtrs, + sResults& results); + +static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + const btVector3& guess, + sResults& results); +}; + +#endif diff --git a/src/BulletDynamics/SoftBody/btSoftBody.cpp b/src/BulletDynamics/SoftBody/btSoftBody.cpp new file mode 100644 index 000000000..670bef316 --- /dev/null +++ b/src/BulletDynamics/SoftBody/btSoftBody.cpp @@ -0,0 +1,1189 @@ +/* +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. +*/ +///btSoftBody implementation by Nathanael Presson + +#include "btSoftBody.h" +#include +#include + +namespace btsoftbody_internals +{ + +// +// Helpers +// + +// +static inline void Trace(const btMatrix3x3& m,const char* name) +{ +printf("%s[0]: %.2f,\t%.2f,\t%.2f\r\n",name,m[0].x(),m[0].y(),m[0].z()); +printf("%s[1]: %.2f,\t%.2f,\t%.2f\r\n",name,m[1].x(),m[1].y(),m[1].z()); +printf("%s[2]: %.2f,\t%.2f,\t%.2f\r\n",name,m[2].x(),m[2].y(),m[2].z()); +} + +// +template +static inline T Lerp(const T& a,const T& b,btScalar t) +{ return(a+(b-a)*t); } +// +template +static inline T Clamp(const T& x,const T& l,const T& h) +{ return(xh?h:x); } +// +template +static inline T Sq(const T& x) +{ return(x*x); } +// +template +static inline T Sign(const T& x) +{ return((T)(x<0?-1:+1)); } +// +static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) +{ +const btScalar xx=a.x()*a.x(); +const btScalar yy=a.y()*a.y(); +const btScalar zz=a.z()*a.z(); +const btScalar xy=a.x()*a.y(); +const btScalar yz=a.y()*a.z(); +const btScalar zx=a.z()*a.x(); +btMatrix3x3 m; +m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx); +m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz); +m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s); +return(m); +} +// +static inline btMatrix3x3 Cross(const btVector3& v) +{ +btMatrix3x3 m; +m[0]=btVector3(0,-v.z(),+v.y()); +m[1]=btVector3(+v.z(),0,-v.x()); +m[2]=btVector3(-v.y(),+v.x(),0); +return(m); +} +// +static inline btMatrix3x3 Diagonal(btScalar x) +{ +btMatrix3x3 m; +m[0]=btVector3(x,0,0); +m[1]=btVector3(0,x,0); +m[2]=btVector3(0,0,x); +return(m); +} +// +static inline btMatrix3x3 Add(const btMatrix3x3& a, + const btMatrix3x3& b) +{ +btMatrix3x3 r; +for(int i=0;i<3;++i) r[i]=a[i]+b[i]; +return(r); +} +// +static inline btMatrix3x3 Sub(const btMatrix3x3& a, + const btMatrix3x3& b) +{ +btMatrix3x3 r; +for(int i=0;i<3;++i) r[i]=a[i]-b[i]; +return(r); +} +// +static inline btMatrix3x3 Mul(const btMatrix3x3& a, + btScalar b) +{ +btMatrix3x3 r; +for(int i=0;i<3;++i) r[i]=a[i]*b; +return(r); +} +// +static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) +{ +const btMatrix3x3 cr=Cross(r); +return(Sub(Diagonal(im),cr*iwi*cr)); +} +// +static inline btMatrix3x3 ImpulseMatrix( btScalar dt, + btScalar ima, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) +{ +return( Diagonal(1/dt)* + Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); +} +// +static inline void PolarDecompose( const btMatrix3x3& m, + btMatrix3x3& q, + btMatrix3x3& s) +{ +static const btScalar half=(btScalar)0.5; +static const btScalar accuracy=(btScalar)0.00001; +static const int maxiterations=64; +btScalar det=m.determinant(); +if(!btFuzzyZero(det)) + { + q=m; + for(int i=0;iaccuracy) det=ndet; else break; + } + s=q.transpose()*m; + } + else + { + q.setIdentity(); + s.setIdentity(); + } +} +// +static inline btVector3 ProjectOnAxis( const btVector3& v, + const btVector3& a) +{ +return(a*dot(v,a)); +} +// +static inline btVector3 ProjectOnPlane( const btVector3& v, + const btVector3& a) +{ +return(v-ProjectOnAxis(v,a)); +} +// +template +static inline T BaryEval( const btVector3& coord, + const T& a, + const T& b, + const T& c) +{ +return(a*coord.x()+b*coord.y()+c*coord.z()); +} + +// +static inline btVector3 NormalizeAny(const btVector3& v) +{ +const btScalar l=v.length(); +if(l>SIMD_EPSILON) + return(v/l); + else + return(btVector3(0,0,0)); +} + +// +static void PointersToIndices(btSoftBody* psb) +{ +#define PTR2IDX(_p_,_b_) reinterpret_cast((_p_)-(_b_)) +btSoftBody::Node* base=&psb->m_nodes[0]; +for(int i=0,ni=psb->m_links.size();im_links[i].m_n[0]=PTR2IDX(psb->m_links[i].m_n[0],base); + psb->m_links[i].m_n[1]=PTR2IDX(psb->m_links[i].m_n[1],base); + } +for(int i=0,ni=psb->m_faces.size();im_faces[i].m_n[0]=PTR2IDX(psb->m_faces[i].m_n[0],base); + psb->m_faces[i].m_n[1]=PTR2IDX(psb->m_faces[i].m_n[1],base); + psb->m_faces[i].m_n[2]=PTR2IDX(psb->m_faces[i].m_n[2],base); + } +#undef PTR2IDX +} + +// +static void IndicesToPointers(btSoftBody* psb) +{ +#define IDX2PTR(_p_,_b_) ((_b_)+(((char*)_p_)-(char*)0)) +btSoftBody::Node* base=&psb->m_nodes[0]; +for(int i=0,ni=psb->m_links.size();im_links[i].m_n[0]=IDX2PTR(psb->m_links[i].m_n[0],base); + psb->m_links[i].m_n[1]=IDX2PTR(psb->m_links[i].m_n[1],base); + } +for(int i=0,ni=psb->m_faces.size();im_faces[i].m_n[0]=IDX2PTR(psb->m_faces[i].m_n[0],base); + psb->m_faces[i].m_n[1]=IDX2PTR(psb->m_faces[i].m_n[1],base); + psb->m_faces[i].m_n[2]=IDX2PTR(psb->m_faces[i].m_n[2],base); + } +#undef IDX2PTR +} + +// +static inline btScalar AreaOf( const btVector3& x0, + const btVector3& x1, + const btVector3& x2) +{ +const btVector3 a=x1-x0; +const btVector3 b=x2-x0; +const btVector3 cr=cross(a,b); +const btScalar area=cr.length(); +return(area); +} + +// +static inline btScalar VolumeOf( const btVector3& x0, + const btVector3& x1, + const btVector3& x2, + const btVector3& x3) +{ +const btVector3 a=x1-x0; +const btVector3 b=x2-x0; +const btVector3 c=x3-x0; +return(dot(a,cross(b,c))); +} + +// +static inline btScalar RayTriangle(const btVector3& org, + const btVector3& dir, + const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar maxt=SIMD_INFINITY) +{ +static const btScalar ceps=-SIMD_EPSILON*10; +static const btScalar teps=SIMD_EPSILON*10; +const btVector3 n=cross(b-a,c-a).normalized(); +const btScalar d=dot(a,n); +const btScalar den=dot(dir,n); +if(!btFuzzyZero(den)) + { + const btScalar num=dot(org,n)-d; + const btScalar t=-num/den; + if((t>teps)&&(tceps) && + (dot(n,cross(b-hit,c-hit))>ceps) && + (dot(n,cross(c-hit,a-hit))>ceps)) + { + return(t); + } + } + } +return(-1); +} + +// +// Private implementation +// + +// +static int RaycastInternal(const btSoftBody* psb, + const btVector3& org, + const btVector3& dir, + btScalar& mint, + bool bcountonly) +{ +int cnt=0; +mint=SIMD_INFINITY; +for(int i=0,ni=psb->m_faces.size();im_faces[i]; + const btScalar t=RayTriangle( org,dir, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + mint); + if(t>0) + { + ++cnt;if(!bcountonly) mint=t; + } + } +return(cnt); +} + + +// +static btVector3 EvaluateCom(btSoftBody* psb) +{ +btVector3 com(0,0,0); +if(psb->m_pose.m_bframe) + { + for(int i=0,ni=psb->m_nodes.size();im_nodes[i].m_x*psb->m_pose.m_wgh[i]; + } + } +return(com); +} + +// +static void UpdateNormals(btSoftBody* psb) +{ +const btVector3 zv(0,0,0); +for(int i=0,ni=psb->m_nodes.size();im_nodes[i].m_n=zv; + } +for(int i=0,ni=psb->m_faces.size();im_faces[i]; + const btVector3 n=cross(f.m_n[1]->m_x-f.m_n[0]->m_x, + f.m_n[2]->m_x-f.m_n[0]->m_x); + f.m_normal=n.normalized(); + f.m_n[0]->m_n+=n; + f.m_n[1]->m_n+=n; + f.m_n[2]->m_n+=n; + } +for(int i=0,ni=psb->m_nodes.size();im_nodes[i].m_n.normalize(); + } +} + +// +static void UpdateBounds(btSoftBody* psb) +{ +if(psb->m_nodes.size()>0) + { + psb->m_bounds[0]= + psb->m_bounds[1]=psb->m_nodes[0].m_x; + for(int i=1,ni=psb->m_nodes.size();im_nodes[i]; + psb->m_bounds[0].setMin(n.m_x); + psb->m_bounds[1].setMax(n.m_x); + psb->m_bounds[0].setMin(n.m_q); + psb->m_bounds[1].setMax(n.m_q); + } + } + else + { + psb->m_bounds[0]= + psb->m_bounds[1]=btVector3(0,0,0); + } +} + +// +static void UpdateTransform(btSoftBody* psb) +{ +UpdateBounds(psb); +} + +// +static void UpdatePose(btSoftBody* psb) +{ +if(psb->m_pose.m_bframe) + { + btSoftBody::Pose& pose=psb->m_pose; + const btVector3 com=EvaluateCom(psb); + /* Com */ + pose.m_com = com; + /* Rotation */ + btMatrix3x3 Apq; + const btScalar eps=1/(btScalar)(100*psb->m_nodes.size()); + Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0); + Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3); + for(int i=0,ni=psb->m_nodes.size();im_nodes[i].m_x-com); + const btVector3& b=pose.m_pos[i]; + Apq[0]+=a.x()*b; + Apq[1]+=a.y()*b; + Apq[2]+=a.z()*b; + } + btMatrix3x3 r,s; + PolarDecompose(Apq,r,s); + psb->m_pose.m_trs=r; + } +} + +// +static void UpdateConstants(btSoftBody* psb) +{ +/* Links */ +for(int i=0,ni=psb->m_links.size();im_links[i]; + l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length(); + l.m_c0 = l.m_n[0]->m_im+l.m_n[1]->m_im; + l.m_c1 = l.m_rl*l.m_rl; + } +/* Faces */ +for(int i=0,ni=psb->m_faces.size();im_faces[i]; + f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x); + } +/* Area's */ +btAlignedObjectArray counts; +counts.resize(psb->m_nodes.size(),0); +for(int i=0,ni=psb->m_nodes.size();im_nodes[i].m_area = 0; + } +for(int i=0,ni=psb->m_faces.size();im_faces[i]; + for(int j=0;j<3;++j) + { + const int index=(int)(f.m_n[j]-&psb->m_nodes[0]); + counts[index]++; + f.m_n[j]->m_area+=btFabs(f.m_ra); + } + } +for(int i=0,ni=psb->m_nodes.size();i0) + psb->m_nodes[i].m_area/=(btScalar)counts[i]; + else + psb->m_nodes[i].m_area=0; + } +} + +// +static inline void ApplyClampedForce( btSoftBody::Node& n, + const btVector3& f, + btScalar dt) +{ +const btScalar dtim=dt*n.m_im; +if((f*dtim).length2()>n.m_v.length2()) + {/* Clamp */ + n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim; + } + else + {/* Apply */ + n.m_f+=f; + } +} + +// +static void ApplyForces(btSoftBody* psb,btScalar dt) +{ +const btScalar kLF=psb->m_cfg.kLF; +const btScalar kDG=psb->m_cfg.kDG; +const btScalar kPR=psb->m_cfg.kPR; +const btScalar kVC=psb->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 && + (psb->m_cfg.aeromodel< + btSoftBody::eAeroModel::F_TwoSided); +const bool as_faero= as_aero && + (psb->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; +btSoftBody::ISoftBody::sMedium medium; +if(use_volume) + { + volume = psb->GetVolume(); + ivolumetp = 1/btFabs(volume)*kPR; + dvolumetv = (psb->m_pose.m_volume-volume)*kVC; + } +/* Per vertex forces */ +for(int i=0,ni=psb->m_nodes.size();im_nodes[i]; + if(n.m_im>0) + { + if(use_medium) + { + psb->m_isb->EvaluateMedium(n.m_x,medium); + /* Aerodynamics */ + if(as_vaero) + { + 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; + /* Setup normal */ + switch(psb->m_cfg.aeromodel) + { + case btSoftBody::eAeroModel::V_Point: + nrm=NormalizeAny(rel_v);break; + case btSoftBody::eAeroModel::V_TwoSided: + nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break; + } + const btScalar dvn=dot(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; + force += nrm*(-c1*kLF); + force += rel_v.normalized()*(-c1*kDG); + ApplyClampedForce(n,force,dt); + } + } + } + } + /* Pressure */ + if(as_pressure) + { + n.m_f += n.m_n*(n.m_area*ivolumetp); + } + /* Volume */ + if(as_volume) + { + n.m_f += n.m_n*(n.m_area*dvolumetv); + } + } + } +/* Per face forces */ +for(int i=0,ni=psb->m_faces.size();im_faces[i]; + if(as_faero) + { + const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3; + const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3; + psb->m_isb->EvaluateMedium(x,medium); + const btVector3 rel_v=v-medium.m_velocity; + const btScalar rel_v2=rel_v.length2(); + if(rel_v2>SIMD_EPSILON) + { + btVector3 nrm=f.m_normal; + /* Setup normal */ + switch(psb->m_cfg.aeromodel) + { + case btSoftBody::eAeroModel::F_TwoSided: + nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break; + } + const btScalar dvn=dot(rel_v,nrm); + /* Compute forces */ + if(dvn>0) + { + btVector3 force(0,0,0); + const btScalar c0 = f.m_ra*dvn*rel_v2; + const btScalar c1 = c0*medium.m_density; + force += nrm*(-c1*kLF); + force += rel_v.normalized()*(-c1*kDG); + force /= 3; + for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt); + } + } + } + } +} + +// +static void PSolve_Anchors(btSoftBody* psb,btScalar sdt) +{ +const btScalar kAHR=psb->m_cfg.kAHR; +for(int i=0,ni=psb->m_anchors.size();im_anchors[i]; + const btTransform& t=a.m_body->getWorldTransform(); + btSoftBody::Node& n=*a.m_node; + const btVector3 wa=t*a.m_local; + const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*sdt; + const btVector3 vb=n.m_x-n.m_q; + const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR; + const btVector3 impulse=a.m_c0*vr; + n.m_x+=impulse*a.m_c2; + a.m_body->applyImpulse(-impulse,a.m_c1); + } +} + +// +static void PSolve_Contacts(btSoftBody* psb,btScalar sdt) +{ +const btScalar kCHR=psb->m_cfg.kCHR; +for(int i=0,ni=psb->m_contacts.size();im_contacts[i]; + const btSoftBody::ISoftBody::sCti& cti=c.m_cti; + const btVector3 va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*sdt; + const btVector3 vb=c.m_node->m_x-c.m_node->m_q; + const btVector3 vr=vb-va; + const btScalar dn=dot(vr,cti.m_normal); + if(dn<=SIMD_EPSILON) + { + const btScalar dp=dot(c.m_node->m_x,cti.m_normal)+cti.m_offset; + const btVector3 fv=vr-cti.m_normal*dn; + const btVector3 impulse=c.m_c0*(vr-fv*c.m_c3+cti.m_normal*(dp*kCHR)); + c.m_node->m_x-=impulse*c.m_c2; + c.m_cti.m_body->applyImpulse(impulse,c.m_c1); + } + } +} + +// +static void PSolve_Links(btSoftBody* psb,btScalar w) +{ +for(int i=0,ni=psb->m_links.size();im_links[i]; + if(l.m_c0>0) + { + btSoftBody::Node& a=*l.m_n[0]; + btSoftBody::Node& b=*l.m_n[1]; + const btVector3 del=b.m_x-a.m_x; + const btScalar len=del.length2(); + const btScalar kst=l.m_kST*w; + const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst; + a.m_x-=del*(k*a.m_im); + b.m_x+=del*(k*b.m_im); + } + } +} + +} + +using namespace btsoftbody_internals; + +// +// Api +// + +// +btSoftBody* btSoftBody::Create( ISoftBody* isoftbody, + int node_count, + const btVector3* x, + const btScalar* m) +{ +btSoftBody* sb=new btSoftBody(); +/* Init */ +sb->m_cfg.aeromodel = eAeroModel::V_Point; +sb->m_cfg.kDG = 0; +sb->m_cfg.kLF = 0; +sb->m_cfg.kDP = 0; +sb->m_cfg.kPR = 0; +sb->m_cfg.kVC = 0; +sb->m_cfg.kDF = (btScalar)0.2; +sb->m_cfg.kLST = 1; +sb->m_cfg.kMT = 0; +sb->m_cfg.kSOR = 1; +sb->m_cfg.kCHR = (btScalar)1.0; +sb->m_cfg.kAHR = (btScalar)0.5; +sb->m_cfg.timescale = 1; +sb->m_cfg.timestep = (btScalar)(1/60.); +sb->m_cfg.maxsteps = 60; +sb->m_cfg.iterations = 1; +sb->m_cfg.becollide = true; +sb->m_cfg.bscollide = false; +sb->m_pose.m_bvolume = false; +sb->m_pose.m_bframe = false; +sb->m_pose.m_volume = 0; +sb->m_pose.m_com = btVector3(0,0,0); +sb->m_pose.m_trs.setIdentity(); +sb->m_isb = isoftbody?isoftbody:new ISoftBody(); +sb->m_tag = 0; +sb->m_timeacc = 0; +sb->m_bUpdateRtCst = true; +sb->m_bounds[0] = btVector3(0,0,0); +sb->m_bounds[1] = btVector3(0,0,0); +/* Nodes */ +sb->m_nodes.resize(node_count); +for(int i=0,ni=node_count;im_nodes[i]; + memset(&n,0,sizeof(n)); + n.m_x = x?*x++:btVector3(0,0,0); + n.m_q = n.m_x; + n.m_im = m?*m++:1; + n.m_im = n.m_im>0?1/n.m_im:0; + } +UpdateTransform(sb); +sb->m_isb->Attach(sb); +return(sb); +} + +// +void btSoftBody::Delete() +{ +m_isb->Detach(this); +delete this; +} + +// +bool btSoftBody::CheckLink(int node0,int node1) const +{ +return(CheckLink(&m_nodes[node0],&m_nodes[node1])); +} + +// +bool btSoftBody::CheckLink(const Node* node0,const Node* node1) const +{ +const Node* n[]={node0,node1}; +for(int i=0,ni=m_links.size();im_x-l.m_n[1]->m_x).length(); + l.m_c0 = 0; + l.m_c1 = 0; + l.m_type = type; + l.m_tag = 0; + m_links.push_back(l); + m_bUpdateRtCst=true; + } +} + +// +void btSoftBody::AppendFace(int node0,int node1,int node2) +{ +Face f; +f.m_n[0] = &m_nodes[node0]; +f.m_n[1] = &m_nodes[node1]; +f.m_n[2] = &m_nodes[node2]; +f.m_ra = AreaOf( f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x); +f.m_tag = 0; +m_faces.push_back(f); +m_bUpdateRtCst=true; +} + +// +void btSoftBody::AppendAnchor(int node,btRigidBody* body) +{ +Anchor a; +a.m_node = &m_nodes[node]; +a.m_body = body; +a.m_local = body->getWorldTransform().inverse()*a.m_node->m_x; +a.m_node->m_battach = 1; +m_anchors.push_back(a); +} + +// +void btSoftBody::AddForce(const btVector3& force) +{ +for(int i=0,ni=m_nodes.size();i0) + { + n.m_f += force; + } +} + +// +void btSoftBody::AddVelocity(const btVector3& velocity) +{ +for(int i=0,ni=m_nodes.size();i0) + { + n.m_v += velocity; + } +} + +// +void btSoftBody::SetMass(int node,btScalar mass) +{ +m_nodes[node].m_im=mass>0?1/mass:0; +m_bUpdateRtCst=true; +} + +// +btScalar btSoftBody::GetMass(int node) const +{ +return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0); +} + +// +btScalar btSoftBody::GetTotalMass() const +{ +btScalar mass=0; +for(int i=0;im_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x); + for(int j=0;j<3;++j) + { + f.m_n[j]->m_im+=twicearea; + } + } + for(int i=0;i0 ? + 1/(m_nodes[i].m_im*tmass) : + kmass/tmass; + } +/* Pos */ +const btVector3 com=EvaluateCom(this); +m_pose.m_pos.resize(m_nodes.size()); +for(int i=0,ni=m_nodes.size();i0) + { + const btVector3 org=m_nodes[0].m_x; + for(int i=0,ni=m_faces.size();im_x-org,cross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org)); + } + vol/=(btScalar)6; + } +return(vol); +} + +// +int btSoftBody::GenerateBendingConstraints( int distance, + btScalar stiffness) +{ +if(distance>1) + { + /* Build graph */ + const int n=m_nodes.size(); + const unsigned inf=(~(unsigned)0)>>1; + unsigned* adj=new unsigned[n*n]; + #define IDX(_x_,_y_) ((_y_)*n+(_x_)) + for(int j=0;jsum) + { + adj[IDX(i,j)]=adj[IDX(j,i)]=sum; + } + } + } + } + /* Build links */ + int nlinks=0; + for(int j=0;j=m_cfg.timestep) + { + /* Update */ + m_timeacc-=m_cfg.timestep; + if(m_bUpdateRtCst) + { + m_bUpdateRtCst=false; + UpdateConstants(this); + } + /* Prepare */ + const btScalar iit = 1/(btScalar)m_cfg.iterations; + const btScalar sdt = m_cfg.timestep*m_cfg.timescale; + const btScalar isdt = 1/sdt; + Node* pnodes = m_nodes.size()>0?&m_nodes[0]:0; + /* Forces */ + ApplyForces(this,sdt); + /* Integrate */ + for(int i=0,ni=m_nodes.size();i0)) + { + for(int i=0,ni=m_nodes.size();i0) + { + const btVector3 x= m_pose.m_trs*m_pose.m_pos[i]+ + m_pose.m_com; + n.m_x=Lerp(n.m_x,x,m_cfg.kMT); + } + } + } + /* External collide */ + m_contacts.resize(0); + if(m_cfg.becollide) + { + m_isb->StartCollide(m_bounds[0],m_bounds[1]); + for(int i=0,ni=m_nodes.size();iCheckContact(n.m_x,c.m_cti)) + { + const btRigidBody* prb=c.m_cti.m_body; + const btScalar ima=n.m_im; + const btScalar imb=prb->getInvMass(); + const btScalar ms=ima+imb; + if(ms>0) + { + const btTransform& wtr=prb->getWorldTransform(); + const btMatrix3x3 iwi=prb->getInvInertiaTensorWorld(); + const btVector3 ra=n.m_x-wtr.getOrigin(); + const btVector3 va=prb->getVelocityInLocalPoint(ra)*sdt; + const btVector3 vb=n.m_x-n.m_q; + const btVector3 vr=vb-va; + const btScalar dn=dot(vr,c.m_cti.m_normal); + const btVector3 fv=vr-c.m_cti.m_normal*dn; + const btScalar fc=m_cfg.kDF*prb->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(sdt,ima,imb,iwi,ra); + c.m_c1 = ra; + c.m_c2 = ima*sdt; + c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; + m_contacts.push_back(c); + } + } + } + m_isb->EndCollide(); + } + /* Prepare anchors */ + for(int i=0,ni=m_anchors.size();igetWorldTransform().getBasis()*a.m_local; + a.m_c0 = ImpulseMatrix( sdt, + a.m_node->m_im, + a.m_body->getInvMass(), + a.m_body->getInvInertiaTensorWorld(), + ra); + a.m_c1 = ra; + a.m_c2 = sdt*a.m_node->m_im; + } + /* Solve */ + for(int isolve=0;isolve(1,m_cfg.kSOR,isolve*iit)*m_cfg.kLST; + PSolve_Anchors(this,sdt); + PSolve_Contacts(this,sdt); + PSolve_Links(this,lw); + } + /* Velocities */ + const btScalar vc=isdt*(1-m_cfg.kDP); + for(int i=0,ni=m_nodes.size();i tScalarArray; + typedef btAlignedObjectArray tVector3Array; + + /* Base type */ + struct Element + { + void* m_tag; // User data + }; + /* Node */ + struct Node : Element + { + btVector3 m_x; // Position + btVector3 m_q; // Previous step position + btVector3 m_v; // Velocity + btVector3 m_f; // Force accumulator + btVector3 m_n; // Normal + btScalar m_im; // 1/mass + btScalar m_area; // Area + int m_battach:1; // Attached + }; + /* Link */ + struct Link : Element + { + Node* m_n[2]; // Node pointers + btScalar m_rl; // Rest length + btScalar m_kST; // Stiffness coefficient + btScalar m_c0; // (ima+imb)*kLST + btScalar m_c1; // rl^2 + eLType::_ m_type; // Link type + }; + /* Face */ + struct Face : Element + { + Node* m_n[3]; // Node pointers + btVector3 m_normal; // Normal + btScalar m_ra; // Rest area + }; + /* Contact */ + struct Contact + { + ISoftBody::sCti m_cti; // Contact infos + Node* m_node; // Owner node + btMatrix3x3 m_c0; // Impulse matrix + btVector3 m_c1; // Relative anchor + btScalar m_c2; // ima*dt + btScalar m_c3; // Friction + }; + /* Anchor */ + struct Anchor + { + Node* m_node; // Node pointer + btVector3 m_local; // Anchor position in body space + btRigidBody* m_body; // Body + btMatrix3x3 m_c0; // Impulse matrix + btVector3 m_c1; // Relative anchor + btScalar m_c2; // ima*dt + }; + /* Pose */ + struct Pose + { + bool m_bvolume; // Is valid + bool m_bframe; // Is frame + btScalar m_volume; // Rest volume + tVector3Array m_pos; // Reference positions + tScalarArray m_wgh; // Weights + btVector3 m_com; // COM + btMatrix3x3 m_trs; // Transform + }; + /* Config */ + struct Config + { + eAeroModel::_ aeromodel; // Aerodynamic model (default: V_Point) + btScalar kLST; // Linear stiffness coefficient [0,1] + btScalar kDP; // Damping coefficient [0,1] + btScalar kDG; // Drag coefficient [0,+inf] + btScalar kLF; // Lift coefficient [0,+inf] + btScalar kPR; // Pressure coefficient [-inf,+inf] + btScalar kVC; // Volume conversation coefficient [0,+inf] + btScalar kDF; // Dynamic friction coefficient [0,1] + btScalar kMT; // Pose matching coefficient [0,1] + btScalar kSOR; // SOR(w) [1,2] default 1, never use with solver!=Accurate + btScalar kCHR; // Contacts hardness [0,1] + btScalar kAHR; // Anchors hardness [0,1] + btScalar timescale; // Time scale + btScalar timestep; // Time step + int maxsteps; // Maximum time steps + int iterations; // Solver iterations + bool becollide; // Enable external collisions + bool bscollide; // Enable self collisions + }; + + // + // Typedef's + // + + typedef btAlignedObjectArray tNodeArray; + typedef btAlignedObjectArray tLinkArray; + typedef btAlignedObjectArray tFaceArray; + typedef btAlignedObjectArray tAnchorArray; + typedef btAlignedObjectArray tContactArray; + + // + // Fields + // + + Config m_cfg; // Configuration + Pose m_pose; // Pose + ISoftBody* m_isb; // ISoftBody + void* m_tag; // User data + tNodeArray m_nodes; // Nodes + tLinkArray m_links; // Links + tFaceArray m_faces; // Faces + tAnchorArray m_anchors; // Anchors + tContactArray m_contacts; // Contacts + btScalar m_timeacc; // Time accumulator + btVector3 m_bounds[2]; // Spatial bounds + bool m_bUpdateRtCst; // Update runtime constants + + // + // Api + // + + /* Create a soft body */ + static btSoftBody* Create( ISoftBody* isoftbody, + int node_count, + const btVector3* x=0, + const btScalar* m=0); + /* Delete a body */ + void Delete(); + /* Check for existing link */ + bool CheckLink( int node0, + int node1) const; + bool CheckLink( const btSoftBody::Node* node0, + const btSoftBody::Node* node1) const; + /* Check for existring face */ + bool CheckFace( int node0, + int node1, + int node2) const; + /* Append link */ + void AppendLink( int node0, + int node1, + btScalar kST, + btSoftBody::eLType::_ type, + bool bcheckexist=false); + void AppendLink( btSoftBody::Node* node0, + btSoftBody::Node* node1, + btScalar kST, + btSoftBody::eLType::_ type, + bool bcheckexist=false); + /* Append face */ + void AppendFace( int node0, + int node1, + int node2); + /* Append anchor */ + void AppendAnchor( int node, + btRigidBody* body); + /* Add force (or gravity) to the entire body */ + void AddForce( const btVector3& force); + /* Add force (or gravity) to a node of the body */ + void AddForce( const btVector3& force, + int node); + /* Add velocity to the entire body */ + void AddVelocity( const btVector3& velocity); + /* Add velocity to a node of the body */ + void AddVelocity( const btVector3& velocity, + int node); + /* Set mass */ + void SetMass( int node, + btScalar mass); + /* Get mass */ + btScalar GetMass( int node) const; + /* Get total mass */ + btScalar GetTotalMass() const; + /* Set total mass (weighted by previous masses) */ + void SetTotalMass( btScalar mass, + bool fromfaces=false); + /* Set total density */ + void SetTotalDensity(btScalar density); + /* Transform */ + void Transform( const btTransform& trs); + /* Scale */ + void Scale( const btVector3& scl); + /* Set current state as pose */ + void SetPose( bool bvolume, + bool bframe); + /* Return the volume */ + btScalar GetVolume() const; + /* Generate bending constraints based on distance in the adjency graph */ + int GenerateBendingConstraints( int distance, + btScalar stiffness); + /* Randomize constraints to reduce solver bias */ + void RandomizeConstraints(); + /* Ray casting */ + btScalar Raycast( const btVector3& org, + const btVector3& dir) const; + /* Step */ + void Step( btScalar dt); + + }; + +// +// Helpers +// + +/* fDrawFlags */ +struct fDrawFlags { enum _ { + Nodes = 0x0001, + SLinks = 0x0002, + BLinks = 0x0004, + Faces = 0x0008, + Tetras = 0x0010, + Normals = 0x0020, + Contacts = 0x0040, + Anchors = 0x0080, + /* presets */ + Links = SLinks+BLinks, + Std = SLinks+Faces+Anchors, + StdTetra = Std-Faces+Tetras, +};}; + +/* Draw body */ +void Draw( btSoftBody* psb, + btIDebugDraw* idraw, + int drawflags=fDrawFlags::Std); +/* Draw body infos */ +void DrawInfos( btSoftBody* psb, + btIDebugDraw* idraw, + bool masses, + bool areas, + bool stress); +/* Draw rigid frame */ +void DrawFrame( btSoftBody* psb, + btIDebugDraw* idraw); +/* Create a rope */ +btSoftBody* CreateRope( btSoftBody::ISoftBody* isoftbody, + const btVector3& from, + const btVector3& to, + int res, + int fixeds); +/* Create a patch */ +btSoftBody* CreatePatch( btSoftBody::ISoftBody* isoftbody, + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags); +/* Create an ellipsoid */ +btSoftBody* CreateEllipsoid(btSoftBody::ISoftBody* isoftbody, + const btVector3& center, + const btVector3& radius, + int res); +/* Create from convex-hull */ +btSoftBody* CreateFromConvexHull( btSoftBody::ISoftBody* isoftbody, + const btVector3* vertices, + int nvertices); +/* Create from trimesh */ +btSoftBody* CreateFromTriMesh( btSoftBody::ISoftBody* isoftbody, + const btScalar* vertices, + const int* triangles, + int ntriangles); + +#endif \ No newline at end of file diff --git a/src/BulletDynamics/SoftBody/btSoftBodyHelpers.cpp b/src/BulletDynamics/SoftBody/btSoftBodyHelpers.cpp new file mode 100644 index 000000000..a45a6d293 --- /dev/null +++ b/src/BulletDynamics/SoftBody/btSoftBodyHelpers.cpp @@ -0,0 +1,429 @@ +/* +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. +*/ +///btSoftBodyHelpers.cpp by Nathanael Presson + +#include "btSoftBody.h" +#include "Extras/ConvexHull/btConvexHull.h" +#include +#include + +namespace btsoftbody_internals +{ + +// +static void drawVertex( btIDebugDraw* idraw, + const btVector3& x,btScalar s,const btVector3& c) +{ +idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); +idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); +idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); +} + +// +static btVector3 stresscolor(btScalar stress) +{ +static const btVector3 spectrum[]= { + btVector3(1,0,1), + btVector3(0,0,1), + btVector3(0,1,1), + btVector3(0,1,0), + btVector3(1,1,0), + btVector3(1,0,0), + btVector3(1,0,0), + }; +static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; +static const btScalar one=1; +stress=btMax(0,btMin(1,stress))*ncolors; +const int sel=(int)stress; +const btScalar frc=stress-sel; +return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); +} + +} + +using namespace btsoftbody_internals; + +// +void Draw( btSoftBody* psb, + btIDebugDraw* idraw, + int drawflags) +{ +const btScalar scl=(btScalar)0.1; +const btScalar nscl=scl*5; +const btScalar alpha=(btScalar)0.5; +const btVector3 scolor=btVector3(0,0,0); +const btVector3 bcolor=btVector3(1,1,0); +const btVector3 ncolor=btVector3(1,1,1); +const btVector3 ccolor=btVector3(1,0,0); +/* Nodes */ +if(0!=(drawflags&fDrawFlags::Nodes)) + { + for(int i=0;im_nodes.size();++i) + { + const btSoftBody::Node& n=psb->m_nodes[i]; + idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0)); + idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0)); + idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1)); + } + } +/* Links */ +if(0!=(drawflags&fDrawFlags::Links)) + { + for(int i=0;im_links.size();++i) + { + const btSoftBody::Link& l=psb->m_links[i]; + switch(l.m_type) + { + case btSoftBody::eLType::Structural: + if(0!=(drawflags&fDrawFlags::SLinks)) idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,scolor);break; + case btSoftBody::eLType::Bending: + if(0!=(drawflags&fDrawFlags::BLinks)) idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,bcolor);break; + } + } + } +/* Normals */ +if(0!=(drawflags&fDrawFlags::Normals)) + { + for(int i=0;im_nodes.size();++i) + { + const btSoftBody::Node& n=psb->m_nodes[i]; + const btVector3 d=n.m_n*nscl; + idraw->drawLine(n.m_x,n.m_x+d,ncolor); + idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5); + } + } +/* Contacts */ +if(0!=(drawflags&fDrawFlags::Contacts)) + { + static const btVector3 axis[]={btVector3(1,0,0), + btVector3(0,1,0), + btVector3(0,0,1)}; + for(int i=0;im_contacts.size();++i) + { + const btSoftBody::Contact& c=psb->m_contacts[i]; + const btVector3 o= c.m_node->m_x-c.m_cti.m_normal* + (dot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset); + const btVector3 x=cross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized(); + const btVector3 y=cross(x,c.m_cti.m_normal).normalized(); + idraw->drawLine(o-x*nscl,o+x*nscl,ccolor); + idraw->drawLine(o-y*nscl,o+y*nscl,ccolor); + idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0)); + } + } +/* Anchors */ +if(0!=(drawflags&fDrawFlags::Anchors)) + { + for(int i=0;im_anchors.size();++i) + { + const btSoftBody::Anchor& a=psb->m_anchors[i]; + const btVector3 q=a.m_body->getWorldTransform()*a.m_local; + drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0)); + drawVertex(idraw,q,0.25,btVector3(0,1,0)); + idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1)); + } + for(int i=0;im_nodes.size();++i) + { + const btSoftBody::Node& n=psb->m_nodes[i]; + if(n.m_im<=0) + { + drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0)); + } + } + } +/* Faces */ +if(0!=(drawflags&fDrawFlags::Faces)) + { + const btScalar scl=(btScalar)0.7; + const btScalar alp=(btScalar)1; + const btVector3 col(0,(btScalar)0.7,0); + for(int i=0;im_faces.size();++i) + { + const btSoftBody::Face& f=psb->m_faces[i]; + const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; + const btVector3 c=(x[0]+x[1]+x[2])/3; + idraw->drawTriangle((x[0]-c)*scl+c, + (x[1]-c)*scl+c, + (x[2]-c)*scl+c, + f.m_n[0]->m_n,f.m_n[1]->m_n,f.m_n[2]->m_n, + col,alp); + } + } +} + +// +void DrawInfos( btSoftBody* psb, + btIDebugDraw* idraw, + bool masses, + bool areas, + bool stress) +{ +for(int i=0;im_nodes.size();++i) + { + const btSoftBody::Node& n=psb->m_nodes[i]; + char text[2048]={0}; + char buff[1024]; + if(masses) + { + sprintf(buff," M(%.2f)",1/n.m_im); + strcat(text,buff); + } + if(areas) + { + sprintf(buff," A(%.2f)",n.m_area); + strcat(text,buff); + } + if(text[0]) idraw->draw3dText(n.m_x,text); + } +} + +// +void DrawFrame( btSoftBody* psb, + btIDebugDraw* idraw) +{ +if(psb->m_pose.m_bframe) + { + static const btScalar ascl=10; + static const btScalar nscl=(btScalar)0.1; + const btVector3 com=psb->m_pose.m_com; + const btMatrix3x3& trs=psb->m_pose.m_trs; + const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized(); + const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized(); + const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized(); + idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0)); + idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0)); + idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1)); + for(int i=0;im_pose.m_pos.size();++i) + { + const btVector3 x=com+trs*psb->m_pose.m_pos[i]; + idraw->drawLine(x-btVector3(1,0,0)*nscl,x+btVector3(1,0,0)*nscl,btVector3(1,0,1)); + idraw->drawLine(x-btVector3(0,1,0)*nscl,x+btVector3(0,1,0)*nscl,btVector3(1,0,1)); + idraw->drawLine(x-btVector3(0,0,1)*nscl,x+btVector3(0,0,1)*nscl,btVector3(1,0,1)); + } + } +} + +// +btSoftBody* CreateRope( btSoftBody::ISoftBody* isoftbody, + const btVector3& from, + const btVector3& to, + int res, + int fixeds) +{ +/* Create nodes */ +const int r=res+2; +btVector3* x=new btVector3[r]; +btScalar* m=new btScalar[r]; +for(int i=0;iSetMass(0,0); +if(fixeds&2) psb->SetMass(r-1,0); +delete[] x; +delete[] m; +/* Create links */ +for(int i=1;iAppendLink(i-1,i,1,btSoftBody::eLType::Structural); + } +/* Finished */ +return(psb); +} + +// +btSoftBody* CreatePatch( btSoftBody::ISoftBody* isoftbody, + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags) +{ +#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) +/* Create nodes */ +if((resx<2)||(resy<2)) return(0); +const int rx=resx; +const int ry=resy; +const int tot=rx*ry; +btVector3* x=new btVector3[tot]; +btScalar* m=new btScalar[tot]; +for(int iy=0;iySetMass(IDX(0,0),0); +if(fixeds&2) psb->SetMass(IDX(rx-1,0),0); +if(fixeds&4) psb->SetMass(IDX(0,ry-1),0); +if(fixeds&8) psb->SetMass(IDX(rx-1,ry-1),0); +delete[] x; +delete[] m; +/* Create links and faces */ +for(int iy=0;iyAppendLink(idx,IDX(ix+1,iy), + 1,btSoftBody::eLType::Structural); + if(mdy) psb->AppendLink(idx,IDX(ix,iy+1), + 1,btSoftBody::eLType::Structural); + if(mdx&&mdy) + { + if((ix+iy)&1) + { + psb->AppendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1)); + psb->AppendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1)); + if(gendiags) + { + psb->AppendLink(IDX(ix,iy),IDX(ix+1,iy+1), + 1,btSoftBody::eLType::Structural); + } + } + else + { + psb->AppendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy)); + psb->AppendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1)); + if(gendiags) + { + psb->AppendLink(IDX(ix+1,iy),IDX(ix,iy+1), + 1,btSoftBody::eLType::Structural); + } + } + } + } + } +/* Finished */ +#undef IDX +return(psb); +} + +// +btSoftBody* CreateEllipsoid(btSoftBody::ISoftBody* isoftbody, + const btVector3& center, + const btVector3& radius, + int res) +{ +struct Hammersley + { + static void Generate(btVector3* x,int n) + { + for(int i=0;i>=1) if(j&1) t+=p; + btScalar w=2*t-1; + btScalar a=(SIMD_PI+2*i*SIMD_PI)/n; + btScalar s=btSqrt(1-w*w); + *x++=btVector3(s*btCos(a),s*btSin(a),w); + } + } + }; +btAlignedObjectArray vtx; +vtx.resize(3+res); +Hammersley::Generate(&vtx[0],vtx.size()); +for(int i=0;iAppendLink( idx[0],idx[1], + 1,btSoftBody::eLType::Structural); + if(idx[1]AppendLink( idx[1],idx[2], + 1,btSoftBody::eLType::Structural); + if(idx[2]AppendLink( idx[2],idx[0], + 1,btSoftBody::eLType::Structural); + psb->AppendFace(idx[0],idx[1],idx[2]); + } +hlib.ReleaseResult(hres); +psb->RandomizeConstraints(); +return(psb); +} + +// +btSoftBody* CreateFromTriMesh( btSoftBody::ISoftBody* isoftbody, + const btScalar* vertices, + const int* triangles, + int ntriangles) +{ +int maxidx=0; +for(int i=0,ni=ntriangles*3;i chks; +btAlignedObjectArray vtx; +chks.resize(maxidx*maxidx,false); +vtx.resize(maxidx); +for(int i=0,j=0,ni=maxidx*3;iAppendLink(idx[j],idx[k],1,btSoftBody::eLType::Structural); + } + } + #undef IDX + psb->AppendFace(idx[0],idx[1],idx[2]); + } +psb->RandomizeConstraints(); +return(psb); +} diff --git a/src/BulletDynamics/SoftBody/btSparseSDF.h b/src/BulletDynamics/SoftBody/btSparseSDF.h new file mode 100644 index 000000000..91f0e4a20 --- /dev/null +++ b/src/BulletDynamics/SoftBody/btSparseSDF.h @@ -0,0 +1,313 @@ +/* +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. +*/ +///btSparseSdf implementation by Nathanael Presson + +#ifndef _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_ +#define _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_ + +template +struct btSparseSdf + { + // + // Inner types + // + struct IntFrac + { + int b; + int i; + btScalar f; + }; + struct Client + { + btCollisionShape* shape; + btVector3 center; + btVector3 extent; + btScalar vsize; + int id; + }; + struct Cell + { + btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; + int c[3]; + int puid; + unsigned hash; + const Client* pclient; + Cell* next; + }; + // + // Fields + // + + btAlignedObjectArray cells; + btAlignedObjectArray clients; + int puid; + int ncells; + int nprobes; + int nqueries; + + // + // Methods + // + + // + void Initialize(int hashsize=2383) + { + cells.resize(hashsize,0); + Reset(); + } + // + void Reset() + { + for(int i=0,ni=cells.size();inext; + delete pc; + pc=pn; + } + } + puid =0; + ncells =0; + nprobes =1; + nqueries =1; + } + // + void GarbageCollect(int lifetime=64) + { + const int life=puid-lifetime; + for(int i=0;inext; + if(pc->puidnext=pn; else root=pn; + delete pc;pc=pp;--ncells; + } + pp=pc;pc=pn; + } + } + //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries); + nqueries=1; + nprobes=1; + ++puid; /* TODO: Reset puid's when int range limit is reached */ + /* else setup a priority list... */ + } + // + Client* GetClient( btCollisionShape* shape) + { + Client* pc=(Client*)shape->getUserPointer(); + if(!pc) + { + pc=new Client(); + clients.push_back(pc); + shape->setUserPointer(pc); + pc->shape = shape; + pc->id = clients.size(); + pc->vsize = 0.25; + SetShapeBounds(*pc); + } + return(pc); + } + // + btScalar Evaluate( const btVector3& x, + btCollisionShape* shape, + btVector3& normal) + { + const Client* pclient=GetClient(shape); + /* Bounds check */ + const btVector3 offset=x-pclient->center; + const btVector3 sqoffset=offset*offset; + if( (sqoffset.x()>pclient->extent.x()) || + (sqoffset.y()>pclient->extent.y()) || + (sqoffset.z()>pclient->extent.z())) return(SIMD_INFINITY); + /* Lookup cell */ + const btVector3 scx=x/pclient->vsize; + const IntFrac ix=Decompose(scx.x()); + const IntFrac iy=Decompose(scx.y()); + const IntFrac iz=Decompose(scx.z()); + const unsigned h=Hash(ix.b,iy.b,iz.b,pclient->id); + Cell*& root=cells[h%cells.size()]; + Cell* c=root; + ++nqueries; + while(c) + { + ++nprobes; + if( (c->hash==h) && + (c->c[0]==ix.b) && + (c->c[1]==iy.b) && + (c->c[2]==iz.b) && + (c->pclient==pclient)) + { break; } + else + { c=c->next; } + } + if(!c) + { + ++nprobes; + ++ncells; + c=new Cell(); + c->next=root;root=c; + c->pclient=pclient;c->hash=h; + c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b; + BuildCell(*c); + } + c->puid=puid; + /* Extract infos */ + const int o[]={ ix.i,iy.i,iz.i}; + const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0], + c->d[o[0]+1][o[1]+0][o[2]+0], + c->d[o[0]+1][o[1]+1][o[2]+0], + c->d[o[0]+0][o[1]+1][o[2]+0], + c->d[o[0]+0][o[1]+0][o[2]+1], + c->d[o[0]+1][o[1]+0][o[2]+1], + c->d[o[0]+1][o[1]+1][o[2]+1], + c->d[o[0]+0][o[1]+1][o[2]+1]}; + /* Normal */ + #if 1 + const btScalar gx[]={ d[1]-d[0],d[2]-d[3], + d[5]-d[4],d[6]-d[7]}; + const btScalar gy[]={ d[3]-d[0],d[2]-d[1], + d[7]-d[4],d[6]-d[5]}; + const btScalar gz[]={ d[4]-d[0],d[5]-d[1], + d[7]-d[3],d[6]-d[2]}; + normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f), + Lerp(gx[2],gx[3],iy.f),iz.f)); + normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f), + Lerp(gy[2],gy[3],ix.f),iz.f)); + normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f), + Lerp(gz[2],gz[3],ix.f),iy.f)); + normal = normal.normalized(); + #else + normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized(); + #endif + /* Distance */ + const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f), + Lerp(d[3],d[2],ix.f),iy.f); + const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f), + Lerp(d[7],d[6],ix.f),iy.f); + return(Lerp(d0,d1,iz.f)); + } + // + void BuildCell(Cell& c) + { + const Client* client=c.pclient; + const btVector3 org=btVector3(c.c[0],c.c[1],c.c[2])*CELLSIZE*client->vsize; + for(int k=0;k<=CELLSIZE;++k) + { + const btScalar z=client->vsize*k+org.z(); + for(int j=0;j<=CELLSIZE;++j) + { + const btScalar y=client->vsize*j+org.y(); + for(int i=0;i<=CELLSIZE;++i) + { + const btScalar x=client->vsize*i+org.x(); + c.d[i][j][k]=DistanceToShape( btVector3(x,y,z), + client->shape, + client->vsize); + } + } + } + } + // + static inline btScalar DistanceToShape(const btVector3& x, + btCollisionShape* shape, + btScalar margin) + { + btTransform unit; + unit.setIdentity(); + if(shape->isConvex()) + { + btGjkEpaSolver2::sResults res; + btConvexShape* csh=static_cast(shape); + return(btGjkEpaSolver2::SignedDistance(x,margin,csh,unit,res)); + } + return(0); + } + // + static inline void SetShapeBounds(Client& c) + { + if(c.shape->isConvex()) + { + btConvexShape* csh=static_cast(c.shape); + const btVector3 x[]={ csh->localGetSupportingVertex(btVector3(+1,0,0)), + csh->localGetSupportingVertex(btVector3(-1,0,0))}; + const btVector3 y[]={ csh->localGetSupportingVertex(btVector3(0,+1,0)), + csh->localGetSupportingVertex(btVector3(0,-1,0))}; + const btVector3 z[]={ csh->localGetSupportingVertex(btVector3(0,0,+1)), + csh->localGetSupportingVertex(btVector3(0,0,-1))}; + c.center = btVector3( x[0].x()+x[1].x(), + y[0].y()+y[1].y(), + z[0].z()+z[1].z())*0.5; + c.extent = btVector3( x[0].x()-x[1].x(), + y[0].y()-y[1].y(), + z[0].z()-z[1].z())*0.5; + c.extent += btVector3(c.vsize,c.vsize,c.vsize); + c.extent *= c.extent; + } + } + // + static inline IntFrac Decompose(btScalar x) + { + /* That one need a lot of improvements... */ + /* Remove test, faster floor... */ + IntFrac r; + x/=CELLSIZE; + const int o=x<0?(int)(-x+1):0; + x+=o;r.b=(int)x; + const btScalar k=(x-r.b)*CELLSIZE; + r.i=(int)k;r.f=k-r.i;r.b-=o; + return(r); + } + // + static inline btScalar Lerp(btScalar a,btScalar b,btScalar t) + { + return(a+(b-a)*t); + } + // + static inline unsigned Hash(int x,int y,int z,int i) + { + const int data[]={x,y,z,i}; + return(HsiehHash(data)); + } + // Modified Paul Hsieh hash + template + static inline unsigned HsiehHash(const void* pdata) + { + const unsigned short* data=(const unsigned short*)pdata; + unsigned hash=DWORDLEN<<2,tmp; + for(int i=0;i>11; + } + hash^=hash<<3;hash+=hash>>5; + hash^=hash<<4;hash+=hash>>17; + hash^=hash<<25;hash+=hash>>6; + return(hash); + } + }; + +#endif \ No newline at end of file diff --git a/src/LinearMath/btIDebugDraw.h b/src/LinearMath/btIDebugDraw.h index 2d96cff50..b37fb4762 100644 --- a/src/LinearMath/btIDebugDraw.h +++ b/src/LinearMath/btIDebugDraw.h @@ -55,6 +55,17 @@ class btIDebugDraw virtual ~btIDebugDraw() {}; virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0; + + virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& n0,const btVector3& n1,const btVector3& n2,const btVector3& color, btScalar alpha) + { + drawTriangle(v0,v1,v2,color,alpha); + } + virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& color, btScalar alpha) + { + drawLine(v0,v1,color); + drawLine(v1,v2,color); + drawLine(v2,v0,color); + } virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)=0;