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;