diff options
| author | Erwin Coumans <blender@erwincoumans.com> | 2011-03-12 23:34:17 +0300 |
|---|---|---|
| committer | Erwin Coumans <blender@erwincoumans.com> | 2011-03-12 23:34:17 +0300 |
| commit | 5e374328a87c1b418f8454d5ef38470484804961 (patch) | |
| tree | 1d6de85165175c5192f74dbd423e1d5cb48f8ff6 /extern | |
| parent | 8c526e79e31d40d56a6fecce9343c74bd9fe62d8 (diff) | |
update Bullet physics sdk to latest trunk/version 2.78
add PhysicsConstraints.exportBulletFile(char* fileName) python command
I'll be checking the bf-committers mailing list, in case this commit broke stuff
scons needs to be updated, I'll do that in a second.
Diffstat (limited to 'extern')
258 files changed, 17823 insertions, 5511 deletions
diff --git a/extern/bullet2/CMakeLists.txt b/extern/bullet2/CMakeLists.txt index 74f0fc2ab56..4b85b0d72d1 100644 --- a/extern/bullet2/CMakeLists.txt +++ b/extern/bullet2/CMakeLists.txt @@ -107,7 +107,6 @@ set(SRC src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp - src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -119,6 +118,7 @@ set(SRC src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp + src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -132,6 +132,7 @@ set(SRC src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp src/BulletDynamics/Vehicle/btRaycastVehicle.cpp src/BulletDynamics/Vehicle/btWheelInfo.cpp + src/BulletSoftBody/btDefaultSoftBodySolver.cpp src/BulletSoftBody/btSoftBody.cpp src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -143,6 +144,8 @@ set(SRC src/LinearMath/btConvexHull.cpp src/LinearMath/btGeometryUtil.cpp src/LinearMath/btQuickprof.cpp + src/LinearMath/btSerializer.cpp + src/Bullet-C-Api.h src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -247,8 +250,7 @@ set(SRC src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h - src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h - src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h +? src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -264,6 +266,7 @@ set(SRC src/BulletDynamics/ConstraintSolver/btContactConstraint.h src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h + src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h src/BulletDynamics/ConstraintSolver/btHingeConstraint.h src/BulletDynamics/ConstraintSolver/btJacobianEntry.h src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -282,6 +285,7 @@ set(SRC src/BulletDynamics/Vehicle/btRaycastVehicle.h src/BulletDynamics/Vehicle/btVehicleRaycaster.h src/BulletDynamics/Vehicle/btWheelInfo.h + src/BulletSoftBody/btDefaultSoftBodySolver.h src/BulletSoftBody/btSoftBody.h src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h src/BulletSoftBody/btSoftBodyHelpers.h @@ -310,11 +314,13 @@ set(SRC src/LinearMath/btQuickprof.h src/LinearMath/btRandom.h src/LinearMath/btScalar.h + src/LinearMath/btSerializer.h src/LinearMath/btSimdMinMax.h src/LinearMath/btStackAlloc.h src/LinearMath/btTransform.h src/LinearMath/btTransformUtil.h src/LinearMath/btVector3.h + src/btBulletCollisionCommon.h src/btBulletDynamicsCommon.h ) diff --git a/extern/bullet2/src/Bullet-C-Api.h b/extern/bullet2/src/Bullet-C-Api.h index d2123047c68..f309aba2816 100644 --- a/extern/bullet2/src/Bullet-C-Api.h +++ b/extern/bullet2/src/Bullet-C-Api.h @@ -65,7 +65,7 @@ extern "C" { Create and Delete a Physics SDK */ - extern plPhysicsSdkHandle plNewBulletSdk(void); //this could be also another sdk, like ODE, PhysX etc. + extern plPhysicsSdkHandle plNewBulletSdk(); //this could be also another sdk, like ODE, PhysX etc. extern void plDeletePhysicsSdk(plPhysicsSdkHandle physicsSdk); /** Collision World, not strictly necessary, you can also just create a Dynamics World with Rigid Bodies which internally manages the Collision World with Collision Objects */ @@ -116,16 +116,16 @@ extern "C" { extern plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height); extern plCollisionShapeHandle plNewConeShape(plReal radius, plReal height); extern plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height); - extern plCollisionShapeHandle plNewCompoundShape(void); + extern plCollisionShapeHandle plNewCompoundShape(); extern void plAddChildShape(plCollisionShapeHandle compoundShape,plCollisionShapeHandle childShape, plVector3 childPos,plQuaternion childOrn); extern void plDeleteShape(plCollisionShapeHandle shape); /* Convex Meshes */ - extern plCollisionShapeHandle plNewConvexHullShape(void); + extern plCollisionShapeHandle plNewConvexHullShape(); extern void plAddVertex(plCollisionShapeHandle convexHull, plReal x,plReal y,plReal z); /* Concave static triangle meshes */ - extern plMeshInterfaceHandle plNewMeshInterface(void); + extern plMeshInterfaceHandle plNewMeshInterface(); extern void plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2); extern plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle); @@ -147,6 +147,7 @@ extern "C" { extern void plSetPosition(plRigidBodyHandle object, const plVector3 position); extern void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation); extern void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient); + extern void plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix); typedef struct plRayCastResult { plRigidBodyHandle m_body; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h index cad21b4cad2..07167af3baf 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -150,6 +150,8 @@ public: virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); + void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const; ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result @@ -285,6 +287,31 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,cons } } +template <typename BP_FP_INT_TYPE> +void btAxisSweep3Internal<BP_FP_INT_TYPE>::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) +{ + if (m_raycastAccelerator) + { + m_raycastAccelerator->aabbTest(aabbMin,aabbMax,callback); + } else + { + //choose axis? + BP_FP_INT_TYPE axis = 0; + //for each proxy + for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++) + { + if (m_pEdges[axis][i].IsMax()) + { + Handle* handle = getHandle(m_pEdges[axis][i].m_handle); + if (TestAabbAgainstAabb2(aabbMin,aabbMax,handle->m_aabbMin,handle->m_aabbMax)) + { + callback.process(handle); + } + } + } + } +} + template <typename BP_FP_INT_TYPE> diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index b7bbaf512ae..fe414effbfc 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -26,7 +26,14 @@ class btOverlappingPairCache; -struct btBroadphaseRayCallback +struct btBroadphaseAabbCallback +{ + virtual ~btBroadphaseAabbCallback() {} + virtual bool process(const btBroadphaseProxy* proxy) = 0; +}; + + +struct btBroadphaseRayCallback : public btBroadphaseAabbCallback { ///added some cached data to accelerate ray-AABB tests btVector3 m_rayDirectionInverse; @@ -34,7 +41,6 @@ struct btBroadphaseRayCallback btScalar m_lambda_max; virtual ~btBroadphaseRayCallback() {} - virtual bool process(const btBroadphaseProxy* proxy) = 0; }; #include "LinearMath/btVector3.h" @@ -54,6 +60,8 @@ public: virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)) = 0; + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) = 0; + ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb virtual void calculateOverlappingPairs(btDispatcher* dispatcher)=0; @@ -65,7 +73,7 @@ public: virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0; ///reset broadphase internal structures, to ensure determinism/reproducability - virtual void resetPool(btDispatcher* dispatcher) {}; + virtual void resetPool(btDispatcher* dispatcher) { (void) dispatcher; }; virtual void printStats() = 0; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index a9f3223798b..62d349739c3 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -46,6 +46,8 @@ IMPLICIT_CONVEX_SHAPES_START_HERE, UNIFORM_SCALING_SHAPE_PROXYTYPE, MINKOWSKI_SUM_SHAPE_PROXYTYPE, MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE, + BOX_2D_SHAPE_PROXYTYPE, + CONVEX_2D_SHAPE_PROXYTYPE, CUSTOM_CONVEX_SHAPE_TYPE, //concave shapes CONCAVE_SHAPES_START_HERE, @@ -139,6 +141,11 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); return (proxyType < CONCAVE_SHAPES_START_HERE); } + static SIMD_FORCE_INLINE bool isNonMoving(int proxyType) + { + return (isConcave(proxyType) && !(proxyType==GIMPACT_SHAPE_PROXYTYPE)); + } + static SIMD_FORCE_INLINE bool isConcave(int proxyType) { return ((proxyType > CONCAVE_SHAPES_START_HERE) && @@ -148,14 +155,22 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); { return (proxyType == COMPOUND_SHAPE_PROXYTYPE); } + + static SIMD_FORCE_INLINE bool isSoftBody(int proxyType) + { + return (proxyType == SOFTBODY_SHAPE_PROXYTYPE); + } + static SIMD_FORCE_INLINE bool isInfinite(int proxyType) { return (proxyType == STATIC_PLANE_PROXYTYPE); } - static SIMD_FORCE_INLINE bool isSoftBody(int proxyType) + + static SIMD_FORCE_INLINE bool isConvex2d(int proxyType) { - return (proxyType == SOFTBODY_SHAPE_PROXYTYPE); + return (proxyType == BOX_2D_SHAPE_PROXYTYPE) || (proxyType == CONVEX_2D_SHAPE_PROXYTYPE); } + } ; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h index 1618ad9fdd3..0d8bca41c8e 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h @@ -44,7 +44,7 @@ struct btCollisionAlgorithmConstructionInfo btDispatcher* m_dispatcher1; btPersistentManifold* m_manifold; - int getDispatcherId(); +// int getDispatcherId(); }; @@ -59,7 +59,7 @@ protected: btDispatcher* m_dispatcher; protected: - int getDispatcherId(); +// int getDispatcherId(); public: diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index a6e36b47049..95443af5070 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -61,7 +61,7 @@ static void getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth) if(node->isinternal()) { getmaxdepth(node->childs[0],depth+1,maxdepth); - getmaxdepth(node->childs[0],depth+1,maxdepth); + getmaxdepth(node->childs[1],depth+1,maxdepth); } else maxdepth=btMax(maxdepth,depth); } @@ -238,7 +238,7 @@ static void split( const tNodeArray& leaves, right.resize(0); for(int i=0,ni=leaves.size();i<ni;++i) { - if(dot(axis,leaves[i]->volume.Center()-org)<0) + if(btDot(axis,leaves[i]->volume.Center()-org)<0) left.push_back(leaves[i]); else right.push_back(leaves[i]); @@ -319,7 +319,7 @@ static btDbvtNode* topdown(btDbvt* pdbvt, const btVector3 x=leaves[i]->volume.Center()-org; for(int j=0;j<3;++j) { - ++splitcount[j][dot(x,axis[j])>0?1:0]; + ++splitcount[j][btDot(x,axis[j])>0?1:0]; } } for( i=0;i<3;++i) diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h index d3cf1e75039..2bb8ef5d2a7 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -32,7 +32,7 @@ subject to the following restrictions: #define DBVT_IMPL_SSE 1 // SSE // Template implementation of ICollide -#ifdef WIN32 +#ifdef _WIN32 #if (defined (_MSC_VER) && _MSC_VER >= 1400) #define DBVT_USE_TEMPLATE 1 #else @@ -57,7 +57,7 @@ subject to the following restrictions: // Specific methods implementation //SSE gives errors on a MSVC 7.1 -#ifdef BT_USE_SSE +#if defined (BT_USE_SSE) && defined (_WIN32) #define DBVT_SELECT_IMPL DBVT_IMPL_SSE #define DBVT_MERGE_IMPL DBVT_IMPL_SSE #define DBVT_INT0_IMPL DBVT_IMPL_SSE @@ -92,7 +92,7 @@ subject to the following restrictions: #endif #if DBVT_USE_MEMMOVE -#ifndef __CELLOS_LV2__ +#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) #include <memory.h> #endif #include <string.h> @@ -484,8 +484,8 @@ DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) con case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z()); pi=btVector3(mi.x(),mi.y(),mi.z());break; } - if((dot(n,px)+o)<0) return(-1); - if((dot(n,pi)+o)>=0) return(+1); + if((btDot(n,px)+o)<0) return(-1); + if((btDot(n,pi)+o)>=0) return(+1); return(0); } @@ -496,7 +496,7 @@ DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned si const btVector3 p( b[(signs>>0)&1]->x(), b[(signs>>1)&1]->y(), b[(signs>>2)&1]->z()); - return(dot(p,v)); + return(btDot(p,v)); } // @@ -947,6 +947,7 @@ inline void btDbvt::rayTestInternal( const btDbvtNode* root, const btVector3& aabbMax, DBVT_IPOLICY) const { + (void) rayTo; DBVT_CHECKTYPE if(root) { @@ -961,8 +962,8 @@ inline void btDbvt::rayTestInternal( const btDbvtNode* root, do { const btDbvtNode* node=stack[--depth]; - bounds[0] = node->volume.Mins()+aabbMin; - bounds[1] = node->volume.Maxs()+aabbMax; + bounds[0] = node->volume.Mins()-aabbMax; + bounds[1] = node->volume.Maxs()-aabbMin; btScalar tmin=1.f,lambda_min=0.f; unsigned int result1=false; result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); @@ -1000,11 +1001,11 @@ inline void btDbvt::rayTest( const btDbvtNode* root, btVector3 rayDir = (rayTo-rayFrom); rayDir.normalize (); - ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT btVector3 rayDirectionInverse; - rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; btScalar lambda_max = rayDir.dot(rayTo-rayFrom); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp index f231717af17..75cfac64368 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -12,6 +12,7 @@ subject to the following restrictions: 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. */ + ///btDbvtBroadphase implementation by Nathanael Presson #include "btDbvtBroadphase.h" @@ -123,7 +124,7 @@ btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) m_deferedcollide = false; m_needcleanup = true; m_releasepaircache = (paircache!=0)?false:true; - m_prediction = 1/(btScalar)2; + m_prediction = 0; m_stageCurrent = 0; m_fixedleft = 0; m_fupdates = 1; @@ -249,6 +250,34 @@ void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, } + +struct BroadphaseAabbTester : btDbvt::ICollide +{ + btBroadphaseAabbCallback& m_aabbCallback; + BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback) + :m_aabbCallback(orgCallback) + { + } + void Process(const btDbvtNode* leaf) + { + btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; + m_aabbCallback.process(proxy); + } +}; + +void btDbvtBroadphase::aabbTest(const btVector3& aabbMin,const btVector3& aabbMax,btBroadphaseAabbCallback& aabbCallback) +{ + BroadphaseAabbTester callback(aabbCallback); + + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(aabbMin,aabbMax); + //process all children, that overlap with the given AABB bounds + m_sets[0].collideTV(m_sets[0].m_root,bounds,callback); + m_sets[1].collideTV(m_sets[1].m_root,bounds,callback); + +} + + + // void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, const btVector3& aabbMin, @@ -316,6 +345,47 @@ void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, } } + +// +void btDbvtBroadphase::setAabbForceUpdate( btBroadphaseProxy* absproxy, + const btVector3& aabbMin, + const btVector3& aabbMax, + btDispatcher* /*dispatcher*/) +{ + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); + bool docollide=false; + if(proxy->stage==STAGECOUNT) + {/* fixed -> dynamic set */ + m_sets[1].remove(proxy->leaf); + proxy->leaf=m_sets[0].insert(aabb,proxy); + docollide=true; + } + else + {/* dynamic set */ + ++m_updates_call; + /* Teleporting */ + m_sets[0].update(proxy->leaf,aabb); + ++m_updates_done; + docollide=true; + } + listremove(proxy,m_stageRoots[proxy->stage]); + proxy->m_aabbMin = aabbMin; + proxy->m_aabbMax = aabbMax; + proxy->stage = m_stageCurrent; + listappend(proxy,m_stageRoots[m_stageCurrent]); + if(docollide) + { + m_needcleanup=true; + if(!m_deferedcollide) + { + btDbvtTreeCollider collider(this); + m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); + } + } +} + // void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) { @@ -571,7 +641,6 @@ void btDbvtBroadphase::resetPool(btDispatcher* dispatcher) m_deferedcollide = false; m_needcleanup = true; - m_prediction = 1/(btScalar)2; m_stageCurrent = 0; m_fixedleft = 0; m_fupdates = 1; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h index fe70bc39c43..18b64ad0e57 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -12,6 +12,7 @@ subject to the following restrictions: 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. */ + ///btDbvtBroadphase implementation by Nathanael Presson #ifndef BT_DBVT_BROADPHASE_H #define BT_DBVT_BROADPHASE_H @@ -101,26 +102,45 @@ struct btDbvtBroadphase : btBroadphaseInterface ~btDbvtBroadphase(); void collide(btDispatcher* dispatcher); void optimize(); - /* btBroadphaseInterface Implementation */ + + /* btBroadphaseInterface Implementation */ btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); - void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); - - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - void calculateOverlappingPairs(btDispatcher* dispatcher); - btOverlappingPairCache* getOverlappingPairCache(); - const btOverlappingPairCache* getOverlappingPairCache() const; - void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; - void printStats(); - static void benchmark(btBroadphaseInterface*); + virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); + virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); + virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); + virtual btOverlappingPairCache* getOverlappingPairCache(); + virtual const btOverlappingPairCache* getOverlappingPairCache() const; + virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; + virtual void printStats(); - void performDeferredRemoval(btDispatcher* dispatcher); ///reset broadphase internal structures, to ensure determinism/reproducability virtual void resetPool(btDispatcher* dispatcher); + void performDeferredRemoval(btDispatcher* dispatcher); + + void setVelocityPrediction(btScalar prediction) + { + m_prediction = prediction; + } + btScalar getVelocityPrediction() const + { + return m_prediction; + } + + ///this setAabbForceUpdate is similar to setAabb but always forces the aabb update. + ///it is not part of the btBroadphaseInterface but specific to btDbvtBroadphase. + ///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see + ///http://code.google.com/p/bullet/issues/detail?id=223 + void setAabbForceUpdate( btBroadphaseProxy* absproxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* /*dispatcher*/); + + static void benchmark(btBroadphaseInterface*); + + }; #endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h index ee57aa96151..8ded0006c3b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -46,8 +46,9 @@ struct btDispatcherInfo m_enableSPU(true), m_useEpa(true), m_allowedCcdPenetration(btScalar(0.04)), - m_useConvexConservativeDistanceUtil(true), + m_useConvexConservativeDistanceUtil(false), m_convexConservativeDistanceThreshold(0.0f), + m_convexMaxDistanceUseCPT(false), m_stackAllocator(0) { @@ -64,6 +65,7 @@ struct btDispatcherInfo btScalar m_allowedCcdPenetration; bool m_useConvexConservativeDistanceUtil; btScalar m_convexConservativeDistanceThreshold; + bool m_convexMaxDistanceUseCPT; btStackAlloc* m_stackAllocator; }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h index 91c504eee22..7bcfe6b132a 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h @@ -133,8 +133,8 @@ public: ///will add some transform later virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const { - aabbMin.setValue(-1e30f,-1e30f,-1e30f); - aabbMax.setValue(1e30f,1e30f,1e30f); + aabbMin.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT); + aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); } void buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index b209bcb9a20..041bbe05ae2 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -240,7 +240,7 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx }*/ int count = m_overlappingPairArray.size(); int oldCapacity = m_overlappingPairArray.capacity(); - void* mem = &m_overlappingPairArray.expand(); + void* mem = &m_overlappingPairArray.expandNonInitializing(); //this is where we add an actual pair, so also call the 'ghost' if (m_ghostPairCallback) @@ -467,7 +467,7 @@ btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseP if (!needsBroadphaseCollision(proxy0,proxy1)) return 0; - void* mem = &m_overlappingPairArray.expand(); + void* mem = &m_overlappingPairArray.expandNonInitializing(); btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); gOverlappingPairs++; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index eda45c47b5b..3945afb8d70 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -457,6 +457,7 @@ public: virtual void sortOverlappingPairs(btDispatcher* dispatcher) { + (void) dispatcher; } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp index 8bef8f0d43e..c911435a946 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -17,6 +17,7 @@ subject to the following restrictions: #include "LinearMath/btAabbUtil2.h" #include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btSerializer.h" #define RAYAABB2 @@ -78,10 +79,10 @@ void btQuantizedBvh::buildInternal() #ifdef DEBUG_PATCH_COLORS btVector3 color[4]= { - btVector3(255,0,0), - btVector3(0,255,0), - btVector3(0,0,255), - btVector3(0,255,255) + btVector3(1,0,0), + btVector3(0,1,0), + btVector3(0,0,1), + btVector3(0,1,1) }; #endif //DEBUG_PATCH_COLORS @@ -474,9 +475,9 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall lambda_max = rayDir.dot(rayTarget-raySource); ///what about division by zero? --> just set rayDirection[i] to 1.0 btVector3 rayDirectionInverse; - rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; #endif @@ -493,8 +494,8 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall bounds[0] = rootNode->m_aabbMinOrg; bounds[1] = rootNode->m_aabbMaxOrg; /* Add box cast extents */ - bounds[0] += aabbMin; - bounds[1] += aabbMax; + bounds[0] -= aabbMax; + bounds[1] -= aabbMin; aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg); //perhaps profile if it is worth doing the aabbOverlap test first @@ -561,9 +562,9 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* rayDirection.normalize (); lambda_max = rayDirection.dot(rayTarget-raySource); ///what about division by zero? --> just set rayDirection[i] to 1.0 - rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[0]; - rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[1]; - rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[2]; + rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[0]; + rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[1]; + rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[2]; unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0}; #endif @@ -617,8 +618,8 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* bounds[0] = unQuantize(rootNode->m_quantizedAabbMin); bounds[1] = unQuantize(rootNode->m_quantizedAabbMax); /* Add box cast extents */ - bounds[0] += aabbMin; - bounds[1] += aabbMax; + bounds[0] -= aabbMax; + bounds[1] -= aabbMin; btVector3 normal; #if 0 bool ra2 = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); @@ -830,7 +831,7 @@ unsigned int btQuantizedBvh::getAlignmentSerializationPadding() return 0;//BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; } -unsigned btQuantizedBvh::calculateSerializeBufferSize() +unsigned btQuantizedBvh::calculateSerializeBufferSize() const { unsigned baseSize = sizeof(btQuantizedBvh) + getAlignmentSerializationPadding(); baseSize += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; @@ -841,7 +842,7 @@ unsigned btQuantizedBvh::calculateSerializeBufferSize() return baseSize + m_curNodeIndex * sizeof(btOptimizedBvhNode); } -bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) +bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) const { btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size()); m_subtreeHeaderCount = m_SubtreeHeaders.size(); @@ -1143,6 +1144,232 @@ m_bulletVersion(BT_BULLET_VERSION) } +void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData) +{ + m_bvhAabbMax.deSerializeFloat(quantizedBvhFloatData.m_bvhAabbMax); + m_bvhAabbMin.deSerializeFloat(quantizedBvhFloatData.m_bvhAabbMin); + m_bvhQuantization.deSerializeFloat(quantizedBvhFloatData.m_bvhQuantization); + + m_curNodeIndex = quantizedBvhFloatData.m_curNodeIndex; + m_useQuantization = quantizedBvhFloatData.m_useQuantization!=0; + + { + int numElem = quantizedBvhFloatData.m_numContiguousLeafNodes; + m_contiguousNodes.resize(numElem); + + if (numElem) + { + btOptimizedBvhNodeFloatData* memPtr = quantizedBvhFloatData.m_contiguousNodesPtr; + + for (int i=0;i<numElem;i++,memPtr++) + { + m_contiguousNodes[i].m_aabbMaxOrg.deSerializeFloat(memPtr->m_aabbMaxOrg); + m_contiguousNodes[i].m_aabbMinOrg.deSerializeFloat(memPtr->m_aabbMinOrg); + m_contiguousNodes[i].m_escapeIndex = memPtr->m_escapeIndex; + m_contiguousNodes[i].m_subPart = memPtr->m_subPart; + m_contiguousNodes[i].m_triangleIndex = memPtr->m_triangleIndex; + } + } + } + + { + int numElem = quantizedBvhFloatData.m_numQuantizedContiguousNodes; + m_quantizedContiguousNodes.resize(numElem); + + if (numElem) + { + btQuantizedBvhNodeData* memPtr = quantizedBvhFloatData.m_quantizedContiguousNodesPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex = memPtr->m_escapeIndexOrTriangleIndex; + m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0]; + m_quantizedContiguousNodes[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1]; + m_quantizedContiguousNodes[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2]; + m_quantizedContiguousNodes[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0]; + m_quantizedContiguousNodes[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1]; + m_quantizedContiguousNodes[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2]; + } + } + } + + m_traversalMode = btTraversalMode(quantizedBvhFloatData.m_traversalMode); + + { + int numElem = quantizedBvhFloatData.m_numSubtreeHeaders; + m_SubtreeHeaders.resize(numElem); + if (numElem) + { + btBvhSubtreeInfoData* memPtr = quantizedBvhFloatData.m_subTreeInfoPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0] ; + m_SubtreeHeaders[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1]; + m_SubtreeHeaders[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2]; + m_SubtreeHeaders[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0]; + m_SubtreeHeaders[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1]; + m_SubtreeHeaders[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2]; + m_SubtreeHeaders[i].m_rootNodeIndex = memPtr->m_rootNodeIndex; + m_SubtreeHeaders[i].m_subtreeSize = memPtr->m_subtreeSize; + } + } + } +} + +void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantizedBvhDoubleData) +{ + m_bvhAabbMax.deSerializeDouble(quantizedBvhDoubleData.m_bvhAabbMax); + m_bvhAabbMin.deSerializeDouble(quantizedBvhDoubleData.m_bvhAabbMin); + m_bvhQuantization.deSerializeDouble(quantizedBvhDoubleData.m_bvhQuantization); + + m_curNodeIndex = quantizedBvhDoubleData.m_curNodeIndex; + m_useQuantization = quantizedBvhDoubleData.m_useQuantization!=0; + + { + int numElem = quantizedBvhDoubleData.m_numContiguousLeafNodes; + m_contiguousNodes.resize(numElem); + + if (numElem) + { + btOptimizedBvhNodeDoubleData* memPtr = quantizedBvhDoubleData.m_contiguousNodesPtr; + + for (int i=0;i<numElem;i++,memPtr++) + { + m_contiguousNodes[i].m_aabbMaxOrg.deSerializeDouble(memPtr->m_aabbMaxOrg); + m_contiguousNodes[i].m_aabbMinOrg.deSerializeDouble(memPtr->m_aabbMinOrg); + m_contiguousNodes[i].m_escapeIndex = memPtr->m_escapeIndex; + m_contiguousNodes[i].m_subPart = memPtr->m_subPart; + m_contiguousNodes[i].m_triangleIndex = memPtr->m_triangleIndex; + } + } + } + + { + int numElem = quantizedBvhDoubleData.m_numQuantizedContiguousNodes; + m_quantizedContiguousNodes.resize(numElem); + + if (numElem) + { + btQuantizedBvhNodeData* memPtr = quantizedBvhDoubleData.m_quantizedContiguousNodesPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex = memPtr->m_escapeIndexOrTriangleIndex; + m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0]; + m_quantizedContiguousNodes[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1]; + m_quantizedContiguousNodes[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2]; + m_quantizedContiguousNodes[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0]; + m_quantizedContiguousNodes[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1]; + m_quantizedContiguousNodes[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2]; + } + } + } + + m_traversalMode = btTraversalMode(quantizedBvhDoubleData.m_traversalMode); + + { + int numElem = quantizedBvhDoubleData.m_numSubtreeHeaders; + m_SubtreeHeaders.resize(numElem); + if (numElem) + { + btBvhSubtreeInfoData* memPtr = quantizedBvhDoubleData.m_subTreeInfoPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0] ; + m_SubtreeHeaders[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1]; + m_SubtreeHeaders[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2]; + m_SubtreeHeaders[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0]; + m_SubtreeHeaders[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1]; + m_SubtreeHeaders[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2]; + m_SubtreeHeaders[i].m_rootNodeIndex = memPtr->m_rootNodeIndex; + m_SubtreeHeaders[i].m_subtreeSize = memPtr->m_subtreeSize; + } + } + } + +} + + + +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btQuantizedBvhData* quantizedData = (btQuantizedBvhData*)dataBuffer; + + m_bvhAabbMax.serialize(quantizedData->m_bvhAabbMax); + m_bvhAabbMin.serialize(quantizedData->m_bvhAabbMin); + m_bvhQuantization.serialize(quantizedData->m_bvhQuantization); + + quantizedData->m_curNodeIndex = m_curNodeIndex; + quantizedData->m_useQuantization = m_useQuantization; + + quantizedData->m_numContiguousLeafNodes = m_contiguousNodes.size(); + quantizedData->m_contiguousNodesPtr = (btOptimizedBvhNodeData*) (m_contiguousNodes.size() ? serializer->getUniquePointer((void*)&m_contiguousNodes[0]) : 0); + if (quantizedData->m_contiguousNodesPtr) + { + int sz = sizeof(btOptimizedBvhNodeData); + int numElem = m_contiguousNodes.size(); + btChunk* chunk = serializer->allocate(sz,numElem); + btOptimizedBvhNodeData* memPtr = (btOptimizedBvhNodeData*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + m_contiguousNodes[i].m_aabbMaxOrg.serialize(memPtr->m_aabbMaxOrg); + m_contiguousNodes[i].m_aabbMinOrg.serialize(memPtr->m_aabbMinOrg); + memPtr->m_escapeIndex = m_contiguousNodes[i].m_escapeIndex; + memPtr->m_subPart = m_contiguousNodes[i].m_subPart; + memPtr->m_triangleIndex = m_contiguousNodes[i].m_triangleIndex; + } + serializer->finalizeChunk(chunk,"btOptimizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_contiguousNodes[0]); + } + + quantizedData->m_numQuantizedContiguousNodes = m_quantizedContiguousNodes.size(); +// printf("quantizedData->m_numQuantizedContiguousNodes=%d\n",quantizedData->m_numQuantizedContiguousNodes); + quantizedData->m_quantizedContiguousNodesPtr =(btQuantizedBvhNodeData*) (m_quantizedContiguousNodes.size() ? serializer->getUniquePointer((void*)&m_quantizedContiguousNodes[0]) : 0); + if (quantizedData->m_quantizedContiguousNodesPtr) + { + int sz = sizeof(btQuantizedBvhNodeData); + int numElem = m_quantizedContiguousNodes.size(); + btChunk* chunk = serializer->allocate(sz,numElem); + btQuantizedBvhNodeData* memPtr = (btQuantizedBvhNodeData*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + memPtr->m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex; + memPtr->m_quantizedAabbMax[0] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[0]; + memPtr->m_quantizedAabbMax[1] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[1]; + memPtr->m_quantizedAabbMax[2] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[2]; + memPtr->m_quantizedAabbMin[0] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[0]; + memPtr->m_quantizedAabbMin[1] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[1]; + memPtr->m_quantizedAabbMin[2] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[2]; + } + serializer->finalizeChunk(chunk,"btQuantizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_quantizedContiguousNodes[0]); + } + + quantizedData->m_traversalMode = int(m_traversalMode); + quantizedData->m_numSubtreeHeaders = m_SubtreeHeaders.size(); + + quantizedData->m_subTreeInfoPtr = (btBvhSubtreeInfoData*) (m_SubtreeHeaders.size() ? serializer->getUniquePointer((void*)&m_SubtreeHeaders[0]) : 0); + if (quantizedData->m_subTreeInfoPtr) + { + int sz = sizeof(btBvhSubtreeInfoData); + int numElem = m_SubtreeHeaders.size(); + btChunk* chunk = serializer->allocate(sz,numElem); + btBvhSubtreeInfoData* memPtr = (btBvhSubtreeInfoData*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + memPtr->m_quantizedAabbMax[0] = m_SubtreeHeaders[i].m_quantizedAabbMax[0]; + memPtr->m_quantizedAabbMax[1] = m_SubtreeHeaders[i].m_quantizedAabbMax[1]; + memPtr->m_quantizedAabbMax[2] = m_SubtreeHeaders[i].m_quantizedAabbMax[2]; + memPtr->m_quantizedAabbMin[0] = m_SubtreeHeaders[i].m_quantizedAabbMin[0]; + memPtr->m_quantizedAabbMin[1] = m_SubtreeHeaders[i].m_quantizedAabbMin[1]; + memPtr->m_quantizedAabbMin[2] = m_SubtreeHeaders[i].m_quantizedAabbMin[2]; + + memPtr->m_rootNodeIndex = m_SubtreeHeaders[i].m_rootNodeIndex; + memPtr->m_subtreeSize = m_SubtreeHeaders[i].m_subtreeSize; + } + serializer->finalizeChunk(chunk,"btBvhSubtreeInfoData",BT_ARRAY_CODE,(void*)&m_SubtreeHeaders[0]); + } + return btQuantizedBvhDataName; +} + + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h index ced457b6036..aa30d43a025 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @@ -16,6 +16,8 @@ subject to the following restrictions: #ifndef QUANTIZED_BVH_H #define QUANTIZED_BVH_H +class btSerializer; + //#define DEBUG_CHECK_DEQUANTIZATION 1 #ifdef DEBUG_CHECK_DEQUANTIZATION #ifdef __SPU__ @@ -29,6 +31,17 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "LinearMath/btAlignedAllocator.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define btQuantizedBvhData btQuantizedBvhDoubleData +#define btOptimizedBvhNodeData btOptimizedBvhNodeDoubleData +#define btQuantizedBvhDataName "btQuantizedBvhDoubleData" +#else +#define btQuantizedBvhData btQuantizedBvhFloatData +#define btOptimizedBvhNodeData btOptimizedBvhNodeFloatData +#define btQuantizedBvhDataName "btQuantizedBvhFloatData" +#endif + + //http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp @@ -190,7 +203,7 @@ protected: BvhSubtreeInfoArray m_SubtreeHeaders; //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray - int m_subtreeHeaderCount; + mutable int m_subtreeHeaderCount; @@ -443,17 +456,32 @@ public: return m_SubtreeHeaders; } +//////////////////////////////////////////////////////////////////// /////Calculate space needed to store BVH for serialization - unsigned calculateSerializeBufferSize(); + unsigned calculateSerializeBufferSize() const; /// Data buffer MUST be 16 byte aligned - virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian); + virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const; ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' static btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); static unsigned int getAlignmentSerializationPadding(); +////////////////////////////////////////////////////////////////////// + + + virtual int calculateSerializeBufferSizeNew() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + virtual void deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData); + + virtual void deSerializeDouble(struct btQuantizedBvhDoubleData& quantizedBvhDoubleData); + + +//////////////////////////////////////////////////////////////////// SIMD_FORCE_INLINE bool isQuantized() { @@ -470,4 +498,82 @@ private: ; +struct btBvhSubtreeInfoData +{ + int m_rootNodeIndex; + int m_subtreeSize; + unsigned short m_quantizedAabbMin[3]; + unsigned short m_quantizedAabbMax[3]; +}; + +struct btOptimizedBvhNodeFloatData +{ + btVector3FloatData m_aabbMinOrg; + btVector3FloatData m_aabbMaxOrg; + int m_escapeIndex; + int m_subPart; + int m_triangleIndex; + char m_pad[4]; +}; + +struct btOptimizedBvhNodeDoubleData +{ + btVector3DoubleData m_aabbMinOrg; + btVector3DoubleData m_aabbMaxOrg; + int m_escapeIndex; + int m_subPart; + int m_triangleIndex; + char m_pad[4]; +}; + + +struct btQuantizedBvhNodeData +{ + unsigned short m_quantizedAabbMin[3]; + unsigned short m_quantizedAabbMax[3]; + int m_escapeIndexOrTriangleIndex; +}; + +struct btQuantizedBvhFloatData +{ + btVector3FloatData m_bvhAabbMin; + btVector3FloatData m_bvhAabbMax; + btVector3FloatData m_bvhQuantization; + int m_curNodeIndex; + int m_useQuantization; + int m_numContiguousLeafNodes; + int m_numQuantizedContiguousNodes; + btOptimizedBvhNodeFloatData *m_contiguousNodesPtr; + btQuantizedBvhNodeData *m_quantizedContiguousNodesPtr; + btBvhSubtreeInfoData *m_subTreeInfoPtr; + int m_traversalMode; + int m_numSubtreeHeaders; + +}; + +struct btQuantizedBvhDoubleData +{ + btVector3DoubleData m_bvhAabbMin; + btVector3DoubleData m_bvhAabbMax; + btVector3DoubleData m_bvhQuantization; + int m_curNodeIndex; + int m_useQuantization; + int m_numContiguousLeafNodes; + int m_numQuantizedContiguousNodes; + btOptimizedBvhNodeDoubleData *m_contiguousNodesPtr; + btQuantizedBvhNodeData *m_quantizedContiguousNodesPtr; + + int m_traversalMode; + int m_numSubtreeHeaders; + btBvhSubtreeInfoData *m_subTreeInfoPtr; +}; + + +SIMD_FORCE_INLINE int btQuantizedBvh::calculateSerializeBufferSizeNew() const +{ + return sizeof(btQuantizedBvhData); +} + + + #endif //QUANTIZED_BVH_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index caed63db005..752fcd0fef2 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -20,6 +20,8 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btAabbUtil2.h" + #include <new> extern int gOverlappingPairs; @@ -166,6 +168,23 @@ void btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo } +void btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) +{ + for (int i=0; i <= m_LastHandleIndex; i++) + { + btSimpleBroadphaseProxy* proxy = &m_pHandles[i]; + if(!proxy->m_clientObject) + { + continue; + } + if (TestAabbAgainstAabb2(aabbMin,aabbMax,proxy->m_aabbMin,proxy->m_aabbMax)) + { + callback.process(proxy); + } + } +} + + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index cc7613bf6a0..3e7c7ee3b62 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -136,6 +136,7 @@ public: virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0)); + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); btOverlappingPairCache* getOverlappingPairCache() { @@ -153,8 +154,8 @@ public: ///will add some transform later virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const { - aabbMin.setValue(-1e30f,-1e30f,-1e30f); - aabbMax.setValue(1e30f,1e30f,1e30f); + aabbMin.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT); + aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); } virtual void printStats() diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index 9a749a03793..23a5c7526b4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -37,7 +37,7 @@ void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Res btVector3 point,normal; btScalar timeOfImpact = btScalar(1.); btScalar depth = btScalar(0.); -// output.m_distance = btScalar(1e30); +// output.m_distance = btScalar(BT_LARGE_FLOAT); //move sphere into triangle space btTransform sphereInTr = transformB.inverseTimes(transformA); @@ -57,8 +57,6 @@ void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Res } -#define MAX_OVERLAP btScalar(0.) - // See also geometrictools.com @@ -93,48 +91,39 @@ bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* ve return pointInTriangle(vertices, lnormal, &lp); } -///combined discrete/continuous sphere-triangle bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold) { const btVector3* vertices = &m_triangle->getVertexPtr(0); - const btVector3& c = sphereCenter; - btScalar r = m_sphere->getRadius(); - - btVector3 delta (0,0,0); + + btScalar radius = m_sphere->getRadius(); + btScalar radiusWithThreshold = radius + contactBreakingThreshold; btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); normal.normalize(); - btVector3 p1ToCentre = c - vertices[0]; + btVector3 p1ToCentre = sphereCenter - vertices[0]; btScalar distanceFromPlane = p1ToCentre.dot(normal); if (distanceFromPlane < btScalar(0.)) { //triangle facing the other way - distanceFromPlane *= btScalar(-1.); normal *= btScalar(-1.); } - btScalar contactMargin = contactBreakingThreshold; - bool isInsideContactPlane = distanceFromPlane < r + contactMargin; - bool isInsideShellPlane = distanceFromPlane < r; + bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; - btScalar deltaDotNormal = delta.dot(normal); - if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0)) - return false; - // Check for contact / intersection bool hasContact = false; btVector3 contactPoint; if (isInsideContactPlane) { - if (facecontains(c,vertices,normal)) { + if (facecontains(sphereCenter,vertices,normal)) { // Inside the contact wedge - touches a point on the shell plane hasContact = true; - contactPoint = c - normal*distanceFromPlane; + contactPoint = sphereCenter - normal*distanceFromPlane; } else { // Could be inside one of the contact capsules - btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); + btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold; btVector3 nearestOnEdge; for (int i = 0; i < m_triangle->getNumEdges(); i++) { @@ -143,7 +132,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po m_triangle->getEdge(i,pa,pb); - btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge); + btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge); if (distanceSqr < contactCapsuleRadiusSqr) { // Yep, we're inside a capsule hasContact = true; @@ -155,24 +144,27 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po } if (hasContact) { - btVector3 contactToCentre = c - contactPoint; + btVector3 contactToCentre = sphereCenter - contactPoint; btScalar distanceSqr = contactToCentre.length2(); - if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) { - btScalar distance = btSqrt(distanceSqr); - resultNormal = contactToCentre; - resultNormal.normalize(); - point = contactPoint; - depth = -(r-distance); + + if (distanceSqr < radiusWithThreshold*radiusWithThreshold) + { + if (distanceSqr>SIMD_EPSILON) + { + btScalar distance = btSqrt(distanceSqr); + resultNormal = contactToCentre; + resultNormal.normalize(); + point = contactPoint; + depth = -(radius-distance); + } else + { + btScalar distance = 0.f; + resultNormal = normal; + point = contactPoint; + depth = -radius; + } return true; } - - if (delta.dot(contactToCentre) >= btScalar(0.0)) - return false; - - // Moving towards the contact point -> collision - point = contactPoint; - timeOfImpact = btScalar(0.0); - return true; } return false; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index 981bd54e76c..f656e5c323a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -34,9 +34,11 @@ struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface virtual ~SphereTriangleDetector() {}; + bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold); + private: - bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold); + bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ); bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp new file mode 100644 index 00000000000..2182d0d7e49 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp @@ -0,0 +1,435 @@ +/* +Bullet Continuous Collision Detection and Physics Library +* The b2CollidePolygons routines are Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + +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. +*/ + +///btBox2dBox2dCollisionAlgorithm, with modified b2CollidePolygons routines from the Box2D library. +///The modifications include: switching from b2Vec to btVector3, redefinition of b2Dot, b2Cross + +#include "btBox2dBox2dCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h" +#include "BulletCollision/CollisionShapes/btBox2dShape.h" + +#define USE_PERSISTENT_CONTACTS 1 + +btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1) +: btActivatingCollisionAlgorithm(ci,obj0,obj1), +m_ownManifold(false), +m_manifoldPtr(mf) +{ + if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1)) + { + m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1); + m_ownManifold = true; + } +} + +btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm() +{ + + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } + +} + + +void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB); + +//#include <stdio.h> +void btBox2dBox2dCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + if (!m_manifoldPtr) + return; + + btCollisionObject* col0 = body0; + btCollisionObject* col1 = body1; + btBox2dShape* box0 = (btBox2dShape*)col0->getCollisionShape(); + btBox2dShape* box1 = (btBox2dShape*)col1->getCollisionShape(); + + resultOut->setPersistentManifold(m_manifoldPtr); + + b2CollidePolygons(resultOut,box0,col0->getWorldTransform(),box1,col1->getWorldTransform()); + + // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + +} + +btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +{ + //not yet + return 1.f; +} + + +struct ClipVertex +{ + btVector3 v; + int id; + //b2ContactID id; + //b2ContactID id; +}; + +#define b2Dot(a,b) (a).dot(b) +#define b2Mul(a,b) (a)*(b) +#define b2MulT(a,b) (a).transpose()*(b) +#define b2Cross(a,b) (a).cross(b) +#define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f) + +int b2_maxManifoldPoints =2; + +static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], + const btVector3& normal, btScalar offset) +{ + // Start with no output points + int numOut = 0; + + // Calculate the distance of end points to the line + btScalar distance0 = b2Dot(normal, vIn[0].v) - offset; + btScalar distance1 = b2Dot(normal, vIn[1].v) - offset; + + // If the points are behind the plane + if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; + if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; + + // If the points are on different sides of the plane + if (distance0 * distance1 < 0.0f) + { + // Find intersection point of edge and plane + btScalar interp = distance0 / (distance0 - distance1); + vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); + if (distance0 > 0.0f) + { + vOut[numOut].id = vIn[0].id; + } + else + { + vOut[numOut].id = vIn[1].id; + } + ++numOut; + } + + return numOut; +} + +// Find the separation between poly1 and poly2 for a give edge normal on poly1. +static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1, + const btBox2dShape* poly2, const btTransform& xf2) +{ + const btVector3* vertices1 = poly1->getVertices(); + const btVector3* normals1 = poly1->getNormals(); + + int count2 = poly2->getVertexCount(); + const btVector3* vertices2 = poly2->getVertices(); + + btAssert(0 <= edge1 && edge1 < poly1->getVertexCount()); + + // Convert normal from poly1's frame into poly2's frame. + btVector3 normal1World = b2Mul(xf1.getBasis(), normals1[edge1]); + btVector3 normal1 = b2MulT(xf2.getBasis(), normal1World); + + // Find support vertex on poly2 for -normal. + int index = 0; + btScalar minDot = BT_LARGE_FLOAT; + + for (int i = 0; i < count2; ++i) + { + btScalar dot = b2Dot(vertices2[i], normal1); + if (dot < minDot) + { + minDot = dot; + index = i; + } + } + + btVector3 v1 = b2Mul(xf1, vertices1[edge1]); + btVector3 v2 = b2Mul(xf2, vertices2[index]); + btScalar separation = b2Dot(v2 - v1, normal1World); + return separation; +} + +// Find the max separation between poly1 and poly2 using edge normals from poly1. +static btScalar FindMaxSeparation(int* edgeIndex, + const btBox2dShape* poly1, const btTransform& xf1, + const btBox2dShape* poly2, const btTransform& xf2) +{ + int count1 = poly1->getVertexCount(); + const btVector3* normals1 = poly1->getNormals(); + + // Vector pointing from the centroid of poly1 to the centroid of poly2. + btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid()); + btVector3 dLocal1 = b2MulT(xf1.getBasis(), d); + + // Find edge normal on poly1 that has the largest projection onto d. + int edge = 0; + btScalar maxDot = -BT_LARGE_FLOAT; + for (int i = 0; i < count1; ++i) + { + btScalar dot = b2Dot(normals1[i], dLocal1); + if (dot > maxDot) + { + maxDot = dot; + edge = i; + } + } + + // Get the separation for the edge normal. + btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); + if (s > 0.0f) + { + return s; + } + + // Check the separation for the previous edge normal. + int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; + btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); + if (sPrev > 0.0f) + { + return sPrev; + } + + // Check the separation for the next edge normal. + int nextEdge = edge + 1 < count1 ? edge + 1 : 0; + btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2); + if (sNext > 0.0f) + { + return sNext; + } + + // Find the best edge and the search direction. + int bestEdge; + btScalar bestSeparation; + int increment; + if (sPrev > s && sPrev > sNext) + { + increment = -1; + bestEdge = prevEdge; + bestSeparation = sPrev; + } + else if (sNext > s) + { + increment = 1; + bestEdge = nextEdge; + bestSeparation = sNext; + } + else + { + *edgeIndex = edge; + return s; + } + + // Perform a local search for the best edge normal. + for ( ; ; ) + { + if (increment == -1) + edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; + else + edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; + + s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); + if (s > 0.0f) + { + return s; + } + + if (s > bestSeparation) + { + bestEdge = edge; + bestSeparation = s; + } + else + { + break; + } + } + + *edgeIndex = bestEdge; + return bestSeparation; +} + +static void FindIncidentEdge(ClipVertex c[2], + const btBox2dShape* poly1, const btTransform& xf1, int edge1, + const btBox2dShape* poly2, const btTransform& xf2) +{ + const btVector3* normals1 = poly1->getNormals(); + + int count2 = poly2->getVertexCount(); + const btVector3* vertices2 = poly2->getVertices(); + const btVector3* normals2 = poly2->getNormals(); + + btAssert(0 <= edge1 && edge1 < poly1->getVertexCount()); + + // Get the normal of the reference edge in poly2's frame. + btVector3 normal1 = b2MulT(xf2.getBasis(), b2Mul(xf1.getBasis(), normals1[edge1])); + + // Find the incident edge on poly2. + int index = 0; + btScalar minDot = BT_LARGE_FLOAT; + for (int i = 0; i < count2; ++i) + { + btScalar dot = b2Dot(normal1, normals2[i]); + if (dot < minDot) + { + minDot = dot; + index = i; + } + } + + // Build the clip vertices for the incident edge. + int i1 = index; + int i2 = i1 + 1 < count2 ? i1 + 1 : 0; + + c[0].v = b2Mul(xf2, vertices2[i1]); +// c[0].id.features.referenceEdge = (unsigned char)edge1; +// c[0].id.features.incidentEdge = (unsigned char)i1; +// c[0].id.features.incidentVertex = 0; + + c[1].v = b2Mul(xf2, vertices2[i2]); +// c[1].id.features.referenceEdge = (unsigned char)edge1; +// c[1].id.features.incidentEdge = (unsigned char)i2; +// c[1].id.features.incidentVertex = 1; +} + +// Find edge normal of max separation on A - return if separating axis is found +// Find edge normal of max separation on B - return if separation axis is found +// Choose reference edge as min(minA, minB) +// Find incident edge +// Clip + +// The normal points from 1 to 2 +void b2CollidePolygons(btManifoldResult* manifold, + const btBox2dShape* polyA, const btTransform& xfA, + const btBox2dShape* polyB, const btTransform& xfB) +{ + + int edgeA = 0; + btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); + if (separationA > 0.0f) + return; + + int edgeB = 0; + btScalar separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); + if (separationB > 0.0f) + return; + + const btBox2dShape* poly1; // reference poly + const btBox2dShape* poly2; // incident poly + btTransform xf1, xf2; + int edge1; // reference edge + unsigned char flip; + const btScalar k_relativeTol = 0.98f; + const btScalar k_absoluteTol = 0.001f; + + // TODO_ERIN use "radius" of poly for absolute tolerance. + if (separationB > k_relativeTol * separationA + k_absoluteTol) + { + poly1 = polyB; + poly2 = polyA; + xf1 = xfB; + xf2 = xfA; + edge1 = edgeB; + flip = 1; + } + else + { + poly1 = polyA; + poly2 = polyB; + xf1 = xfA; + xf2 = xfB; + edge1 = edgeA; + flip = 0; + } + + ClipVertex incidentEdge[2]; + FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); + + int count1 = poly1->getVertexCount(); + const btVector3* vertices1 = poly1->getVertices(); + + btVector3 v11 = vertices1[edge1]; + btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0]; + + btVector3 dv = v12 - v11; + btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11); + sideNormal.normalize(); + btVector3 frontNormal = btCrossS(sideNormal, 1.0f); + + + v11 = b2Mul(xf1, v11); + v12 = b2Mul(xf1, v12); + + btScalar frontOffset = b2Dot(frontNormal, v11); + btScalar sideOffset1 = -b2Dot(sideNormal, v11); + btScalar sideOffset2 = b2Dot(sideNormal, v12); + + // Clip incident edge against extruded edge1 side edges. + ClipVertex clipPoints1[2]; + clipPoints1[0].v.setValue(0,0,0); + clipPoints1[1].v.setValue(0,0,0); + + ClipVertex clipPoints2[2]; + clipPoints2[0].v.setValue(0,0,0); + clipPoints2[1].v.setValue(0,0,0); + + + int np; + + // Clip to box side 1 + np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1); + + if (np < 2) + return; + + // Clip to negative box side 1 + np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2); + + if (np < 2) + { + return; + } + + // Now clipPoints2 contains the clipped points. + btVector3 manifoldNormal = flip ? -frontNormal : frontNormal; + + int pointCount = 0; + for (int i = 0; i < b2_maxManifoldPoints; ++i) + { + btScalar separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset; + + if (separation <= 0.0f) + { + + //b2ManifoldPoint* cp = manifold->points + pointCount; + //btScalar separation = separation; + //cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v); + //cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v); + + manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation); + +// cp->id = clipPoints2[i].id; +// cp->id.features.flip = flip; + ++pointCount; + } + } + +// manifold->pointCount = pointCount;} +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h new file mode 100644 index 00000000000..21342175238 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BOX_2D_BOX_2D__COLLISION_ALGORITHM_H +#define BOX_2D_BOX_2D__COLLISION_ALGORITHM_H + +#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" + +class btPersistentManifold; + +///box-box collision detection +class btBox2dBox2dCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + +public: + btBox2dBox2dCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btActivatingCollisionAlgorithm(ci) {} + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); + + virtual ~btBox2dBox2dCollisionAlgorithm(); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + int bbsize = sizeof(btBox2dBox2dCollisionAlgorithm); + void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); + return new(ptr) btBox2dBox2dCollisionAlgorithm(0,ci,body0,body1); + } + }; + +}; + +#endif //BOX_2D_BOX_2D__COLLISION_ALGORITHM_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp index d3342c547b5..49628853493 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp @@ -61,7 +61,7 @@ void btBoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCo #endif //USE_PERSISTENT_CONTACTS btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = 1e30f; + input.m_maximumDistanceSquared = BT_LARGE_FLOAT; input.m_transformA = body0->getWorldTransform(); input.m_transformB = body1->getWorldTransform(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp index 31353f1b2c4..a7c8cf140ce 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -1,4 +1,3 @@ - /* * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. @@ -212,7 +211,7 @@ void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) a = 1.f/(btScalar(3.0)*(a+q)); } else { - a=1e30f; + a=BT_LARGE_FLOAT; } cx = a*(cx + q*(p[n*2-2]+p[0])); cy = a*(cy + q*(p[n*2-1]+p[1])); @@ -267,7 +266,7 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output) { const btScalar fudge_factor = btScalar(1.05); - btVector3 p,pp,normalC; + btVector3 p,pp,normalC(0.f,0.f,0.f); const btScalar *normalR = 0; btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l; @@ -333,9 +332,9 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, #undef TST #define TST(expr1,expr2,n1,n2,n3,cc) \ s2 = btFabs(expr1) - (expr2); \ - if (s2 > 0) return 0; \ + if (s2 > SIMD_EPSILON) return 0; \ l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ - if (l > 0) { \ + if (l > SIMD_EPSILON) { \ s2 /= l; \ if (s2*fudge_factor > s) { \ s = s2; \ @@ -346,6 +345,20 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, } \ } + btScalar fudge2 (1.0e-5f); + + Q11 += fudge2; + Q12 += fudge2; + Q13 += fudge2; + + Q21 += fudge2; + Q22 += fudge2; + Q23 += fudge2; + + Q31 += fudge2; + Q32 += fudge2; + Q33 += fudge2; + // separating axis = u1 x (v1,v2,v3) TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); @@ -424,6 +437,7 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, output.addContactPoint(-normal,pointInWorld,-*depth); #else output.addContactPoint(-normal,pb,-*depth); + #endif // *return_code = code; } @@ -593,21 +607,30 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, if (maxc < 1) maxc = 1; if (cnum <= maxc) { - // we have less contacts than we need, so we use them all - for (j=0; j < cnum; j++) { - - //AddContactPoint... - - //dContactGeom *con = CONTACT(contact,skip*j); - //for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i]; - //con->depth = dep[j]; + if (code<4) + { + // we have less contacts than we need, so we use them all + for (j=0; j < cnum; j++) + { btVector3 pointInWorld; for (i=0; i<3; i++) pointInWorld[i] = point[j*3+i] + pa[i]; output.addContactPoint(-normal,pointInWorld,-dep[j]); } + } else + { + // we have less contacts than we need, so we use them all + for (j=0; j < cnum; j++) + { + btVector3 pointInWorld; + for (i=0; i<3; i++) + pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j]; + //pointInWorld[i] = point[j*3+i] + pa[i]; + output.addContactPoint(-normal,pointInWorld,-dep[j]); + } + } } else { // we have more contacts than are wanted, some of them must be culled. @@ -632,7 +655,13 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, btVector3 posInWorld; for (i=0; i<3; i++) posInWorld[i] = point[iret[j]*3+i] + pa[i]; - output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); + if (code<4) + { + output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); + } else + { + output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]); + } } cnum = maxc; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index e6ff2130aad..9fed44a19f7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -34,9 +34,7 @@ int gNumManifold = 0; btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): - m_count(0), - m_useIslands(true), - m_staticWarningReported(false), +m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD), m_collisionConfiguration(collisionConfiguration) { int i; @@ -79,9 +77,11 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) btCollisionObject* body0 = (btCollisionObject*)b0; btCollisionObject* body1 = (btCollisionObject*)b1; - //test for Bullet 2.74: use a relative contact breaking threshold without clamping against 'gContactBreakingThreshold' - //btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold())); - btScalar contactBreakingThreshold = btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()); + //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance) + + btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? + btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold) , body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold)) + : gContactBreakingThreshold ; btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold()); @@ -169,13 +169,13 @@ bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionO bool needsCollision = true; #ifdef BT_DEBUG - if (!m_staticWarningReported) + if (!(m_dispatcherFlags & btCollisionDispatcher::CD_STATIC_STATIC_REPORTED)) { //broadphase filtering already deals with this if ((body0->isStaticObject() || body0->isKinematicObject()) && (body1->isStaticObject() || body1->isKinematicObject())) { - m_staticWarningReported = true; + m_dispatcherFlags |= btCollisionDispatcher::CD_STATIC_STATIC_REPORTED; printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index a9c9cd414c1..3c4f039504a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -42,14 +42,13 @@ typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispa ///Time of Impact, Closest Points and Penetration Depth. class btCollisionDispatcher : public btDispatcher { - int m_count; - - btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr; - bool m_useIslands; +protected: + + int m_dispatcherFlags; + + btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr; - bool m_staticWarningReported; - btManifoldResult m_defaultManifoldResult; btNearCallback m_nearCallback; @@ -59,13 +58,28 @@ class btCollisionDispatcher : public btDispatcher btPoolAllocator* m_persistentManifoldPoolAllocator; btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; - btCollisionConfiguration* m_collisionConfiguration; public: + enum DispatcherFlags + { + CD_STATIC_STATIC_REPORTED = 1, + CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2 + }; + + int getDispatcherFlags() const + { + return m_dispatcherFlags; + } + + void setDispatcherFlags(int flags) + { + m_dispatcherFlags = flags; + } + ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 285b8f174e4..580ea345860 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -15,13 +15,15 @@ subject to the following restrictions: #include "btCollisionObject.h" +#include "LinearMath/btSerializer.h" btCollisionObject::btCollisionObject() : m_anisotropicFriction(1.f,1.f,1.f), m_hasAnisotropicFriction(false), - m_contactProcessingThreshold(0.f), + m_contactProcessingThreshold(BT_LARGE_FLOAT), m_broadphaseHandle(0), m_collisionShape(0), + m_extensionPointer(0), m_rootCollisionShape(0), m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT), m_islandTag1(-1), @@ -30,14 +32,14 @@ btCollisionObject::btCollisionObject() m_deactivationTime(btScalar(0.)), m_friction(btScalar(0.5)), m_restitution(btScalar(0.)), - m_userObjectPointer(0), m_internalType(CO_COLLISION_OBJECT), + m_userObjectPointer(0), m_hitFraction(btScalar(1.)), m_ccdSweptSphereRadius(btScalar(0.)), m_ccdMotionThreshold(btScalar(0.)), m_checkCollideWith(false) { - + m_worldTransform.setIdentity(); } btCollisionObject::~btCollisionObject() @@ -64,5 +66,51 @@ void btCollisionObject::activate(bool forceActivation) } } +const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btCollisionObjectData* dataOut = (btCollisionObjectData*)dataBuffer; + m_worldTransform.serialize(dataOut->m_worldTransform); + m_interpolationWorldTransform.serialize(dataOut->m_interpolationWorldTransform); + m_interpolationLinearVelocity.serialize(dataOut->m_interpolationLinearVelocity); + m_interpolationAngularVelocity.serialize(dataOut->m_interpolationAngularVelocity); + m_anisotropicFriction.serialize(dataOut->m_anisotropicFriction); + dataOut->m_hasAnisotropicFriction = m_hasAnisotropicFriction; + dataOut->m_contactProcessingThreshold = m_contactProcessingThreshold; + dataOut->m_broadphaseHandle = 0; + dataOut->m_collisionShape = serializer->getUniquePointer(m_collisionShape); + dataOut->m_rootCollisionShape = 0;//@todo + dataOut->m_collisionFlags = m_collisionFlags; + dataOut->m_islandTag1 = m_islandTag1; + dataOut->m_companionId = m_companionId; + dataOut->m_activationState1 = m_activationState1; + dataOut->m_activationState1 = m_activationState1; + dataOut->m_deactivationTime = m_deactivationTime; + dataOut->m_friction = m_friction; + dataOut->m_restitution = m_restitution; + dataOut->m_internalType = m_internalType; + + char* name = (char*) serializer->findNameForPointer(this); + dataOut->m_name = (char*)serializer->getUniquePointer(name); + if (dataOut->m_name) + { + serializer->serializeName(name); + } + dataOut->m_hitFraction = m_hitFraction; + dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius; + dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold; + dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold; + dataOut->m_checkCollideWith = m_checkCollideWith; + + return btCollisionObjectDataName; +} + + +void btCollisionObject::serializeSingleObject(class btSerializer* serializer) const +{ + int len = calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(len,1); + const char* structType = serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_COLLISIONOBJECT_CODE,(void*)this); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 0d5b7886443..5de829824ff 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -27,13 +27,21 @@ subject to the following restrictions: struct btBroadphaseProxy; class btCollisionShape; +struct btCollisionShapeData; #include "LinearMath/btMotionState.h" #include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btAlignedObjectArray.h" - typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray; +#ifdef BT_USE_DOUBLE_PRECISION +#define btCollisionObjectData btCollisionObjectDoubleData +#define btCollisionObjectDataName "btCollisionObjectDoubleData" +#else +#define btCollisionObjectData btCollisionObjectFloatData +#define btCollisionObjectDataName "btCollisionObjectFloatData" +#endif + /// btCollisionObject can be used to manage collision detection objects. /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. @@ -53,12 +61,14 @@ protected: btVector3 m_interpolationLinearVelocity; btVector3 m_interpolationAngularVelocity; - btVector3 m_anisotropicFriction; - bool m_hasAnisotropicFriction; - btScalar m_contactProcessingThreshold; + btVector3 m_anisotropicFriction; + int m_hasAnisotropicFriction; + btScalar m_contactProcessingThreshold; btBroadphaseProxy* m_broadphaseHandle; btCollisionShape* m_collisionShape; + ///m_extensionPointer is used by some internal low-level Bullet extensions. + void* m_extensionPointer; ///m_rootCollisionShape is temporarily used to store the original collision shape ///The m_collisionShape might be temporarily replaced by a child collision shape during collision detection purposes @@ -76,13 +86,13 @@ protected: btScalar m_friction; btScalar m_restitution; - ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer - void* m_userObjectPointer; - ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc. ///do not assign your own m_internalType unless you write a new dynamics object class. int m_internalType; + ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer + void* m_userObjectPointer; + ///time of impact calculation btScalar m_hitFraction; @@ -93,9 +103,7 @@ protected: btScalar m_ccdMotionThreshold; /// If some object should have elaborate collision filtering by sub-classes - bool m_checkCollideWith; - - char m_pad[7]; + int m_checkCollideWith; virtual bool checkCollideWithOverride(btCollisionObject* /* co */) { @@ -112,18 +120,21 @@ public: CF_KINEMATIC_OBJECT= 2, CF_NO_CONTACT_RESPONSE = 4, CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) - CF_CHARACTER_OBJECT = 16 + CF_CHARACTER_OBJECT = 16, + CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing + CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing }; enum CollisionObjectTypes { CO_COLLISION_OBJECT =1, - CO_RIGID_BODY, + CO_RIGID_BODY=2, ///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter ///It is useful for collision sensors, explosion objects, character controller etc. - CO_GHOST_OBJECT, - CO_SOFT_BODY, - CO_HF_FLUID + CO_GHOST_OBJECT=4, + CO_SOFT_BODY=8, + CO_HF_FLUID=16, + CO_USER_TYPE=32 }; SIMD_FORCE_INLINE bool mergesSimulationIslands() const @@ -143,7 +154,7 @@ public: } bool hasAnisotropicFriction() const { - return m_hasAnisotropicFriction; + return m_hasAnisotropicFriction!=0; } ///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default. @@ -213,6 +224,19 @@ public: m_collisionShape = collisionShape; } + ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions. + ///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead. + void* internalGetExtensionPointer() const + { + return m_extensionPointer; + } + ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions + ///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead. + void internalSetExtensionPointer(void* pointer) + { + m_extensionPointer = pointer; + } + SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1;} void setActivationState(int newState); @@ -393,7 +417,7 @@ public: /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold void setCcdMotionThreshold(btScalar ccdMotionThreshold) { - m_ccdMotionThreshold = ccdMotionThreshold*ccdMotionThreshold; + m_ccdMotionThreshold = ccdMotionThreshold; } ///users can point to their objects, userPointer is not used by Bullet @@ -416,6 +440,85 @@ public: return true; } + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; + + virtual void serializeSingleObject(class btSerializer* serializer) const; + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btCollisionObjectDoubleData +{ + void *m_broadphaseHandle; + void *m_collisionShape; + btCollisionShapeData *m_rootCollisionShape; + char *m_name; + + btTransformDoubleData m_worldTransform; + btTransformDoubleData m_interpolationWorldTransform; + btVector3DoubleData m_interpolationLinearVelocity; + btVector3DoubleData m_interpolationAngularVelocity; + btVector3DoubleData m_anisotropicFriction; + double m_contactProcessingThreshold; + double m_deactivationTime; + double m_friction; + double m_restitution; + double m_hitFraction; + double m_ccdSweptSphereRadius; + double m_ccdMotionThreshold; + + int m_hasAnisotropicFriction; + int m_collisionFlags; + int m_islandTag1; + int m_companionId; + int m_activationState1; + int m_internalType; + int m_checkCollideWith; + + char m_padding[4]; }; +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btCollisionObjectFloatData +{ + void *m_broadphaseHandle; + void *m_collisionShape; + btCollisionShapeData *m_rootCollisionShape; + char *m_name; + + btTransformFloatData m_worldTransform; + btTransformFloatData m_interpolationWorldTransform; + btVector3FloatData m_interpolationLinearVelocity; + btVector3FloatData m_interpolationAngularVelocity; + btVector3FloatData m_anisotropicFriction; + float m_contactProcessingThreshold; + float m_deactivationTime; + float m_friction; + float m_restitution; + float m_hitFraction; + float m_ccdSweptSphereRadius; + float m_ccdMotionThreshold; + + int m_hasAnisotropicFriction; + int m_collisionFlags; + int m_islandTag1; + int m_companionId; + int m_activationState1; + int m_internalType; + int m_checkCollideWith; +}; + + + +SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const +{ + return sizeof(btCollisionObjectData); +} + + + #endif //COLLISION_OBJECT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 5c645f82a45..bfe8d4f52fb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -26,12 +26,16 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" - +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "BulletCollision/BroadphaseCollision/btDbvt.h" #include "LinearMath/btAabbUtil2.h" #include "LinearMath/btQuickprof.h" #include "LinearMath/btStackAlloc.h" -#include "BulletSoftBody/btSoftBody.h" +#include "LinearMath/btSerializer.h" + +//#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION + //#define USE_BRUTEFORCE_RAYBROADPHASE 1 //RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation' or 'updateAabbs' before using a rayTest @@ -43,10 +47,29 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" +///for debug drawing + +//for debug rendering +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/CollisionShapes/btConeShape.h" +#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btCylinderShape.h" +#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" +#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" + + + btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration) :m_dispatcher1(dispatcher), m_broadphasePairCache(pairCache), -m_debugDrawer(0) +m_debugDrawer(0), +m_forceUpdateAllAabbs(true) { m_stackAlloc = collisionConfiguration->getStackAllocator(); m_dispatchInfo.m_stackAllocator = m_stackAlloc; @@ -89,28 +112,30 @@ btCollisionWorld::~btCollisionWorld() void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) { + btAssert(collisionObject); + //check that the object isn't already added - btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); + btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); - m_collisionObjects.push_back(collisionObject); + m_collisionObjects.push_back(collisionObject); - //calculate new AABB - btTransform trans = collisionObject->getWorldTransform(); + //calculate new AABB + btTransform trans = collisionObject->getWorldTransform(); - btVector3 minAabb; - btVector3 maxAabb; - collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb); + btVector3 minAabb; + btVector3 maxAabb; + collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb); - int type = collisionObject->getCollisionShape()->getShapeType(); - collisionObject->setBroadphaseHandle( getBroadphase()->createProxy( - minAabb, - maxAabb, - type, - collisionObject, - collisionFilterGroup, - collisionFilterMask, - m_dispatcher1,0 - )) ; + int type = collisionObject->getCollisionShape()->getShapeType(); + collisionObject->setBroadphaseHandle( getBroadphase()->createProxy( + minAabb, + maxAabb, + type, + collisionObject, + collisionFilterGroup, + collisionFilterMask, + m_dispatcher1,0 + )) ; @@ -129,6 +154,16 @@ void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) minAabb -= contactThreshold; maxAabb += contactThreshold; + if(getDispatchInfo().m_convexMaxDistanceUseCPT) + { + btVector3 minAabb2,maxAabb2; + colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); + minAabb2 -= contactThreshold; + maxAabb2 += contactThreshold; + minAabb.setMin(minAabb2); + maxAabb.setMax(maxAabb2); + } + btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; //moving objects should be moderately sized, probably something wrong if not @@ -163,7 +198,7 @@ void btCollisionWorld::updateAabbs() btCollisionObject* colObj = m_collisionObjects[i]; //only update aabb of active objects - if (colObj->isActive()) + if (m_forceUpdateAllAabbs || colObj->isActive()) { updateSingleAabb(colObj); } @@ -226,10 +261,10 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) { btSphereShape pointShape(btScalar(0.0)); pointShape.setMargin(0.f); @@ -237,7 +272,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra if (collisionShape->isConvex()) { -// BT_PROFILE("rayTestConvex"); + // BT_PROFILE("rayTestConvex"); btConvexCast::CastResult castResult; castResult.m_fraction = resultCallback.m_closestHitFraction; @@ -266,10 +301,10 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra castResult.m_normal.normalize(); btCollisionWorld::LocalRayResult localRayResult ( - collisionObject, - 0, - castResult.m_normal, - castResult.m_fraction + collisionObject, + 0, + castResult.m_normal, + castResult.m_fraction ); bool normalInWorldSpace = true; @@ -281,7 +316,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra } else { if (collisionShape->isConcave()) { -// BT_PROFILE("rayTestConcave"); + // BT_PROFILE("rayTestConcave"); if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) { ///optimized version for btBvhTriangleMeshShape @@ -297,15 +332,18 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra btCollisionObject* m_collisionObject; btTriangleMeshShape* m_triangleMesh; + btTransform m_colObjWorldTransform; + BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): - //@BP Mod - btTriangleRaycastCallback(from,to, resultCallback->m_flags), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) - { - } + btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh,const btTransform& colObjWorldTransform): + //@BP Mod + btTriangleRaycastCallback(from,to, resultCallback->m_flags), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh), + m_colObjWorldTransform(colObjWorldTransform) + { + } virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) @@ -314,19 +352,21 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; + btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; + btCollisionWorld::LocalRayResult rayResult - (m_collisionObject, + (m_collisionObject, &shapeInfo, - hitNormalLocal, + hitNormalWorld, hitFraction); - bool normalInWorldSpace = false; + bool normalInWorldSpace = true; return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); } }; - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); } else @@ -347,15 +387,18 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra btCollisionObject* m_collisionObject; btConcaveShape* m_triangleMesh; + btTransform m_colObjWorldTransform; + BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh): - //@BP Mod - btTriangleRaycastCallback(from,to, resultCallback->m_flags), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) - { - } + btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform): + //@BP Mod + btTriangleRaycastCallback(from,to, resultCallback->m_flags), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh), + m_colObjWorldTransform(colObjWorldTransform) + { + } virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) @@ -364,22 +407,22 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; + btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; + btCollisionWorld::LocalRayResult rayResult - (m_collisionObject, + (m_collisionObject, &shapeInfo, - hitNormalLocal, + hitNormalWorld, hitFraction); - bool normalInWorldSpace = false; + bool normalInWorldSpace = true; return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); - - } }; - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape); + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 rayAabbMinLocal = rayFromLocal; @@ -390,27 +433,118 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); } } else { -// BT_PROFILE("rayTestCompound"); - ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt + // BT_PROFILE("rayTestCompound"); if (collisionShape->isCompound()) { + struct LocalInfoAdder2 : public RayResultCallback + { + RayResultCallback* m_userCallback; + int m_i; + + LocalInfoAdder2 (int i, RayResultCallback *user) + : m_userCallback(user), m_i(i) + { + m_closestHitFraction = m_userCallback->m_closestHitFraction; + } + virtual bool needsCollision(btBroadphaseProxy* p) const + { + return m_userCallback->needsCollision(p); + } + + virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = -1; + shapeInfo.m_triangleIndex = m_i; + if (r.m_localShapeInfo == NULL) + r.m_localShapeInfo = &shapeInfo; + + const btScalar result = m_userCallback->addSingleResult(r, b); + m_closestHitFraction = m_userCallback->m_closestHitFraction; + return result; + } + }; + + struct RayTester : btDbvt::ICollide + { + btCollisionObject* m_collisionObject; + const btCompoundShape* m_compoundShape; + const btTransform& m_colObjWorldTransform; + const btTransform& m_rayFromTrans; + const btTransform& m_rayToTrans; + RayResultCallback& m_resultCallback; + + RayTester(btCollisionObject* collisionObject, + const btCompoundShape* compoundShape, + const btTransform& colObjWorldTransform, + const btTransform& rayFromTrans, + const btTransform& rayToTrans, + RayResultCallback& resultCallback): + m_collisionObject(collisionObject), + m_compoundShape(compoundShape), + m_colObjWorldTransform(colObjWorldTransform), + m_rayFromTrans(rayFromTrans), + m_rayToTrans(rayToTrans), + m_resultCallback(resultCallback) + { + + } + + void Process(int i) + { + const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i); + const btTransform& childTrans = m_compoundShape->getChildTransform(i); + btTransform childWorldTrans = m_colObjWorldTransform * childTrans; + + // replace collision shape so that callback can determine the triangle + btCollisionShape* saveCollisionShape = m_collisionObject->getCollisionShape(); + m_collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); + + LocalInfoAdder2 my_cb(i, &m_resultCallback); + + rayTestSingle( + m_rayFromTrans, + m_rayToTrans, + m_collisionObject, + childCollisionShape, + childWorldTrans, + my_cb); + + // restore + m_collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); + } + + void Process(const btDbvtNode* leaf) + { + Process(leaf->dataAsInt); + } + }; + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); - int i=0; - for (i=0;i<compoundShape->getNumChildShapes();i++) + const btDbvt* dbvt = compoundShape->getDynamicAabbTree(); + + + RayTester rayCB( + collisionObject, + compoundShape, + colObjWorldTransform, + rayFromTrans, + rayToTrans, + resultCallback); +#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION + if (dbvt) { - btTransform childTrans = compoundShape->getChildTransform(i); - const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); - btTransform childWorldTrans = colObjWorldTransform * childTrans; - // replace collision shape so that callback can determine the triangle - btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape(); - collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); - rayTestSingle(rayFromTrans,rayToTrans, - collisionObject, - childCollisionShape, - childWorldTrans, - resultCallback); - // restore - collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); + btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin(); + btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin(); + btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB); + } + else +#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION + { + for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i) + { + rayCB.Process(i); + } } } } @@ -418,10 +552,10 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra } void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - ConvexResultCallback& resultCallback, btScalar allowedPenetration) + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback, btScalar allowedPenetration) { if (collisionShape->isConvex()) { @@ -433,15 +567,15 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt btConvexShape* convexShape = (btConvexShape*) collisionShape; btVoronoiSimplexSolver simplexSolver; btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; - + btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver); //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver); //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver); btConvexCast* castPtr = &convexCaster1; - - - + + + if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) { //add hit @@ -451,13 +585,13 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt { castResult.m_normal.normalize(); btCollisionWorld::LocalConvexResult localConvexResult - ( - collisionObject, - 0, - castResult.m_normal, - castResult.m_hitPoint, - castResult.m_fraction - ); + ( + collisionObject, + 0, + castResult.m_normal, + castResult.m_hitPoint, + castResult.m_fraction + ); bool normalInWorldSpace = true; resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); @@ -487,12 +621,12 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld): - btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) - { - } + btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) @@ -504,7 +638,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt { btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, + (m_collisionObject, &shapeInfo, hitNormalLocal, hitPointLocal, @@ -522,6 +656,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform); tccb.m_hitFraction = resultCallback.m_closestHitFraction; + tccb.m_allowedPenetration = allowedPenetration; btVector3 boxMinLocal, boxMaxLocal; castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal); @@ -544,12 +679,12 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): - btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) - { - } + btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) @@ -561,7 +696,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt { btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, + (m_collisionObject, &shapeInfo, hitNormalLocal, hitPointLocal, @@ -578,6 +713,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); tccb.m_hitFraction = resultCallback.m_closestHitFraction; + tccb.m_allowedPenetration = allowedPenetration; btVector3 boxMinLocal, boxMaxLocal; castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); @@ -604,11 +740,41 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt // replace collision shape so that callback can determine the triangle btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape(); collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); + struct LocalInfoAdder : public ConvexResultCallback { + ConvexResultCallback* m_userCallback; + int m_i; + + LocalInfoAdder (int i, ConvexResultCallback *user) + : m_userCallback(user), m_i(i) + { + m_closestHitFraction = m_userCallback->m_closestHitFraction; + } + virtual bool needsCollision(btBroadphaseProxy* p) const + { + return m_userCallback->needsCollision(p); + } + virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = -1; + shapeInfo.m_triangleIndex = m_i; + if (r.m_localShapeInfo == NULL) + r.m_localShapeInfo = &shapeInfo; + const btScalar result = m_userCallback->addSingleResult(r, b); + m_closestHitFraction = m_userCallback->m_closestHitFraction; + return result; + + } + }; + + LocalInfoAdder my_cb(i, &resultCallback); + + objectQuerySingle(castShape, convexFromTrans,convexToTrans, collisionObject, childCollisionShape, childWorldTrans, - resultCallback, allowedPenetration); + my_cb, allowedPenetration); // restore collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); } @@ -631,10 +797,10 @@ struct btSingleRayCallback : public btBroadphaseRayCallback btCollisionWorld::RayResultCallback& m_resultCallback; btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_world(world), - m_resultCallback(resultCallback) + :m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_world(world), + m_resultCallback(resultCallback) { m_rayFromTrans.setIdentity(); m_rayFromTrans.setOrigin(m_rayFromWorld); @@ -644,10 +810,10 @@ struct btSingleRayCallback : public btBroadphaseRayCallback btVector3 rayDir = (rayToWorld-rayFromWorld); rayDir.normalize (); - ///what about division by zero? --> just set rayDirection[i] to INF/1e30 - m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; m_signs[0] = m_rayDirectionInverse[0] < 0.0; m_signs[1] = m_rayDirectionInverse[1] < 0.0; m_signs[2] = m_rayDirectionInverse[2] < 0.0; @@ -656,7 +822,7 @@ struct btSingleRayCallback : public btBroadphaseRayCallback } - + virtual bool process(const btBroadphaseProxy* proxy) { @@ -687,9 +853,9 @@ struct btSingleRayCallback : public btBroadphaseRayCallback { m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback); + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback); } } return true; @@ -698,7 +864,7 @@ struct btSingleRayCallback : public btBroadphaseRayCallback void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const { - BT_PROFILE("rayTest"); + //BT_PROFILE("rayTest"); /// use the broadphase to accelerate the search for objects, based on their aabb /// and for each object with ray-aabb overlap, perform an exact ray test btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); @@ -737,10 +903,10 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback { btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin()); btVector3 rayDir = unnormalizedRayDir.normalized(); - ///what about division by zero? --> just set rayDirection[i] to INF/1e30 - m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; m_signs[0] = m_rayDirectionInverse[0] < 0.0; m_signs[1] = m_rayDirectionInverse[1] < 0.0; m_signs[2] = m_rayDirectionInverse[2] < 0.0; @@ -761,13 +927,13 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback, - m_allowedCcdPenetration); + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback, + m_allowedCcdPenetration); } - + return true; } }; @@ -782,7 +948,7 @@ void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT /// and for each object with ray-aabb overlap, perform an exact ray test /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical - + btTransform convexFromTrans,convexToTrans; convexFromTrans = convexFromWorld; @@ -825,12 +991,455 @@ void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT { objectQuerySingle(castShape, convexFromTrans,convexToTrans, collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback, - allowedCcdPenetration); + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + allowedCcdPenetration); } } } #endif //USE_BRUTEFORCE_RAYBROADPHASE } + + + +struct btBridgedManifoldResult : public btManifoldResult +{ + + btCollisionWorld::ContactResultCallback& m_resultCallback; + + btBridgedManifoldResult( btCollisionObject* obj0,btCollisionObject* obj1,btCollisionWorld::ContactResultCallback& resultCallback ) + :btManifoldResult(obj0,obj1), + m_resultCallback(resultCallback) + { + } + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + { + bool isSwapped = m_manifoldPtr->getBody0() != m_body0; + btVector3 pointA = pointInWorld + normalOnBInWorld * depth; + btVector3 localA; + btVector3 localB; + if (isSwapped) + { + localA = m_rootTransB.invXform(pointA ); + localB = m_rootTransA.invXform(pointInWorld); + } else + { + localA = m_rootTransA.invXform(pointA ); + localB = m_rootTransB.invXform(pointInWorld); + } + + btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + newPt.m_positionWorldOnA = pointA; + newPt.m_positionWorldOnB = pointInWorld; + + //BP mod, store contact triangles. + if (isSwapped) + { + newPt.m_partId0 = m_partId1; + newPt.m_partId1 = m_partId0; + newPt.m_index0 = m_index1; + newPt.m_index1 = m_index0; + } else + { + newPt.m_partId0 = m_partId0; + newPt.m_partId1 = m_partId1; + newPt.m_index0 = m_index0; + newPt.m_index1 = m_index1; + } + + //experimental feature info, for per-triangle material etc. + btCollisionObject* obj0 = isSwapped? m_body1 : m_body0; + btCollisionObject* obj1 = isSwapped? m_body0 : m_body1; + m_resultCallback.addSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1); + + } + +}; + + + +struct btSingleContactCallback : public btBroadphaseAabbCallback +{ + + btCollisionObject* m_collisionObject; + btCollisionWorld* m_world; + btCollisionWorld::ContactResultCallback& m_resultCallback; + + + btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback) + :m_collisionObject(collisionObject), + m_world(world), + m_resultCallback(resultCallback) + { + } + + virtual bool process(const btBroadphaseProxy* proxy) + { + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + if (collisionObject == m_collisionObject) + return true; + + //only perform raycast if filterMask matches + if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { + btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(m_collisionObject,collisionObject); + if (algorithm) + { + btBridgedManifoldResult contactPointResult(m_collisionObject,collisionObject, m_resultCallback); + //discrete collision detection query + algorithm->processCollision(m_collisionObject,collisionObject, m_world->getDispatchInfo(),&contactPointResult); + + algorithm->~btCollisionAlgorithm(); + m_world->getDispatcher()->freeCollisionAlgorithm(algorithm); + } + } + return true; + } +}; + + +///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback. +///it reports one or more contact points for every overlapping object (including the one with deepest penetration) +void btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback) +{ + btVector3 aabbMin,aabbMax; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax); + btSingleContactCallback contactCB(colObj,this,resultCallback); + + m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB); +} + + +///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected. +///it reports one or more contact points (including the one with deepest penetration) +void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback) +{ + btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(colObjA,colObjB); + if (algorithm) + { + btBridgedManifoldResult contactPointResult(colObjA,colObjB, resultCallback); + //discrete collision detection query + algorithm->processCollision(colObjA,colObjB, getDispatchInfo(),&contactPointResult); + + algorithm->~btCollisionAlgorithm(); + getDispatcher()->freeCollisionAlgorithm(algorithm); + } + +} + + + + +class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback +{ + btIDebugDraw* m_debugDrawer; + btVector3 m_color; + btTransform m_worldTrans; + +public: + + DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) : + m_debugDrawer(debugDrawer), + m_color(color), + m_worldTrans(worldTrans) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + { + processTriangle(triangle,partId,triangleIndex); + } + + virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex) + { + (void)partId; + (void)triangleIndex; + + btVector3 wv0,wv1,wv2; + wv0 = m_worldTrans*triangle[0]; + wv1 = m_worldTrans*triangle[1]; + wv2 = m_worldTrans*triangle[2]; + btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.); + + btVector3 normal = (wv1-wv0).cross(wv2-wv0); + normal.normalize(); + btVector3 normalColor(1,1,0); + m_debugDrawer->drawLine(center,center+normal,normalColor); + + + + + m_debugDrawer->drawLine(wv0,wv1,m_color); + m_debugDrawer->drawLine(wv1,wv2,m_color); + m_debugDrawer->drawLine(wv2,wv0,m_color); + } +}; + + +void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) +{ + // Draw a small simplex at the center of the object + getDebugDrawer()->drawTransform(worldTransform,1); + + if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) + { + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape); + for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) + { + btTransform childTrans = compoundShape->getChildTransform(i); + const btCollisionShape* colShape = compoundShape->getChildShape(i); + debugDrawObject(worldTransform*childTrans,colShape,color); + } + + } else + { + switch (shape->getShapeType()) + { + + case BOX_SHAPE_PROXYTYPE: + { + const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); + btVector3 halfExtents = boxShape->getHalfExtentsWithMargin(); + getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color); + break; + } + + case SPHERE_SHAPE_PROXYTYPE: + { + const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); + btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin + + getDebugDrawer()->drawSphere(radius, worldTransform, color); + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); + + btTransform childTransform; + childTransform.setIdentity(); + + for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) + { + childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); + getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color); + } + + break; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape); + + btScalar radius = capsuleShape->getRadius(); + btScalar halfHeight = capsuleShape->getHalfHeight(); + + int upAxis = capsuleShape->getUpAxis(); + getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color); + break; + } + case CONE_SHAPE_PROXYTYPE: + { + const btConeShape* coneShape = static_cast<const btConeShape*>(shape); + btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); + btScalar height = coneShape->getHeight();//+coneShape->getMargin(); + + int upAxis= coneShape->getConeUpIndex(); + getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color); + break; + + } + case CYLINDER_SHAPE_PROXYTYPE: + { + const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); + int upAxis = cylinder->getUpAxis(); + btScalar radius = cylinder->getRadius(); + btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; + getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color); + break; + } + + case STATIC_PLANE_PROXYTYPE: + { + const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); + btScalar planeConst = staticPlaneShape->getPlaneConstant(); + const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); + getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color); + break; + + } + default: + { + + if (shape->isConcave()) + { + btConcaveShape* concaveMesh = (btConcaveShape*) shape; + + ///@todo pass camera, for some culling? no -> we are not a graphics lib + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); + + } + + if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + { + btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; + //todo: pass camera for some culling + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + //DebugDrawcallback drawCallback; + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + } + + + /// for polyhedral shapes + if (shape->isPolyhedral()) + { + btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; + + int i; + for (i=0;i<polyshape->getNumEdges();i++) + { + btVector3 a,b; + polyshape->getEdge(i,a,b); + btVector3 wa = worldTransform * a; + btVector3 wb = worldTransform * b; + getDebugDrawer()->drawLine(wa,wb,color); + + } + + + } + } + } + } +} + + +void btCollisionWorld::debugDrawWorld() +{ + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) + { + int numManifolds = getDispatcher()->getNumManifolds(); + btVector3 color(0,0,0); + for (int i=0;i<numManifolds;i++) + { + btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i); + //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0()); + //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1()); + + int numContacts = contactManifold->getNumContacts(); + for (int j=0;j<numContacts;j++) + { + btManifoldPoint& cp = contactManifold->getContactPoint(j); + getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); + } + } + } + + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) + { + int i; + + for ( i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0) + { + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) + { + btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.)); + switch(colObj->getActivationState()) + { + case ACTIVE_TAG: + color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break; + case ISLAND_SLEEPING: + color = btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break; + case WANTS_DEACTIVATION: + color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break; + case DISABLE_DEACTIVATION: + color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break; + case DISABLE_SIMULATION: + color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break; + default: + { + color = btVector3(btScalar(1),btScalar(0.),btScalar(0.)); + } + }; + + debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); + } + if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + btVector3 minAabb,maxAabb; + btVector3 colorvec(1,0,0); + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + minAabb -= contactThreshold; + maxAabb += contactThreshold; + + btVector3 minAabb2,maxAabb2; + + colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); + minAabb2 -= contactThreshold; + maxAabb2 += contactThreshold; + + minAabb.setMin(minAabb2); + maxAabb.setMax(maxAabb2); + + m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); + } + } + + } + } +} + + +void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) +{ + int i; + //serialize all collision objects + for (i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) + { + colObj->serializeSingleObject(serializer); + } + } + + ///keep track of shapes already serialized + btHashMap<btHashPtr,btCollisionShape*> serializedShapes; + + for (i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + btCollisionShape* shape = colObj->getCollisionShape(); + + if (!serializedShapes.find(shape)) + { + serializedShapes.insert(shape,shape); + shape->serializeSingleShape(serializer); + } + } + +} + + +void btCollisionWorld::serialize(btSerializer* serializer) +{ + + serializer->startSerialization(); + + serializeCollisionObjects(serializer); + + serializer->finishSerialization(); +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index 1f785cf69e4..b42e2c40b21 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -1,5 +1,5 @@ /* -Bullet Continuous Collision Detection and Physics %Library +Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2006 Erwin Coumans http://bulletphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. @@ -15,50 +15,48 @@ subject to the following restrictions: /** - * \file btCollisionWorld.h - * \ingroup bullet - * - * @page bulletdoc Bullet Documentation + * @mainpage Bullet Documentation * * @section intro_sec Introduction * Bullet Collision Detection & Physics SDK * - * Bullet is a Collision Detection and Rigid Body Dynamics %Library. The %Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ). + * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ). * + * The main documentation is Bullet_User_Manual.pdf, included in the source code distribution. * There is the Physics Forum for feedback and general Collision Detection and Physics discussions. * Please visit http://www.bulletphysics.com * * @section install_sec Installation * * @subsection step1 Step 1: Download - * You can download the Bullet Physics %Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list + * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list + * * @subsection step2 Step 2: Building - * Bullet comes with autogenerated Project Files for Microsoft Visual Studio 6, 7, 7.1 and 8. - * The main Workspace/Solution is located in Bullet/msvc/8/wksbullet.sln (replace 8 with your version). - * - * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using make, cmake, http://www.cmake.org , or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet. - * So if you are not using MSVC or cmake, you can run ./autogen.sh ./configure to create both Makefile and Jamfile and then run make or jam. - * Jam is a build system that can build the library, demos and also autogenerate the MSVC Project Files. - * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/jam + * Bullet main build system for all platforms is cmake, you can download http://www.cmake.org + * cmake can autogenerate projectfiles for Microsoft Visual Studio, Apple Xcode, KDevelop and Unix Makefiles. + * The easiest is to run the CMake cmake-gui graphical user interface and choose the options and generate projectfiles. + * You can also use cmake in the command-line. Here are some examples for various platforms: + * cmake . -G "Visual Studio 9 2008" + * cmake . -G Xcode + * cmake . -G "Unix Makefiles" + * Although cmake is recommended, you can also use autotools for UNIX: ./autogen.sh ./configure to create a Makefile and then run make. * * @subsection step3 Step 3: Testing demos * Try to run and experiment with BasicDemo executable as a starting point. - * Bullet can be used in several ways, as Full Rigid Body simulation, as Collision Detector %Library or Low Level / Snippets like the GJK Closest Point calculation. + * Bullet can be used in several ways, as Full Rigid Body simulation, as Collision Detector Library or Low Level / Snippets like the GJK Closest Point calculation. * The Dependencies can be seen in this documentation under Directories * * @subsection step4 Step 4: Integrating in your application, full Rigid Body and Soft Body simulation * Check out BasicDemo how to create a btDynamicsWorld, btRigidBody and btCollisionShape, Stepping the simulation and synchronizing your graphics object transform. * Check out SoftDemo how to use soft body dynamics, using btSoftRigidDynamicsWorld. - * @subsection step5 Step 5 : Integrate the Collision Detection %Library (without Dynamics and other Extras) + * @subsection step5 Step 5 : Integrate the Collision Detection Library (without Dynamics and other Extras) * Bullet Collision Detection can also be used without the Dynamics/Extras. * Check out btCollisionWorld and btCollisionObject, and the CollisionInterfaceDemo. * @subsection step6 Step 6 : Use Snippets like the GJK Closest Point calculation. * Bullet has been designed in a modular way keeping dependencies to a minimum. The ConvexHullDistance demo demonstrates direct use of btGjkPairDetector. * * @section copyright Copyright - * Copyright (C) 2005-2008 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon - * Special thanks to all visitors of the Bullet Physics forum, and in particular above contributors, John McCutchan, Nathanael Presson, Dave Eberle, Dirk Gregorius, Erin Catto, Dave Eberle, Adam Moravanszky, - * Pierre Terdiman, Kenny Erleben, Russell Smith, Oliver Strunk, Jan Paul van Waveren, Marten Svanfeldt. + * For up-to-data information and copyright and contributors list check out the Bullet_User_Manual.pdf * */ @@ -71,6 +69,8 @@ class btStackAlloc; class btCollisionShape; class btConvexShape; class btBroadphaseInterface; +class btSerializer; + #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "btCollisionObject.h" @@ -97,7 +97,12 @@ protected: btIDebugDraw* m_debugDrawer; - + ///m_forceUpdateAllAabbs can be set to false as an optimization to only update active object AABBs + ///it is true by default, because it is error-prone (setting the position of static objects wouldn't update their AABB) + bool m_forceUpdateAllAabbs; + + void serializeCollisionObjects(btSerializer* serializer); + public: //this constructor doesn't own the dispatcher and paircache/broadphase @@ -150,6 +155,10 @@ public: return m_debugDrawer; } + virtual void debugDrawWorld(); + + virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); + ///LocalShapeInfo gives extra information for complex shapes ///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart @@ -255,6 +264,45 @@ public: } }; + struct AllHitsRayResultCallback : public RayResultCallback + { + AllHitsRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) + :m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld) + { + } + + btAlignedObjectArray<btCollisionObject*> m_collisionObjects; + + btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction + btVector3 m_rayToWorld; + + btAlignedObjectArray<btVector3> m_hitNormalWorld; + btAlignedObjectArray<btVector3> m_hitPointWorld; + btAlignedObjectArray<btScalar> m_hitFractions; + + virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) + { + m_collisionObject = rayResult.m_collisionObject; + m_collisionObjects.push_back(rayResult.m_collisionObject); + btVector3 hitNormalWorld; + if (normalInWorldSpace) + { + hitNormalWorld = rayResult.m_hitNormalLocal; + } else + { + ///need to transform normal into worldspace + hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; + } + m_hitNormalWorld.push_back(hitNormalWorld); + btVector3 hitPointWorld; + hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction); + m_hitPointWorld.push_back(hitPointWorld); + m_hitFractions.push_back(rayResult.m_hitFraction); + return m_closestHitFraction; + } + }; + struct LocalConvexResult { @@ -350,6 +398,34 @@ public: } }; + ///ContactResultCallback is used to report contact points + struct ContactResultCallback + { + short int m_collisionFilterGroup; + short int m_collisionFilterMask; + + ContactResultCallback() + :m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter) + { + } + + virtual ~ContactResultCallback() + { + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; + collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask); + return collides; + } + + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1) = 0; + }; + + + int getNumCollisionObjects() const { return int(m_collisionObjects.size()); @@ -359,10 +435,18 @@ public: /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; - // convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback - // This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback. + /// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback + /// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback. void convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = btScalar(0.)) const; + ///contactTest performs a discrete collision test between colObj against all objects in the btCollisionWorld, and calls the resultCallback. + ///it reports one or more contact points for every overlapping object (including the one with deepest penetration) + void contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback); + + ///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected. + ///it reports one or more contact points (including the one with deepest penetration) + void contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback); + /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. @@ -380,7 +464,7 @@ public: const btTransform& colObjWorldTransform, ConvexResultCallback& resultCallback, btScalar allowedPenetration); - void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); + virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); btCollisionObjectArray& getCollisionObjectArray() { @@ -393,7 +477,7 @@ public: } - void removeCollisionObject(btCollisionObject* collisionObject); + virtual void removeCollisionObject(btCollisionObject* collisionObject); virtual void performDiscreteCollisionDetection(); @@ -406,6 +490,18 @@ public: { return m_dispatchInfo; } + + bool getForceUpdateAllAabbs() const + { + return m_forceUpdateAllAabbs; + } + void setForceUpdateAllAabbs( bool forceUpdateAllAabbs) + { + m_forceUpdateAllAabbs = forceUpdateAllAabbs; + } + + ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (Bullet/Demos/SerializeDemo) + virtual void serialize(btSerializer* serializer); }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 1dea91a0b0b..54889a6375d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -114,8 +114,9 @@ public: void ProcessChildShape(btCollisionShape* childShape,int index) { - + btAssert(index>=0); btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape()); + btAssert(index<compoundShape->getNumChildShapes()); //backup @@ -142,6 +143,15 @@ public: if (!m_childCollisionAlgorithms[index]) m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold); + ///detect swapping case + if (m_resultOut->getBody0Internal() == m_compoundColObj) + { + m_resultOut->setShapeIdentifiersA(-1,index); + } else + { + m_resultOut->setShapeIdentifiersB(-1,index); + } + m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut); if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { @@ -224,7 +234,7 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt resultOut->setPersistentManifold(0);//??necessary? } } - manifoldArray.clear(); + manifoldArray.resize(0); } } } @@ -257,20 +267,24 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt int numChildren = m_childCollisionAlgorithms.size(); int i; btManifoldArray manifoldArray; - + btCollisionShape* childShape = 0; + btTransform orgTrans; + btTransform orgInterpolationTrans; + btTransform newChildWorldTrans; + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + for (i=0;i<numChildren;i++) { if (m_childCollisionAlgorithms[i]) { - btCollisionShape* childShape = compoundShape->getChildShape(i); + childShape = compoundShape->getChildShape(i); //if not longer overlapping, remove the algorithm - btTransform orgTrans = colObj->getWorldTransform(); - btTransform orgInterpolationTrans = colObj->getInterpolationWorldTransform(); + orgTrans = colObj->getWorldTransform(); + orgInterpolationTrans = colObj->getInterpolationWorldTransform(); const btTransform& childTrans = compoundShape->getChildTransform(i); - btTransform newChildWorldTrans = orgTrans*childTrans ; + newChildWorldTrans = orgTrans*childTrans ; //perform an AABB check first - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); otherObj->getCollisionShape()->getAabb(otherObj->getWorldTransform(),aabbMin1,aabbMax1); @@ -280,13 +294,8 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); m_childCollisionAlgorithms[i] = 0; } - } - } - - - } } @@ -311,13 +320,15 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* int numChildren = m_childCollisionAlgorithms.size(); int i; + btTransform orgTrans; + btScalar frac; for (i=0;i<numChildren;i++) { //temporarily exchange parent btCollisionShape with childShape, and recurse btCollisionShape* childShape = compoundShape->getChildShape(i); //backup - btTransform orgTrans = colObj->getWorldTransform(); + orgTrans = colObj->getWorldTransform(); const btTransform& childTrans = compoundShape->getChildTransform(i); //btTransform newChildWorldTrans = orgTrans*childTrans ; @@ -325,7 +336,7 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* btCollisionShape* tmpShape = colObj->getCollisionShape(); colObj->internalSetTemporaryCollisionShape( childShape ); - btScalar frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); + frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); if (frac<hitFraction) { hitFraction = frac; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp new file mode 100644 index 00000000000..db7f884ac82 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp @@ -0,0 +1,247 @@ +/* +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 "btConvex2dConvex2dAlgorithm.h" + +//#include <stdio.h> +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" + + +#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" + +#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" + + + +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + +#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" + +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" + + +btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) +{ + m_numPerturbationIterations = 0; + m_minimumPointsPerturbationThreshold = 3; + m_simplexSolver = simplexSolver; + m_pdSolver = pdSolver; +} + +btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() +{ +} + +btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold) +: btActivatingCollisionAlgorithm(ci,body0,body1), +m_simplexSolver(simplexSolver), +m_pdSolver(pdSolver), +m_ownManifold (false), +m_manifoldPtr(mf), +m_lowLevelOfDetail(false), + m_numPerturbationIterations(numPerturbationIterations), +m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) +{ + (void)body0; + (void)body1; +} + + + + +btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel) +{ + m_lowLevelOfDetail = useLowLevel; +} + + + +extern btScalar gContactBreakingThreshold; + + +// +// Convex-Convex collision algorithm +// +void btConvex2dConvex2dAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + if (!m_manifoldPtr) + { + //swapped? + m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); + m_ownManifold = true; + } + resultOut->setPersistentManifold(m_manifoldPtr); + + //comment-out next line to test multi-contact generation + //resultOut->getPersistentManifold()->clearManifold(); + + + btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape()); + btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape()); + + btVector3 normalOnB; + btVector3 pointOnBWorld; + + { + + + btGjkPairDetector::ClosestPointInput input; + + btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); + //TODO: if (dispatchInfo.m_useContinuous) + gjkPairDetector.setMinkowskiA(min0); + gjkPairDetector.setMinkowskiB(min1); + + { + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); + input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; + } + + input.m_stackAlloc = dispatchInfo.m_stackAllocator; + input.m_transformA = body0->getWorldTransform(); + input.m_transformB = body1->getWorldTransform(); + + gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + + btVector3 v0,v1; + btVector3 sepNormalWorldSpace; + + } + + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + +} + + + + +btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold + + ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold + ///col0->m_worldTransform, + btScalar resultFraction = btScalar(1.); + + + btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); + btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); + + if (squareMot0 < col0->getCcdSquareMotionThreshold() && + squareMot1 < col1->getCcdSquareMotionThreshold()) + return resultFraction; + + + //An adhoc way of testing the Continuous Collision Detection algorithms + //One object is approximated as a sphere, to simplify things + //Starting in penetration should report no time of impact + //For proper CCD, better accuracy and handling of 'allowed' penetration should be added + //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) + + + /// Convex0 against sphere for Convex1 + { + btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape()); + + btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btConvexCast::CastResult result; + btVoronoiSimplexSolver voronoiSimplex; + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), + col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + { + + //store result.m_fraction in both bodies + + if (col0->getHitFraction()> result.m_fraction) + col0->setHitFraction( result.m_fraction ); + + if (col1->getHitFraction() > result.m_fraction) + col1->setHitFraction( result.m_fraction); + + if (resultFraction > result.m_fraction) + resultFraction = result.m_fraction; + + } + + + + + } + + /// Sphere (for convex0) against Convex1 + { + btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape()); + + btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btConvexCast::CastResult result; + btVoronoiSimplexSolver voronoiSimplex; + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), + col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + { + + //store result.m_fraction in both bodies + + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction( result.m_fraction); + + if (col1->getHitFraction() > result.m_fraction) + col1->setHitFraction( result.m_fraction); + + if (resultFraction > result.m_fraction) + resultFraction = result.m_fraction; + + } + } + + return resultFraction; + +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h new file mode 100644 index 00000000000..5738401401e --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h @@ -0,0 +1,95 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef CONVEX_2D_CONVEX_2D_ALGORITHM_H +#define CONVEX_2D_CONVEX_2D_ALGORITHM_H + +#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil + +class btConvexPenetrationDepthSolver; + + +///The convex2dConvex2dAlgorithm collision algorithm support 2d collision detection for btConvex2dShape +///Currently it requires the btMinkowskiPenetrationDepthSolver, it has support for 2d penetration depth computation +class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm +{ + btSimplexSolverInterface* m_simplexSolver; + btConvexPenetrationDepthSolver* m_pdSolver; + + + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_lowLevelOfDetail; + + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + +public: + + btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); + + + virtual ~btConvex2dConvex2dAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + ///should we use m_ownManifold to avoid adding duplicates? + if (m_manifoldPtr && m_ownManifold) + manifoldArray.push_back(m_manifoldPtr); + } + + + void setLowLevelOfDetail(bool useLowLevel); + + + const btPersistentManifold* getManifold() + { + return m_manifoldPtr; + } + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + + btConvexPenetrationDepthSolver* m_pdSolver; + btSimplexSolverInterface* m_simplexSolver; + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + + CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); + + virtual ~CreateFunc(); + + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm)); + return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + } + }; + + +}; + +#endif //CONVEX_2D_CONVEX_2D_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index cbc5530732b..268ec4b6c7e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -95,7 +95,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i ///debug drawing of the overlapping triangles if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe )) { - btVector3 color(255,255,0); + btVector3 color(1,1,0); btTransform& tr = ob->getWorldTransform(); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); @@ -121,12 +121,16 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i ob->internalSetTemporaryCollisionShape( &tm ); btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr); - ///this should use the btDispatcher, so the actual registered algorithm is used - // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); - m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex); - // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); -// cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); + if (m_resultOut->getBody0Internal() == m_triBody) + { + m_resultOut->setShapeIdentifiersA(partId,triangleIndex); + } + else + { + m_resultOut->setShapeIdentifiersB(partId,triangleIndex); + } + colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 496fd996f8c..ede1afb2a03 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -13,6 +13,11 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance +///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums +///with reproduction case +//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1 + #include "btConvexConvexAlgorithm.h" //#include <stdio.h> @@ -20,6 +25,9 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" + + #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" @@ -43,8 +51,127 @@ subject to the following restrictions: +/////////// + + + +static SIMD_FORCE_INLINE void segmentsClosestPoints( + btVector3& ptsVector, + btVector3& offsetA, + btVector3& offsetB, + btScalar& tA, btScalar& tB, + const btVector3& translation, + const btVector3& dirA, btScalar hlenA, + const btVector3& dirB, btScalar hlenB ) +{ + // compute the parameters of the closest points on each line segment + + btScalar dirA_dot_dirB = btDot(dirA,dirB); + btScalar dirA_dot_trans = btDot(dirA,translation); + btScalar dirB_dot_trans = btDot(dirB,translation); + + btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; + + if ( denom == 0.0f ) { + tA = 0.0f; + } else { + tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } + + tB = tA * dirA_dot_dirB - dirB_dot_trans; + + if ( tB < -hlenB ) { + tB = -hlenB; + tA = tB * dirA_dot_dirB + dirA_dot_trans; + + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } else if ( tB > hlenB ) { + tB = hlenB; + tA = tB * dirA_dot_dirB + dirA_dot_trans; + + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } + + // compute the closest points relative to segment centers. + + offsetA = dirA * tA; + offsetB = dirB * tB; + + ptsVector = translation - offsetA + offsetB; +} + + +static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance( + btVector3& normalOnB, + btVector3& pointOnB, + btScalar capsuleLengthA, + btScalar capsuleRadiusA, + btScalar capsuleLengthB, + btScalar capsuleRadiusB, + int capsuleAxisA, + int capsuleAxisB, + const btTransform& transformA, + const btTransform& transformB, + btScalar distanceThreshold ) +{ + btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA); + btVector3 translationA = transformA.getOrigin(); + btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB); + btVector3 translationB = transformB.getOrigin(); + + // translation between centers + + btVector3 translation = translationB - translationA; + + // compute the closest points of the capsule line segments + + btVector3 ptsVector; // the vector between the closest points + + btVector3 offsetA, offsetB; // offsets from segment centers to their closest points + btScalar tA, tB; // parameters on line segment + + segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation, + directionA, capsuleLengthA, directionB, capsuleLengthB ); + + btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB; + + if ( distance > distanceThreshold ) + return distance; + + btScalar lenSqr = ptsVector.length2(); + if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON)) + { + //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA' + btVector3 q; + btPlaneSpace1(directionA,normalOnB,q); + } else + { + // compute the contact normal + normalOnB = ptsVector*-btRecipSqrt(lenSqr); + } + pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB; + + return distance; +} + + + + + +////////// + @@ -69,7 +196,7 @@ m_ownManifold (false), m_manifoldPtr(mf), m_lowLevelOfDetail(false), #ifdef USE_SEPDISTANCE_UTIL2 -,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(), +m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(), (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()), #endif m_numPerturbationIterations(numPerturbationIterations), @@ -111,8 +238,8 @@ struct btPerturbedContactResult : public btManifoldResult :m_originalManifoldResult(originalResult), m_transformA(transformA), m_transformB(transformB), - m_perturbA(perturbA), m_unPerturbedTransform(unPerturbedTransform), + m_perturbA(perturbA), m_debugDrawer(debugDrawer) { } @@ -155,6 +282,7 @@ struct btPerturbedContactResult : public btManifoldResult extern btScalar gContactBreakingThreshold; + // // Convex-Convex collision algorithm // @@ -176,8 +304,39 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape()); btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape()); + btVector3 normalOnB; + btVector3 pointOnBWorld; +#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER + if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) + { + btCapsuleShape* capsuleA = (btCapsuleShape*) min0; + btCapsuleShape* capsuleB = (btCapsuleShape*) min1; + btVector3 localScalingA = capsuleA->getLocalScaling(); + btVector3 localScalingB = capsuleB->getLocalScaling(); + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), + capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(), + body0->getWorldTransform(),body1->getWorldTransform(),threshold); + + if (dist<threshold) + { + btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); + } + resultOut->refreshContactPoints(); + return; + } +#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER + + #ifdef USE_SEPDISTANCE_UTIL2 - m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); + if (dispatchInfo.m_useConvexConservativeDistanceUtil) + { + m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); + } + if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) #endif //USE_SEPDISTANCE_UTIL2 @@ -194,31 +353,55 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { - input.m_maximumDistanceSquared = 1e30f; + input.m_maximumDistanceSquared = BT_LARGE_FLOAT; } else #endif //USE_SEPDISTANCE_UTIL2 { - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); + if (dispatchInfo.m_convexMaxDistanceUseCPT) + { + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); + } else + { + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); + } input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; } + input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0->getWorldTransform(); input.m_transformB = body1->getWorldTransform(); gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); - btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold; - //now perturbe directions to get multiple contact points - btVector3 v0,v1; - btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); - btPlaneSpace1(sepNormalWorldSpace,v0,v1); + + +#ifdef USE_SEPDISTANCE_UTIL2 + btScalar sepDist = 0.f; + if (dispatchInfo.m_useConvexConservativeDistanceUtil) + { + sepDist = gjkPairDetector.getCachedSeparatingDistance(); + if (sepDist>SIMD_EPSILON) + { + sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; + //now perturbe directions to get multiple contact points + + } + } +#endif //USE_SEPDISTANCE_UTIL2 + //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points - if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) + if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) { int i; + btVector3 v0,v1; + btVector3 sepNormalWorldSpace; + + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); + btPlaneSpace1(sepNormalWorldSpace,v0,v1); + bool perturbeA = true; const btScalar angleLimit = 0.125f * SIMD_PI; @@ -248,6 +431,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl for ( i=0;i<m_numPerturbationIterations;i++) { + if (v0.length2()>SIMD_EPSILON) + { btQuaternion perturbeRot(v0,perturbeAngle); btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); btQuaternion rotq(sepNormalWorldSpace,iterationAngle); @@ -271,7 +456,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); - + } } } @@ -279,7 +464,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl #ifdef USE_SEPDISTANCE_UTIL2 - if (dispatchInfo.m_useConvexConservativeDistanceUtil) + if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) { m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform()); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index 62dd33eb98d..d38aff6862c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -31,7 +31,8 @@ class btConvexPenetrationDepthSolver; ///so the distance is not conservative. In that case, enabling this USE_SEPDISTANCE_UTIL2 would result in failing/missing collisions. ///Either improve GJK for large size ratios (testing a 100 units versus a 0.1 unit object) or only enable the util ///for certain pairs that have a small size ratio -///#define USE_SEPDISTANCE_UTIL2 1 + +//#define USE_SEPDISTANCE_UTIL2 1 ///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects. ///Multiple contact points are calculated by perturbing the orientation of the smallest object orthogonal to the separating normal. diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp index a7b3b163d66..dda85dc693f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -102,9 +102,9 @@ void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0 btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); - bool hasCollision = false; + const btVector3& planeNormal = planeShape->getPlaneNormal(); - const btScalar& planeConstant = planeShape->getPlaneConstant(); + //const btScalar& planeConstant = planeShape->getPlaneConstant(); //first perform a collision query with the non-perturbated collision objects { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h index 368ca71dda0..f49ac45e772 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -60,8 +60,8 @@ public: int m_minimumPointsPerturbationThreshold; CreateFunc() - : m_numPerturbationIterations(3), - m_minimumPointsPerturbationThreshold(3) + : m_numPerturbationIterations(1), + m_minimumPointsPerturbationThreshold(1) { } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index 3ae25f109c2..c27d8ce0752 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -45,17 +45,17 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16); m_simplexSolver = new (mem)btVoronoiSimplexSolver(); - -#define USE_EPA 1 -#ifdef USE_EPA - mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16); - m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver; -#else - mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16); - m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver; -#endif//USE_EPA - + if (constructionInfo.m_useEpaPenetrationAlgorithm) + { + mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16); + m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver; + }else + { + mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16); + m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver; + } + //default CreationFunctions, filling the m_doubleDispatch table mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16); m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver); @@ -102,7 +102,8 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault int maxSize3 = sizeof(btCompoundCollisionAlgorithm); int sl = sizeof(btConvexSeparatingDistanceUtil); sl = sizeof(btGjkPairDetector); - int collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2); + int collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); if (constructionInfo.m_stackAlloc) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 6d8cab726bd..6aa0d8c270f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -27,7 +27,9 @@ struct btDefaultCollisionConstructionInfo btPoolAllocator* m_collisionAlgorithmPool; int m_defaultMaxPersistentManifoldPoolSize; int m_defaultMaxCollisionAlgorithmPoolSize; + int m_customCollisionAlgorithmMaxElementSize; int m_defaultStackAllocatorSize; + int m_useEpaPenetrationAlgorithm; btDefaultCollisionConstructionInfo() :m_stackAlloc(0), @@ -35,7 +37,9 @@ struct btDefaultCollisionConstructionInfo m_collisionAlgorithmPool(0), m_defaultMaxPersistentManifoldPoolSize(4096), m_defaultMaxCollisionAlgorithmPoolSize(4096), - m_defaultStackAllocatorSize(0) + m_customCollisionAlgorithmMaxElementSize(0), + m_defaultStackAllocatorSize(0), + m_useEpaPenetrationAlgorithm(true) { } }; @@ -108,6 +112,11 @@ public: return m_stackAlloc; } + virtual btVoronoiSimplexSolver* getSimplexSolver() + { + return m_simplexSolver; + } + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h index 95b5750240c..8ec86138575 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h @@ -160,7 +160,7 @@ public: return 0; } - virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) { btAssert(0); //need to keep track of all ghost objects and call them here @@ -171,4 +171,5 @@ public: }; -#endif
\ No newline at end of file +#endif + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp new file mode 100644 index 00000000000..5cceb04dbb4 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -0,0 +1,772 @@ +#include "btInternalEdgeUtility.h" + +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" +#include "LinearMath/btIDebugDraw.h" + + +//#define DEBUG_INTERNAL_EDGE + + +#ifdef DEBUG_INTERNAL_EDGE +#include <stdio.h> +#endif //DEBUG_INTERNAL_EDGE + + +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW +static btIDebugDraw* gDebugDrawer = 0; + +void btSetDebugDrawer(btIDebugDraw* debugDrawer) +{ + gDebugDrawer = debugDrawer; +} + +static void btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color) +{ + if (gDebugDrawer) + gDebugDrawer->drawLine(from,to,color); +} +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + +static int btGetHash(int partId, int triangleIndex) +{ + int hash = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; + return hash; +} + + + +static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA,const btVector3& normalB) +{ + const btVector3 refAxis0 = edgeA; + const btVector3 refAxis1 = normalA; + const btVector3 swingAxis = normalB; + btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); + return angle; +} + + +struct btConnectivityProcessor : public btTriangleCallback +{ + int m_partIdA; + int m_triangleIndexA; + btVector3* m_triangleVerticesA; + btTriangleInfoMap* m_triangleInfoMap; + + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + //skip self-collisions + if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex)) + return; + + //skip duplicates (disabled for now) + //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex)) + // return; + + //search for shared vertices and edges + int numshared = 0; + int sharedVertsA[3]={-1,-1,-1}; + int sharedVertsB[3]={-1,-1,-1}; + + ///skip degenerate triangles + btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2(); + if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold) + return; + + + btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2(); + ///skip degenerate triangles + if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold) + return; + +#if 0 + printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n", + m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(), + m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(), + m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ()); + + printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex); + printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n", + triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(), + triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(), + triangle[2].getX(),triangle[2].getY(),triangle[2].getZ()); +#endif + + for (int i=0;i<3;i++) + { + for (int j=0;j<3;j++) + { + if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold) + { + sharedVertsA[numshared] = i; + sharedVertsB[numshared] = j; + numshared++; + ///degenerate case + if(numshared >= 3) + return; + } + } + ///degenerate case + if(numshared >= 3) + return; + } + switch (numshared) + { + case 0: + { + break; + } + case 1: + { + //shared vertex + break; + } + case 2: + { + //shared edge + //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct + if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2) + { + sharedVertsA[0] = 2; + sharedVertsA[1] = 0; + int tmp = sharedVertsB[1]; + sharedVertsB[1] = sharedVertsB[0]; + sharedVertsB[0] = tmp; + } + + int hash = btGetHash(m_partIdA,m_triangleIndexA); + + btTriangleInfo* info = m_triangleInfoMap->find(hash); + if (!info) + { + btTriangleInfo tmp; + m_triangleInfoMap->insert(hash,tmp); + info = m_triangleInfoMap->find(hash); + } + + int sumvertsA = sharedVertsA[0]+sharedVertsA[1]; + int otherIndexA = 3-sumvertsA; + + + btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]); + + btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]); + int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]); + + btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]); + //btTriangleShape tB(triangle[0],triangle[1],triangle[2]); + + btVector3 normalA; + btVector3 normalB; + tA.calcNormal(normalA); + tB.calcNormal(normalB); + edge.normalize(); + btVector3 edgeCrossA = edge.cross(normalA).normalize(); + + { + btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]]; + if (edgeCrossA.dot(tmp) < 0) + { + edgeCrossA*=-1; + } + } + + btVector3 edgeCrossB = edge.cross(normalB).normalize(); + + { + btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]]; + if (edgeCrossB.dot(tmp) < 0) + { + edgeCrossB*=-1; + } + } + + btScalar angle2 = 0; + btScalar ang4 = 0.f; + + + btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB); + btScalar len2 = calculatedEdge.length2(); + + btScalar correctedAngle(0); + btVector3 calculatedNormalB = normalA; + bool isConvex = false; + + if (len2<m_triangleInfoMap->m_planarEpsilon) + { + angle2 = 0.f; + ang4 = 0.f; + } else + { + + calculatedEdge.normalize(); + btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA); + calculatedNormalA.normalize(); + angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB); + ang4 = SIMD_PI-angle2; + btScalar dotA = normalA.dot(edgeCrossB); + ///@todo: check if we need some epsilon, due to floating point imprecision + isConvex = (dotA<0.); + + correctedAngle = isConvex ? ang4 : -ang4; + btQuaternion orn2(calculatedEdge,-correctedAngle); + calculatedNormalB = btMatrix3x3(orn2)*normalA; + + + } + + + + + + //alternatively use + //btVector3 calculatedNormalB2 = quatRotate(orn,normalA); + + + switch (sumvertsA) + { + case 1: + { + btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1]; + btQuaternion orn(edge,-correctedAngle); + btVector3 computedNormalB = quatRotate(orn,normalA); + btScalar bla = computedNormalB.dot(normalB); + if (bla<0) + { + computedNormalB*=-1; + info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB; + } +#ifdef DEBUG_INTERNAL_EDGE + if ((computedNormalB-normalB).length()>0.0001) + { + printf("warning: normals not identical\n"); + } +#endif//DEBUG_INTERNAL_EDGE + + info->m_edgeV0V1Angle = -correctedAngle; + + if (isConvex) + info->m_flags |= TRI_INFO_V0V1_CONVEX; + break; + } + case 2: + { + btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0]; + btQuaternion orn(edge,-correctedAngle); + btVector3 computedNormalB = quatRotate(orn,normalA); + if (computedNormalB.dot(normalB)<0) + { + computedNormalB*=-1; + info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB; + } + +#ifdef DEBUG_INTERNAL_EDGE + if ((computedNormalB-normalB).length()>0.0001) + { + printf("warning: normals not identical\n"); + } +#endif //DEBUG_INTERNAL_EDGE + info->m_edgeV2V0Angle = -correctedAngle; + if (isConvex) + info->m_flags |= TRI_INFO_V2V0_CONVEX; + break; + } + case 3: + { + btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2]; + btQuaternion orn(edge,-correctedAngle); + btVector3 computedNormalB = quatRotate(orn,normalA); + if (computedNormalB.dot(normalB)<0) + { + info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB; + computedNormalB*=-1; + } +#ifdef DEBUG_INTERNAL_EDGE + if ((computedNormalB-normalB).length()>0.0001) + { + printf("warning: normals not identical\n"); + } +#endif //DEBUG_INTERNAL_EDGE + info->m_edgeV1V2Angle = -correctedAngle; + + if (isConvex) + info->m_flags |= TRI_INFO_V1V2_CONVEX; + break; + } + } + + break; + } + default: + { + // printf("warning: duplicate triangle\n"); + } + + } + } +}; +///////////////////////////////////////////////////////// +///////////////////////////////////////////////////////// + +void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap) +{ + //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there! + if (trimeshShape->getTriangleInfoMap()) + return; + + trimeshShape->setTriangleInfoMap(triangleInfoMap); + + btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface(); + const btVector3& meshScaling = meshInterface->getScaling(); + + for (int partId = 0; partId< meshInterface->getNumSubParts();partId++) + { + const unsigned char *vertexbase = 0; + int numverts = 0; + PHY_ScalarType type = PHY_INTEGER; + int stride = 0; + const unsigned char *indexbase = 0; + int indexstride = 0; + int numfaces = 0; + PHY_ScalarType indicestype = PHY_INTEGER; + //PHY_ScalarType indexType=0; + + btVector3 triangleVerts[3]; + meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId); + btVector3 aabbMin,aabbMax; + + for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++) + { + unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride); + + for (int j=2;j>=0;j--) + { + + int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + if (type == PHY_FLOAT) + { + float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + triangleVerts[j] = btVector3( + graphicsbase[0]*meshScaling.getX(), + graphicsbase[1]*meshScaling.getY(), + graphicsbase[2]*meshScaling.getZ()); + } + else + { + double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); + triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ())); + } + } + aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + aabbMin.setMin(triangleVerts[0]); + aabbMax.setMax(triangleVerts[0]); + aabbMin.setMin(triangleVerts[1]); + aabbMax.setMax(triangleVerts[1]); + aabbMin.setMin(triangleVerts[2]); + aabbMax.setMax(triangleVerts[2]); + + btConnectivityProcessor connectivityProcessor; + connectivityProcessor.m_partIdA = partId; + connectivityProcessor.m_triangleIndexA = triangleIndex; + connectivityProcessor.m_triangleVerticesA = &triangleVerts[0]; + connectivityProcessor.m_triangleInfoMap = triangleInfoMap; + + trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax); + } + + } + +} + + + + +// Given a point and a line segment (defined by two points), compute the closest point +// in the line. Cap the point at the endpoints of the line segment. +void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint) +{ + btVector3 lineDelta = line1 - line0; + + // Handle degenerate lines + if ( lineDelta.fuzzyZero()) + { + nearestPoint = line0; + } + else + { + btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta); + + // Clamp the point to conform to the segment's endpoints + if ( delta < 0 ) + delta = 0; + else if ( delta > 1 ) + delta = 1; + + nearestPoint = line0 + lineDelta*delta; + } +} + + + + +bool btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal) +{ + btVector3 tri_normal = tri_normal_org; + //we only have a local triangle normal, not a local contact normal -> only normal in world space... + //either compute the current angle all in local space, or all in world space + + btVector3 edgeCross = edge.cross(tri_normal).normalize(); + btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB); + + if (correctedEdgeAngle<0) + { + if (curAngle < correctedEdgeAngle) + { + btScalar diffAngle = correctedEdgeAngle-curAngle; + btQuaternion rotation(edge,diffAngle ); + clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB; + return true; + } + } + + if (correctedEdgeAngle>=0) + { + if (curAngle > correctedEdgeAngle) + { + btScalar diffAngle = correctedEdgeAngle-curAngle; + btQuaternion rotation(edge,diffAngle ); + clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB; + return true; + } + } + return false; +} + + + +/// Changes a btManifoldPoint collision normal to the normal from the mesh. +void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* colObj0,const btCollisionObject* colObj1, int partId0, int index0, int normalAdjustFlags) +{ + //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE); + if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE) + return; + + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape(); + btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); + if (!triangleInfoMapPtr) + return; + + int hash = btGetHash(partId0,index0); + + + btTriangleInfo* info = triangleInfoMapPtr->find(hash); + if (!info) + return; + + btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f; + + const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0->getCollisionShape()); + btVector3 v0,v1,v2; + tri_shape->getVertex(0,v0); + tri_shape->getVertex(1,v1); + tri_shape->getVertex(2,v2); + + btVector3 center = (v0+v1+v2)*btScalar(1./3.); + + btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0); + btVector3 tri_normal; + tri_shape->calcNormal(tri_normal); + + //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB); + btVector3 nearest; + btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest); + + btVector3 contact = cp.m_localPointB; +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + const btTransform& tr = colObj0->getWorldTransform(); + btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + + + bool isNearEdge = false; + + int numConcaveEdgeHits = 0; + int numConvexEdgeHits = 0; + + btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + localContactNormalOnB.normalize();//is this necessary? + + if ((info->m_edgeV0V1Angle)< SIMD_2_PI) + { +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); +#endif + btScalar len = (contact-nearest).length(); + if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) + { + btVector3 edge(v0-v1); + isNearEdge = true; + + if (info->m_edgeV0V1Angle==btScalar(0)) + { + numConcaveEdgeHits++; + } else + { + + bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX); + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); + #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); + #endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 nA = swapFactor * tri_normal; + + btQuaternion orn(edge,info->m_edgeV0V1Angle); + btVector3 computedNormalB = quatRotate(orn,tri_normal); + if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB) + computedNormalB*=-1; + btVector3 nB = swapFactor*computedNormalB; + + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); + +#ifdef DEBUG_INTERNAL_EDGE + { + + btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); + } +#endif //DEBUG_INTERNAL_EDGE + + + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else + { + numConvexEdgeHits++; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal); + if (isClamped) + { + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + { + btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. (what about cp.m_distance1?) + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); + + } + } + } + } + } + } + + btNearestPointInLineSegment(contact,v1,v2,nearest); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + if ((info->m_edgeV1V2Angle)< SIMD_2_PI) + { +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + + + btScalar len = (contact-nearest).length(); + if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) + { + isNearEdge = true; +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 edge(v1-v2); + + isNearEdge = true; + + if (info->m_edgeV1V2Angle == btScalar(0)) + { + numConcaveEdgeHits++; + } else + { + bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0; + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); + #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); + #endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 nA = swapFactor * tri_normal; + + btQuaternion orn(edge,info->m_edgeV1V2Angle); + btVector3 computedNormalB = quatRotate(orn,tri_normal); + if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB) + computedNormalB*=-1; + btVector3 nB = swapFactor*computedNormalB; + +#ifdef DEBUG_INTERNAL_EDGE + { + btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); + } +#endif //DEBUG_INTERNAL_EDGE + + + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); + + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else + { + numConvexEdgeHits++; + btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal); + if (isClamped) + { + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + { + btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); + } + } + } + } + } + } + + btNearestPointInLineSegment(contact,v2,v0,nearest); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + if ((info->m_edgeV2V0Angle)< SIMD_2_PI) + { + +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btScalar len = (contact-nearest).length(); + if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) + { + isNearEdge = true; +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 edge(v2-v0); + + if (info->m_edgeV2V0Angle==btScalar(0)) + { + numConcaveEdgeHits++; + } else + { + + bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0; + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); + #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); + #endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 nA = swapFactor * tri_normal; + btQuaternion orn(edge,info->m_edgeV2V0Angle); + btVector3 computedNormalB = quatRotate(orn,tri_normal); + if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB) + computedNormalB*=-1; + btVector3 nB = swapFactor*computedNormalB; + +#ifdef DEBUG_INTERNAL_EDGE + { + btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); + } +#endif //DEBUG_INTERNAL_EDGE + + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); + + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else + { + numConvexEdgeHits++; + // printf("hitting convex edge\n"); + + + btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal); + if (isClamped) + { + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + { + btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); + } + } + } + } + + + } + } + +#ifdef DEBUG_INTERNAL_EDGE + { + btVector3 color(0,1,1); + btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color); + } +#endif //DEBUG_INTERNAL_EDGE + + if (isNearEdge) + { + + if (numConcaveEdgeHits>0) + { + if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0) + { + //fix tri_normal so it pointing the same direction as the current local contact normal + if (tri_normal.dot(localContactNormalOnB) < 0) + { + tri_normal *= -1; + } + cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal; + } else + { + //modify the normal to be the triangle normal (or backfacing normal) + cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *(tri_normal *frontFacing); + } + + + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); + } + } +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h new file mode 100644 index 00000000000..9efb0122bb8 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h @@ -0,0 +1,46 @@ + +#ifndef BT_INTERNAL_EDGE_UTILITY_H +#define BT_INTERNAL_EDGE_UTILITY_H + +#include "LinearMath/btHashMap.h" +#include "LinearMath/btVector3.h" + +#include "BulletCollision/CollisionShapes/btTriangleInfoMap.h" + +///The btInternalEdgeUtility helps to avoid or reduce artifacts due to wrong collision normals caused by internal edges. +///See also http://code.google.com/p/bullet/issues/detail?id=27 + +class btBvhTriangleMeshShape; +class btCollisionObject; +class btManifoldPoint; +class btIDebugDraw; + + + +enum btInternalEdgeAdjustFlags +{ + BT_TRIANGLE_CONVEX_BACKFACE_MODE = 1, + BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended + BT_TRIANGLE_CONVEX_DOUBLE_SIDED = 4 +}; + + +///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo' +void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap); + + +///Call the btFixMeshNormal to adjust the collision normal, using the triangle info map (generated using btGenerateInternalEdgeInfo) +///If this info map is missing, or the triangle is not store in this map, nothing will be done +void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* trimeshColObj0,const btCollisionObject* otherColObj1, int partId0, int index0, int normalAdjustFlags = 0); + +///Enable the BT_INTERNAL_EDGE_DEBUG_DRAW define and call btSetDebugDrawer, to get visual info to see if the internal edge utility works properly. +///If the utility doesn't work properly, you might have to adjust the threshold values in btTriangleInfoMap +//#define BT_INTERNAL_EDGE_DEBUG_DRAW + +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW +void btSetDebugDrawer(btIDebugDraw* debugDrawer); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + +#endif //BT_INTERNAL_EDGE_UTILITY_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index f8dfa5b101f..fd684c056f7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -47,6 +47,12 @@ btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* b :m_manifoldPtr(0), m_body0(body0), m_body1(body1) +#ifdef DEBUG_PART_INDEX + ,m_partId0(-1), + m_partId1(-1), + m_index0(-1), + m_index1(-1) +#endif //DEBUG_PART_INDEX { m_rootTransA = body0->getWorldTransform(); m_rootTransB = body1->getWorldTransform(); @@ -57,8 +63,9 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b { btAssert(m_manifoldPtr); //order in manifold needs to match - - if (depth > m_manifoldPtr->getContactBreakingThreshold()) + +// if (depth > m_manifoldPtr->getContactBreakingThreshold()) + if (depth > m_manifoldPtr->getContactProcessingThreshold()) return; bool isSwapped = m_manifoldPtr->getBody0() != m_body0; @@ -88,10 +95,19 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1); //BP mod, store contact triangles. - newPt.m_partId0 = m_partId0; - newPt.m_partId1 = m_partId1; - newPt.m_index0 = m_index0; - newPt.m_index1 = m_index1; + if (isSwapped) + { + newPt.m_partId0 = m_partId1; + newPt.m_partId1 = m_partId0; + newPt.m_index0 = m_index1; + newPt.m_index1 = m_index0; + } else + { + newPt.m_partId0 = m_partId0; + newPt.m_partId1 = m_partId1; + newPt.m_index0 = m_index0; + newPt.m_index1 = m_index1; + } //printf("depth=%f\n",depth); ///@todo, check this for any side effects if (insertIndex >= 0) @@ -112,7 +128,7 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b //experimental feature info, for per-triangle material etc. btCollisionObject* obj0 = isSwapped? m_body1 : m_body0; btCollisionObject* obj1 = isSwapped? m_body0 : m_body1; - (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0,m_partId0,m_index0,obj1,m_partId1,m_index1); + (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 964b6a04483..927e2bc4f76 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -28,11 +28,14 @@ class btManifoldPoint; typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1); extern ContactAddedCallback gContactAddedCallback; +//#define DEBUG_PART_INDEX 1 ///btManifoldResult is a helper class to manage contact results. class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result { +protected: + btPersistentManifold* m_manifoldPtr; //we need this for compounds @@ -50,6 +53,13 @@ class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result public: btManifoldResult() +#ifdef DEBUG_PART_INDEX + : + m_partId0(-1), + m_partId1(-1), + m_index0(-1), + m_index1(-1) +#endif //DEBUG_PART_INDEX { } @@ -71,12 +81,16 @@ public: return m_manifoldPtr; } - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + virtual void setShapeIdentifiersA(int partId0,int index0) { - m_partId0=partId0; - m_partId1=partId1; - m_index0=index0; - m_index1=index1; + m_partId0=partId0; + m_index0=index0; + } + + virtual void setShapeIdentifiersB( int partId1,int index1) + { + m_partId1=partId1; + m_index1=index1; } @@ -99,7 +113,16 @@ public: } } + const btCollisionObject* getBody0Internal() const + { + return m_body0; + } + const btCollisionObject* getBody1Internal() const + { + return m_body1; + } + }; #endif //MANIFOLD_RESULT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 0328d0f738f..bb2a7f23985 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -1,3 +1,4 @@ + /* Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ @@ -44,10 +45,12 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl { { + btOverlappingPairCache* pairCachePtr = colWorld->getPairCache(); + const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs(); + btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr(); - for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++) + for (int i=0;i<numOverlappingPairs;i++) { - btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr(); const btBroadphasePair& collisionPair = pairPtr[i]; btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; @@ -63,15 +66,69 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl } } +#ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION +void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) +{ + + // put the index into m_controllers into m_tag + int index = 0; + { + + int i; + for (i=0;i<colWorld->getCollisionObjectArray().size(); i++) + { + btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + //Adding filtering here + if (!collisionObject->isStaticOrKinematicObject()) + { + collisionObject->setIslandTag(index++); + } + collisionObject->setCompanionId(-1); + collisionObject->setHitFraction(btScalar(1.)); + } + } + // do the union find + + initUnionFind( index ); + + findUnions(dispatcher,colWorld); +} + +void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) +{ + // put the islandId ('find' value) into m_tag + { + int index = 0; + int i; + for (i=0;i<colWorld->getCollisionObjectArray().size();i++) + { + btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + if (!collisionObject->isStaticOrKinematicObject()) + { + collisionObject->setIslandTag( m_unionFind.find(index) ); + //Set the correct object offset in Collision Object Array + m_unionFind.getElement(index).m_sz = i; + collisionObject->setCompanionId(-1); + index++; + } else + { + collisionObject->setIslandTag(-1); + collisionObject->setCompanionId(-2); + } + } + } +} + +#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) { - + initUnionFind( int (colWorld->getCollisionObjectArray().size())); - + // put the index into m_controllers into m_tag { - + int index = 0; int i; for (i=0;i<colWorld->getCollisionObjectArray().size(); i++) @@ -81,26 +138,20 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld collisionObject->setCompanionId(-1); collisionObject->setHitFraction(btScalar(1.)); index++; - + } } // do the union find - - findUnions(dispatcher,colWorld); - - + findUnions(dispatcher,colWorld); } - - - void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) { // put the islandId ('find' value) into m_tag { - - + + int index = 0; int i; for (i=0;i<colWorld->getCollisionObjectArray().size();i++) @@ -120,6 +171,8 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col } } +#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION + inline int getIslandId(const btPersistentManifold* lhs) { int islandId; @@ -339,15 +392,15 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, int islandId = getUnionFind().getElement(startIslandIndex).m_id; - bool islandSleeping = false; + bool islandSleeping = true; for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) { int i = getUnionFind().getElement(endIslandIndex).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; m_islandBodies.push_back(colObj0); - if (!colObj0->isActive()) - islandSleeping = true; + if (colObj0->isActive()) + islandSleeping = false; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index 2d5efcf56ba..c327c3ff72a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -59,7 +59,7 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()); btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = btScalar(1e30);///@todo: tighter bounds + input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds input.m_transformA = sphereObj->getWorldTransform(); input.m_transformB = triObj->getWorldTransform(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp index c561df06109..4c4f58d44fa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -70,7 +70,9 @@ void btUnionFind::sortIslands() for (int i=0;i<numElements;i++) { m_elements[i].m_id = find(i); +#ifndef STATIC_SIMULATION_ISLAND_OPTIMIZATION m_elements[i].m_sz = i; +#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION } // Sort the vector using predicate and std::sort @@ -78,4 +80,3 @@ void btUnionFind::sortIslands() m_elements.quickSort(btUnionFindElementSortPredicate()); } - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h index e105ecbff18..2cce335145b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h @@ -18,7 +18,10 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" - #define USE_PATH_COMPRESSION 1 +#define USE_PATH_COMPRESSION 1 + +///see for discussion of static island optimizations by Vroonsh here: http://code.google.com/p/bullet/issues/detail?id=406 +#define STATIC_SIMULATION_ISLAND_OPTIMIZATION 1 struct btElement { @@ -106,10 +109,12 @@ class btUnionFind //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically #ifdef USE_PATH_COMPRESSION - // - m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id; - #endif // + const btElement* elementPtr = &m_elements[m_elements[x].m_id]; + m_elements[x].m_id = elementPtr->m_id; + x = elementPtr->m_id; + #else// x = m_elements[x].m_id; + #endif //btAssert(x < m_N); //btAssert(x >= 0); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp new file mode 100644 index 00000000000..ecce028c2e6 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp @@ -0,0 +1,42 @@ +/* +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 "btBox2dShape.h" + + +//{ + + +void btBox2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); +} + + +void btBox2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + //btScalar margin = btScalar(0.); + btVector3 halfExtents = getHalfExtentsWithMargin(); + + btScalar lx=btScalar(2.)*(halfExtents.x()); + btScalar ly=btScalar(2.)*(halfExtents.y()); + btScalar lz=btScalar(2.)*(halfExtents.z()); + + inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), + mass/(btScalar(12.0)) * (lx*lx + lz*lz), + mass/(btScalar(12.0)) * (lx*lx + ly*ly)); + +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h new file mode 100644 index 00000000000..fc032069c03 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h @@ -0,0 +1,363 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef OBB_BOX_2D_SHAPE_H +#define OBB_BOX_2D_SHAPE_H + +#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btMinMax.h" + +///The btBox2dShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space. +class btBox2dShape: public btPolyhedralConvexShape +{ + + //btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead + + btVector3 m_centroid; + btVector3 m_vertices[4]; + btVector3 m_normals[4]; + +public: + + btVector3 getHalfExtentsWithMargin() const + { + btVector3 halfExtents = getHalfExtentsWithoutMargin(); + btVector3 margin(getMargin(),getMargin(),getMargin()); + halfExtents += margin; + return halfExtents; + } + + const btVector3& getHalfExtentsWithoutMargin() const + { + return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included + } + + + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + { + btVector3 halfExtents = getHalfExtentsWithoutMargin(); + btVector3 margin(getMargin(),getMargin(),getMargin()); + halfExtents += margin; + + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + } + + SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + { + const btVector3& halfExtents = getHalfExtentsWithoutMargin(); + + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + } + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const + { + const btVector3& halfExtents = getHalfExtentsWithoutMargin(); + + for (int i=0;i<numVectors;i++) + { + const btVector3& vec = vectors[i]; + supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + } + + } + + + btBox2dShape( const btVector3& boxHalfExtents) + : btPolyhedralConvexShape(), + m_centroid(0,0,0) + { + m_vertices[0].setValue(-boxHalfExtents.getX(),-boxHalfExtents.getY(),0); + m_vertices[1].setValue(boxHalfExtents.getX(),-boxHalfExtents.getY(),0); + m_vertices[2].setValue(boxHalfExtents.getX(),boxHalfExtents.getY(),0); + m_vertices[3].setValue(-boxHalfExtents.getX(),boxHalfExtents.getY(),0); + + m_normals[0].setValue(0,-1,0); + m_normals[1].setValue(1,0,0); + m_normals[2].setValue(0,1,0); + m_normals[3].setValue(-1,0,0); + + m_shapeType = BOX_2D_SHAPE_PROXYTYPE; + btVector3 margin(getMargin(),getMargin(),getMargin()); + m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; + }; + + virtual void setMargin(btScalar collisionMargin) + { + //correct the m_implicitShapeDimensions for the margin + btVector3 oldMargin(getMargin(),getMargin(),getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + + btConvexInternalShape::setMargin(collisionMargin); + btVector3 newMargin(getMargin(),getMargin(),getMargin()); + m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin; + + } + virtual void setLocalScaling(const btVector3& scaling) + { + btVector3 oldMargin(getMargin(),getMargin(),getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling; + + btConvexInternalShape::setLocalScaling(scaling); + + m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin; + + } + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + + + + + int getVertexCount() const + { + return 4; + } + + virtual int getNumVertices()const + { + return 4; + } + + const btVector3* getVertices() const + { + return &m_vertices[0]; + } + + const btVector3* getNormals() const + { + return &m_normals[0]; + } + + + + + + + + virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const + { + //this plane might not be aligned... + btVector4 plane ; + getPlaneEquation(plane,i); + planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ()); + planeSupport = localGetSupportingVertex(-planeNormal); + } + + + const btVector3& getCentroid() const + { + return m_centroid; + } + + virtual int getNumPlanes() const + { + return 6; + } + + + + virtual int getNumEdges() const + { + return 12; + } + + + virtual void getVertex(int i,btVector3& vtx) const + { + btVector3 halfExtents = getHalfExtentsWithoutMargin(); + + vtx = btVector3( + halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1), + halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1), + halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2)); + } + + + virtual void getPlaneEquation(btVector4& plane,int i) const + { + btVector3 halfExtents = getHalfExtentsWithoutMargin(); + + switch (i) + { + case 0: + plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x()); + break; + case 1: + plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x()); + break; + case 2: + plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y()); + break; + case 3: + plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y()); + break; + case 4: + plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z()); + break; + case 5: + plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z()); + break; + default: + btAssert(0); + } + } + + + virtual void getEdge(int i,btVector3& pa,btVector3& pb) const + //virtual void getEdge(int i,Edge& edge) const + { + int edgeVert0 = 0; + int edgeVert1 = 0; + + switch (i) + { + case 0: + edgeVert0 = 0; + edgeVert1 = 1; + break; + case 1: + edgeVert0 = 0; + edgeVert1 = 2; + break; + case 2: + edgeVert0 = 1; + edgeVert1 = 3; + + break; + case 3: + edgeVert0 = 2; + edgeVert1 = 3; + break; + case 4: + edgeVert0 = 0; + edgeVert1 = 4; + break; + case 5: + edgeVert0 = 1; + edgeVert1 = 5; + + break; + case 6: + edgeVert0 = 2; + edgeVert1 = 6; + break; + case 7: + edgeVert0 = 3; + edgeVert1 = 7; + break; + case 8: + edgeVert0 = 4; + edgeVert1 = 5; + break; + case 9: + edgeVert0 = 4; + edgeVert1 = 6; + break; + case 10: + edgeVert0 = 5; + edgeVert1 = 7; + break; + case 11: + edgeVert0 = 6; + edgeVert1 = 7; + break; + default: + btAssert(0); + + } + + getVertex(edgeVert0,pa ); + getVertex(edgeVert1,pb ); + } + + + + + + virtual bool isInside(const btVector3& pt,btScalar tolerance) const + { + btVector3 halfExtents = getHalfExtentsWithoutMargin(); + + //btScalar minDist = 2*tolerance; + + bool result = (pt.x() <= (halfExtents.x()+tolerance)) && + (pt.x() >= (-halfExtents.x()-tolerance)) && + (pt.y() <= (halfExtents.y()+tolerance)) && + (pt.y() >= (-halfExtents.y()-tolerance)) && + (pt.z() <= (halfExtents.z()+tolerance)) && + (pt.z() >= (-halfExtents.z()-tolerance)); + + return result; + } + + + //debugging + virtual const char* getName()const + { + return "Box2d"; + } + + virtual int getNumPreferredPenetrationDirections() const + { + return 6; + } + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + { + switch (index) + { + case 0: + penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); + break; + case 1: + penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); + break; + case 2: + penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); + break; + case 3: + penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); + break; + case 4: + penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); + break; + case 5: + penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); + break; + default: + btAssert(0); + } + } + +}; + +#endif //OBB_BOX_2D_SHAPE_H + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp index 8bc2cfa6ffa..c6644efbef3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -12,7 +12,6 @@ subject to the following restrictions: 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 "btBoxShape.h" diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h index 14502d485dc..b405efc8e3c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -41,7 +41,7 @@ public: const btVector3& getHalfExtentsWithoutMargin() const { - return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included + return m_implicitShapeDimensions;//scaling is included, margin is not } @@ -312,6 +312,7 @@ public: }; + #endif //OBB_BOX_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index 4d350196943..ace4cfa2646 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -17,12 +17,14 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btOptimizedBvh.h" +#include "LinearMath/btSerializer.h" ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization. ///Uses an interface to access the triangles to allow for sharing graphics/physics triangles. btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh) :btTriangleMeshShape(meshInterface), m_bvh(0), +m_triangleInfoMap(0), m_useQuantizedAabbCompression(useQuantizedAabbCompression), m_ownsBvh(false) { @@ -42,6 +44,7 @@ m_ownsBvh(false) btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,bool buildBvh) :btTriangleMeshShape(meshInterface), m_bvh(0), +m_triangleInfoMap(0), m_useQuantizedAabbCompression(useQuantizedAabbCompression), m_ownsBvh(false) { @@ -274,13 +277,13 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co nodeSubPart); unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); + btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT||indicestype==PHY_UCHAR); const btVector3& meshScaling = m_meshInterface->getScaling(); for (int j=2;j>=0;j--) { - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:indicestype==PHY_INTEGER?gfxbase[j]:((unsigned char*)gfxbase)[j]; #ifdef DEBUG_TRIANGLE_MESH @@ -364,3 +367,100 @@ void btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVect } + +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*) dataBuffer; + + btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer); + + m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer); + + trimeshData->m_collisionMargin = float(m_collisionMargin); + + + + if (m_bvh && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_BVH)) + { + void* chunk = serializer->findPointer(m_bvh); + if (chunk) + { +#ifdef BT_USE_DOUBLE_PRECISION + trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)chunk; + trimeshData->m_quantizedFloatBvh = 0; +#else + trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)chunk; + trimeshData->m_quantizedDoubleBvh= 0; +#endif //BT_USE_DOUBLE_PRECISION + } else + { + +#ifdef BT_USE_DOUBLE_PRECISION + trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh); + trimeshData->m_quantizedFloatBvh = 0; +#else + trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh); + trimeshData->m_quantizedDoubleBvh= 0; +#endif //BT_USE_DOUBLE_PRECISION + + int sz = m_bvh->calculateSerializeBufferSizeNew(); + btChunk* chunk = serializer->allocate(sz,1); + const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,m_bvh); + } + } else + { + trimeshData->m_quantizedFloatBvh = 0; + trimeshData->m_quantizedDoubleBvh = 0; + } + + + + if (m_triangleInfoMap && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_TRIANGLEINFOMAP)) + { + void* chunk = serializer->findPointer(m_triangleInfoMap); + if (chunk) + { + trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)chunk; + } else + { + trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)serializer->getUniquePointer(m_triangleInfoMap); + int sz = m_triangleInfoMap->calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(sz,1); + const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,m_triangleInfoMap); + } + } else + { + trimeshData->m_triangleInfoMap = 0; + } + + return "btTriangleMeshShapeData"; +} + +void btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const +{ + if (m_bvh) + { + int len = m_bvh->calculateSerializeBufferSizeNew(); //make sure not to use calculateSerializeBufferSize because it is used for in-place + btChunk* chunk = serializer->allocate(len,1); + const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,(void*)m_bvh); + } +} + +void btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const +{ + if (m_triangleInfoMap) + { + int len = m_triangleInfoMap->calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(len,1); + const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,(void*)m_triangleInfoMap); + } +} + + + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index ab54bd1a1ed..c269ef27bdb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -19,7 +19,7 @@ subject to the following restrictions: #include "btTriangleMeshShape.h" #include "btOptimizedBvh.h" #include "LinearMath/btAlignedAllocator.h" - +#include "btTriangleInfoMap.h" ///The btBvhTriangleMeshShape is a static-triangle mesh shape with several optimizations, such as bounding volume hierarchy and cache friendly traversal for PlayStation 3 Cell SPU. It is recommended to enable useQuantizedAabbCompression for better memory usage. ///It takes a triangle mesh as input, for example a btTriangleMesh or btTriangleIndexVertexArray. The btBvhTriangleMeshShape class allows for triangle mesh deformations by a refit or partialRefit method. @@ -29,6 +29,8 @@ ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape { btOptimizedBvh* m_bvh; + btTriangleInfoMap* m_triangleInfoMap; + bool m_useQuantizedAabbCompression; bool m_ownsBvh; bool m_pad[11];////need padding due to alignment @@ -37,7 +39,7 @@ public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btBvhTriangleMeshShape() : btTriangleMeshShape(0),m_bvh(0),m_ownsBvh(false) {m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;}; + btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true); ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb @@ -73,7 +75,6 @@ public: return m_bvh; } - void setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& localScaling=btVector3(1,1,1)); void buildOptimizedBvh(); @@ -82,7 +83,57 @@ public: { return m_useQuantizedAabbCompression; } + + void setTriangleInfoMap(btTriangleInfoMap* triangleInfoMap) + { + m_triangleInfoMap = triangleInfoMap; + } + + const btTriangleInfoMap* getTriangleInfoMap() const + { + return m_triangleInfoMap; + } + + btTriangleInfoMap* getTriangleInfoMap() + { + return m_triangleInfoMap; + } + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + virtual void serializeSingleBvh(btSerializer* serializer) const; + + virtual void serializeSingleTriangleInfoMap(btSerializer* serializer) const; + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btTriangleMeshShapeData +{ + btCollisionShapeData m_collisionShapeData; + + btStridingMeshInterfaceData m_meshInterface; + + btQuantizedBvhFloatData *m_quantizedFloatBvh; + btQuantizedBvhDoubleData *m_quantizedDoubleBvh; + + btTriangleInfoMapData *m_triangleInfoMap; + + float m_collisionMargin; + + char m_pad3[4]; + +}; + + +SIMD_FORCE_INLINE int btBvhTriangleMeshShape::calculateSerializeBufferSize() const +{ + return sizeof(btTriangleMeshShapeData); } -; + + #endif //BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp index 60a96a542f9..2faa11d4360 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -32,7 +32,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter btVector3 supVec(0,0,0); - btScalar maxDot(btScalar(-1e30)); + btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); @@ -88,7 +88,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter for (int j=0;j<numVectors;j++) { - btScalar maxDot(btScalar(-1e30)); + btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); const btVector3& vec = vectors[j]; btVector3 vtx; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h index 828c1b3a565..ab763abf808 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -43,6 +43,18 @@ public: virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual void setMargin(btScalar collisionMargin) + { + //correct the m_implicitShapeDimensions for the margin + btVector3 oldMargin(getMargin(),getMargin(),getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + + btConvexInternalShape::setMargin(collisionMargin); + btVector3 newMargin(getMargin(),getMargin(),getMargin()); + m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin; + + } + virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { btVector3 halfExtents(getRadius(),getRadius(),getRadius()); @@ -77,6 +89,24 @@ public: return m_implicitShapeDimensions[m_upAxis]; } + virtual void setLocalScaling(const btVector3& scaling) + { + btVector3 oldMargin(getMargin(),getMargin(),getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling; + + btConvexInternalShape::setLocalScaling(scaling); + + m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin; + + } + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + }; ///btCapsuleShapeX represents a capsule around the Z axis @@ -113,6 +143,31 @@ public: }; +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btCapsuleShapeData +{ + btConvexInternalShapeData m_convexInternalShapeData; + + int m_upAxis; + + char m_padding[4]; +}; + +SIMD_FORCE_INLINE int btCapsuleShape::calculateSerializeBufferSize() const +{ + return sizeof(btCapsuleShapeData); +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btCapsuleShapeData* shapeData = (btCapsuleShapeData*) dataBuffer; + + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer); + shapeData->m_upAxis = m_upAxis; + + return "btCapsuleShapeData"; +} #endif //BT_CAPSULE_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h index 4730264d3df..18fd026041f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp index d242cba1b72..39ee21cad73 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -12,12 +12,8 @@ subject to the following restrictions: 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ - #include "BulletCollision/CollisionShapes/btCollisionShape.h" - - -btScalar gContactThresholdFactor=btScalar(0.02); - +#include "LinearMath/btSerializer.h" /* Make sure this dummy function never changes so that it @@ -45,10 +41,12 @@ void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) con center = (aabbMin+aabbMax)*btScalar(0.5); } -btScalar btCollisionShape::getContactBreakingThreshold() const + +btScalar btCollisionShape::getContactBreakingThreshold(btScalar defaultContactThreshold) const { - return getAngularMotionDisc() * gContactThresholdFactor; + return getAngularMotionDisc() * defaultContactThreshold; } + btScalar btCollisionShape::getAngularMotionDisc() const { ///@todo cache this value, to improve performance @@ -96,3 +94,26 @@ void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const b temporalAabbMin -= angularMotion3d; temporalAabbMax += angularMotion3d; } + +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btCollisionShapeData* shapeData = (btCollisionShapeData*) dataBuffer; + char* name = (char*) serializer->findNameForPointer(this); + shapeData->m_name = (char*)serializer->getUniquePointer(name); + if (shapeData->m_name) + { + serializer->serializeName(name); + } + shapeData->m_shapeType = m_shapeType; + //shapeData->m_padding//?? + return "btCollisionShapeData"; +} + +void btCollisionShape::serializeSingleShape(btSerializer* serializer) const +{ + int len = calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(len,1); + const char* structType = serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,(void*)this); +}
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h index b6374e64153..f32bd736a99 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -20,6 +20,8 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types +class btSerializer; + ///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects. class btCollisionShape @@ -46,24 +48,33 @@ public: ///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations. virtual btScalar getAngularMotionDisc() const; - virtual btScalar getContactBreakingThreshold() const; + virtual btScalar getContactBreakingThreshold(btScalar defaultContactThresholdFactor) const; ///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) ///result is conservative void calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const; -#ifndef __SPU__ + SIMD_FORCE_INLINE bool isPolyhedral() const { return btBroadphaseProxy::isPolyhedral(getShapeType()); } + SIMD_FORCE_INLINE bool isConvex2d() const + { + return btBroadphaseProxy::isConvex2d(getShapeType()); + } + SIMD_FORCE_INLINE bool isConvex() const { return btBroadphaseProxy::isConvex(getShapeType()); } + SIMD_FORCE_INLINE bool isNonMoving() const + { + return btBroadphaseProxy::isNonMoving(getShapeType()); + } SIMD_FORCE_INLINE bool isConcave() const { return btBroadphaseProxy::isConcave(getShapeType()); @@ -72,6 +83,7 @@ public: { return btBroadphaseProxy::isCompound(getShapeType()); } + SIMD_FORCE_INLINE bool isSoftBody() const { return btBroadphaseProxy::isSoftBody(getShapeType()); @@ -83,7 +95,7 @@ public: return btBroadphaseProxy::isInfinite(getShapeType()); } - +#ifndef __SPU__ virtual void setLocalScaling(const btVector3& scaling) =0; virtual const btVector3& getLocalScaling() const =0; virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const = 0; @@ -110,7 +122,29 @@ public: return m_userPointer; } + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + virtual void serializeSingleShape(btSerializer* serializer) const; + }; +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btCollisionShapeData +{ + char *m_name; + int m_shapeType; + char m_padding[4]; +}; + +SIMD_FORCE_INLINE int btCollisionShape::calculateSerializeBufferSize() const +{ + return sizeof(btCollisionShapeData); +} + + + #endif //COLLISION_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index 9c2b04d18fa..4eb860c57f1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -16,14 +16,15 @@ subject to the following restrictions: #include "btCompoundShape.h" #include "btCollisionShape.h" #include "BulletCollision/BroadphaseCollision/btDbvt.h" +#include "LinearMath/btSerializer.h" btCompoundShape::btCompoundShape(bool enableDynamicAabbTree) -: m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)), -m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)), -m_collisionMargin(btScalar(0.)), -m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), +: m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)), +m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)), m_dynamicAabbTree(0), -m_updateRevision(1) +m_updateRevision(1), +m_collisionMargin(btScalar(0.)), +m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) { m_shapeType = COMPOUND_SHAPE_PROXYTYPE; @@ -51,6 +52,7 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio //m_childTransforms.push_back(localTransform); //m_childShapes.push_back(shape); btCompoundShapeChild child; + child.m_node = 0; child.m_transform = localTransform; child.m_childShape = shape; child.m_childShapeType = shape->getShapeType(); @@ -83,7 +85,7 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio } -void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform) +void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform,bool shouldRecalculateLocalAabb) { m_children[childIndex].m_transform = newChildTransform; @@ -93,11 +95,14 @@ void btCompoundShape::updateChildTransform(int childIndex, const btTransform& ne btVector3 localAabbMin,localAabbMax; m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax); ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); - int index = m_children.size()-1; + //int index = m_children.size()-1; m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds); } - recalculateLocalAabb(); + if (shouldRecalculateLocalAabb) + { + recalculateLocalAabb(); + } } void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) @@ -109,6 +114,8 @@ void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node); } m_children.swap(childShapeIndex,m_children.size()-1); + if (m_dynamicAabbTree) + m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex; m_children.pop_back(); } @@ -124,14 +131,7 @@ void btCompoundShape::removeChildShape(btCollisionShape* shape) { if(m_children[i].m_childShape == shape) { - m_children.swap(i,m_children.size()-1); - m_children.pop_back(); - //remove it from the m_dynamicAabbTree too - //@todo: this leads to problems due to caching in the btCompoundCollisionAlgorithm - //so effectively, removeChildShape is broken at the moment - //m_dynamicAabbTree->remove(m_aabbProxies[i]); - //m_aabbProxies.swap(i,m_children.size()-1); - //m_aabbProxies.pop_back(); + removeChildShapeByIndex(i); } } @@ -145,8 +145,8 @@ void btCompoundShape::recalculateLocalAabb() // Recalculate the local aabb // Brute force, it iterates over all the shapes left. - m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); //extend the local aabbMin/aabbMax for (int j = 0; j < m_children.size(); j++) @@ -223,9 +223,13 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf for (k = 0; k < n; k++) { + btAssert(masses[k]>0); center += m_children[k].m_transform.getOrigin() * masses[k]; totalMass += masses[k]; } + + btAssert(totalMass>0); + center /= totalMass; principal.setOrigin(center); @@ -271,3 +275,82 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf +void btCompoundShape::setLocalScaling(const btVector3& scaling) +{ + + for(int i = 0; i < m_children.size(); i++) + { + btTransform childTrans = getChildTransform(i); + btVector3 childScale = m_children[i].m_childShape->getLocalScaling(); +// childScale = childScale * (childTrans.getBasis() * scaling); + childScale = childScale * scaling / m_localScaling; + m_children[i].m_childShape->setLocalScaling(childScale); + childTrans.setOrigin((childTrans.getOrigin())*scaling); + updateChildTransform(i, childTrans,false); + } + + m_localScaling = scaling; + recalculateLocalAabb(); + +} + + +void btCompoundShape::createAabbTreeFromChildren() +{ + if ( !m_dynamicAabbTree ) + { + void* mem = btAlignedAlloc(sizeof(btDbvt),16); + m_dynamicAabbTree = new(mem) btDbvt(); + btAssert(mem==m_dynamicAabbTree); + + for ( int index = 0; index < m_children.size(); index++ ) + { + btCompoundShapeChild &child = m_children[index]; + + //extend the local aabbMin/aabbMax + btVector3 localAabbMin,localAabbMax; + child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax); + + const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index); + } + } +} + + +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const +{ + + btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer; + btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer); + + shapeData->m_collisionMargin = float(m_collisionMargin); + shapeData->m_numChildShapes = m_children.size(); + shapeData->m_childShapePtr = 0; + if (shapeData->m_numChildShapes) + { + btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes); + btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr; + shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr); + + for (int i=0;i<shapeData->m_numChildShapes;i++,memPtr++) + { + memPtr->m_childMargin = float(m_children[i].m_childMargin); + memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape); + //don't serialize shapes that already have been serialized + if (!serializer->findPointer(m_children[i].m_childShape)) + { + btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1); + const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer); + serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape); + } + + memPtr->m_childShapeType = m_children[i].m_childShapeType; + m_children[i].m_transform.serializeFloat(memPtr->m_transform); + } + serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr); + } + return "btCompoundShapeData"; +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index 434860c8633..7f41dd4517b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -62,6 +62,11 @@ ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape ///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated int m_updateRevision; + btScalar m_collisionMargin; + +protected: + btVector3 m_localScaling; + public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -101,7 +106,7 @@ public: } ///set a new transform for a child, and update internal data structures (local aabb and dynamic tree) - void updateChildTransform(int childIndex, const btTransform& newChildTransform); + void updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb = true); btCompoundShapeChild* getChildList() @@ -116,10 +121,8 @@ public: Use this yourself if you modify the children or their transforms. */ virtual void recalculateLocalAabb(); - virtual void setLocalScaling(const btVector3& scaling) - { - m_localScaling = scaling; - } + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const { return m_localScaling; @@ -140,14 +143,18 @@ public: return "Compound"; } - //this is optional, but should make collision queries faster, by culling non-overlapping nodes - void createAabbTreeFromChildren(); - - btDbvt* getDynamicAabbTree() + const btDbvt* getDynamicAabbTree() const + { + return m_dynamicAabbTree; + } + + btDbvt* getDynamicAabbTree() { return m_dynamicAabbTree; } + void createAabbTreeFromChildren(); + ///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia ///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform ///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound @@ -160,13 +167,46 @@ public: return m_updateRevision; } -private: - btScalar m_collisionMargin; -protected: - btVector3 m_localScaling; + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + }; +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btCompoundShapeChildData +{ + btTransformFloatData m_transform; + btCollisionShapeData *m_childShape; + int m_childShapeType; + float m_childMargin; +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btCompoundShapeData +{ + btCollisionShapeData m_collisionShapeData; + + btCompoundShapeChildData *m_childShapePtr; + + int m_numChildShapes; + + float m_collisionMargin; + +}; + + +SIMD_FORCE_INLINE int btCompoundShape::calculateSerializeBufferSize() const +{ + return sizeof(btCompoundShapeData); +} + + + + + #endif //COMPOUND_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp index 5103500a012..58ff84a5b02 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp @@ -1,7 +1,6 @@ - /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h index 30065d55a12..2a370a47c75 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp index d887be61ada..5e83087b320 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -131,3 +131,13 @@ btVector3 btConeShape::localGetSupportingVertex(const btVector3& vec) const } +void btConeShape::setLocalScaling(const btVector3& scaling) +{ + int axis = m_coneIndices[1]; + int r1 = m_coneIndices[0]; + int r2 = m_coneIndices[2]; + m_height *= scaling[axis] / m_localScaling[axis]; + m_radius *= (scaling[r1] / m_localScaling[r1] + scaling[r2] / m_localScaling[r2]) / 2; + m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height)); + btConvexInternalShape::setLocalScaling(scaling); +}
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h index 1869893f37a..bd7d1443ac2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -81,6 +81,9 @@ public: { return m_coneIndices[1]; } + + virtual void setLocalScaling(const btVector3& scaling); + }; ///btConeShape implements a Cone shape, around the X axis diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp new file mode 100644 index 00000000000..10ea3e981af --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp @@ -0,0 +1,92 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btConvex2dShape.h" + +btConvex2dShape::btConvex2dShape( btConvexShape* convexChildShape): +btConvexShape (), m_childConvexShape(convexChildShape) +{ + m_shapeType = CONVEX_2D_SHAPE_PROXYTYPE; +} + +btConvex2dShape::~btConvex2dShape() +{ +} + + + +btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +{ + return m_childConvexShape->localGetSupportingVertexWithoutMargin(vec); +} + +void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors); +} + + +btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec)const +{ + return m_childConvexShape->localGetSupportingVertex(vec); +} + + +void btConvex2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + ///this linear upscaling is not realistic, but we don't deal with large mass ratios... + m_childConvexShape->calculateLocalInertia(mass,inertia); +} + + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btConvex2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + m_childConvexShape->getAabb(t,aabbMin,aabbMax); +} + +void btConvex2dShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax); +} + +void btConvex2dShape::setLocalScaling(const btVector3& scaling) +{ + m_childConvexShape->setLocalScaling(scaling); +} + +const btVector3& btConvex2dShape::getLocalScaling() const +{ + return m_childConvexShape->getLocalScaling(); +} + +void btConvex2dShape::setMargin(btScalar margin) +{ + m_childConvexShape->setMargin(margin); +} +btScalar btConvex2dShape::getMargin() const +{ + return m_childConvexShape->getMargin(); +} + +int btConvex2dShape::getNumPreferredPenetrationDirections() const +{ + return m_childConvexShape->getNumPreferredPenetrationDirections(); +} + +void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const +{ + m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h new file mode 100644 index 00000000000..caf54329da9 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h @@ -0,0 +1,80 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_CONVEX_2D_SHAPE_H +#define BT_CONVEX_2D_SHAPE_H + +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types + +///The btConvex2dShape allows to use arbitrary convex shapes as 2d convex shapes, with the Z component assumed to be 0. +///For 2d boxes, the btBox2dShape is recommended. +class btConvex2dShape : public btConvexShape +{ + btConvexShape* m_childConvexShape; + + public: + + btConvex2dShape( btConvexShape* convexChildShape); + + virtual ~btConvex2dShape(); + + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + btConvexShape* getChildShape() + { + return m_childConvexShape; + } + + const btConvexShape* getChildShape() const + { + return m_childConvexShape; + } + + virtual const char* getName()const + { + return "Convex2dShape"; + } + + + + /////////////////////////// + + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + virtual void setLocalScaling(const btVector3& scaling) ; + virtual const btVector3& getLocalScaling() const ; + + virtual void setMargin(btScalar margin); + virtual btScalar getMargin() const; + + virtual int getNumPreferredPenetrationDirections() const; + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; + + +}; + +#endif //BT_CONVEX_2D_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 7b45fad12df..69bc67cafab 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -12,14 +12,14 @@ subject to the following restrictions: 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 "btConvexHullShape.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "LinearMath/btQuaternion.h" +#include "LinearMath/btSerializer.h" - - -btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexShape () +btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexAabbCachingShape () { m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE; m_unscaledPoints.resize(numPoints); @@ -52,22 +52,10 @@ void btConvexHullShape::addPoint(const btVector3& point) } -btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const { btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); - btScalar newDot,maxDot = btScalar(-1e30); - - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - + btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT); for (int i=0;i<m_unscaledPoints.size();i++) { @@ -90,7 +78,7 @@ void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const { for (int i=0;i<numVectors;i++) { - supportVerticesOut[i][3] = btScalar(-1e30); + supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT); } } for (int i=0;i<m_unscaledPoints.size();i++) @@ -186,3 +174,38 @@ bool btConvexHullShape::isInside(const btVector3& ,btScalar ) const return false; } +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* serializer) const +{ + //int szc = sizeof(btConvexHullShapeData); + btConvexHullShapeData* shapeData = (btConvexHullShapeData*) dataBuffer; + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); + + int numElem = m_unscaledPoints.size(); + shapeData->m_numUnscaledPoints = numElem; +#ifdef BT_USE_DOUBLE_PRECISION + shapeData->m_unscaledPointsFloatPtr = 0; + shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]): 0; +#else + shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]): 0; + shapeData->m_unscaledPointsDoublePtr = 0; +#endif + + if (numElem) + { + int sz = sizeof(btVector3Data); + // int sz2 = sizeof(btVector3DoubleData); + // int sz3 = sizeof(btVector3FloatData); + btChunk* chunk = serializer->allocate(sz,numElem); + btVector3Data* memPtr = (btVector3Data*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + m_unscaledPoints[i].serialize(*memPtr); + } + serializer->finalizeChunk(chunk,btVector3DataName,BT_ARRAY_CODE,(void*)&m_unscaledPoints[0]); + } + + return "btConvexHullShapeData"; +} + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h index baf074be6c3..bf960f4df92 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -20,9 +20,10 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "LinearMath/btAlignedObjectArray.h" + ///The btConvexHullShape implements an implicit convex hull of an array of vertices. ///Bullet provides a general and fast collision detector for convex shapes based on GJK and EPA using localGetSupportingVertex. -ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexAabbCachingShape { btAlignedObjectArray<btVector3> m_unscaledPoints; @@ -88,8 +89,32 @@ public: ///in case we receive negative scaling virtual void setLocalScaling(const btVector3& scaling); + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btConvexHullShapeData +{ + btConvexInternalShapeData m_convexInternalShapeData; + + btVector3FloatData *m_unscaledPointsFloatPtr; + btVector3DoubleData *m_unscaledPointsDoublePtr; + + int m_numUnscaledPoints; + char m_padding3[4]; + }; +SIMD_FORCE_INLINE int btConvexHullShape::calculateSerializeBufferSize() const +{ + return sizeof(btConvexHullShapeData); +} + + #endif //CONVEX_HULL_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp index bfb741310e1..083d60b1b1e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -34,7 +34,8 @@ void btConvexInternalShape::setLocalScaling(const btVector3& scaling) void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const { - +#ifndef __SPU__ + //use localGetSupportingVertexWithoutMargin? btScalar margin = getMargin(); for (int i=0;i<3;i++) { @@ -49,6 +50,7 @@ void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAa tmp = trans(localGetSupportingVertex(vec*trans.getBasis())); minAabb[i] = tmp[i]-margin; } +#endif } @@ -79,3 +81,71 @@ btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)c } +btConvexInternalAabbCachingShape::btConvexInternalAabbCachingShape() + : btConvexInternalShape(), +m_localAabbMin(1,1,1), +m_localAabbMax(-1,-1,-1), +m_isLocalAabbValid(false) +{ +} + + +void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +{ + getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin()); +} + +void btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling) +{ + btConvexInternalShape::setLocalScaling(scaling); + recalcLocalAabb(); +} + + +void btConvexInternalAabbCachingShape::recalcLocalAabb() +{ + m_isLocalAabbValid = true; + + #if 1 + static const btVector3 _directions[] = + { + btVector3( 1., 0., 0.), + btVector3( 0., 1., 0.), + btVector3( 0., 0., 1.), + btVector3( -1., 0., 0.), + btVector3( 0., -1., 0.), + btVector3( 0., 0., -1.) + }; + + btVector3 _supporting[] = + { + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.) + }; + + batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); + + for ( int i = 0; i < 3; ++i ) + { + m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin; + m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin; + } + + #else + + for (int i=0;i<3;i++) + { + btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + vec[i] = btScalar(1.); + btVector3 tmp = localGetSupportingVertex(vec); + m_localAabbMax[i] = tmp[i]+m_collisionMargin; + vec[i] = btScalar(-1.); + tmp = localGetSupportingVertex(vec); + m_localAabbMin[i] = tmp[i]-m_collisionMargin; + } + #endif +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h index bab720d7b6d..12527731804 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h @@ -1,8 +1,24 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ #ifndef BT_CONVEX_INTERNAL_SHAPE_H #define BT_CONVEX_INTERNAL_SHAPE_H #include "btConvexShape.h" +#include "LinearMath/btAabbUtil2.h" + ///The btConvexInternalShape is an internal base class, shared by most convex shape implementations. class btConvexInternalShape : public btConvexShape @@ -37,6 +53,15 @@ public: return m_implicitShapeDimensions; } + ///warning: use setImplicitShapeDimensions with care + ///changing a collision shape while the body is in the world is not recommended, + ///it is best to remove the body from the world, then make the change, and re-add it + ///alternatively flush the contact points, see documentation for 'cleanProxyFromPairs' + void setImplicitShapeDimensions(const btVector3& dimensions) + { + m_implicitShapeDimensions = dimensions; + } + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { @@ -85,9 +110,93 @@ public: btAssert(0); } + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btConvexInternalShapeData +{ + btCollisionShapeData m_collisionShapeData; + + btVector3FloatData m_localScaling; + btVector3FloatData m_implicitShapeDimensions; + float m_collisionMargin; + + int m_padding; + }; + +SIMD_FORCE_INLINE int btConvexInternalShape::calculateSerializeBufferSize() const +{ + return sizeof(btConvexInternalShapeData); +} + +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btConvexInternalShape::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btConvexInternalShapeData* shapeData = (btConvexInternalShapeData*) dataBuffer; + btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer); + + m_implicitShapeDimensions.serializeFloat(shapeData->m_implicitShapeDimensions); + m_localScaling.serializeFloat(shapeData->m_localScaling); + shapeData->m_collisionMargin = float(m_collisionMargin); + + return "btConvexInternalShapeData"; +} + + + + +///btConvexInternalAabbCachingShape adds local aabb caching for convex shapes, to avoid expensive bounding box calculations +class btConvexInternalAabbCachingShape : public btConvexInternalShape +{ + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + bool m_isLocalAabbValid; + +protected: + + btConvexInternalAabbCachingShape(); + + void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax) + { + m_isLocalAabbValid = true; + m_localAabbMin = aabbMin; + m_localAabbMax = aabbMax; + } + + inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const + { + btAssert(m_isLocalAabbValid); + aabbMin = m_localAabbMin; + aabbMax = m_localAabbMax; + } + + inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const + { + + //lazy evaluation of local aabb + btAssert(m_isLocalAabbValid); + btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax); + } + +public: + + virtual void setLocalScaling(const btVector3& scaling); + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + void recalcLocalAabb(); + +}; + #endif //BT_CONVEX_INTERNAL_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp index 4ab5d692945..c1b155aef45 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -12,6 +12,7 @@ subject to the following restrictions: 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 "btConvexPointCloudShape.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" @@ -27,7 +28,7 @@ void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling) btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const { btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); - btScalar newDot,maxDot = btScalar(-1e30); + btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); @@ -62,7 +63,7 @@ void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin( { for (int i=0;i<numVectors;i++) { - supportVerticesOut[i][3] = btScalar(-1e30); + supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT); } } for (int i=0;i<m_numPoints;i++) diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h index 7e1c13b29c1..54b5afac3ec 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -21,7 +21,7 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" ///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices. -ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexAabbCachingShape { btVector3* m_unscaledPoints; int m_numPoints; @@ -29,6 +29,14 @@ ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexSh public: BT_DECLARE_ALIGNED_ALLOCATOR(); + btConvexPointCloudShape() + { + m_localScaling.setValue(1.f,1.f,1.f); + m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; + m_unscaledPoints = 0; + m_numPoints = 0; + } + btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true) { m_localScaling = localScaling; @@ -40,10 +48,11 @@ public: recalcLocalAabb(); } - void setPoints (btVector3* points, int numPoints, bool computeAabb = true) + void setPoints (btVector3* points, int numPoints, bool computeAabb = true,const btVector3& localScaling=btVector3(1.f,1.f,1.f)) { m_unscaledPoints = points; m_numPoints = numPoints; + m_localScaling = localScaling; if (computeAabb) recalcLocalAabb(); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp index 7e67696f4c7..f5f3aa58aa4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -21,6 +21,18 @@ subject to the following restrictions: #include "btConvexHullShape.h" #include "btConvexPointCloudShape.h" +///not supported on IBM SDK, until we fix the alignment of btVector3 +#if defined (__CELLOS_LV2__) && defined (__SPU__) +#include <spu_intrinsics.h> +static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 ) +{ + vec_float4 result; + result = spu_mul( vec0, vec1 ); + result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result ); + return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result ); +} +#endif //__SPU__ + btConvexShape::btConvexShape () { } @@ -32,35 +44,71 @@ btConvexShape::~btConvexShape() -static btVector3 convexHullSupport (const btVector3& localDir, const btVector3* points, int numPoints, const btVector3& localScaling) -{ - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); - btScalar newDot,maxDot = btScalar(-1e30); - - btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; +static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling) +{ + + btVector3 vec = localDirOrg * localScaling; + +#if defined (__CELLOS_LV2__) && defined (__SPU__) + + btVector3 localDir = vec; + + vec_float4 v_distMax = {-FLT_MAX,0,0,0}; + vec_int4 v_idxMax = {-999,0,0,0}; + int v=0; + int numverts = numPoints; + + for(;v<(int)numverts-4;v+=4) { + vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128()); + vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128()); + vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128()); + vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128()); + const vec_int4 i0 = {v ,0,0,0}; + const vec_int4 i1 = {v+1,0,0,0}; + const vec_int4 i2 = {v+2,0,0,0}; + const vec_int4 i3 = {v+3,0,0,0}; + vec_uint4 retGt01 = spu_cmpgt(p0,p1); + vec_float4 pmax01 = spu_sel(p1,p0,retGt01); + vec_int4 imax01 = spu_sel(i1,i0,retGt01); + vec_uint4 retGt23 = spu_cmpgt(p2,p3); + vec_float4 pmax23 = spu_sel(p3,p2,retGt23); + vec_int4 imax23 = spu_sel(i3,i2,retGt23); + vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23); + vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123); + vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123); + vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123); + v_distMax = spu_sel(pmax0123,v_distMax,retGtMax); + v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax); } + for(;v<(int)numverts;v++) { + vec_float4 p = vec_dot3(points[v].get128(),localDir.get128()); + const vec_int4 i = {v,0,0,0}; + vec_uint4 retGtMax = spu_cmpgt(v_distMax,p); + v_distMax = spu_sel(p,v_distMax,retGtMax); + v_idxMax = spu_sel(i,v_idxMax,retGtMax); + } + int ptIndex = spu_extract(v_idxMax,0); + const btVector3& supVec= points[ptIndex] * localScaling; + return supVec; +#else + btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT); + int ptIndex = -1; for (int i=0;i<numPoints;i++) { - btVector3 vtx = points[i] * localScaling; - newDot = vec.dot(vtx); + newDot = vec.dot(points[i]); if (newDot > maxDot) { maxDot = newDot; - supVec = vtx; + ptIndex = i; } } - return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); + btAssert(ptIndex >= 0); + btVector3 supVec = points[ptIndex] * localScaling; + return supVec; +#endif //__SPU__ } btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const @@ -160,7 +208,7 @@ btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btV btScalar radius = capsuleShape->getRadius(); btVector3 supVec(0,0,0); - btScalar maxDot(btScalar(-1e30)); + btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); @@ -292,7 +340,7 @@ btScalar btConvexShape::getMarginNonVirtual () const btAssert (0); return btScalar(0.0f); } - +#ifndef __SPU__ void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { switch (m_shapeType) @@ -360,7 +408,7 @@ void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: case CONVEX_HULL_SHAPE_PROXYTYPE: { - btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; + btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this; btScalar margin = convexHullShape->getMarginNonVirtual(); convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin); } @@ -377,3 +425,5 @@ void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, // should never reach here btAssert (0); } + +#endif //__SPU__ diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h index 0cc7b349521..9c158259c1c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp index 4bd986bb7f7..0f9ced554b8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -12,6 +12,7 @@ subject to the following restrictions: 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 "btConvexTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" @@ -20,7 +21,7 @@ subject to the following restrictions: btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) -: btPolyhedralConvexShape(), m_stridingMesh(meshInterface) +: btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface) { m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; if ( calcAabb ) @@ -43,7 +44,7 @@ public: LocalSupportVertexCallback(const btVector3& supportVecLocal) : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), - m_maxDot(btScalar(-1e30)), + m_maxDot(btScalar(-BT_LARGE_FLOAT)), m_supportVecLocal(supportVecLocal) { } @@ -91,7 +92,7 @@ btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const } LocalSupportVertexCallback supportCallback(vec); - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); supVec = supportCallback.GetSupportVertexLocal(); @@ -104,7 +105,7 @@ void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargi { for (int i=0;i<numVectors;i++) { - supportVerticesOut[i][3] = btScalar(-1e30); + supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT); } } @@ -115,7 +116,7 @@ void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargi { const btVector3& vec = vectors[j]; LocalSupportVertexCallback supportCallback(vec); - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); } @@ -297,7 +298,7 @@ void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& pri }; CenterCallback centerCallback; - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax); btVector3 center = centerCallback.getCenter(); principal.setOrigin(center); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h index 9d7e39fce69..f5167e74b80 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h @@ -1,3 +1,17 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ #ifndef CONVEX_TRIANGLEMESH_SHAPE_H #define CONVEX_TRIANGLEMESH_SHAPE_H @@ -8,7 +22,7 @@ /// The btConvexTriangleMeshShape is a convex hull of a triangle mesh, but the performance is not as good as btConvexHullShape. /// A small benefit of this class is that it uses the btStridingMeshInterface, so you can avoid the duplication of the triangle mesh data. Nevertheless, most users should use the much better performing btConvexHullShape instead. -class btConvexTriangleMeshShape : public btPolyhedralConvexShape +class btConvexTriangleMeshShape : public btPolyhedralConvexAabbCachingShape { class btStridingMeshInterface* m_stridingMesh; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp index c9fa907edf3..c2e534b0b46 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -12,14 +12,16 @@ subject to the following restrictions: 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 "btCylinderShape.h" btCylinderShape::btCylinderShape (const btVector3& halfExtents) -:btBoxShape(halfExtents), +:btConvexInternalShape(), m_upAxis(1) { + btVector3 margin(getMargin(),getMargin(),getMargin()); + m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin; m_shapeType = CYLINDER_SHAPE_PROXYTYPE; - recalcLocalAabb(); } @@ -27,7 +29,7 @@ btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents) :btCylinderShape(halfExtents) { m_upAxis = 0; - recalcLocalAabb(); + } @@ -35,13 +37,84 @@ btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents) :btCylinderShape(halfExtents) { m_upAxis = 2; - recalcLocalAabb(); + } void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { - //skip the box 'getAabb' - btPolyhedralConvexShape::getAabb(t,aabbMin,aabbMax); + btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); +} + +void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + +//Until Bullet 2.77 a box approximation was used, so uncomment this if you need backwards compatibility +//#define USE_BOX_INERTIA_APPROXIMATION 1 +#ifndef USE_BOX_INERTIA_APPROXIMATION + + /* + cylinder is defined as following: + * + * - principle axis aligned along y by default, radius in x, z-value not used + * - for btCylinderShapeX: principle axis aligned along x, radius in y direction, z-value not used + * - for btCylinderShapeZ: principle axis aligned along z, radius in x direction, y-value not used + * + */ + + btScalar radius2; // square of cylinder radius + btScalar height2; // square of cylinder height + btVector3 halfExtents = getHalfExtentsWithMargin(); // get cylinder dimension + btScalar div12 = mass / 12.f; + btScalar div4 = mass / 4.f; + btScalar div2 = mass / 2.f; + int idxRadius, idxHeight; + + switch (m_upAxis) // get indices of radius and height of cylinder + { + case 0: // cylinder is aligned along x + idxRadius = 1; + idxHeight = 0; + break; + case 2: // cylinder is aligned along z + idxRadius = 0; + idxHeight = 2; + break; + default: // cylinder is aligned along y + idxRadius = 0; + idxHeight = 1; + } + + // calculate squares + radius2 = halfExtents[idxRadius] * halfExtents[idxRadius]; + height2 = btScalar(4.) * halfExtents[idxHeight] * halfExtents[idxHeight]; + + // calculate tensor terms + btScalar t1 = div12 * height2 + div4 * radius2; + btScalar t2 = div2 * radius2; + + switch (m_upAxis) // set diagonal elements of inertia tensor + { + case 0: // cylinder is aligned along x + inertia.setValue(t2,t1,t1); + break; + case 2: // cylinder is aligned along z + inertia.setValue(t1,t1,t2); + break; + default: // cylinder is aligned along y + inertia.setValue(t1,t2,t1); + } +#else //USE_BOX_INERTIA_APPROXIMATION + //approximation of box shape + btVector3 halfExtents = getHalfExtentsWithMargin(); + + btScalar lx=btScalar(2.)*(halfExtents.x()); + btScalar ly=btScalar(2.)*(halfExtents.y()); + btScalar lz=btScalar(2.)*(halfExtents.z()); + + inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), + mass/(btScalar(12.0)) * (lx*lx + lz*lz), + mass/(btScalar(12.0)) * (lx*lx + ly*ly)); +#endif //USE_BOX_INERTIA_APPROXIMATION } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h index bda74a8612d..f7899265d31 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -21,7 +21,7 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" /// The btCylinderShape class implements a cylinder shape primitive, centered around the origin. Its central axis aligned with the Y axis. btCylinderShapeX is aligned with the X axis and btCylinderShapeZ around the Z axis. -class btCylinderShape : public btBoxShape +class btCylinderShape : public btConvexInternalShape { @@ -30,15 +30,42 @@ protected: int m_upAxis; public: + + btVector3 getHalfExtentsWithMargin() const + { + btVector3 halfExtents = getHalfExtentsWithoutMargin(); + btVector3 margin(getMargin(),getMargin(),getMargin()); + halfExtents += margin; + return halfExtents; + } + + const btVector3& getHalfExtentsWithoutMargin() const + { + return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included + } + btCylinderShape (const btVector3& halfExtents); - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual void setMargin(btScalar collisionMargin) + { + //correct the m_implicitShapeDimensions for the margin + btVector3 oldMargin(getMargin(),getMargin(),getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + + btConvexInternalShape::setMargin(collisionMargin); + btVector3 newMargin(getMargin(),getMargin(),getMargin()); + m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin; + + } + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { @@ -73,13 +100,28 @@ public: return getHalfExtentsWithMargin().getX(); } + virtual void setLocalScaling(const btVector3& scaling) + { + btVector3 oldMargin(getMargin(),getMargin(),getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling; + + btConvexInternalShape::setLocalScaling(scaling); + + m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin; + + } + //debugging virtual const char* getName()const { return "CylinderY"; } + virtual int calculateSerializeBufferSize() const; + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; @@ -112,10 +154,6 @@ public: virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual int getUpAxis() const - { - return 2; - } //debugging virtual const char* getName()const { @@ -129,6 +167,34 @@ public: }; +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btCylinderShapeData +{ + btConvexInternalShapeData m_convexInternalShapeData; + + int m_upAxis; + + char m_padding[4]; +}; + +SIMD_FORCE_INLINE int btCylinderShape::calculateSerializeBufferSize() const +{ + return sizeof(btCylinderShapeData); +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btCylinderShape::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btCylinderShapeData* shapeData = (btCylinderShapeData*) dataBuffer; + + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer); + + shapeData->m_upAxis = m_upAxis; + + return "btCylinderShapeData"; +} + + #endif //CYLINDER_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp index 8387b9584bb..a9e6df5c58e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h index 8720d53ae63..9f6b4435c29 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 7d4875d8037..3a1e6f4a2b9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 36489a0ebc3..4f5d1e35bf3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h index 7cb6d5ab6fe..030e167da50 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -31,4 +31,5 @@ public: btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; } }; -#endif // MATERIAL_H
\ No newline at end of file +#endif // MATERIAL_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp index b107b0bea6e..06707e24e55 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -13,6 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ + #include "btMinkowskiSumShape.h" diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h index 6f637fbf40b..d6fd040213b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index 9423505290d..c996bfcdaba 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -13,29 +13,31 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ + + #include "btMultiSphereShape.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "LinearMath/btQuaternion.h" +#include "LinearMath/btSerializer.h" -btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres) -:btConvexInternalShape (), m_inertiaHalfExtents(inertiaHalfExtents) +btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres) +:btConvexInternalAabbCachingShape () { m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE; - btScalar startMargin = btScalar(1e30); + //btScalar startMargin = btScalar(BT_LARGE_FLOAT); - m_numSpheres = numSpheres; - for (int i=0;i<m_numSpheres;i++) + m_localPositionArray.resize(numSpheres); + m_radiArray.resize(numSpheres); + for (int i=0;i<numSpheres;i++) { - m_localPositions[i] = positions[i]; - m_radi[i] = radi[i]; - if (radi[i] < startMargin) - startMargin = radi[i]; + m_localPositionArray[i] = positions[i]; + m_radiArray[i] = radi[i]; + } - setMargin(startMargin); - -} + recalcLocalAabb(); +} btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const @@ -43,7 +45,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons int i; btVector3 supVec(0,0,0); - btScalar maxDot(btScalar(-1e30)); + btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); btVector3 vec = vec0; @@ -60,10 +62,11 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons btVector3 vtx; btScalar newDot; - const btVector3* pos = &m_localPositions[0]; - const btScalar* rad = &m_radi[0]; + const btVector3* pos = &m_localPositionArray[0]; + const btScalar* rad = &m_radiArray[0]; + int numSpheres = m_localPositionArray.size(); - for (i=0;i<m_numSpheres;i++) + for (i=0;i<numSpheres;i++) { vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin(); pos++; @@ -85,17 +88,17 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons for (int j=0;j<numVectors;j++) { - btScalar maxDot(btScalar(-1e30)); + btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); const btVector3& vec = vectors[j]; btVector3 vtx; btScalar newDot; - const btVector3* pos = &m_localPositions[0]; - const btScalar* rad = &m_radi[0]; - - for (int i=0;i<m_numSpheres;i++) + const btVector3* pos = &m_localPositionArray[0]; + const btScalar* rad = &m_radiArray[0]; + int numSpheres = m_localPositionArray.size(); + for (int i=0;i<numSpheres;i++) { vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin(); pos++; @@ -121,29 +124,44 @@ void btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) { //as an approximation, take the inertia of the box that bounds the spheres - btTransform ident; - ident.setIdentity(); -// btVector3 aabbMin,aabbMax; - -// getAabb(ident,aabbMin,aabbMax); + btVector3 localAabbMin,localAabbMax; + getCachedLocalAabb(localAabbMin,localAabbMax); + btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5); - btVector3 halfExtents = m_inertiaHalfExtents;//(aabbMax - aabbMin)* btScalar(0.5); + btScalar lx=btScalar(2.)*(halfExtents.x()); + btScalar ly=btScalar(2.)*(halfExtents.y()); + btScalar lz=btScalar(2.)*(halfExtents.z()); - btScalar margin = CONVEX_DISTANCE_MARGIN; + inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), + mass/(btScalar(12.0)) * (lx*lx + lz*lz), + mass/(btScalar(12.0)) * (lx*lx + ly*ly)); - btScalar lx=btScalar(2.)*(halfExtents[0]+margin); - btScalar ly=btScalar(2.)*(halfExtents[1]+margin); - btScalar lz=btScalar(2.)*(halfExtents[2]+margin); - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; - const btScalar scaledmass = mass * btScalar(.08333333); +} - inertia[0] = scaledmass * (y2+z2); - inertia[1] = scaledmass * (x2+z2); - inertia[2] = scaledmass * (x2+y2); +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*) dataBuffer; + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); + + int numElem = m_localPositionArray.size(); + shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]): 0; + + shapeData->m_localPositionArraySize = numElem; + if (numElem) + { + btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius),numElem); + btPositionAndRadius* memPtr = (btPositionAndRadius*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + m_localPositionArray[i].serializeFloat(memPtr->m_pos); + memPtr->m_radius = float(m_radiArray[i]); + } + serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]); + } + + return "btMultiSphereShapeData"; } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h index 97309fbbed9..3db7e320889 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -18,26 +18,21 @@ subject to the following restrictions: #include "btConvexInternalShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btAabbUtil2.h" -#define MAX_NUM_SPHERES 5 -///The btMultiSphereShape represents the convex hull of a collection of spheres. You can create special capsules or other smooth volumes. -///It is possible to animate the spheres for deformation. -class btMultiSphereShape : public btConvexInternalShape +///The btMultiSphereShape represents the convex hull of a collection of spheres. You can create special capsules or other smooth volumes. +///It is possible to animate the spheres for deformation, but call 'recalcLocalAabb' after changing any sphere position/radius +class btMultiSphereShape : public btConvexInternalAabbCachingShape { - btVector3 m_localPositions[MAX_NUM_SPHERES]; - btScalar m_radi[MAX_NUM_SPHERES]; - btVector3 m_inertiaHalfExtents; - - int m_numSpheres; + btAlignedObjectArray<btVector3> m_localPositionArray; + btAlignedObjectArray<btScalar> m_radiArray; - - - public: - btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres); + btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres); ///CollisionShape Interface virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; @@ -49,17 +44,17 @@ public: int getSphereCount() const { - return m_numSpheres; + return m_localPositionArray.size(); } const btVector3& getSpherePosition(int index) const { - return m_localPositions[index]; + return m_localPositionArray[index]; } btScalar getSphereRadius(int index) const { - return m_radi[index]; + return m_radiArray[index]; } @@ -68,7 +63,37 @@ public: return "MultiSphere"; } + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + +}; + + +struct btPositionAndRadius +{ + btVector3FloatData m_pos; + float m_radius; +}; + +struct btMultiSphereShapeData +{ + btConvexInternalShapeData m_convexInternalShapeData; + + btPositionAndRadius *m_localPositionArrayPtr; + int m_localPositionArraySize; + char m_padding[4]; }; + +SIMD_FORCE_INLINE int btMultiSphereShape::calculateSerializeBufferSize() const +{ + return sizeof(btMultiSphereShapeData); +} + + + #endif //MULTI_SPHERE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp index fc47e86411d..58799ac96ad 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h index d025a5c4450..96b2ad95b6d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -31,13 +31,11 @@ public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;} btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true): btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh) { m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; - btVector3 m_triangle[3]; const unsigned char *vertexbase; int numverts; PHY_ScalarType type; @@ -71,7 +69,6 @@ public: { m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; - btVector3 m_triangle[3]; const unsigned char *vertexbase; int numverts; PHY_ScalarType type; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 3e73059394c..981b8a2652c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -13,6 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ + #include "btOptimizedBvh.h" #include "btStridingMeshInterface.h" #include "LinearMath/btAabbUtil2.h" @@ -55,8 +56,8 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized { btOptimizedBvhNode node; btVector3 aabbMin,aabbMax; - aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangle[0]); aabbMax.setMax(triangle[0]); aabbMin.setMin(triangle[1]); @@ -103,8 +104,8 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized btQuantizedBvhNode node; btVector3 aabbMin,aabbMax; - aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangle[0]); aabbMax.setMax(triangle[0]); aabbMin.setMin(triangle[1]); @@ -167,8 +168,8 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized { NodeTriangleCallback callback(m_leafNodes); - btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); @@ -336,8 +337,8 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int f - aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangleVerts[0]); aabbMax.setMax(triangleVerts[0]); aabbMin.setMin(triangleVerts[1]); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index d7536417d7e..749fe6005dd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -13,6 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +///Contains contributions from Disney Studio's + #ifndef OPTIMIZED_BVH_H #define OPTIMIZED_BVH_H @@ -45,7 +47,7 @@ public: void updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index); /// Data buffer MUST be 16 byte aligned - virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) + virtual bool serializeInPlace(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const { return btQuantizedBvh::serialize(o_alignedDataBuffer,i_dataBufferSize,i_swapEndian); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 7a79ac25791..b1ecb3e432c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -15,11 +15,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" -btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(), -m_localAabbMin(1,1,1), -m_localAabbMax(-1,-1,-1), -m_isLocalAabbValid(false), -m_optionalHull(0) +btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape() { } @@ -27,10 +23,12 @@ m_optionalHull(0) btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const { - int i; - btVector3 supVec(0,0,0); - btScalar maxDot(btScalar(-1e30)); + + btVector3 supVec(0,0,0); +#ifndef __SPU__ + int i; + btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); @@ -57,12 +55,16 @@ btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const b } } + +#endif //__SPU__ return supVec; - } + + void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const { +#ifndef __SPU__ int i; btVector3 vtx; @@ -70,7 +72,7 @@ void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin( for (i=0;i<numVectors;i++) { - supportVerticesOut[i][3] = btScalar(-1e30); + supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT); } for (int j=0;j<numVectors;j++) @@ -90,12 +92,14 @@ void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin( } } } +#endif //__SPU__ } void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { +#ifndef __SPU__ //not yet, return box inertia btScalar margin = getMargin(); @@ -115,25 +119,31 @@ void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& ine const btScalar scaledmass = mass * btScalar(0.08333333); inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); - +#endif //__SPU__ } -void btPolyhedralConvexShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling) { - getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin()); + btConvexInternalShape::setLocalScaling(scaling); + recalcLocalAabb(); } +btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape() +:btPolyhedralConvexShape(), +m_localAabbMin(1,1,1), +m_localAabbMax(-1,-1,-1), +m_isLocalAabbValid(false) +{ +} - -void btPolyhedralConvexShape::setLocalScaling(const btVector3& scaling) +void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const { - btConvexInternalShape::setLocalScaling(scaling); - recalcLocalAabb(); + getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin()); } -void btPolyhedralConvexShape::recalcLocalAabb() +void btPolyhedralConvexAabbCachingShape::recalcLocalAabb() { m_isLocalAabbValid = true; @@ -181,5 +191,3 @@ void btPolyhedralConvexShape::recalcLocalAabb() #endif } - - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index 7567a42d3ba..2c691b95652 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -17,7 +17,6 @@ subject to the following restrictions: #define BU_SHAPE #include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btAabbUtil2.h" #include "btConvexInternalShape.h" @@ -26,10 +25,7 @@ class btPolyhedralConvexShape : public btConvexInternalShape { protected: - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - bool m_isLocalAabbValid; - + public: btPolyhedralConvexShape(); @@ -38,10 +34,32 @@ public: virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + + virtual int getNumVertices() const = 0 ; + virtual int getNumEdges() const = 0; + virtual void getEdge(int i,btVector3& pa,btVector3& pb) const = 0; + virtual void getVertex(int i,btVector3& vtx) const = 0; + virtual int getNumPlanes() const = 0; + virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const = 0; +// virtual int getIndex(int i) const = 0 ; + virtual bool isInside(const btVector3& pt,btScalar tolerance) const = 0; + +}; + + +///The btPolyhedralConvexAabbCachingShape adds aabb caching to the btPolyhedralConvexShape +class btPolyhedralConvexAabbCachingShape : public btPolyhedralConvexShape +{ + + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + bool m_isLocalAabbValid; + +protected: void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax) { @@ -57,6 +75,10 @@ public: aabbMax = m_localAabbMax; } +public: + + btPolyhedralConvexAabbCachingShape(); + inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const { @@ -65,25 +87,11 @@ public: btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax); } - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - virtual void setLocalScaling(const btVector3& scaling); - void recalcLocalAabb(); - - virtual int getNumVertices() const = 0 ; - virtual int getNumEdges() const = 0; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const = 0; - virtual void getVertex(int i,btVector3& vtx) const = 0; - virtual int getNumPlanes() const = 0; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const = 0; -// virtual int getIndex(int i) const = 0 ; + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const = 0; - - /// optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp - class Hull* m_optionalHull; + void recalcLocalAabb(); }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp index 203f5b7596c..25d58d61bb1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -13,6 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ + #include "btScaledBvhTriangleMeshShape.h" btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling) @@ -61,12 +62,12 @@ void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callb scaledAabbMin[0] = m_localScaling.getX() >= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; scaledAabbMin[1] = m_localScaling.getY() >= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; scaledAabbMin[2] = m_localScaling.getZ() >= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; - scaledAabbMin[3] = 0.0; /* otherwise un-initialized stack memory: uninitialized_stack_vec.patch, blender patch */ + scaledAabbMin[3] = 0.f; scaledAabbMax[0] = m_localScaling.getX() <= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; scaledAabbMax[1] = m_localScaling.getY() <= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; scaledAabbMax[2] = m_localScaling.getZ() <= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; - scaledAabbMax[3] = 0.0; /* otherwise un-initialized stack memory: uninitialized_stack_vec.patch, blender patch */ + scaledAabbMax[3] = 0.f; m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h index 5da6fe8bc95..4ab28555ba8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -57,6 +57,37 @@ public: //debugging virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";} + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + }; +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btScaledTriangleMeshShapeData +{ + btTriangleMeshShapeData m_trimeshShapeData; + + btVector3FloatData m_localScaling; +}; + + +SIMD_FORCE_INLINE int btScaledBvhTriangleMeshShape::calculateSerializeBufferSize() const +{ + return sizeof(btScaledTriangleMeshShapeData); +} + + +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btScaledBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btScaledTriangleMeshShapeData* scaledMeshData = (btScaledTriangleMeshShapeData*) dataBuffer; + m_bvhTriMeshShape->serialize(&scaledMeshData->m_trimeshShapeData,serializer); + scaledMeshData->m_trimeshShapeData.m_collisionShapeData.m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; + m_localScaling.serializeFloat(scaledMeshData->m_localScaling); + return "btScaledTriangleMeshShapeData"; +} + + #endif //BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp index a87b87f1a6f..3beaf865801 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -1,13 +1,11 @@ /* -btbtShapeHull implemented by John McCutchan. - Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +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. @@ -15,57 +13,14 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +//btShapeHull was implemented by John McCutchan. + + #include "btShapeHull.h" #include "LinearMath/btConvexHull.h" #define NUM_UNITSPHERE_POINTS 42 -static btVector3 btUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = -{ - btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), - btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), - btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), - btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), - btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), - btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), - btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), - btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), - btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), - btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), - btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), - btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), - btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), - btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), - btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), - btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), - btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), - btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), - btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), - btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), - btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), - btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), - btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), - btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), - btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), - btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), - btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), - btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), - btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), - btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), - btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), - btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), - btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), - btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), - btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), - btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), - btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), - btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), - btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), - btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), - btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), - btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) -}; - btShapeHull::btShapeHull (const btConvexShape* shape) { m_shape = shape; @@ -92,7 +47,7 @@ btShapeHull::buildHull (btScalar /*margin*/) { btVector3 norm; m_shape->getPreferredPenetrationDirection(i,norm); - btUnitSpherePoints[numSampleDirections] = norm; + getUnitSpherePoints()[numSampleDirections] = norm; numSampleDirections++; } } @@ -102,7 +57,7 @@ btShapeHull::buildHull (btScalar /*margin*/) int i; for (i = 0; i < numSampleDirections; i++) { - supportPoints[i] = m_shape->localGetSupportingVertex(btUnitSpherePoints[i]); + supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints()[i]); } HullDesc hd; @@ -162,3 +117,54 @@ btShapeHull::numIndices () const return static_cast<int>(m_numIndices); } + +btVector3* btShapeHull::getUnitSpherePoints() +{ + static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = + { + btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), + btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), + btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), + btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), + btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), + btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), + btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), + btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), + btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), + btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), + btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), + btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), + btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), + btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), + btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), + btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), + btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), + btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), + btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), + btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), + btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), + btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), + btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), + btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), + btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), + btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), + btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), + btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), + btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), + btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), + btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), + btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), + btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), + btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), + btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), + btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), + btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), + btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), + btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), + btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), + btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), + btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) + }; + return sUnitSpherePoints; +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h index 583c6b99eb6..07b3500f994 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h @@ -1,13 +1,11 @@ /* -btShapeHull implemented by John McCutchan. - Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +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. @@ -15,6 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +///btShapeHull implemented by John McCutchan. + #ifndef _SHAPE_HULL_H #define _SHAPE_HULL_H @@ -27,6 +27,15 @@ subject to the following restrictions: ///It approximates the convex hull using the supporting vertex of 42 directions. class btShapeHull { +protected: + + btAlignedObjectArray<btVector3> m_vertices; + btAlignedObjectArray<unsigned int> m_indices; + unsigned int m_numIndices; + const btConvexShape* m_shape; + + static btVector3* getUnitSpherePoints(); + public: btShapeHull (const btConvexShape* shape); ~btShapeHull (); @@ -45,12 +54,6 @@ public: { return &m_indices[0]; } - -protected: - btAlignedObjectArray<btVector3> m_vertices; - btAlignedObjectArray<unsigned int> m_indices; - unsigned int m_numIndices; - const btConvexShape* m_shape; }; #endif //_SHAPE_HULL_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp index cab7b277289..b9a736c0fdd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h index bf163d3b501..f98372442ba 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -12,7 +12,6 @@ subject to the following restrictions: 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ - #ifndef SPHERE_MINKOWSKI_H #define SPHERE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp index 17b8776d459..38ef8f03706 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -38,7 +38,7 @@ void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVecto { (void)t; /* - btVector3 infvec (btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 infvec (btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); btVector3 center = m_planeNormal*m_planeConstant; aabbMin = center + infvec*m_planeNormal; @@ -47,8 +47,8 @@ void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVecto aabbMax.setMax(center - infvec*m_planeNormal); */ - aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); - aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h index a496013c077..beb53ef33a1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -20,7 +20,7 @@ subject to the following restrictions: ///The btStaticPlaneShape simulates an infinite non-moving (static) collision plane. -class btStaticPlaneShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) btStaticPlaneShape : public btConcaveShape { protected: btVector3 m_localAabbMin; @@ -58,7 +58,46 @@ public: //debugging virtual const char* getName()const {return "STATICPLANE";} + virtual int calculateSerializeBufferSize() const; + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btStaticPlaneShapeData +{ + btCollisionShapeData m_collisionShapeData; + + btVector3FloatData m_localScaling; + btVector3FloatData m_planeNormal; + float m_planeConstant; + char m_pad[4]; }; + +SIMD_FORCE_INLINE int btStaticPlaneShape::calculateSerializeBufferSize() const +{ + return sizeof(btStaticPlaneShapeData); +} + +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btStaticPlaneShape::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*) dataBuffer; + btCollisionShape::serialize(&planeData->m_collisionShapeData,serializer); + + m_localScaling.serializeFloat(planeData->m_localScaling); + m_planeNormal.serializeFloat(planeData->m_planeNormal); + planeData->m_planeConstant = float(m_planeConstant); + + return "btStaticPlaneShapeData"; +} + + #endif //STATIC_PLANE_SHAPE_H + + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp index 752a5fc1dfa..bc2f9f21448 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -14,6 +14,7 @@ subject to the following restrictions: */ #include "btStridingMeshInterface.h" +#include "LinearMath/btSerializer.h" btStridingMeshInterface::~btStridingMeshInterface() { @@ -35,7 +36,6 @@ void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde int stride,numverts,numtriangles; int gfxindex; btVector3 triangle[3]; - btScalar* graphicsbase; btVector3 meshScaling = getScaling(); @@ -45,40 +45,128 @@ void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part); numtotalphysicsverts+=numtriangles*3; //upper bound - switch (gfxindextype) + ///unlike that developers want to pass in double-precision meshes in single-precision Bullet build + ///so disable this feature by default + ///see patch http://code.google.com/p/bullet/issues/detail?id=213 + + switch (type) { - case PHY_INTEGER: - { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) - { - unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); - graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (btScalar*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); - } - break; - } - case PHY_SHORT: + case PHY_FLOAT: + { + + float* graphicsbase; + + switch (gfxindextype) + { + case PHY_INTEGER: + { + for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + { + unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); + graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); + triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); + triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); + triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + case PHY_SHORT: + { + for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + { + unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); + graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); + triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); + triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); + triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + case PHY_UCHAR: + { + for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + { + unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride); + graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); + triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); + triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); + triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + default: + btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); + } + break; + } + + case PHY_DOUBLE: { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + double* graphicsbase; + + switch (gfxindextype) { - unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); - graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (btScalar*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); + case PHY_INTEGER: + { + for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + { + unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); + graphicsbase = (double*)(vertexbase+tri_indices[0]*stride); + triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (double*)(vertexbase+tri_indices[1]*stride); + triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (double*)(vertexbase+tri_indices[2]*stride); + triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + case PHY_SHORT: + { + for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + { + unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); + graphicsbase = (double*)(vertexbase+tri_indices[0]*stride); + triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (double*)(vertexbase+tri_indices[1]*stride); + triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (double*)(vertexbase+tri_indices[2]*stride); + triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + case PHY_UCHAR: + { + for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + { + unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride); + graphicsbase = (double*)(vertexbase+tri_indices[0]*stride); + triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (double*)(vertexbase+tri_indices[1]*stride); + triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (double*)(vertexbase+tri_indices[2]*stride); + triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + default: + btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); } break; } default: - btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); + btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); } unLockReadOnlyVertexBase(part); @@ -95,8 +183,8 @@ void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVecto AabbCalculationCallback() { - m_aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - m_aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + m_aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + m_aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); } virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) @@ -113,12 +201,179 @@ void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVecto } }; - //first calculate the total aabb for all triangles + //first calculate the total aabb for all triangles AabbCalculationCallback aabbCallback; - aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); - aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax); aabbMin = aabbCallback.m_aabbMin; aabbMax = aabbCallback.m_aabbMax; } + + + +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btStridingMeshInterfaceData* trimeshData = (btStridingMeshInterfaceData*) dataBuffer; + + trimeshData->m_numMeshParts = getNumSubParts(); + + //void* uniquePtr = 0; + + trimeshData->m_meshPartsPtr = 0; + + if (trimeshData->m_numMeshParts) + { + btChunk* chunk = serializer->allocate(sizeof(btMeshPartData),trimeshData->m_numMeshParts); + btMeshPartData* memPtr = (btMeshPartData*)chunk->m_oldPtr; + trimeshData->m_meshPartsPtr = (btMeshPartData *)serializer->getUniquePointer(memPtr); + + + // int numtotalphysicsverts = 0; + int part,graphicssubparts = getNumSubParts(); + const unsigned char * vertexbase; + const unsigned char * indexbase; + int indexstride; + PHY_ScalarType type; + PHY_ScalarType gfxindextype; + int stride,numverts,numtriangles; + int gfxindex; + // btVector3 triangle[3]; + + btVector3 meshScaling = getScaling(); + + ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype + for (part=0;part<graphicssubparts ;part++,memPtr++) + { + getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part); + memPtr->m_numTriangles = numtriangles;//indices = 3*numtriangles + memPtr->m_numVertices = numverts; + memPtr->m_indices16 = 0; + memPtr->m_indices32 = 0; + memPtr->m_3indices16 = 0; + memPtr->m_vertices3f = 0; + memPtr->m_vertices3d = 0; + + switch (gfxindextype) + { + case PHY_INTEGER: + { + int numindices = numtriangles*3; + + if (numindices) + { + btChunk* chunk = serializer->allocate(sizeof(btIntIndexData),numindices); + btIntIndexData* tmpIndices = (btIntIndexData*)chunk->m_oldPtr; + memPtr->m_indices32 = (btIntIndexData*)serializer->getUniquePointer(tmpIndices); + for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + { + unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); + tmpIndices[gfxindex*3].m_value = tri_indices[0]; + tmpIndices[gfxindex*3+1].m_value = tri_indices[1]; + tmpIndices[gfxindex*3+2].m_value = tri_indices[2]; + } + serializer->finalizeChunk(chunk,"btIntIndexData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + } + break; + } + case PHY_SHORT: + { + if (numtriangles) + { + btChunk* chunk = serializer->allocate(sizeof(btShortIntIndexTripletData),numtriangles); + btShortIntIndexTripletData* tmpIndices = (btShortIntIndexTripletData*)chunk->m_oldPtr; + memPtr->m_3indices16 = (btShortIntIndexTripletData*) serializer->getUniquePointer(tmpIndices); + for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + { + unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); + tmpIndices[gfxindex].m_values[0] = tri_indices[0]; + tmpIndices[gfxindex].m_values[1] = tri_indices[1]; + tmpIndices[gfxindex].m_values[2] = tri_indices[2]; + } + serializer->finalizeChunk(chunk,"btShortIntIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + } + break; + } + case PHY_UCHAR: + { + if (numtriangles) + { + btChunk* chunk = serializer->allocate(sizeof(btCharIndexTripletData),numtriangles); + btCharIndexTripletData* tmpIndices = (btCharIndexTripletData*)chunk->m_oldPtr; + memPtr->m_3indices8 = (btCharIndexTripletData*) serializer->getUniquePointer(tmpIndices); + for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + { + unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride); + tmpIndices[gfxindex].m_values[0] = tri_indices[0]; + tmpIndices[gfxindex].m_values[1] = tri_indices[1]; + tmpIndices[gfxindex].m_values[2] = tri_indices[2]; + } + serializer->finalizeChunk(chunk,"btCharIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + } + break; + } + default: + { + btAssert(0); + //unknown index type + } + } + + switch (type) + { + case PHY_FLOAT: + { + float* graphicsbase; + + if (numverts) + { + btChunk* chunk = serializer->allocate(sizeof(btVector3FloatData),numverts); + btVector3FloatData* tmpVertices = (btVector3FloatData*) chunk->m_oldPtr; + memPtr->m_vertices3f = (btVector3FloatData *)serializer->getUniquePointer(tmpVertices); + for (int i=0;i<numverts;i++) + { + graphicsbase = (float*)(vertexbase+i*stride); + tmpVertices[i].m_floats[0] = graphicsbase[0]; + tmpVertices[i].m_floats[1] = graphicsbase[1]; + tmpVertices[i].m_floats[2] = graphicsbase[2]; + } + serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + } + break; + } + + case PHY_DOUBLE: + { + if (numverts) + { + btChunk* chunk = serializer->allocate(sizeof(btVector3DoubleData),numverts); + btVector3DoubleData* tmpVertices = (btVector3DoubleData*) chunk->m_oldPtr; + memPtr->m_vertices3d = (btVector3DoubleData *) serializer->getUniquePointer(tmpVertices); + for (int i=0;i<numverts;i++) + { + double* graphicsbase = (double*)(vertexbase+i*stride);//for now convert to float, might leave it at double + tmpVertices[i].m_floats[0] = graphicsbase[0]; + tmpVertices[i].m_floats[1] = graphicsbase[1]; + tmpVertices[i].m_floats[2] = graphicsbase[2]; + } + serializer->finalizeChunk(chunk,"btVector3DoubleData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + } + break; + } + + default: + btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); + } + + unLockReadOnlyVertexBase(part); + } + + serializer->finalizeChunk(chunk,"btMeshPartData",BT_ARRAY_CODE,chunk->m_oldPtr); + } + + + m_scaling.serializeFloat(trimeshData->m_scaling); + return "btStridingMeshInterfaceData"; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h index e61b11499d4..9e3e2ab0f59 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -22,6 +22,8 @@ subject to the following restrictions: + + /// The btStridingMeshInterface is the interface class for high performance generic access to triangle meshes, used in combination with btBvhTriangleMeshShape and some other collision shapes. /// Using index striding of 3*sizeof(integer) it can use triangle arrays, using index striding of 1*sizeof(integer) it can handle triangle strips. /// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory. @@ -89,8 +91,72 @@ class btStridingMeshInterface m_scaling = scaling; } - + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + +}; + +struct btIntIndexData +{ + int m_value; +}; +struct btShortIntIndexData +{ + short m_value; + char m_pad[2]; +}; + +struct btShortIntIndexTripletData +{ + short m_values[3]; + char m_pad[2]; }; +struct btCharIndexTripletData +{ + unsigned char m_values[3]; + char m_pad; +}; + + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btMeshPartData +{ + btVector3FloatData *m_vertices3f; + btVector3DoubleData *m_vertices3d; + + btIntIndexData *m_indices32; + btShortIntIndexTripletData *m_3indices16; + btCharIndexTripletData *m_3indices8; + + btShortIntIndexData *m_indices16;//backwards compatibility + + int m_numTriangles;//length of m_indices = m_numTriangles + int m_numVertices; +}; + + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btStridingMeshInterfaceData +{ + btMeshPartData *m_meshPartsPtr; + btVector3FloatData m_scaling; + int m_numMeshParts; + char m_padding[4]; +}; + + + + +SIMD_FORCE_INLINE int btStridingMeshInterface::calculateSerializeBufferSize() const +{ + return sizeof(btStridingMeshInterfaceData); +} + + + #endif //STRIDING_MESHINTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp index 7fd8fb4691b..52f346bf726 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp @@ -1,7 +1,6 @@ - /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -13,23 +12,24 @@ subject to the following restrictions: 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 "btTetrahedronShape.h" #include "LinearMath/btMatrix3x3.h" -btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexShape (), +btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexAabbCachingShape (), m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexShape (), +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexAabbCachingShape (), m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1) : btPolyhedralConvexShape (), +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1) : btPolyhedralConvexAabbCachingShape (), m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; @@ -37,7 +37,7 @@ m_numVertices(0) addVertex(pt1); } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2) : btPolyhedralConvexShape (), +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2) : btPolyhedralConvexAabbCachingShape (), m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; @@ -46,7 +46,7 @@ m_numVertices(0) addVertex(pt2); } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3) : btPolyhedralConvexShape (), +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3) : btPolyhedralConvexAabbCachingShape (), m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; @@ -57,13 +57,31 @@ m_numVertices(0) } +void btBU_Simplex1to4::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ +#if 1 + btPolyhedralConvexAabbCachingShape::getAabb(t,aabbMin,aabbMax); +#else + aabbMin.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); + aabbMax.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT); + + //just transform the vertices in worldspace, and take their AABB + for (int i=0;i<m_numVertices;i++) + { + btVector3 worldVertex = t(m_vertices[i]); + aabbMin.setMin(worldVertex); + aabbMax.setMax(worldVertex); + } +#endif +} + + void btBU_Simplex1to4::addVertex(const btVector3& pt) { m_vertices[m_numVertices++] = pt; - recalcLocalAabb(); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h index 114ae288bb0..72e9f232876 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -22,7 +22,7 @@ subject to the following restrictions: ///The btBU_Simplex1to4 implements tetrahedron, triangle, line, vertex collision shapes. In most cases it is better to use btConvexHullShape instead. -class btBU_Simplex1to4 : public btPolyhedralConvexShape +class btBU_Simplex1to4 : public btPolyhedralConvexAabbCachingShape { protected: @@ -43,7 +43,7 @@ public: m_numVertices = 0; } - + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; void addVertex(const btVector3& pt); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp index 5194219f256..3027e65b256 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h index adca38041b2..b71fc8b3746 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp index a020746db5f..f558bf6d241 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h index cb760be80ca..0499702b05b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp index 5f292532861..a665024cb61 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp @@ -1,11 +1,11 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +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. @@ -44,11 +44,9 @@ void btTriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertex numverts = mesh.m_numVertices; (*vertexbase) = (unsigned char *) mesh.m_vertexBase; - #ifdef BT_USE_DOUBLE_PRECISION - type = PHY_DOUBLE; - #else - type = PHY_FLOAT; - #endif + + type = mesh.m_vertexType; + vertexStride = mesh.m_vertexStride; numfaces = mesh.m_numTriangles; @@ -64,11 +62,9 @@ void btTriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned numverts = mesh.m_numVertices; (*vertexbase) = (const unsigned char *)mesh.m_vertexBase; - #ifdef BT_USE_DOUBLE_PRECISION - type = PHY_DOUBLE; - #else - type = PHY_FLOAT; - #endif + + type = mesh.m_vertexType; + vertexStride = mesh.m_vertexStride; numfaces = mesh.m_numTriangles; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index eb79ff5e47d..c64ea6e7043 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -27,16 +27,32 @@ ATTRIBUTE_ALIGNED16( struct) btIndexedMesh { BT_DECLARE_ALIGNED_ALLOCATOR(); - int m_numTriangles; - const unsigned char * m_triangleIndexBase; - int m_triangleIndexStride; - int m_numVertices; - const unsigned char * m_vertexBase; - int m_vertexStride; - // The index type is set when adding an indexed mesh to the - // btTriangleIndexVertexArray, do not set it manually - PHY_ScalarType m_indexType; - int pad; + int m_numTriangles; + const unsigned char * m_triangleIndexBase; + int m_triangleIndexStride; + int m_numVertices; + const unsigned char * m_vertexBase; + int m_vertexStride; + + // The index type is set when adding an indexed mesh to the + // btTriangleIndexVertexArray, do not set it manually + PHY_ScalarType m_indexType; + + // The vertex type has a default type similar to Bullet's precision mode (float or double) + // but can be set manually if you for example run Bullet with double precision but have + // mesh data in single precision.. + PHY_ScalarType m_vertexType; + + + btIndexedMesh() + :m_indexType(PHY_INTEGER), +#ifdef BT_USE_DOUBLE_PRECISION + m_vertexType(PHY_DOUBLE) +#else // BT_USE_DOUBLE_PRECISION + m_vertexType(PHY_FLOAT) +#endif // BT_USE_DOUBLE_PRECISION + { + } } ; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp index abefa71250a..dc562941ad6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp @@ -1,12 +1,11 @@ - /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +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. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h index 69a2e631458..ba4f7b46076 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h new file mode 100644 index 00000000000..282a7702e80 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h @@ -0,0 +1,238 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2010 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef _BT_TRIANGLE_INFO_MAP_H +#define _BT_TRIANGLE_INFO_MAP_H + + +#include "LinearMath/btHashMap.h" +#include "LinearMath/btSerializer.h" + + +///for btTriangleInfo m_flags +#define TRI_INFO_V0V1_CONVEX 1 +#define TRI_INFO_V1V2_CONVEX 2 +#define TRI_INFO_V2V0_CONVEX 4 + +#define TRI_INFO_V0V1_SWAP_NORMALB 8 +#define TRI_INFO_V1V2_SWAP_NORMALB 16 +#define TRI_INFO_V2V0_SWAP_NORMALB 32 + + +///The btTriangleInfo structure stores information to adjust collision normals to avoid collisions against internal edges +///it can be generated using +struct btTriangleInfo +{ + btTriangleInfo() + { + m_edgeV0V1Angle = SIMD_2_PI; + m_edgeV1V2Angle = SIMD_2_PI; + m_edgeV2V0Angle = SIMD_2_PI; + m_flags=0; + } + + int m_flags; + + btScalar m_edgeV0V1Angle; + btScalar m_edgeV1V2Angle; + btScalar m_edgeV2V0Angle; + +}; + +typedef btHashMap<btHashInt,btTriangleInfo> btInternalTriangleInfoMap; + + +///The btTriangleInfoMap stores edge angle information for some triangles. You can compute this information yourself or using btGenerateInternalEdgeInfo. +struct btTriangleInfoMap : public btInternalTriangleInfoMap +{ + btScalar m_convexEpsilon;///used to determine if an edge or contact normal is convex, using the dot product + btScalar m_planarEpsilon; ///used to determine if a triangle edge is planar with zero angle + btScalar m_equalVertexThreshold; ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared' + btScalar m_edgeDistanceThreshold; ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge" + btScalar m_zeroAreaThreshold; ///used to determine if a triangle is degenerate (length squared of cross product of 2 triangle edges < threshold) + + + btTriangleInfoMap() + { + m_convexEpsilon = 0.00f; + m_planarEpsilon = 0.0001f; + m_equalVertexThreshold = btScalar(0.0001)*btScalar(0.0001); + m_edgeDistanceThreshold = btScalar(0.1); + m_zeroAreaThreshold = btScalar(0.0001)*btScalar(0.0001); + } + virtual ~btTriangleInfoMap() {} + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + void deSerialize(struct btTriangleInfoMapData& data); + +}; + +struct btTriangleInfoData +{ + int m_flags; + float m_edgeV0V1Angle; + float m_edgeV1V2Angle; + float m_edgeV2V0Angle; +}; + +struct btTriangleInfoMapData +{ + int *m_hashTablePtr; + int *m_nextPtr; + btTriangleInfoData *m_valueArrayPtr; + int *m_keyArrayPtr; + + float m_convexEpsilon; + float m_planarEpsilon; + float m_equalVertexThreshold; + float m_edgeDistanceThreshold; + float m_zeroAreaThreshold; + + int m_nextSize; + int m_hashTableSize; + int m_numValues; + int m_numKeys; + char m_padding[4]; +}; + +SIMD_FORCE_INLINE int btTriangleInfoMap::calculateSerializeBufferSize() const +{ + return sizeof(btTriangleInfoMapData); +} + +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btTriangleInfoMap::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btTriangleInfoMapData* tmapData = (btTriangleInfoMapData*) dataBuffer; + tmapData->m_convexEpsilon = m_convexEpsilon; + tmapData->m_planarEpsilon = m_planarEpsilon; + tmapData->m_equalVertexThreshold = m_equalVertexThreshold; + tmapData->m_edgeDistanceThreshold = m_edgeDistanceThreshold; + tmapData->m_zeroAreaThreshold = m_zeroAreaThreshold; + + tmapData->m_hashTableSize = m_hashTable.size(); + + tmapData->m_hashTablePtr = tmapData->m_hashTableSize ? (int*)serializer->getUniquePointer((void*)&m_hashTable[0]) : 0; + if (tmapData->m_hashTablePtr) + { + //serialize an int buffer + int sz = sizeof(int); + int numElem = tmapData->m_hashTableSize; + btChunk* chunk = serializer->allocate(sz,numElem); + int* memPtr = (int*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + *memPtr = m_hashTable[i]; + } + serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_hashTable[0]); + + } + + tmapData->m_nextSize = m_next.size(); + tmapData->m_nextPtr = tmapData->m_nextSize? (int*)serializer->getUniquePointer((void*)&m_next[0]): 0; + if (tmapData->m_nextPtr) + { + int sz = sizeof(int); + int numElem = tmapData->m_nextSize; + btChunk* chunk = serializer->allocate(sz,numElem); + int* memPtr = (int*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + *memPtr = m_next[i]; + } + serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_next[0]); + } + + tmapData->m_numValues = m_valueArray.size(); + tmapData->m_valueArrayPtr = tmapData->m_numValues ? (btTriangleInfoData*)serializer->getUniquePointer((void*)&m_valueArray[0]): 0; + if (tmapData->m_valueArrayPtr) + { + int sz = sizeof(btTriangleInfoData); + int numElem = tmapData->m_numValues; + btChunk* chunk = serializer->allocate(sz,numElem); + btTriangleInfoData* memPtr = (btTriangleInfoData*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + memPtr->m_edgeV0V1Angle = m_valueArray[i].m_edgeV0V1Angle; + memPtr->m_edgeV1V2Angle = m_valueArray[i].m_edgeV1V2Angle; + memPtr->m_edgeV2V0Angle = m_valueArray[i].m_edgeV2V0Angle; + memPtr->m_flags = m_valueArray[i].m_flags; + } + serializer->finalizeChunk(chunk,"btTriangleInfoData",BT_ARRAY_CODE,(void*) &m_valueArray[0]); + } + + tmapData->m_numKeys = m_keyArray.size(); + tmapData->m_keyArrayPtr = tmapData->m_numKeys ? (int*)serializer->getUniquePointer((void*)&m_keyArray[0]) : 0; + if (tmapData->m_keyArrayPtr) + { + int sz = sizeof(int); + int numElem = tmapData->m_numValues; + btChunk* chunk = serializer->allocate(sz,numElem); + int* memPtr = (int*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + *memPtr = m_keyArray[i].getUid1(); + } + serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*) &m_keyArray[0]); + + } + return "btTriangleInfoMapData"; +} + + + +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE void btTriangleInfoMap::deSerialize(btTriangleInfoMapData& tmapData ) +{ + + + m_convexEpsilon = tmapData.m_convexEpsilon; + m_planarEpsilon = tmapData.m_planarEpsilon; + m_equalVertexThreshold = tmapData.m_equalVertexThreshold; + m_edgeDistanceThreshold = tmapData.m_edgeDistanceThreshold; + m_zeroAreaThreshold = tmapData.m_zeroAreaThreshold; + m_hashTable.resize(tmapData.m_hashTableSize); + int i =0; + for (i=0;i<tmapData.m_hashTableSize;i++) + { + m_hashTable[i] = tmapData.m_hashTablePtr[i]; + } + m_next.resize(tmapData.m_nextSize); + for (i=0;i<tmapData.m_nextSize;i++) + { + m_next[i] = tmapData.m_nextPtr[i]; + } + m_valueArray.resize(tmapData.m_numValues); + for (i=0;i<tmapData.m_numValues;i++) + { + m_valueArray[i].m_edgeV0V1Angle = tmapData.m_valueArrayPtr[i].m_edgeV0V1Angle; + m_valueArray[i].m_edgeV1V2Angle = tmapData.m_valueArrayPtr[i].m_edgeV1V2Angle; + m_valueArray[i].m_edgeV2V0Angle = tmapData.m_valueArrayPtr[i].m_edgeV2V0Angle; + m_valueArray[i].m_flags = tmapData.m_valueArrayPtr[i].m_flags; + } + + m_keyArray.resize(tmapData.m_numKeys,btHashInt(0)); + for (i=0;i<tmapData.m_numKeys;i++) + { + m_keyArray[i].setUid1(tmapData.m_keyArrayPtr[i]); + } +} + + +#endif //_BT_TRIANGLE_INFO_MAP_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp index 5dcfa51c280..b29e0f71e62 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -13,6 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ + #include "btTriangleMesh.h" diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h index f06fbb7c22d..d2624fe18e1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef TRIANGLE_MESH_H #define TRIANGLE_MESH_H @@ -41,9 +40,6 @@ class btTriangleMesh : public btTriangleIndexVertexArray btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true); - int findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices); - void addIndex(int index); - bool getUse32bitIndices() const { return m_use32bitIndices; @@ -62,6 +58,10 @@ class btTriangleMesh : public btTriangleIndexVertexArray virtual void preallocateVertices(int numverts){(void) numverts;} virtual void preallocateIndices(int numindices){(void) numindices;} + ///findOrAddVertex is an internal method, use addTriangle instead + int findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices); + ///addIndex is an internal method, use addTriangle instead + void addIndex(int index); }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp index 1bd7e84a4e7..683684da770 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -91,7 +91,7 @@ public: btVector3 m_supportVecLocal; SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans) - : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-1e30)) + : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-BT_LARGE_FLOAT)) { m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis(); @@ -199,7 +199,7 @@ btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) co SupportVertexCallback supportCallback(vec,ident); - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); processAllTriangles(&supportCallback,-aabbMax,aabbMax); @@ -207,3 +207,5 @@ btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) co return supportVertex; } + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h index 53d65799001..2216698d275 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -79,7 +79,11 @@ public: //debugging virtual const char* getName()const {return "TRIANGLEMESH";} + }; + + + #endif //TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h index 4bfb0b9e881..847147cf6b4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -19,7 +19,7 @@ subject to the following restrictions: #include "btConvexShape.h" #include "btBoxShape.h" -class btTriangleShape : public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) btTriangleShape : public btPolyhedralConvexShape { @@ -32,6 +32,11 @@ public: return 3; } + btVector3& getVertexPtr(int index) + { + return m_vertices1[index]; + } + const btVector3& getVertexPtr(int index) const { return m_vertices1[index]; @@ -77,7 +82,10 @@ public: } - + btTriangleShape() : btPolyhedralConvexShape () + { + m_shapeType = TRIANGLE_SHAPE_PROXYTYPE; + } btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) : btPolyhedralConvexShape () { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp index 4c1c716d519..b148bbd99a5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -64,25 +64,70 @@ void btUniformScalingShape::calculateLocalInertia(btScalar mass,btVector3& inert ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btUniformScalingShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btUniformScalingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const { - m_childConvexShape->getAabb(t,aabbMin,aabbMax); - btVector3 aabbCenter = (aabbMax+aabbMin)*btScalar(0.5); - btVector3 scaledAabbHalfExtends = (aabbMax-aabbMin)*btScalar(0.5)*m_uniformScalingFactor; - - aabbMin = aabbCenter - scaledAabbHalfExtends; - aabbMax = aabbCenter + scaledAabbHalfExtends; + getAabbSlow(trans,aabbMin,aabbMax); } void btUniformScalingShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { - m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax); - btVector3 aabbCenter = (aabbMax+aabbMin)*btScalar(0.5); - btVector3 scaledAabbHalfExtends = (aabbMax-aabbMin)*btScalar(0.5)*m_uniformScalingFactor; +#if 1 + btVector3 _directions[] = + { + btVector3( 1., 0., 0.), + btVector3( 0., 1., 0.), + btVector3( 0., 0., 1.), + btVector3( -1., 0., 0.), + btVector3( 0., -1., 0.), + btVector3( 0., 0., -1.) + }; + + btVector3 _supporting[] = + { + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.), + btVector3( 0., 0., 0.) + }; + + for (int i=0;i<6;i++) + { + _directions[i] = _directions[i]*t.getBasis(); + } + + batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); + + btVector3 aabbMin1(0,0,0),aabbMax1(0,0,0); + + for ( int i = 0; i < 3; ++i ) + { + aabbMax1[i] = t(_supporting[i])[i]; + aabbMin1[i] = t(_supporting[i + 3])[i]; + } + btVector3 marginVec(getMargin(),getMargin(),getMargin()); + aabbMin = aabbMin1-marginVec; + aabbMax = aabbMax1+marginVec; + +#else + + btScalar margin = getMargin(); + for (int i=0;i<3;i++) + { + btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + vec[i] = btScalar(1.); + btVector3 sv = localGetSupportingVertex(vec*t.getBasis()); + btVector3 tmp = t(sv); + aabbMax[i] = tmp[i]+margin; + vec[i] = btScalar(-1.); + sv = localGetSupportingVertex(vec*t.getBasis()); + tmp = t(sv); + aabbMin[i] = tmp[i]-margin; + } - aabbMin = aabbCenter - scaledAabbHalfExtends; - aabbMax = aabbCenter + scaledAabbHalfExtends; +#endif } void btUniformScalingShape::setLocalScaling(const btVector3& scaling) diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h index 945976ac036..cbf7e6fd3ed 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h b/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h index 827a3c89587..d5676aaa80e 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h @@ -2,7 +2,7 @@ #define BT_BOX_COLLISION_H_INCLUDED /*! \file gim_box_collision.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* This source file is part of GIMPACT Library. diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h b/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h index 5de391a7561..de0a5231baf 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h @@ -2,7 +2,7 @@ #define BT_CLIP_POLYGON_H_INCLUDED /*! \file btClipPolygon.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* This source file is part of GIMPACT Library. diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h index 4b0b70293b4..0c66f8e106c 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h @@ -2,7 +2,7 @@ #define BT_CONTACT_H_INCLUDED /*! \file gim_contact.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* This source file is part of GIMPACT Library. diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp index 38fed6b4fd2..863233163a6 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp @@ -1,5 +1,5 @@ /*! \file gim_box_set.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* This source file is part of GIMPACT Library. diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h index b432b615f8a..6174ae97a97 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h @@ -2,7 +2,7 @@ #define GIM_BOX_SET_H_INCLUDED /*! \file gim_box_set.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* This source file is part of GIMPACT Library. @@ -231,6 +231,8 @@ class btPrimitiveManagerBase { public: + virtual ~btPrimitiveManagerBase() {} + //! determines if this manager consist on only triangles, which special case will be optimized virtual bool is_trimesh() const = 0; virtual int get_primitive_count() const = 0; @@ -381,9 +383,9 @@ public: return m_box_tree.get_node_pointer(index); } - +#ifdef TRI_COLLISION_PROFILING static float getAverageTreeCollisionTime(); - +#endif //TRI_COLLISION_PROFILING static void find_collision(btGImpactBvh * boxset1, const btTransform & trans1, btGImpactBvh * boxset2, const btTransform & trans2, diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp index 4d6fa88f62d..2f2c09ffc03 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp @@ -102,6 +102,7 @@ public: { return m_parent->m_gim_shape->getChildShape(index); } + virtual ~ChildShapeRetriever() {} }; class TriangleShapeRetriever:public ChildShapeRetriever @@ -113,6 +114,7 @@ public: m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape); return &m_parent->m_trishape; } + virtual ~TriangleShapeRetriever() {} }; class TetraShapeRetriever:public ChildShapeRetriever @@ -213,7 +215,8 @@ void btGImpactCollisionAlgorithm::addContactPoint(btCollisionObject * body0, const btVector3 & normal, btScalar distance) { - m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1); + m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); checkManifold(body0,body1); m_resultOut->addContactPoint(normal,point,distance); } @@ -236,7 +239,8 @@ void btGImpactCollisionAlgorithm::shape_vs_shape_collision( btCollisionAlgorithm* algor = newAlgorithm(body0,body1); // post : checkManifold is called - m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1); + m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); algor->processCollision(body0,body1,*m_dispatchInfo,m_resultOut); @@ -262,7 +266,8 @@ void btGImpactCollisionAlgorithm::convex_vs_convex_collision( body1->internalSetTemporaryCollisionShape(shape1); - m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1); + m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); checkConvexAlgorithm(body0,body1); m_convex_algorithm->processCollision(body0,body1,*m_dispatchInfo,m_resultOut); @@ -876,22 +881,24 @@ btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* b ///////////////////////////////////// REGISTERING ALGORITHM ////////////////////////////////////////////// -btGImpactCollisionAlgorithm::CreateFunc g_gimpact_cf; + //! Use this function for register the algorithm externally void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher) { + static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf; + int i; for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) { - dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&g_gimpact_cf); + dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&s_gimpact_cf); } for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) { - dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&g_gimpact_cf); + dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&s_gimpact_cf); } } diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h index 453472aa034..ae5d55ab7d9 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h @@ -1,5 +1,5 @@ /*! \file btGImpactShape.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* This source file is part of GIMPACT Library. @@ -222,13 +222,13 @@ public: //! Use this function for register the algorithm externally static void registerAlgorithm(btCollisionDispatcher * dispatcher); - +#ifdef TRI_COLLISION_PROFILING //! Gets the average time in miliseconds of tree collisions static float getAverageTreeCollisionTime(); //! Gets the average time in miliseconds of triangle collisions static float getAverageTriangleCollisionTime(); - +#endif //TRI_COLLISION_PROFILING //! Collides two gimpact shapes /*! diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h index 0a10f3cdb7b..2543aefcfc6 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h @@ -1,5 +1,5 @@ /*! \file btGImpactMassUtil.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* This source file is part of GIMPACT Library. diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp index ea1647a8129..cd4dfdb60e9 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp @@ -1,5 +1,5 @@ /*! \file gim_box_set.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* This source file is part of GIMPACT Library. diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h index e9cccac75f5..9c990774739 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h @@ -2,7 +2,7 @@ #define GIM_QUANTIZED_SET_H_INCLUDED /*! \file btGImpactQuantizedBvh.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* This source file is part of GIMPACT Library. @@ -359,9 +359,9 @@ public: return m_box_tree.get_node_pointer(index); } - +#ifdef TRI_COLLISION_PROFILING static float getAverageTreeCollisionTime(); - +#endif //TRI_COLLISION_PROFILING static void find_collision(btGImpactQuantizedBvh * boxset1, const btTransform & trans1, btGImpactQuantizedBvh * boxset2, const btTransform & trans2, diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp index da6a4dbfc28..cceace55e4b 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp @@ -181,3 +181,23 @@ void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback,const m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax); } } + + +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btGImpactMeshShapeData* trimeshData = (btGImpactMeshShapeData*) dataBuffer; + + btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer); + + m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer); + + trimeshData->m_collisionMargin = float(m_collisionMargin); + + localScaling.serializeFloat(trimeshData->m_localScaling); + + trimeshData->m_gimpactSubType = int(getGImpactShapeType()); + + return "btGImpactMeshShapeData"; +} + diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h index 05431cfce15..90015bb9ac0 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h @@ -189,7 +189,7 @@ public: //!@{ //! Base method for determinig which kind of GIMPACT shape we get - virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() = 0; + virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const = 0 ; //! gets boxset SIMD_FORCE_INLINE btGImpactBoxSet * getBoxSet() @@ -276,6 +276,7 @@ public: //! virtual method for ray collision virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const { + (void) rayFrom; (void) rayTo; (void) resultCallback; } //! Function for retrieve triangles. @@ -284,6 +285,7 @@ public: */ virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { + (void) callback; (void) aabbMin; (void) aabbMax; } //!@} @@ -302,10 +304,12 @@ public: class CompoundPrimitiveManager:public btPrimitiveManagerBase { public: + virtual ~CompoundPrimitiveManager() {} btGImpactCompoundShape * m_compoundShape; CompoundPrimitiveManager(const CompoundPrimitiveManager& compound) + : btPrimitiveManagerBase() { m_compoundShape = compound.m_compoundShape; } @@ -348,6 +352,7 @@ public: virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const { btAssert(0); + (void) prim_index; (void) triangle; } }; @@ -364,6 +369,7 @@ public: btGImpactCompoundShape(bool children_has_transform = true) { + (void) children_has_transform; m_primitive_manager.m_compoundShape = this; m_box_set.setPrimitiveManager(&m_primitive_manager); } @@ -477,11 +483,13 @@ public: virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const { + (void) prim_index; (void) triangle; btAssert(0); } virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const { + (void) prim_index; (void) tetrahedron; btAssert(0); } @@ -494,7 +502,7 @@ public: return "GImpactCompound"; } - virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() + virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const { return CONST_GIMPACT_COMPOUND_SHAPE; } @@ -550,6 +558,7 @@ public: } TrimeshPrimitiveManager(const TrimeshPrimitiveManager & manager) + : btPrimitiveManagerBase() { m_meshInterface = manager.m_meshInterface; m_part = manager.m_part; @@ -582,6 +591,7 @@ public: } + virtual ~TrimeshPrimitiveManager() {} void lock() { @@ -746,6 +756,7 @@ public: //! Gets the children virtual btCollisionShape* getChildShape(int index) { + (void) index; btAssert(0); return NULL; } @@ -755,6 +766,7 @@ public: //! Gets the child virtual const btCollisionShape* getChildShape(int index) const { + (void) index; btAssert(0); return NULL; } @@ -762,6 +774,7 @@ public: //! Gets the children transform virtual btTransform getChildTransform(int index) const { + (void) index; btAssert(0); return btTransform(); } @@ -772,6 +785,8 @@ public: */ virtual void setChildTransform(int index, const btTransform & transform) { + (void) index; + (void) transform; btAssert(0); } @@ -801,7 +816,7 @@ public: return "GImpactMeshShapePart"; } - virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() + virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const { return CONST_GIMPACT_TRIMESH_SHAPE_PART; } @@ -825,6 +840,8 @@ public: virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const { + (void) prim_index; + (void) tetrahedron; btAssert(0); } @@ -1036,11 +1053,13 @@ public: virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const { + (void) prim_index; (void) triangle; btAssert(0); } virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const { + (void) prim_index; (void) tetrahedron; btAssert(0); } @@ -1063,12 +1082,14 @@ public: */ virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { + (void) child_index; (void) t; (void) aabbMin; (void) aabbMax; btAssert(0); } //! Gets the children virtual btCollisionShape* getChildShape(int index) { + (void) index; btAssert(0); return NULL; } @@ -1077,6 +1098,7 @@ public: //! Gets the child virtual const btCollisionShape* getChildShape(int index) const { + (void) index; btAssert(0); return NULL; } @@ -1084,6 +1106,7 @@ public: //! Gets the children transform virtual btTransform getChildTransform(int index) const { + (void) index; btAssert(0); return btTransform(); } @@ -1094,11 +1117,12 @@ public: */ virtual void setChildTransform(int index, const btTransform & transform) { + (void) index; (void) transform; btAssert(0); } - virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() + virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const { return CONST_GIMPACT_TRIMESH_SHAPE; } @@ -1116,7 +1140,32 @@ public: It gives the triangles in local space */ virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btGImpactMeshShapeData +{ + btCollisionShapeData m_collisionShapeData; + + btStridingMeshInterfaceData m_meshInterface; + + btVector3FloatData m_localScaling; + + float m_collisionMargin; + + int m_gimpactSubType; }; +SIMD_FORCE_INLINE int btGImpactMeshShape::calculateSerializeBufferSize() const +{ + return sizeof(btGImpactMeshShapeData); +} + #endif //GIMPACT_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp index 956fa0430e3..5d07d1adb97 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp @@ -1,5 +1,5 @@ /*! \file btGenericPoolAllocator.cpp -\author Francisco Len Nßjera. email projectileman@yahoo.com +\author Francisco Leon Najera. email projectileman@yahoo.com General purpose allocator class */ diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h index e883bcd23d2..b46d8516347 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h @@ -1,5 +1,5 @@ /*! \file btGenericPoolAllocator.h -\author Francisco Len Nßjera. email projectileman@yahoo.com +\author Francisco Leon Najera. email projectileman@yahoo.com General purpose allocator class */ @@ -23,7 +23,7 @@ subject to the following restrictions: #include <limits.h> #include <stdio.h> -#include <memory.h> +#include <string.h> #include "LinearMath/btAlignedAllocator.h" #define BT_UINT_MAX UINT_MAX diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h b/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h index bc5a416ddb7..60f06510ad7 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h @@ -2,7 +2,7 @@ #define BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED /*! \file btGeometryOperations.h -*\author Francisco Len Nßjera +*\author Francisco Leon Najera */ /* diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h index 7faada61c95..e0487c22766 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h @@ -2,7 +2,7 @@ #define BT_QUANTIZATION_H_INCLUDED /*! \file btQuantization.h -*\author Francisco Len Nßjera +*\author Francisco Leon Najera */ /* diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp index 1e8706f60ad..ca76cc54a12 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp @@ -1,5 +1,5 @@ -/*! \file btTriangleShapeEx.cpp -\author Francisco Len Nßjera +/*! \file btGImpactTriangleShape.h +\author Francisco Leon Najera */ /* This source file is part of GIMPACT Library. diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h index bbd6b630c02..fcc981dfb69 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h @@ -1,5 +1,5 @@ /*! \file btGImpactShape.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* This source file is part of GIMPACT Library. diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h index c8161d252d5..cfd5da8f49a 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h @@ -1,7 +1,7 @@ #ifndef GIM_ARRAY_H_INCLUDED #define GIM_ARRAY_H_INCLUDED /*! \file gim_array.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* ----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h index 666abf7917a..915277404d6 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h @@ -2,7 +2,7 @@ #define GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED /*! \file gim_basic_geometry_operations.h -*\author Francisco Len Nßjera +*\author Francisco Leon Najera type independant geometry routines */ diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h index 322004a8d5b..7dee48a4c76 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h @@ -1,7 +1,7 @@ #ifndef GIM_BITSET_H_INCLUDED #define GIM_BITSET_H_INCLUDED /*! \file gim_bitset.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* ----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h index 0add5e4b99f..b360dd47038 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h @@ -2,7 +2,7 @@ #define GIM_BOX_COLLISION_H_INCLUDED /*! \file gim_box_collision.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* ----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h index 1058a0872a5..61d190a7dfe 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h @@ -2,7 +2,7 @@ #define GIM_BOX_SET_H_INCLUDED /*! \file gim_box_set.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* ----------------------------------------------------------------------------- @@ -91,6 +91,7 @@ class GIM_PRIMITIVE_MANAGER_PROTOTYPE { public: + virtual ~GIM_PRIMITIVE_MANAGER_PROTOTYPE() {} //! determines if this manager consist on only triangles, which special case will be optimized virtual bool is_trimesh() = 0; virtual GUINT get_primitive_count() = 0; @@ -669,3 +670,5 @@ public: #endif // GIM_BOXPRUNING_H_INCLUDED + + diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h index a91fd3aa422..e342459ce56 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h @@ -2,7 +2,7 @@ #define GIM_CLIP_POLYGON_H_INCLUDED /*! \file gim_tri_collision.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* ----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h index ad3e12298fe..5d9f8ef8180 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h @@ -2,7 +2,7 @@ #define GIM_CONTACT_H_INCLUDED /*! \file gim_contact.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* ----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h index 2fb1e3f9e45..6b8f9ea6c21 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h @@ -2,7 +2,7 @@ #define GIM_GEOM_TYPES_H_INCLUDED /*! \file gim_geom_types.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* ----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h index 6cc41672101..c67a6991c02 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h @@ -2,7 +2,7 @@ #define GIM_GEOMETRY_H_INCLUDED /*! \file gim_geometry.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* ----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h index a1ca6ed7c4a..e4237c2c570 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h @@ -1,7 +1,7 @@ #ifndef GIM_HASH_TABLE_H_INCLUDED #define GIM_HASH_TABLE_H_INCLUDED -/*! \file gim_hash_table.h -\author Francisco Len Nßjera +/*! \file gim_trimesh_data.h +\author Francisco Leon Najera */ /* ----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h index 0247d4e61cc..64f11b49543 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h @@ -2,7 +2,7 @@ #define GIM_LINEAR_H_INCLUDED /*! \file gim_linear_math.h -*\author Francisco Len Nßjera +*\author Francisco Leon Najera Type Independant Vector and matrix operations. */ /* diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h index 8b9e6806ef7..939079e1040 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h @@ -1,7 +1,7 @@ #ifndef GIM_MATH_H_INCLUDED #define GIM_MATH_H_INCLUDED /*! \file gim_math.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* ----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h index fa99eebb60e..e203888a1e2 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h @@ -1,7 +1,7 @@ #ifndef GIM_MEMORY_H_INCLUDED #define GIM_MEMORY_H_INCLUDED /*! \file gim_memory.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* ----------------------------------------------------------------------------- @@ -34,7 +34,7 @@ email: projectileman@yahoo.com #include "gim_math.h" -#include <memory.h> +#include <string.h> #ifdef PREFETCH #include <xmmintrin.h> // for prefetch @@ -117,7 +117,7 @@ void gim_free(void *ptr); -#if defined (WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__) +#if defined (_WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__) #define GIM_SIMD_MEMORY 1 #endif diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h index f7dadbbca93..c246ef12543 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h @@ -1,7 +1,7 @@ #ifndef GIM_RADIXSORT_H_INCLUDED #define GIM_RADIXSORT_H_INCLUDED /*! \file gim_radixsort.h -\author Francisco Len Nßjera. +\author Francisco Leon Najera. Based on the work of Michael Herf : "fast floating-point radix sort" Avaliable on http://www.stereopsis.com/radix.html */ diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp index 74d734146a7..f9727e1d53b 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp @@ -1,6 +1,6 @@ /*! \file gim_tri_collision.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* ----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h index 2d6e43a1a2e..5b552a1ed51 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h @@ -2,7 +2,7 @@ #define GIM_TRI_COLLISION_H_INCLUDED /*! \file gim_tri_collision.h -\author Francisco Len Nßjera +\author Francisco Leon Najera */ /* ----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index 2f41b3c2757..9ee83e7d561 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -96,7 +96,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( { - btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver); + btGjkPairDetector gjk(m_convexA,m_convexB,m_convexA->getShapeType(),m_convexB->getShapeType(),m_convexA->getMargin(),m_convexB->getMargin(),m_simplexSolver,m_penetrationDepthSolver); btGjkPairDetector::ClosestPointInput input; //we don't use margins during CCD @@ -121,6 +121,10 @@ bool btContinuousConvexCollision::calcTimeOfImpact( //not close enough while (dist > radius) { + if (result.m_debugDrawer) + { + result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1)); + } numIter++; if (numIter > maxIter) { @@ -170,6 +174,11 @@ bool btContinuousConvexCollision::calcTimeOfImpact( btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB); relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA); + if (result.m_debugDrawer) + { + result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0)); + } + result.DebugDraw( lambda ); btPointCollector pointCollector; @@ -197,6 +206,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( //?? return false; } + } @@ -224,4 +234,3 @@ bool btContinuousConvexCollision::calcTimeOfImpact( */ } - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h index 0edf4dcd496..b0bce341e41 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h @@ -41,7 +41,7 @@ public: virtual void drawCoordSystem(const btTransform& trans) {(void)trans;} CastResult() - :m_fraction(btScalar(1e30)), + :m_fraction(btScalar(BT_LARGE_FLOAT)), m_debugDrawer(0), m_allowedPenetration(btScalar(0)) { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h index 2989daeb44e..7e3fde8e291 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -14,8 +14,8 @@ subject to the following restrictions: */ -#ifndef CONVEX_PENETRATION_DEPTH_H -#define CONVEX_PENETRATION_DEPTH_H +#ifndef __CONVEX_PENETRATION_DEPTH_H +#define __CONVEX_PENETRATION_DEPTH_H class btStackAlloc; class btVector3; @@ -33,7 +33,7 @@ public: const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw + class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ) = 0; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index b011bb9ae58..bc711ad495c 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -33,21 +33,24 @@ struct btDiscreteCollisionDetectorInterface virtual ~Result(){} - ///setShapeIdentifiers provides experimental support for per-triangle material / custom material combiner - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)=0; + ///setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material combiner + virtual void setShapeIdentifiersA(int partId0,int index0)=0; + virtual void setShapeIdentifiersB(int partId1,int index1)=0; virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0; }; struct ClosestPointInput { ClosestPointInput() - :m_maximumDistanceSquared(btScalar(1e30)) + :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)), + m_stackAlloc(0) { } btTransform m_transformA; btTransform m_transformB; btScalar m_maximumDistanceSquared; + btStackAlloc* m_stackAlloc; }; virtual ~btDiscreteCollisionDetectorInterface() {}; @@ -66,7 +69,7 @@ struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result btVector3 m_closestPointInB; btScalar m_distance; //negative means penetration ! - btStorageResult() : m_distance(btScalar(1e30)) + btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT)) { } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp deleted file mode 100644 index 36cdeeaefdb..00000000000 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp +++ /dev/null @@ -1,628 +0,0 @@ -/* -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. -*/ - -/* -GJK-EPA collision solver by Nathanael Presson -Nov.2006 -*/ - -#include "btGjkEpa.h" -#include <string.h> //for memset -#include "LinearMath/btStackAlloc.h" - -#if defined(DEBUG) || defined (_DEBUG) -#include <stdio.h> //for debug printf -#ifdef __SPU__ -#include <spu_printf.h> -#define printf spu_printf -#endif //__SPU__ -#endif - -namespace gjkepa_impl -{ - -// -// Port. typedefs -// - -typedef btScalar F; -typedef bool Z; -typedef int I; -typedef unsigned int U; -typedef unsigned char U1; -typedef unsigned short U2; - -typedef btVector3 Vector3; -typedef btMatrix3x3 Rotation; - -// -// Config -// - -#if 0 -#define BTLOCALSUPPORT localGetSupportingVertexWithoutMargin -#else -#define BTLOCALSUPPORT localGetSupportingVertex -#endif - -// -// Const -// - - -#define cstInf SIMD_INFINITY -#define cstPi SIMD_PI -#define cst2Pi SIMD_2_PI -#define GJK_maxiterations (128) -#define GJK_hashsize (1<<6) -#define GJK_hashmask (GJK_hashsize-1) -#define GJK_insimplex_eps F(0.0001) -#define GJK_sqinsimplex_eps (GJK_insimplex_eps*GJK_insimplex_eps) -#define EPA_maxiterations 256 -#define EPA_inface_eps F(0.01) -#define EPA_accuracy F(0.001) - -// -// Utils -// - -static inline F Abs(F v) { return(v<0?-v:v); } -static inline F Sign(F v) { return(F(v<0?-1:1)); } -template <typename T> static inline void Swap(T& a,T& b) { T -t(a);a=b;b=t; } -template <typename T> static inline T Min(const T& a,const T& b) { -return(a<b?a:b); } -template <typename T> static inline T Max(const T& a,const T& b) { -return(a>b?a:b); } -static inline void ClearMemory(void* p,U sz) { memset(p,0,(size_t)sz); -} -#if 0 -template <typename T> static inline void Raise(const T& object) { -throw(object); } -#else -template <typename T> static inline void Raise(const T&) {} -#endif - - - -// -// GJK -// -struct GJK - { - struct Mkv - { - Vector3 w; /* Minkowski vertice */ - Vector3 r; /* Ray */ - }; - struct He - { - Vector3 v; - He* n; - }; - btStackAlloc* sa; - btBlock* sablock; - He* table[GJK_hashsize]; - Rotation wrotations[2]; - Vector3 positions[2]; - const btConvexShape* shapes[2]; - Mkv simplex[5]; - Vector3 ray; - U order; - U iterations; - F margin; - Z failed; - // - GJK(btStackAlloc* psa, - const Rotation& wrot0,const Vector3& pos0,const btConvexShape* shape0, - const Rotation& wrot1,const Vector3& pos1,const btConvexShape* shape1, - F pmargin=0) - { - wrotations[0]=wrot0;positions[0]=pos0;shapes[0]=shape0; - wrotations[1]=wrot1;positions[1]=pos1;shapes[1]=shape1; - sa =psa; - sablock =sa->beginBlock(); - margin =pmargin; - failed =false; - } - // - ~GJK() - { - sa->endBlock(sablock); - } - // vdh : very dumm hash - static inline U Hash(const Vector3& v) - { - //this doesn't compile under GCC 3.3.5, so add the ()... - //const U h(U(v[0]*15461)^U(v[1]*83003)^U(v[2]*15473)); - //return(((*((const U*)&h))*169639)&GJK_hashmask); - const U h((U)(v[0]*15461)^(U)(v[1]*83003)^(U)(v[2]*15473)); - return(((*((const U*)&h))*169639)&GJK_hashmask); - } - // - inline Vector3 LocalSupport(const Vector3& d,U i) const - { - return(wrotations[i]*shapes[i]->BTLOCALSUPPORT(d*wrotations[i])+positions[i]); - } - // - inline void Support(const Vector3& d,Mkv& v) const - { - v.r = d; - v.w = LocalSupport(d,0)-LocalSupport(-d,1)+d*margin; - } - #define SPX(_i_) simplex[_i_] - #define SPXW(_i_) simplex[_i_].w - // - inline Z FetchSupport() - { - const U h(Hash(ray)); - He* e = (He*)(table[h]); - while(e) { if(e->v==ray) { --order;return(false); } else e=e->n; } - e=(He*)sa->allocate(sizeof(He));e->v=ray;e->n=table[h];table[h]=e; - Support(ray,simplex[++order]); - return(ray.dot(SPXW(order))>0); - } - // - inline Z SolveSimplex2(const Vector3& ao,const Vector3& ab) - { - if(ab.dot(ao)>=0) - { - const Vector3 cabo(cross(ab,ao)); - if(cabo.length2()>GJK_sqinsimplex_eps) - { ray=cross(cabo,ab); } - else - { return(true); } - } - else - { order=0;SPX(0)=SPX(1);ray=ao; } - return(false); - } - // - inline Z SolveSimplex3(const Vector3& ao,const Vector3& ab,const Vector3& -ac) - { - return(SolveSimplex3a(ao,ab,ac,cross(ab,ac))); - } - // - inline Z SolveSimplex3a(const Vector3& ao,const Vector3& ab,const Vector3& -ac,const Vector3& cabc) - { - if((cross(cabc,ab)).dot(ao)<-GJK_insimplex_eps) - { order=1;SPX(0)=SPX(1);SPX(1)=SPX(2);return(SolveSimplex2(ao,ab)); } - else if((cross(cabc,ac)).dot(ao)>+GJK_insimplex_eps) - { order=1;SPX(1)=SPX(2);return(SolveSimplex2(ao,ac)); } - else - { - const F d(cabc.dot(ao)); - if(Abs(d)>GJK_insimplex_eps) - { - if(d>0) - { ray=cabc; } - else - { ray=-cabc;Swap(SPX(0),SPX(1)); } - return(false); - } else return(true); - } - } - // - inline Z SolveSimplex4(const Vector3& ao,const Vector3& ab,const Vector3& -ac,const Vector3& ad) - { - Vector3 crs; - if((crs=cross(ab,ac)).dot(ao)>GJK_insimplex_eps) - { -order=2;SPX(0)=SPX(1);SPX(1)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ab,ac,crs)); -} - else if((crs=cross(ac,ad)).dot(ao)>GJK_insimplex_eps) - { order=2;SPX(2)=SPX(3);return(SolveSimplex3a(ao,ac,ad,crs)); } - else if((crs=cross(ad,ab)).dot(ao)>GJK_insimplex_eps) - { -order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab,crs)); -} - else return(true); - } - // - inline Z SearchOrigin(const Vector3& initray=Vector3(1,0,0)) - { - iterations = 0; - order = (U)-1; - failed = false; - ray = initray.normalized(); - ClearMemory(table,sizeof(void*)*GJK_hashsize); - FetchSupport(); - ray = -SPXW(0); - for(;iterations<GJK_maxiterations;++iterations) - { - const F rl(ray.length()); - ray/=rl>0?rl:1; - if(FetchSupport()) - { - Z found(false); - switch(order) - { - case 1: found=SolveSimplex2(-SPXW(1),SPXW(0)-SPXW(1));break; - case 2: found=SolveSimplex3(-SPXW(2),SPXW(1)-SPXW(2),SPXW(0)-SPXW(2));break; - case 3: found=SolveSimplex4(-SPXW(3),SPXW(2)-SPXW(3),SPXW(1)-SPXW(3),SPXW(0)-SPXW(3));break; - } - if(found) return(true); - } else return(false); - } - failed=true; - return(false); - } - // - inline Z EncloseOrigin() - { - switch(order) - { - /* Point */ - case 0: break; - /* Line */ - case 1: - { - const Vector3 ab(SPXW(1)-SPXW(0)); - const Vector3 b[]={ cross(ab,Vector3(1,0,0)), - cross(ab,Vector3(0,1,0)), - cross(ab,Vector3(0,0,1))}; - const F m[]={b[0].length2(),b[1].length2(),b[2].length2()}; - const Rotation r(btQuaternion(ab.normalized(),cst2Pi/3)); - Vector3 w(b[m[0]>m[1]?m[0]>m[2]?0:2:m[1]>m[2]?1:2]); - Support(w.normalized(),simplex[4]);w=r*w; - Support(w.normalized(),simplex[2]);w=r*w; - Support(w.normalized(),simplex[3]);w=r*w; - order=4; - return(true); - } - break; - /* Triangle */ - case 2: - { - const -Vector3 n(cross((SPXW(1)-SPXW(0)),(SPXW(2)-SPXW(0))).normalized()); - Support( n,simplex[3]); - Support(-n,simplex[4]); - order=4; - return(true); - } - break; - /* Tetrahedron */ - case 3: return(true); - /* Hexahedron */ - case 4: return(true); - } - return(false); - } - #undef SPX - #undef SPXW - }; - -// -// EPA -// -struct EPA - { - // - struct Face - { - const GJK::Mkv* v[3]; - Face* f[3]; - U e[3]; - Vector3 n; - F d; - U mark; - Face* prev; - Face* next; - Face() {} - }; - // - GJK* gjk; - btStackAlloc* sa; - Face* root; - U nfaces; - U iterations; - Vector3 features[2][3]; - Vector3 nearest[2]; - Vector3 normal; - F depth; - Z failed; - // - EPA(GJK* pgjk) - { - gjk = pgjk; - sa = pgjk->sa; - } - // - ~EPA() - { - } - // - inline Vector3 GetCoordinates(const Face* face) const - { - const Vector3 o(face->n*-face->d); - const F a[]={ cross(face->v[0]->w-o,face->v[1]->w-o).length(), - cross(face->v[1]->w-o,face->v[2]->w-o).length(), - cross(face->v[2]->w-o,face->v[0]->w-o).length()}; - const F sm(a[0]+a[1]+a[2]); - return(Vector3(a[1],a[2],a[0])/(sm>0?sm:1)); - } - // - inline Face* FindBest() const - { - Face* bf = 0; - if(root) - { - Face* cf = root; - F bd(cstInf); - do { - if(cf->d<bd) { bd=cf->d;bf=cf; } - } while(0!=(cf=cf->next)); - } - return(bf); - } - // - inline Z Set(Face* f,const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* -c) const - { - const Vector3 nrm(cross(b->w-a->w,c->w-a->w)); - const F len(nrm.length()); - const Z valid( (cross(a->w,b->w).dot(nrm)>=-EPA_inface_eps)&& - (cross(b->w,c->w).dot(nrm)>=-EPA_inface_eps)&& - (cross(c->w,a->w).dot(nrm)>=-EPA_inface_eps)); - f->v[0] = a; - f->v[1] = b; - f->v[2] = c; - f->mark = 0; - f->n = nrm/(len>0?len:cstInf); - f->d = Max<F>(0,-f->n.dot(a->w)); - return(valid); - } - // - inline Face* NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c) - { - Face* pf = (Face*)sa->allocate(sizeof(Face)); - if(Set(pf,a,b,c)) - { - if(root) root->prev=pf; - pf->prev=0; - pf->next=root; - root =pf; - ++nfaces; - } - else - { - pf->prev=pf->next=0; - } - return(pf); - } - // - inline void Detach(Face* face) - { - if(face->prev||face->next) - { - --nfaces; - if(face==root) - { root=face->next;root->prev=0; } - else - { - if(face->next==0) - { face->prev->next=0; } - else - { face->prev->next=face->next;face->next->prev=face->prev; } - } - face->prev=face->next=0; - } - } - // - inline void Link(Face* f0,U e0,Face* f1,U e1) const - { - f0->f[e0]=f1;f1->e[e1]=e0; - f1->f[e1]=f0;f0->e[e0]=e1; - } - // - GJK::Mkv* Support(const Vector3& w) const - { - GJK::Mkv* v =(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv)); - gjk->Support(w,*v); - return(v); - } - // - U BuildHorizon(U markid,const GJK::Mkv* w,Face& f,U e,Face*& cf,Face*& -ff) - { - static const U mod3[]={0,1,2,0,1}; - U ne(0); - if(f.mark!=markid) - { - const U e1(mod3[e+1]); - if((f.n.dot(w->w)+f.d)>0) - { - Face* nf = NewFace(f.v[e1],f.v[e],w); - Link(nf,0,&f,e); - if(cf) Link(cf,1,nf,2); else ff=nf; - cf=nf;ne=1; - } - else - { - const U e2(mod3[e+2]); - Detach(&f); - f.mark = markid; - ne += BuildHorizon(markid,w,*f.f[e1],f.e[e1],cf,ff); - ne += BuildHorizon(markid,w,*f.f[e2],f.e[e2],cf,ff); - } - } - return(ne); - } - // - inline F EvaluatePD(F accuracy=EPA_accuracy) - { - btBlock* sablock = sa->beginBlock(); - Face* bestface = 0; - U markid(1); - depth = -cstInf; - normal = Vector3(0,0,0); - root = 0; - nfaces = 0; - iterations = 0; - failed = false; - /* Prepare hull */ - if(gjk->EncloseOrigin()) - { - const U* pfidx = 0; - U nfidx= 0; - const U* peidx = 0; - U neidx = 0; - GJK::Mkv* basemkv[5]; - Face* basefaces[6]; - switch(gjk->order) - { - /* Tetrahedron */ - case 3: { - static const U fidx[4][3]={{2,1,0},{3,0,1},{3,1,2},{3,2,0}}; - static const -U eidx[6][4]={{0,0,2,1},{0,1,1,1},{0,2,3,1},{1,0,3,2},{2,0,1,2},{3,0,2,2}}; - pfidx=(const U*)fidx;nfidx=4;peidx=(const U*)eidx;neidx=6; - } break; - /* Hexahedron */ - case 4: { - static const -U fidx[6][3]={{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}}; - static const -U eidx[9][4]={{0,0,4,0},{0,1,2,1},{0,2,1,2},{1,1,5,2},{1,0,2,0},{2,2,3,2},{3,1,5,0},{3,0,4,2},{5,1,4,1}}; - pfidx=(const U*)fidx;nfidx=6;peidx=(const U*)eidx;neidx=9; - } break; - } - U i; - - for( i=0;i<=gjk->order;++i) { -basemkv[i]=(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));*basemkv[i]=gjk->simplex[i]; -} - for( i=0;i<nfidx;++i,pfidx+=3) { -basefaces[i]=NewFace(basemkv[pfidx[0]],basemkv[pfidx[1]],basemkv[pfidx[2]]); -} - for( i=0;i<neidx;++i,peidx+=4) { -Link(basefaces[peidx[0]],peidx[1],basefaces[peidx[2]],peidx[3]); } - } - if(0==nfaces) - { - sa->endBlock(sablock); - return(depth); - } - /* Expand hull */ - for(;iterations<EPA_maxiterations;++iterations) - { - Face* bf = FindBest(); - if(bf) - { - GJK::Mkv* w = Support(-bf->n); - const F d(bf->n.dot(w->w)+bf->d); - bestface = bf; - if(d<-accuracy) - { - Face* cf =0; - Face* ff =0; - U nf = 0; - Detach(bf); - bf->mark=++markid; - for(U i=0;i<3;++i) { -nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); } - if(nf<=2) { break; } - Link(cf,1,ff,2); - } else break; - } else break; - } - /* Extract contact */ - if(bestface) - { - const Vector3 b(GetCoordinates(bestface)); - normal = bestface->n; - depth = Max<F>(0,bestface->d); - for(U i=0;i<2;++i) - { - const F s(F(i?-1:1)); - for(U j=0;j<3;++j) - { - features[i][j]=gjk->LocalSupport(s*bestface->v[j]->r,i); - } - } - nearest[0] = features[0][0]*b.x()+features[0][1]*b.y()+features[0][2]*b.z(); - nearest[1] = features[1][0]*b.x()+features[1][1]*b.y()+features[1][2]*b.z(); - } else failed=true; - sa->endBlock(sablock); - return(depth); - } - }; -} - -// -// Api -// - -using namespace gjkepa_impl; - - - -// -bool btGjkEpaSolver::Collide(const btConvexShape *shape0,const btTransform &wtrs0, - const btConvexShape *shape1,const btTransform &wtrs1, - btScalar radialmargin, - btStackAlloc* stackAlloc, - sResults& results) -{ - - -/* Initialize */ -results.witnesses[0] = -results.witnesses[1] = -results.normal = Vector3(0,0,0); -results.depth = 0; -results.status = sResults::Separated; -results.epa_iterations = 0; -results.gjk_iterations = 0; -/* Use GJK to locate origin */ -GJK gjk(stackAlloc, - wtrs0.getBasis(),wtrs0.getOrigin(),shape0, - wtrs1.getBasis(),wtrs1.getOrigin(),shape1, - radialmargin+EPA_accuracy); -const Z collide(gjk.SearchOrigin()); -results.gjk_iterations = static_cast<int>(gjk.iterations+1); -if(collide) - { - /* Then EPA for penetration depth */ - EPA epa(&gjk); - const F pd(epa.EvaluatePD()); - results.epa_iterations = static_cast<int>(epa.iterations+1); - if(pd>0) - { - results.status = sResults::Penetrating; - results.normal = epa.normal; - results.depth = pd; - results.witnesses[0] = epa.nearest[0]; - results.witnesses[1] = epa.nearest[1]; - return(true); - } else { if(epa.failed) results.status=sResults::EPA_Failed; } - } else { if(gjk.failed) results.status=sResults::GJK_Failed; } -return(false); -} - - - - - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h deleted file mode 100644 index 1c256f41939..00000000000 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h +++ /dev/null @@ -1,53 +0,0 @@ -/* -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. -*/ - -/* -GJK-EPA collision solver by Nathanael Presson -Nov.2006 -*/ - - -#ifndef _05E48D53_04E0_49ad_BB0A_D74FE62E7366_ -#define _05E48D53_04E0_49ad_BB0A_D74FE62E7366_ -#include "BulletCollision/CollisionShapes/btConvexShape.h" - -class btStackAlloc; - -///btGjkEpaSolver contributed under zlib by Nathanael Presson -struct btGjkEpaSolver -{ -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; - btScalar depth; - int epa_iterations; - int gjk_iterations; - }; -static bool Collide(const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - btScalar radialmargin, - btStackAlloc* stackAlloc, - sResults& results); -}; - -#endif diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index ada20d3ef7a..f74261d4b21 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -68,7 +68,43 @@ namespace gjkepa2_impl const btConvexShape* m_shapes[2]; btMatrix3x3 m_toshape1; btTransform m_toshape0; +#ifdef __SPU__ + bool m_enableMargin; +#else btVector3 (btConvexShape::*Ls)(const btVector3&) const; +#endif//__SPU__ + + + MinkowskiDiff() + { + + } +#ifdef __SPU__ + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline btVector3 Support0(const btVector3& d) const + { + if (m_enableMargin) + { + return m_shapes[0]->localGetSupportVertexNonVirtual(d); + } else + { + return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d); + } + } + inline btVector3 Support1(const btVector3& d) const + { + if (m_enableMargin) + { + return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d)); + } else + { + return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d)); + } + } +#else void EnableMargin(bool enable) { if(enable) @@ -84,6 +120,8 @@ namespace gjkepa2_impl { return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); } +#endif //__SPU__ + inline btVector3 Support(const btVector3& d) const { return(Support0(d)-Support1(-d)); @@ -202,7 +240,7 @@ namespace gjkepa2_impl lastw[clastw=(clastw+1)&3]=w; } /* Check for termination */ - const btScalar omega=dot(m_ray,w)/rl; + const btScalar omega=btDot(m_ray,w)/rl; alpha=btMax(omega,alpha); if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) {/* Return old simplex */ @@ -259,6 +297,9 @@ namespace gjkepa2_impl { case eStatus::Valid: m_distance=m_ray.length();break; case eStatus::Inside: m_distance=0;break; + default: + { + } } return(m_status); } @@ -288,7 +329,7 @@ namespace gjkepa2_impl { btVector3 axis=btVector3(0,0,0); axis[i]=1; - const btVector3 p=cross(d,axis); + const btVector3 p=btCross(d,axis); if(p.length2()>0) { appendvertice(*m_simplex, p); @@ -303,7 +344,7 @@ namespace gjkepa2_impl break; case 3: { - const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, + const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w, m_simplex->c[2]->w-m_simplex->c[0]->w); if(n.length2()>0) { @@ -357,7 +398,7 @@ namespace gjkepa2_impl const btScalar l=d.length2(); if(l>GJK_SIMPLEX2_EPS) { - const btScalar t(l>0?-dot(a,d)/l:0); + const btScalar t(l>0?-btDot(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()); } @@ -372,16 +413,16 @@ namespace gjkepa2_impl 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 btVector3 n=btCross(dl[0],dl[1]); const btScalar l=n.length2(); if(l>GJK_SIMPLEX3_EPS) { btScalar mindist=-1; - btScalar subw[2]; - U subm; + btScalar subw[2]={0.f,0.f}; + U subm(0); for(U i=0;i<3;++i) { - if(dot(*vt[i],cross(dl[i],n))>0) + if(btDot(*vt[i],btCross(dl[i],n))>0) { const U j=imd3[i]; const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); @@ -397,13 +438,13 @@ namespace gjkepa2_impl } if(mindist<0) { - const btScalar d=dot(a,n); + const btScalar d=btDot(a,n); const btScalar s=btSqrt(l); const btVector3 p=n*(d/l); mindist = p.length2(); m = 7; - w[0] = (cross(dl[1],b-p)).length()/s; - w[1] = (cross(dl[2],c-p)).length()/s; + w[0] = (btCross(dl[1],b-p)).length()/s; + w[1] = (btCross(dl[2],c-p)).length()/s; w[2] = 1-(w[0]+w[1]); } return(mindist); @@ -420,16 +461,16 @@ namespace gjkepa2_impl const btVector3* vt[]={&a,&b,&c,&d}; const btVector3 dl[]={a-d,b-d,c-d}; const btScalar vl=det(dl[0],dl[1],dl[2]); - const bool ng=(vl*dot(a,cross(b-c,a-b)))<=0; + const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0; if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) { btScalar mindist=-1; - btScalar subw[3]; - U subm; + btScalar subw[3]={0.f,0.f,0.f}; + U subm(0); for(U i=0;i<3;++i) { const U j=imd3[i]; - const btScalar s=vl*dot(d,cross(dl[i],dl[j])); + const btScalar s=vl*btDot(d,btCross(dl[i],dl[j])); if(s>0) { const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); @@ -601,7 +642,7 @@ namespace gjkepa2_impl bool valid=true; best->pass = (U1)(++pass); gjk.getsupport(best->n,*w); - const btScalar wdist=dot(best->n,w->w)-best->d; + const btScalar wdist=btDot(best->n,w->w)-best->d; if(wdist>EPA_ACCURACY) { for(U j=0;(j<3)&&valid;++j) @@ -628,11 +669,11 @@ namespace gjkepa2_impl 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, + m_result.p[0] = btCross( outer.c[1]->w-projection, outer.c[2]->w-projection).length(); - m_result.p[1] = cross( outer.c[2]->w-projection, + m_result.p[1] = btCross( outer.c[2]->w-projection, outer.c[0]->w-projection).length(); - m_result.p[2] = cross( outer.c[0]->w-projection, + m_result.p[2] = btCross( 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; @@ -666,18 +707,18 @@ namespace gjkepa2_impl face->c[0] = a; face->c[1] = b; face->c[2] = c; - face->n = cross(b->w-a->w,c->w-a->w); + face->n = btCross(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))) / + btDot(a->w,btCross(face->n,a->w-b->w)), + btDot(b->w,btCross(face->n,b->w-c->w))), + btDot(c->w,btCross(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->d = btDot(a->w,face->n)/l; face->n /= l; if(forced||(face->d>=-EPA_PLANE_EPS)) { @@ -715,7 +756,7 @@ namespace gjkepa2_impl if(f->pass!=pass) { const U e1=i1m3[e]; - if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) { sFace* nf=newface(f->c[e1],f->c[e],w,false); if(nf) @@ -854,10 +895,14 @@ bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, case GJK::eStatus::Failed: results.status=sResults::GJK_Failed; break; + default: + { + } } return(false); } +#ifndef __SPU__ // btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, btScalar margin, @@ -923,6 +968,7 @@ bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, else return(true); } +#endif //__SPU__ /* Symbols cleanup */ diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h index a55214203d3..2296527d7db 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h @@ -55,7 +55,7 @@ static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0, const btVector3& guess, sResults& results, bool usemargins=true); - +#ifndef __SPU__ static btScalar SignedDistance( const btVector3& position, btScalar margin, const btConvexShape* shape, @@ -66,6 +66,8 @@ static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs const btConvexShape* shape1,const btTransform& wtrs1, const btVector3& guess, sResults& results); +#endif //__SPU__ + }; #endif diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 55c23ee8549..c6dc3f3a672 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -25,17 +25,19 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btTransform& transformA, const btTransform& transformB, btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw ) + class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc ) { (void)debugDraw; (void)v; (void)simplexSolver; - const btScalar radialmargin(btScalar(0.)); +// const btScalar radialmargin(btScalar(0.)); btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin()); btGjkEpaSolver2::sResults results; + + if(btGjkEpaSolver2::Penetration(pConvexA,transformA, pConvexB,transformB, guessVector,results)) @@ -45,8 +47,18 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); wWitnessOnA = results.witnesses[0]; wWitnessOnB = results.witnesses[1]; + v = results.normal; return true; + } else + { + if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results)) + { + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return false; } + } return false; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h index 4db18628021..a49689a1501 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -25,11 +25,15 @@ class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver { public : + btGjkEpaPenetrationDepthSolver() + { + } + bool calcPenDepth( btSimplexSolverInterface& simplexSolver, const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btTransform& transformA, const btTransform& transformB, btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw ); + class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ); private : diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 331d25623df..1a561957368 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -38,20 +38,49 @@ int gNumDeepPenetrationChecks = 0; int gNumGjkChecks = 0; - btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)), +:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), +m_penetrationDepthSolver(penetrationDepthSolver), +m_simplexSolver(simplexSolver), +m_minkowskiA(objectA), +m_minkowskiB(objectB), +m_shapeTypeA(objectA->getShapeType()), +m_shapeTypeB(objectB->getShapeType()), +m_marginA(objectA->getMargin()), +m_marginB(objectB->getMargin()), +m_ignoreMargin(false), +m_lastUsedMethod(-1), +m_catchDegeneracies(1) +{ +} +btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) +:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), m_penetrationDepthSolver(penetrationDepthSolver), m_simplexSolver(simplexSolver), m_minkowskiA(objectA), m_minkowskiB(objectB), +m_shapeTypeA(shapeTypeA), +m_shapeTypeB(shapeTypeB), +m_marginA(marginA), +m_marginB(marginB), m_ignoreMargin(false), m_lastUsedMethod(-1), m_catchDegeneracies(1) { } -void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +{ + (void)swapResults; + + getClosestPointsNonVirtual(input,output,debugDraw); +} + +#ifdef __SPU__ +void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +#else +void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +#endif { m_cachedSeparatingDistance = 0.f; @@ -64,21 +93,10 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& localTransA.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset; -#ifdef __SPU__ - btScalar marginA = m_minkowskiA->getMarginNonVirtual(); - btScalar marginB = m_minkowskiB->getMarginNonVirtual(); -#else - btScalar marginA = m_minkowskiA->getMargin(); - btScalar marginB = m_minkowskiB->getMargin(); -#ifdef TEST_NON_VIRTUAL - btScalar marginAv = m_minkowskiA->getMarginNonVirtual(); - btScalar marginBv = m_minkowskiB->getMarginNonVirtual(); - btAssert(marginA == marginAv); - btAssert(marginB == marginBv); -#endif //TEST_NON_VIRTUAL -#endif - + bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d(); + btScalar marginA = m_marginA; + btScalar marginB = m_marginB; gNumGjkChecks++; @@ -107,7 +125,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& m_lastUsedMethod = -1; { - btScalar squaredDistance = SIMD_INFINITY; + btScalar squaredDistance = BT_LARGE_FLOAT; btScalar delta = btScalar(0.); btScalar margin = marginA + marginB; @@ -123,6 +141,15 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); +#if 1 + + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); + +// btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA); +// btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB); + +#else #ifdef __SPU__ btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); @@ -136,6 +163,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& btAssert((qInBv-qInB).length() < 0.0001); #endif // #endif //__SPU__ +#endif + btVector3 pWorld = localTransA(pInA); btVector3 qWorld = localTransB(qInB); @@ -144,12 +173,19 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& spu_printf("got local supporting vertices\n"); #endif + if (check2d) + { + pWorld[2] = 0.f; + qWorld[2] = 0.f; + } + btVector3 w = pWorld - qWorld; delta = m_cachedSeparatingAxis.dot(w); // potential exit, they don't overlap if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) { + m_degenerateSimplex = 10; checkSimplex=true; //checkPenetration = false; break; @@ -171,6 +207,9 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (f0 <= btScalar(0.)) { m_degenerateSimplex = 2; + } else + { + m_degenerateSimplex = 11; } checkSimplex = true; break; @@ -184,24 +223,39 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& #ifdef DEBUG_SPU_COLLISION_DETECTION spu_printf("addVertex 2\n"); #endif + btVector3 newCachedSeparatingAxis; + //calculate the closest point to the origin (update vector v) - if (!m_simplexSolver->closest(m_cachedSeparatingAxis)) + if (!m_simplexSolver->closest(newCachedSeparatingAxis)) { m_degenerateSimplex = 3; checkSimplex = true; break; } - if(m_cachedSeparatingAxis.length2()<REL_ERROR2) + if(newCachedSeparatingAxis.length2()<REL_ERROR2) { + m_cachedSeparatingAxis = newCachedSeparatingAxis; m_degenerateSimplex = 6; checkSimplex = true; break; } btScalar previousSquaredDistance = squaredDistance; - squaredDistance = m_cachedSeparatingAxis.length2(); + squaredDistance = newCachedSeparatingAxis.length2(); +#if 0 +///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo + if (squaredDistance>previousSquaredDistance) + { + m_degenerateSimplex = 7; + squaredDistance = previousSquaredDistance; + checkSimplex = false; + break; + } +#endif // + m_cachedSeparatingAxis = newCachedSeparatingAxis; + //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); //are we getting any closer ? @@ -209,6 +263,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& { m_simplexSolver->backup_closest(m_cachedSeparatingAxis); checkSimplex = true; + m_degenerateSimplex = 12; + break; } @@ -239,6 +295,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& { //do we need this backup_closest here ? m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + m_degenerateSimplex = 13; break; } } @@ -247,7 +304,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& { m_simplexSolver->compute_points(pointOnA, pointOnB); normalInB = pointOnA-pointOnB; - btScalar lenSqr = m_cachedSeparatingAxis.length2(); + btScalar lenSqr =m_cachedSeparatingAxis.length2(); + //valid normal if (lenSqr < 0.0001) { @@ -279,7 +337,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) { //penetration case - + //if there is no way to handle penetrations, bail out if (m_penetrationDepthSolver) { @@ -287,19 +345,27 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& btVector3 tmpPointOnA,tmpPointOnB; gNumDeepPenetrationChecks++; + m_cachedSeparatingAxis.setZero(); bool isValid2 = m_penetrationDepthSolver->calcPenDepth( *m_simplexSolver, m_minkowskiA,m_minkowskiB, localTransA,localTransB, m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, - debugDraw + debugDraw,input.m_stackAlloc ); + if (isValid2) { btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; btScalar lenSqr = tmpNormalInB.length2(); + if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) + { + tmpNormalInB = m_cachedSeparatingAxis; + lenSqr = m_cachedSeparatingAxis.length2(); + } + if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) { tmpNormalInB /= btSqrt(lenSqr); @@ -315,32 +381,62 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& m_lastUsedMethod = 3; } else { - + m_lastUsedMethod = 8; } } else { - //isValid = false; - m_lastUsedMethod = 4; + m_lastUsedMethod = 9; } } else + { - m_lastUsedMethod = 5; + ///this is another degenerate case, where the initial GJK calculation reports a degenerate case + ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) + ///reports a valid positive distance. Use the results of the second GJK instead of failing. + ///thanks to Jacob.Langford for the reproduction case + ///http://code.google.com/p/bullet/issues/detail?id=250 + + + if (m_cachedSeparatingAxis.length2() > btScalar(0.)) + { + btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin; + //only replace valid distances when the distance is less + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + pointOnA -= m_cachedSeparatingAxis * marginA ; + pointOnB += m_cachedSeparatingAxis * marginB ; + normalInB = m_cachedSeparatingAxis; + normalInB.normalize(); + isValid = true; + m_lastUsedMethod = 6; + } else + { + m_lastUsedMethod = 5; + } + } } } + } } - if (isValid) - { -#ifdef __SPU__ - //spu_printf("distance\n"); -#endif //__CELLOS_LV2__ + + if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared))) + { +#if 0 +///some debugging +// if (check2d) + { + printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]); + printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex); + } +#endif -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("output 1\n"); -#endif m_cachedSeparatingAxis = normalInB; m_cachedSeparatingDistance = distance; @@ -349,10 +445,6 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& pointOnB+positionOffset, distance); -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("output 2\n"); -#endif - //printf("gjk add:%f",distance); } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index 0ad4aab8a59..cc6287c86b0 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -36,6 +36,11 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface btSimplexSolverInterface* m_simplexSolver; const btConvexShape* m_minkowskiA; const btConvexShape* m_minkowskiB; + int m_shapeTypeA; + int m_shapeTypeB; + btScalar m_marginA; + btScalar m_marginB; + bool m_ignoreMargin; btScalar m_cachedSeparatingDistance; @@ -50,10 +55,14 @@ public: btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); + btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); virtual ~btGjkPairDetector() {}; virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + void getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw); + + void setMinkowskiA(btConvexShape* minkA) { m_minkowskiA = minkA; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index c7c9812985d..cd310570e06 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -19,7 +19,21 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "LinearMath/btTransformUtil.h" - +#ifdef PFX_USE_FREE_VECTORMATH + #include "physics_effects\base_level\solver\pfx_constraint_row.h" +typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow; +#else + // Don't change following order of parameters + ATTRIBUTE_ALIGNED16(struct) btConstraintRow { + btScalar m_normal[3]; + btScalar m_rhs; + btScalar m_jacDiagInv; + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_accumImpulse; + }; + typedef btConstraintRow PfxConstraintRow; +#endif //PFX_USE_FREE_VECTORMATH @@ -34,6 +48,10 @@ class btManifoldPoint m_lateralFrictionInitialized(false), m_appliedImpulseLateral1(0.f), m_appliedImpulseLateral2(0.f), + m_contactMotion1(0.f), + m_contactMotion2(0.f), + m_contactCFM1(0.f), + m_contactCFM2(0.f), m_lifeTime(0) { } @@ -52,10 +70,15 @@ class btManifoldPoint m_lateralFrictionInitialized(false), m_appliedImpulseLateral1(0.f), m_appliedImpulseLateral2(0.f), + m_contactMotion1(0.f), + m_contactMotion2(0.f), + m_contactCFM1(0.f), + m_contactCFM2(0.f), m_lifeTime(0) { - - + mConstraintRow[0].m_accumImpulse = 0.f; + mConstraintRow[1].m_accumImpulse = 0.f; + mConstraintRow[2].m_accumImpulse = 0.f; } @@ -83,11 +106,21 @@ class btManifoldPoint bool m_lateralFrictionInitialized; btScalar m_appliedImpulseLateral1; btScalar m_appliedImpulseLateral2; + btScalar m_contactMotion1; + btScalar m_contactMotion2; + btScalar m_contactCFM1; + btScalar m_contactCFM2; + int m_lifeTime;//lifetime of the contactpoint in frames btVector3 m_lateralFrictionDir1; btVector3 m_lateralFrictionDir2; + + + btConstraintRow mConstraintRow[3]; + + btScalar getDistance() const { return m_distance1; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index 1fdbb2457d1..fe31f08d61a 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -20,63 +20,20 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #define NUM_UNITSPHERE_POINTS 42 -static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = -{ -btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), -btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), -btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), -btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), -btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), -btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), -btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), -btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), -btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), -btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), -btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), -btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), -btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), -btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), -btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), -btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), -btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), -btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), -btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), -btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), -btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), -btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), -btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), -btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), -btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), -btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), -btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), -btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), -btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), -btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), -btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), -btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), -btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), -btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), -btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), -btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), -btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), -btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), -btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), -btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), -btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), -btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) -}; bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw + class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ) { + (void)stackAlloc; (void)v; + bool check2d= convexA->isConvex2d() && convexB->isConvex2d(); struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result { @@ -90,10 +47,13 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btScalar m_depth; bool m_hasResult; - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + virtual void setShapeIdentifiersA(int partId0,int index0) { (void)partId0; (void)index0; + } + virtual void setShapeIdentifiersB(int partId1,int index1) + { (void)partId1; (void)index1; } @@ -107,7 +67,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s }; //just take fixed number of orientation, and sample the penetration depth in that direction - btScalar minProj = btScalar(1e30); + btScalar minProj = btScalar(BT_LARGE_FLOAT); btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.)); btVector3 minA,minB; btVector3 seperatingAxisInA,seperatingAxisInB; @@ -128,7 +88,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s for (i=0;i<numSampleDirections;i++) { - const btVector3& norm = sPenetrationDirections[i]; + btVector3 norm = getPenetrationDirections()[i]; seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ; seperatingAxisInBBatch[i] = norm * transB.getBasis() ; } @@ -142,7 +102,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btVector3 norm; convexA->getPreferredPenetrationDirection(i,norm); norm = transA.getBasis() * norm; - sPenetrationDirections[numSampleDirections] = norm; + getPenetrationDirections()[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); numSampleDirections++; @@ -159,7 +119,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btVector3 norm; convexB->getPreferredPenetrationDirection(i,norm); norm = transB.getBasis() * norm; - sPenetrationDirections[numSampleDirections] = norm; + getPenetrationDirections()[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); numSampleDirections++; @@ -169,29 +129,44 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s + convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); for (i=0;i<numSampleDirections;i++) { - const btVector3& norm = sPenetrationDirections[i]; - seperatingAxisInA = seperatingAxisInABatch[i]; - seperatingAxisInB = seperatingAxisInBBatch[i]; + btVector3 norm = getPenetrationDirections()[i]; + if (check2d) + { + norm[2] = 0.f; + } + if (norm.length2()>0.01) + { - pInA = supportVerticesABatch[i]; - qInB = supportVerticesBBatch[i]; + seperatingAxisInA = seperatingAxisInABatch[i]; + seperatingAxisInB = seperatingAxisInBBatch[i]; - pWorld = transA(pInA); - qWorld = transB(qInB); - w = qWorld - pWorld; - btScalar delta = norm.dot(w); - //find smallest delta - if (delta < minProj) - { - minProj = delta; - minNorm = norm; - minA = pWorld; - minB = qWorld; + pInA = supportVerticesABatch[i]; + qInB = supportVerticesBBatch[i]; + + pWorld = transA(pInA); + qWorld = transB(qInB); + if (check2d) + { + pWorld[2] = 0.f; + qWorld[2] = 0.f; + } + + w = qWorld - pWorld; + btScalar delta = norm.dot(w); + //find smallest delta + if (delta < minProj) + { + minProj = delta; + minNorm = norm; + minA = pWorld; + minB = qWorld; + } } } #else @@ -208,7 +183,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btVector3 norm; convexA->getPreferredPenetrationDirection(i,norm); norm = transA.getBasis() * norm; - sPenetrationDirections[numSampleDirections] = norm; + getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } } @@ -223,7 +198,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btVector3 norm; convexB->getPreferredPenetrationDirection(i,norm); norm = transB.getBasis() * norm; - sPenetrationDirections[numSampleDirections] = norm; + getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } } @@ -232,7 +207,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s for (int i=0;i<numSampleDirections;i++) { - const btVector3& norm = sPenetrationDirections[i]; + const btVector3& norm = getPenetrationDirections()[i]; seperatingAxisInA = (-norm)* transA.getBasis(); seperatingAxisInB = norm* transB.getBasis(); pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); @@ -260,7 +235,8 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s if (minProj < btScalar(0.)) return false; - minProj += (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); + btScalar extraSeparation = 0.5f;///scale dependent + minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); @@ -298,9 +274,10 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s input.m_transformA = displacedTrans; input.m_transformB = transB; - input.m_maximumDistanceSquared = btScalar(1e30);//minProj; + input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj; btIntermediateResult res; + gjkdet.setCachedSeperatingAxis(-minNorm); gjkdet.getClosestPoints(input,res,debugDraw); btScalar correctedMinNorm = minProj - res.m_depth; @@ -309,12 +286,14 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s //the penetration depth is over-estimated, relax it btScalar penetration_relaxation= btScalar(1.); minNorm*=penetration_relaxation; + if (res.m_hasResult) { pa = res.m_pointInWorld - minNorm * correctedMinNorm; pb = res.m_pointInWorld; + v = minNorm; #ifdef DEBUG_DRAW if (debugDraw) @@ -329,5 +308,55 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s return res.m_hasResult; } +btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections() +{ + static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = + { + btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), + btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), + btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), + btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), + btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), + btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), + btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), + btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), + btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), + btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), + btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), + btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), + btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), + btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), + btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), + btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), + btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), + btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), + btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), + btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), + btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), + btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), + btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), + btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), + btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), + btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), + btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), + btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), + btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), + btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), + btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), + btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), + btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), + btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), + btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), + btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), + btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), + btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), + btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), + btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), + btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), + btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) + }; + + return sPenetrationDirections; +} diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h index e93e4e4bb4e..7b6c8a63779 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h @@ -22,13 +22,17 @@ subject to the following restrictions: ///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver { +protected: + + static btVector3* getPenetrationDirections(); + public: virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw + class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ); }; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index eecf927ee10..924a8af87d1 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -25,7 +25,8 @@ ContactProcessedCallback gContactProcessedCallback = 0; btPersistentManifold::btPersistentManifold() -:m_body0(0), +:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), +m_body0(0), m_body1(0), m_cachedPoints (0), m_index1a(0) @@ -172,9 +173,6 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) #if MANIFOLD_CACHE_SIZE >= 4 //sort cache so best points come first, based on area insertIndex = sortCachedPoints(newPoint); - - if (insertIndex<0) - insertIndex=0; #else insertIndex = 0; #endif @@ -183,7 +181,11 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) } else { m_cachedPoints++; + + } + if (insertIndex<0) + insertIndex=0; btAssert(m_pointCache[insertIndex].m_userPersistentData==0); m_pointCache[insertIndex] = newPoint; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 0b3c734d1d7..c8aac637307 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -30,9 +30,14 @@ extern btScalar gContactBreakingThreshold; typedef bool (*ContactDestroyedCallback)(void* userPersistentData); typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1); extern ContactDestroyedCallback gContactDestroyedCallback; +extern ContactProcessedCallback gContactProcessedCallback; - - +//the enum starts at 1024 to avoid type conflicts with btTypedConstraint +enum btContactManifoldTypes +{ + MIN_CONTACT_MANIFOLD_TYPE = 1024, + BT_PERSISTENT_MANIFOLD_TYPE +}; #define MANIFOLD_CACHE_SIZE 4 @@ -43,7 +48,10 @@ extern ContactDestroyedCallback gContactDestroyedCallback; ///reduces the cache to 4 points, when more then 4 points are added, using following rules: ///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points ///note that some pairs of objects might have more then one contact manifold. -ATTRIBUTE_ALIGNED16( class) btPersistentManifold + + +ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject +//ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject { btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]; @@ -52,6 +60,7 @@ ATTRIBUTE_ALIGNED16( class) btPersistentManifold /// void* will allow any rigidbody class void* m_body0; void* m_body1; + int m_cachedPoints; btScalar m_contactBreakingThreshold; @@ -67,16 +76,19 @@ public: BT_DECLARE_ALIGNED_ALLOCATOR(); + int m_companionIdA; + int m_companionIdB; + int m_index1a; btPersistentManifold(); btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold) - : m_body0(body0),m_body1(body1),m_cachedPoints(0), + : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), + m_body0(body0),m_body1(body1),m_cachedPoints(0), m_contactBreakingThreshold(contactBreakingThreshold), m_contactProcessingThreshold(contactProcessingThreshold) { - } SIMD_FORCE_INLINE void* getBody0() { return m_body0;} @@ -134,6 +146,10 @@ public: m_pointCache[index] = m_pointCache[lastUsedIndex]; //get rid of duplicated userPersistentData pointer m_pointCache[lastUsedIndex].m_userPersistentData = 0; + m_pointCache[lastUsedIndex].mConstraintRow[0].m_accumImpulse = 0.f; + m_pointCache[lastUsedIndex].mConstraintRow[1].m_accumImpulse = 0.f; + m_pointCache[lastUsedIndex].mConstraintRow[2].m_accumImpulse = 0.f; + m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f; m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false; m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f; @@ -151,10 +167,13 @@ public: #define MAINTAIN_PERSISTENCY 1 #ifdef MAINTAIN_PERSISTENCY int lifeTime = m_pointCache[insertIndex].getLifeTime(); - btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse; - btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; - btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; - + btScalar appliedImpulse = m_pointCache[insertIndex].mConstraintRow[0].m_accumImpulse; + btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].mConstraintRow[1].m_accumImpulse; + btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].mConstraintRow[2].m_accumImpulse; +// bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized; + + + btAssert(lifeTime>=0); void* cache = m_pointCache[insertIndex].m_userPersistentData; @@ -165,6 +184,11 @@ public: m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; + m_pointCache[insertIndex].mConstraintRow[0].m_accumImpulse = appliedImpulse; + m_pointCache[insertIndex].mConstraintRow[1].m_accumImpulse = appliedLateralImpulse1; + m_pointCache[insertIndex].mConstraintRow[2].m_accumImpulse = appliedLateralImpulse2; + + m_pointCache[insertIndex].m_lifeTime = lifeTime; #else clearUserCache(m_pointCache[insertIndex]); @@ -175,7 +199,12 @@ public: bool validContactDistance(const btManifoldPoint& pt) const { - return pt.m_distance1 <= getContactBreakingThreshold(); + if (pt.m_lifeTime >1) + { + return pt.m_distance1 <= getContactBreakingThreshold(); + } + return pt.m_distance1 <= getContactProcessingThreshold(); + } /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin void refreshContactPoints( const btTransform& trA,const btTransform& trB); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h index 6262f44b9f1..6ca60548e71 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h @@ -31,17 +31,20 @@ struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result bool m_hasResult; btPointCollector () - : m_distance(btScalar(1e30)),m_hasResult(false) + : m_distance(btScalar(BT_LARGE_FLOAT)),m_hasResult(false) { } - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + virtual void setShapeIdentifiersA(int partId0,int index0) { (void)partId0; (void)index0; + + } + virtual void setShapeIdentifiersB(int partId1,int index1) + { (void)partId1; (void)index1; - //?? } virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index cdb1d22444d..fbe579ce1e5 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -124,8 +124,9 @@ btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* m_convexShapeFrom = convexShapeFrom; m_convexShapeTo = convexShapeTo; m_triangleToWorld = triangleToWorld; - m_hitFraction = 1.0; - m_triangleCollisionMargin = triangleCollisionMargin; + m_hitFraction = 1.0f; + m_triangleCollisionMargin = triangleCollisionMargin; + m_allowedPenetration = 0.f; } void @@ -148,6 +149,7 @@ btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, btConvexCast::CastResult castResult; castResult.m_fraction = btScalar(1.); + castResult.m_allowedPenetration = m_allowedPenetration; if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult)) { //add hit diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index 3a1ab388c13..bdd1add36d2 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -58,7 +58,8 @@ public: btTransform m_convexShapeTo; btTransform m_triangleToWorld; btScalar m_hitFraction; - btScalar m_triangleCollisionMargin; + btScalar m_triangleCollisionMargin; + btScalar m_allowedPenetration; btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index 4c709a8c3a9..18eb662de2f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -114,7 +114,10 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( hasResult = true; } } - m_simplexSolver->addVertex( w, supVertexA , supVertexB); + ///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc. + if (!m_simplexSolver->inSimplex(w)) + m_simplexSolver->addVertex( w, supVertexA , supVertexB); + if (m_simplexSolver->closest(v)) { dist2 = v.length2(); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp index a7ffeda8c62..a775198ab29 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -68,7 +68,7 @@ void btVoronoiSimplexSolver::reset() m_cachedValidClosest = false; m_numVertices = 0; m_needsUpdate = true; - m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); m_cachedBC.reset(); } @@ -289,7 +289,11 @@ bool btVoronoiSimplexSolver::inSimplex(const btVector3& w) //w is in the current (reduced) simplex for (i=0;i<numverts;i++) { +#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD + if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold) +#else if (m_simplexVectorW[i] == w) +#endif found = true; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h index d3162d9fbeb..9a4f552924c 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -24,6 +24,11 @@ subject to the following restrictions: #define VORONOI_SIMPLEX_MAX_VERTS 5 +///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure +#define BT_USE_EQUAL_VERTEX_THRESHOLD +#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f + + struct btUsageBitfield{ btUsageBitfield() { @@ -106,8 +111,11 @@ public: btVector3 m_cachedP2; btVector3 m_cachedV; btVector3 m_lastW; + + btScalar m_equalVertexThreshold; bool m_cachedValidClosest; + btSubSimplexClosestResult m_cachedBC; bool m_needsUpdate; @@ -122,10 +130,23 @@ public: public: + btVoronoiSimplexSolver() + : m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD) + { + } void reset(); void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); + void setEqualVertexThreshold(btScalar threshold) + { + m_equalVertexThreshold = threshold; + } + + btScalar getEqualVertexThreshold() const + { + return m_equalVertexThreshold; + } bool closest(btVector3& v); diff --git a/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h b/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h new file mode 100644 index 00000000000..19373daa241 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h @@ -0,0 +1,45 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef CHARACTER_CONTROLLER_INTERFACE_H +#define CHARACTER_CONTROLLER_INTERFACE_H + +#include "LinearMath/btVector3.h" +#include "BulletDynamics/Dynamics/btActionInterface.h" + +class btCollisionShape; +class btRigidBody; +class btCollisionWorld; + +class btCharacterControllerInterface : public btActionInterface +{ +public: + btCharacterControllerInterface () {}; + virtual ~btCharacterControllerInterface () {}; + + virtual void setWalkDirection(const btVector3& walkDirection) = 0; + virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0; + virtual void reset () = 0; + virtual void warp (const btVector3& origin) = 0; + + virtual void preStep ( btCollisionWorld* collisionWorld) = 0; + virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0; + virtual bool canJump () const = 0; + virtual void jump () = 0; + + virtual bool onGround () const = 0; +}; + +#endif diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp new file mode 100644 index 00000000000..9732553130d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -0,0 +1,641 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +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 "LinearMath/btIDebugDraw.h" +#include "BulletCollision/CollisionDispatch/btGhostObject.h" +#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "LinearMath/btDefaultMotionState.h" +#include "btKinematicCharacterController.h" + + +// static helper method +static btVector3 +getNormalizedVector(const btVector3& v) +{ + btVector3 n = v.normalized(); + if (n.length() < SIMD_EPSILON) { + n.setValue(0, 0, 0); + } + return n; +} + + +///@todo Interact with dynamic objects, +///Ride kinematicly animated platforms properly +///More realistic (or maybe just a config option) falling +/// -> Should integrate falling velocity manually and use that in stepDown() +///Support jumping +///Support ducking +class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback +{ +public: + btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) + { + m_me = me; + } + + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) + { + if (rayResult.m_collisionObject == m_me) + return 1.0; + + return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace); + } +protected: + btCollisionObject* m_me; +}; + +class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback +{ +public: + btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, const btVector3& up, btScalar minSlopeDot) + : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) + , m_me(me) + , m_up(up) + , m_minSlopeDot(minSlopeDot) + { + } + + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) + { + if (convexResult.m_hitCollisionObject == m_me) + return btScalar(1.0); + + btVector3 hitNormalWorld; + if (normalInWorldSpace) + { + hitNormalWorld = convexResult.m_hitNormalLocal; + } else + { + ///need to transform normal into worldspace + hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal; + } + + btScalar dotUp = m_up.dot(hitNormalWorld); + if (dotUp < m_minSlopeDot) { + return btScalar(1.0); + } + + return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); + } +protected: + btCollisionObject* m_me; + const btVector3 m_up; + btScalar m_minSlopeDot; +}; + +/* + * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal' + * + * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html + */ +btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal) +{ + return direction - (btScalar(2.0) * direction.dot(normal)) * normal; +} + +/* + * Returns the portion of 'direction' that is parallel to 'normal' + */ +btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal) +{ + btScalar magnitude = direction.dot(normal); + return normal * magnitude; +} + +/* + * Returns the portion of 'direction' that is perpindicular to 'normal' + */ +btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal) +{ + return direction - parallelComponent(direction, normal); +} + +btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis) +{ + m_upAxis = upAxis; + m_addedMargin = 0.02; + m_walkDirection.setValue(0,0,0); + m_useGhostObjectSweepTest = true; + m_ghostObject = ghostObject; + m_stepHeight = stepHeight; + m_turnAngle = btScalar(0.0); + m_convexShape=convexShape; + m_useWalkDirection = true; // use walk direction by default, legacy behavior + m_velocityTimeInterval = 0.0; + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + m_gravity = 9.8 * 3 ; // 3G acceleration. + m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s. + m_jumpSpeed = 10.0; // ? + m_wasOnGround = false; + m_wasJumping = false; + setMaxSlope(btRadians(45.0)); +} + +btKinematicCharacterController::~btKinematicCharacterController () +{ +} + +btPairCachingGhostObject* btKinematicCharacterController::getGhostObject() +{ + return m_ghostObject; +} + +bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld) +{ + + bool penetration = false; + + collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher()); + + m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); + + btScalar maxPen = btScalar(0.0); + for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) + { + m_manifoldArray.resize(0); + + btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i]; + + if (collisionPair->m_algorithm) + collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray); + + + for (int j=0;j<m_manifoldArray.size();j++) + { + btPersistentManifold* manifold = m_manifoldArray[j]; + btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0); + for (int p=0;p<manifold->getNumContacts();p++) + { + const btManifoldPoint&pt = manifold->getContactPoint(p); + + btScalar dist = pt.getDistance(); + + if (dist < 0.0) + { + if (dist < maxPen) + { + maxPen = dist; + m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? + + } + m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2); + penetration = true; + } else { + //printf("touching %f\n", dist); + } + } + + //manifold->clearManifold(); + } + } + btTransform newTrans = m_ghostObject->getWorldTransform(); + newTrans.setOrigin(m_currentPosition); + m_ghostObject->setWorldTransform(newTrans); +// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]); + return penetration; +} + +void btKinematicCharacterController::stepUp ( btCollisionWorld* world) +{ + // phase 1: up + btTransform start, end; + m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f)); + + start.setIdentity (); + end.setIdentity (); + + /* FIXME: Handle penetration properly */ + start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin)); + end.setOrigin (m_targetPosition); + + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071)); + callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; + + if (m_useGhostObjectSweepTest) + { + m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); + } + else + { + world->convexSweepTest (m_convexShape, start, end, callback); + } + + if (callback.hasHit()) + { + // Only modify the position if the hit was a slope and not a wall or ceiling. + if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0) + { + // we moved up only a fraction of the step height + m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction; + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + } + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + } else { + m_currentStepOffset = m_stepHeight; + m_currentPosition = m_targetPosition; + } +} + +void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) +{ + btVector3 movementDirection = m_targetPosition - m_currentPosition; + btScalar movementLength = movementDirection.length(); + if (movementLength>SIMD_EPSILON) + { + movementDirection.normalize(); + + btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal); + reflectDir.normalize(); + + btVector3 parallelDir, perpindicularDir; + + parallelDir = parallelComponent (reflectDir, hitNormal); + perpindicularDir = perpindicularComponent (reflectDir, hitNormal); + + m_targetPosition = m_currentPosition; + if (0)//tangentMag != 0.0) + { + btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength); +// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]); + m_targetPosition += parComponent; + } + + if (normalMag != 0.0) + { + btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength); +// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]); + m_targetPosition += perpComponent; + } + } else + { +// printf("movementLength don't normalize a zero vector\n"); + } +} + +void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove) +{ + // printf("m_normalizedDirection=%f,%f,%f\n", + // m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]); + // phase 2: forward and strafe + btTransform start, end; + m_targetPosition = m_currentPosition + walkMove; + + start.setIdentity (); + end.setIdentity (); + + btScalar fraction = 1.0; + btScalar distance2 = (m_currentPosition-m_targetPosition).length2(); +// printf("distance2=%f\n",distance2); + + if (m_touchingContact) + { + if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) + { + updateTargetPositionBasedOnCollision (m_touchingNormal); + } + } + + int maxIter = 10; + + while (fraction > btScalar(0.01) && maxIter-- > 0) + { + start.setOrigin (m_currentPosition); + end.setOrigin (m_targetPosition); + btVector3 sweepDirNegative(m_currentPosition - m_targetPosition); + + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0)); + callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; + + + btScalar margin = m_convexShape->getMargin(); + m_convexShape->setMargin(margin + m_addedMargin); + + + if (m_useGhostObjectSweepTest) + { + m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } else + { + collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } + + m_convexShape->setMargin(margin); + + + fraction -= callback.m_closestHitFraction; + + if (callback.hasHit()) + { + // we moved only a fraction + btScalar hitDistance; + hitDistance = (callback.m_hitPointWorld - m_currentPosition).length(); + +// m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + + updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld); + btVector3 currentDir = m_targetPosition - m_currentPosition; + distance2 = currentDir.length2(); + if (distance2 > SIMD_EPSILON) + { + currentDir.normalize(); + /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */ + if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0)) + { + break; + } + } else + { +// printf("currentDir: don't normalize a zero vector\n"); + break; + } + + } else { + // we moved whole way + m_currentPosition = m_targetPosition; + } + + // if (callback.m_closestHitFraction == 0.f) + // break; + + } +} + +void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt) +{ + btTransform start, end; + + // phase 3: down + /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0; + btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep); + btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt; + btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; + m_targetPosition -= (step_drop + gravity_drop);*/ + + btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; + if(downVelocity > 0.0 && downVelocity < m_stepHeight + && (m_wasOnGround || !m_wasJumping)) + { + downVelocity = m_stepHeight; + } + + btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + m_targetPosition -= step_drop; + + start.setIdentity (); + end.setIdentity (); + + start.setOrigin (m_currentPosition); + end.setOrigin (m_targetPosition); + + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; + + if (m_useGhostObjectSweepTest) + { + m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } else + { + collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } + + if (callback.hasHit()) + { + // we dropped a fraction of the height -> hit floor + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + m_wasJumping = false; + } else { + // we dropped the full height + + m_currentPosition = m_targetPosition; + } +} + + + +void btKinematicCharacterController::setWalkDirection +( +const btVector3& walkDirection +) +{ + m_useWalkDirection = true; + m_walkDirection = walkDirection; + m_normalizedDirection = getNormalizedVector(m_walkDirection); +} + + + +void btKinematicCharacterController::setVelocityForTimeInterval +( +const btVector3& velocity, +btScalar timeInterval +) +{ +// printf("setVelocity!\n"); +// printf(" interval: %f\n", timeInterval); +// printf(" velocity: (%f, %f, %f)\n", +// velocity.x(), velocity.y(), velocity.z()); + + m_useWalkDirection = false; + m_walkDirection = velocity; + m_normalizedDirection = getNormalizedVector(m_walkDirection); + m_velocityTimeInterval = timeInterval; +} + + + +void btKinematicCharacterController::reset () +{ +} + +void btKinematicCharacterController::warp (const btVector3& origin) +{ + btTransform xform; + xform.setIdentity(); + xform.setOrigin (origin); + m_ghostObject->setWorldTransform (xform); +} + + +void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld) +{ + + int numPenetrationLoops = 0; + m_touchingContact = false; + while (recoverFromPenetration (collisionWorld)) + { + numPenetrationLoops++; + m_touchingContact = true; + if (numPenetrationLoops > 4) + { + //printf("character could not recover from penetration = %d\n", numPenetrationLoops); + break; + } + } + + m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); + m_targetPosition = m_currentPosition; +// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]); + + +} + +#include <stdio.h> + +void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt) +{ +// printf("playerStep(): "); +// printf(" dt = %f", dt); + + // quick check... + if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) { +// printf("\n"); + return; // no motion + } + + m_wasOnGround = onGround(); + + // Update fall velocity. + m_verticalVelocity -= m_gravity * dt; + if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) + { + m_verticalVelocity = m_jumpSpeed; + } + if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) + { + m_verticalVelocity = -btFabs(m_fallSpeed); + } + m_verticalOffset = m_verticalVelocity * dt; + + + btTransform xform; + xform = m_ghostObject->getWorldTransform (); + +// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]); +// printf("walkSpeed=%f\n",walkSpeed); + + stepUp (collisionWorld); + if (m_useWalkDirection) { + stepForwardAndStrafe (collisionWorld, m_walkDirection); + } else { + //printf(" time: %f", m_velocityTimeInterval); + // still have some time left for moving! + btScalar dtMoving = + (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval; + m_velocityTimeInterval -= dt; + + // how far will we move while we are moving? + btVector3 move = m_walkDirection * dtMoving; + + //printf(" dtMoving: %f", dtMoving); + + // okay, step + stepForwardAndStrafe(collisionWorld, move); + } + stepDown (collisionWorld, dt); + + // printf("\n"); + + xform.setOrigin (m_currentPosition); + m_ghostObject->setWorldTransform (xform); +} + +void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) +{ + m_fallSpeed = fallSpeed; +} + +void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed) +{ + m_jumpSpeed = jumpSpeed; +} + +void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight) +{ + m_maxJumpHeight = maxJumpHeight; +} + +bool btKinematicCharacterController::canJump () const +{ + return onGround(); +} + +void btKinematicCharacterController::jump () +{ + if (!canJump()) + return; + + m_verticalVelocity = m_jumpSpeed; + m_wasJumping = true; + +#if 0 + currently no jumping. + btTransform xform; + m_rigidBody->getMotionState()->getWorldTransform (xform); + btVector3 up = xform.getBasis()[1]; + up.normalize (); + btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0); + m_rigidBody->applyCentralImpulse (up * magnitude); +#endif +} + +void btKinematicCharacterController::setGravity(btScalar gravity) +{ + m_gravity = gravity; +} + +btScalar btKinematicCharacterController::getGravity() const +{ + return m_gravity; +} + +void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians) +{ + m_maxSlopeRadians = slopeRadians; + m_maxSlopeCosine = btCos(slopeRadians); +} + +btScalar btKinematicCharacterController::getMaxSlope() const +{ + return m_maxSlopeRadians; +} + +bool btKinematicCharacterController::onGround () const +{ + return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0; +} + + +btVector3* btKinematicCharacterController::getUpAxisDirections() +{ + static btVector3 sUpAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) }; + + return sUpAxisDirection; +} + +void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer) +{ +} diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h new file mode 100644 index 00000000000..704355c0ae1 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h @@ -0,0 +1,162 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef KINEMATIC_CHARACTER_CONTROLLER_H +#define KINEMATIC_CHARACTER_CONTROLLER_H + +#include "LinearMath/btVector3.h" + +#include "btCharacterControllerInterface.h" + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" + + +class btCollisionShape; +class btRigidBody; +class btCollisionWorld; +class btCollisionDispatcher; +class btPairCachingGhostObject; + +///btKinematicCharacterController is an object that supports a sliding motion in a world. +///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations. +///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user. +class btKinematicCharacterController : public btCharacterControllerInterface +{ +protected: + + btScalar m_halfHeight; + + btPairCachingGhostObject* m_ghostObject; + btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast + + btScalar m_verticalVelocity; + btScalar m_verticalOffset; + btScalar m_fallSpeed; + btScalar m_jumpSpeed; + btScalar m_maxJumpHeight; + btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value) + btScalar m_maxSlopeCosine; // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization) + btScalar m_gravity; + + btScalar m_turnAngle; + + btScalar m_stepHeight; + + btScalar m_addedMargin;//@todo: remove this and fix the code + + ///this is the desired walk direction, set by the user + btVector3 m_walkDirection; + btVector3 m_normalizedDirection; + + //some internal variables + btVector3 m_currentPosition; + btScalar m_currentStepOffset; + btVector3 m_targetPosition; + + ///keep track of the contact manifolds + btManifoldArray m_manifoldArray; + + bool m_touchingContact; + btVector3 m_touchingNormal; + + bool m_wasOnGround; + bool m_wasJumping; + bool m_useGhostObjectSweepTest; + bool m_useWalkDirection; + btScalar m_velocityTimeInterval; + int m_upAxis; + + static btVector3* getUpAxisDirections(); + + btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal); + btVector3 parallelComponent (const btVector3& direction, const btVector3& normal); + btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal); + + bool recoverFromPenetration ( btCollisionWorld* collisionWorld); + void stepUp (btCollisionWorld* collisionWorld); + void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0)); + void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove); + void stepDown (btCollisionWorld* collisionWorld, btScalar dt); +public: + btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1); + ~btKinematicCharacterController (); + + + ///btActionInterface interface + virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime) + { + preStep ( collisionWorld); + playerStep (collisionWorld, deltaTime); + } + + ///btActionInterface interface + void debugDraw(btIDebugDraw* debugDrawer); + + void setUpAxis (int axis) + { + if (axis < 0) + axis = 0; + if (axis > 2) + axis = 2; + m_upAxis = axis; + } + + /// This should probably be called setPositionIncrementPerSimulatorStep. + /// This is neither a direction nor a velocity, but the amount to + /// increment the position each simulation iteration, regardless + /// of dt. + /// This call will reset any velocity set by setVelocityForTimeInterval(). + virtual void setWalkDirection(const btVector3& walkDirection); + + /// Caller provides a velocity with which the character should move for + /// the given time period. After the time period, velocity is reset + /// to zero. + /// This call will reset any walk direction set by setWalkDirection(). + /// Negative time intervals will result in no motion. + virtual void setVelocityForTimeInterval(const btVector3& velocity, + btScalar timeInterval); + + void reset (); + void warp (const btVector3& origin); + + void preStep ( btCollisionWorld* collisionWorld); + void playerStep ( btCollisionWorld* collisionWorld, btScalar dt); + + void setFallSpeed (btScalar fallSpeed); + void setJumpSpeed (btScalar jumpSpeed); + void setMaxJumpHeight (btScalar maxJumpHeight); + bool canJump () const; + + void jump (); + + void setGravity(btScalar gravity); + btScalar getGravity() const; + + /// The max slope determines the maximum angle that the controller can walk up. + /// The slope angle is measured in radians. + void setMaxSlope(btScalar slopeRadians); + btScalar getMaxSlope() const; + + btPairCachingGhostObject* getGhostObject(); + void setUseGhostSweepTest(bool useGhostObjectSweepTest) + { + m_useGhostObjectSweepTest = useGhostObjectSweepTest; + } + + bool onGround () const; +}; + +#endif // KINEMATIC_CHARACTER_CONTROLLER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 07262b95b73..bc371e4062d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -29,14 +29,15 @@ Written by: Marcus Hennix #define CONETWIST_DEF_FIX_THRESH btScalar(.05f) - -btConeTwistConstraint::btConeTwistConstraint() -:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE), -m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) +SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld) { + btVector3 vec = axis * invInertiaWorld; + return axis.dot(vec); } + + btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame,const btTransform& rbBFrame) :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), @@ -67,11 +68,13 @@ void btConeTwistConstraint::init() setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); m_damping = btScalar(0.01); m_fixThresh = CONETWIST_DEF_FIX_THRESH; + m_flags = 0; + m_linCFM = btScalar(0.f); + m_linERP = btScalar(0.7f); + m_angCFM = btScalar(0.f); } - - void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) { if (m_useSolveConstraintObsolete) @@ -83,7 +86,7 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) { info->m_numConstraintRows = 3; info->nub = 3; - calcAngleInfo2(); + calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); if(m_solveSwingLimit) { info->m_numConstraintRows++; @@ -101,22 +104,31 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) } } } - +void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info) +{ + //always reserve 6 rows: object transform is not available on SPU + info->m_numConstraintRows = 6; + info->nub = 0; + +} + void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) { + getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); +} + +void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) +{ + calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB); + btAssert(!m_useSolveConstraintObsolete); - //retrieve matrices - btTransform body0_trans; - body0_trans = m_rbA.getCenterOfMassTransform(); - btTransform body1_trans; - body1_trans = m_rbB.getCenterOfMassTransform(); // set jacobian info->m_J1linearAxis[0] = 1; info->m_J1linearAxis[info->rowskip+1] = 1; info->m_J1linearAxis[2*info->rowskip+2] = 1; - btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin(); + btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); @@ -124,7 +136,7 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) btVector3 a1neg = -a1; a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); } - btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin(); + btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); @@ -132,13 +144,18 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) a2.getSkewSymmetricMatrix(angular0,angular1,angular2); } // set right hand side - btScalar k = info->fps * info->erp; + btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp; + btScalar k = info->fps * linERP; int j; for (j=0; j<3; j++) { - info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); + info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]); info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY; info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY; + if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM) + { + info->cfm[j*info->rowskip] = m_linCFM; + } } int row = 3; int srow = row * info->rowskip; @@ -150,7 +167,7 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) btScalar *J2 = info->m_J2angularAxis; if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) { - btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame; + btTransform trA = transA*m_rbAFrame; btVector3 p = trA.getBasis().getColumn(1); btVector3 q = trA.getBasis().getColumn(2); int srow1 = srow + info->rowskip; @@ -187,7 +204,10 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) btScalar k = info->fps * m_biasFactor; info->m_constraintError[srow] = k * m_swingCorrection; - info->cfm[srow] = 0.0f; + if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) + { + info->cfm[srow] = m_angCFM; + } // m_swingCorrection is always positive or 0 info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; @@ -207,7 +227,10 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) J2[srow+2] = -ax1[2]; btScalar k = info->fps * m_biasFactor; info->m_constraintError[srow] = k * m_twistCorrection; - info->cfm[srow] = 0.0f; + if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) + { + info->cfm[srow] = m_angCFM; + } if(m_twistSpan > 0.0f) { @@ -275,14 +298,15 @@ void btConeTwistConstraint::buildJacobian() } } - calcAngleInfo2(); + calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); } } -void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +void btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep) { + #ifndef __SPU__ if (m_useSolveConstraintObsolete) { btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); @@ -297,9 +321,9 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 vel1; - bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); btVector3 vel2; - bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); btVector3 vel = vel1 - vel2; for (int i=0;i<3;i++) @@ -316,8 +340,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver btVector3 ftorqueAxis1 = rel_pos1.cross(normal); btVector3 ftorqueAxis2 = rel_pos2.cross(normal); - bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); - bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); + bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); + bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); } } @@ -328,8 +352,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver // compute current and predicted transforms btTransform trACur = m_rbA.getCenterOfMassTransform(); btTransform trBCur = m_rbB.getCenterOfMassTransform(); - btVector3 omegaA; bodyA.getAngularVelocity(omegaA); - btVector3 omegaB; bodyB.getAngularVelocity(omegaB); + btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA); + btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB); btTransform trAPred; trAPred.setIdentity(); btVector3 zerovec(0,0,0); btTransformUtil::integrateTransform( @@ -403,15 +427,15 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver btScalar impulseMag = impulse.length(); btVector3 impulseAxis = impulse / impulseMag; - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); + bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); } } else if (m_damping > SIMD_EPSILON) // no motor: do a little damping { - btVector3 angVelA; bodyA.getAngularVelocity(angVelA); - btVector3 angVelB; bodyB.getAngularVelocity(angVelB); + btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA); + btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB); btVector3 relVel = angVelB - angVelA; if (relVel.length2() > SIMD_EPSILON) { @@ -423,8 +447,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver btScalar impulseMag = impulse.length(); btVector3 impulseAxis = impulse / impulseMag; - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); + bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); } } @@ -432,9 +456,9 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver { ///solve angular part btVector3 angVelA; - bodyA.getAngularVelocity(angVelA); + bodyA.internalGetAngularVelocity(angVelA); btVector3 angVelB; - bodyB.getAngularVelocity(angVelB); + bodyB.internalGetAngularVelocity(angVelB); // solve swing limit if (m_solveSwingLimit) @@ -463,8 +487,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver impulseMag = impulse.length(); btVector3 noTwistSwingAxis = impulse / impulseMag; - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); + bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); } @@ -484,16 +508,19 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver btVector3 impulse = m_twistAxis * impulseMag; - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); + bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); + bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); } } } - +#else +btAssert(0); +#endif //__SPU__ } + void btConeTwistConstraint::updateRHS(btScalar timeStep) { (void)timeStep; @@ -501,7 +528,7 @@ void btConeTwistConstraint::updateRHS(btScalar timeStep) } - +#ifndef __SPU__ void btConeTwistConstraint::calcAngleInfo() { m_swingCorrection = btScalar(0.); @@ -587,13 +614,13 @@ void btConeTwistConstraint::calcAngleInfo() } } } - +#endif //__SPU__ static btVector3 vTwist(1,0,0); // twist axis in constraint's space -void btConeTwistConstraint::calcAngleInfo2() +void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) { m_swingCorrection = btScalar(0.); m_twistLimitSign = btScalar(0.); @@ -601,13 +628,13 @@ void btConeTwistConstraint::calcAngleInfo2() m_solveSwingLimit = false; // compute rotation of A wrt B (in constraint space) if (m_bMotorEnabled && (!m_useSolveConstraintObsolete)) - { // it is assumed that setMotorTarget() was already called + { // it is assumed that setMotorTarget() was alredy called // and motor target m_qTarget is within constraint limits // TODO : split rotation to pure swing and pure twist // compute desired transforms in world btTransform trPose(m_qTarget); - btTransform trA = getRigidBodyA().getCenterOfMassTransform() * m_rbAFrame; - btTransform trB = getRigidBodyB().getCenterOfMassTransform() * m_rbBFrame; + btTransform trA = transA * m_rbAFrame; + btTransform trB = transB * m_rbBFrame; btTransform trDeltaAB = trB * trPose * trA.inverse(); btQuaternion qDeltaAB = trDeltaAB.getRotation(); btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); @@ -624,8 +651,8 @@ void btConeTwistConstraint::calcAngleInfo2() { // compute rotation of A wrt B (in constraint space) - btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation(); - btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation(); + btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation(); + btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation(); btQuaternion qAB = qB.inverse() * qA; // split rotation into cone and twist // (all this is done from B's perspective. Maybe I should be averaging axes...) @@ -664,8 +691,8 @@ void btConeTwistConstraint::calcAngleInfo2() m_twistAxisA.setValue(0,0,0); m_kSwing = btScalar(1.) / - (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) + - getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis)); + (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) + + computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB)); } } else @@ -673,10 +700,10 @@ void btConeTwistConstraint::calcAngleInfo2() // you haven't set any limits; // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?) // anyway, we have either hinge or fixed joint - btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); - btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); - btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0); + btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); + btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); + btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2); + btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0); btVector3 target; btScalar x = ivB.dot(ivA); btScalar y = ivB.dot(jvA); @@ -767,8 +794,8 @@ void btConeTwistConstraint::calcAngleInfo2() m_twistAxis = quatRotate(qB, -twistAxis); m_kTwist = btScalar(1.) / - (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + - getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); + (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) + + computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB)); } if (m_solveSwingLimit) @@ -1004,6 +1031,97 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion & } } +///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///If no axis is provided, it uses the default axis for this constraint. +void btConeTwistConstraint::setParam(int num, btScalar value, int axis) +{ + switch(num) + { + case BT_CONSTRAINT_ERP : + case BT_CONSTRAINT_STOP_ERP : + if((axis >= 0) && (axis < 3)) + { + m_linERP = value; + m_flags |= BT_CONETWIST_FLAGS_LIN_ERP; + } + else + { + m_biasFactor = value; + } + break; + case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_STOP_CFM : + if((axis >= 0) && (axis < 3)) + { + m_linCFM = value; + m_flags |= BT_CONETWIST_FLAGS_LIN_CFM; + } + else + { + m_angCFM = value; + m_flags |= BT_CONETWIST_FLAGS_ANG_CFM; + } + break; + default: + btAssertConstrParams(0); + break; + } +} + +///return the local value of parameter +btScalar btConeTwistConstraint::getParam(int num, int axis) const +{ + btScalar retVal = 0; + switch(num) + { + case BT_CONSTRAINT_ERP : + case BT_CONSTRAINT_STOP_ERP : + if((axis >= 0) && (axis < 3)) + { + btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP); + retVal = m_linERP; + } + else if((axis >= 3) && (axis < 6)) + { + retVal = m_biasFactor; + } + else + { + btAssertConstrParams(0); + } + break; + case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_STOP_CFM : + if((axis >= 0) && (axis < 3)) + { + btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM); + retVal = m_linCFM; + } + else if((axis >= 3) && (axis < 6)) + { + btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM); + retVal = m_angCFM; + } + else + { + btAssertConstrParams(0); + } + break; + default : + btAssertConstrParams(0); + } + return retVal; +} + + +void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) +{ + m_rbAFrame = frameA; + m_rbBFrame = frameB; + buildJacobian(); + //calculateTransforms(); +} + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index 8a893d4fb8c..c1c5aa851b6 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -42,6 +42,12 @@ and swing 1 and 2 are along the z and y axes respectively. class btRigidBody; +enum btConeTwistFlags +{ + BT_CONETWIST_FLAGS_LIN_CFM = 1, + BT_CONETWIST_FLAGS_LIN_ERP = 2, + BT_CONETWIST_FLAGS_ANG_CFM = 4 +}; ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc) class btConeTwistConstraint : public btTypedConstraint @@ -99,25 +105,46 @@ public: btScalar m_maxMotorImpulse; btVector3 m_accMotorImpulse; + // parameters + int m_flags; + btScalar m_linCFM; + btScalar m_linERP; + btScalar m_angCFM; + +protected: + + void init(); + + void computeConeLimitInfo(const btQuaternion& qCone, // in + btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs + + void computeTwistLimitInfo(const btQuaternion& qTwist, // in + btScalar& twistAngle, btVector3& vTwistAxis); // all outs + + void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const; + + public: btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame); btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame); - btConeTwistConstraint(); - virtual void buildJacobian(); virtual void getInfo1 (btConstraintInfo1* info); + + void getInfo1NonVirtual(btConstraintInfo1* info); virtual void getInfo2 (btConstraintInfo2* info); + void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); + virtual void solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); + const btRigidBody& getRigidBodyA() const { return m_rbA; @@ -198,7 +225,7 @@ public: } void calcAngleInfo(); - void calcAngleInfo2(); + void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); inline btScalar getSwingSpan1() { @@ -218,7 +245,6 @@ public: } bool isPastSwingLimit() { return m_solveSwingLimit; } - void setDamping(btScalar damping) { m_damping = damping; } void enableMotor(bool b) { m_bMotorEnabled = b; } @@ -239,18 +265,82 @@ public: btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const; + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///If no axis is provided, it uses the default axis for this constraint. + virtual void setParam(int num, btScalar value, int axis = -1); + virtual void setFrames(const btTransform& frameA, const btTransform& frameB); -protected: - void init(); + const btTransform& getFrameOffsetA() const + { + return m_rbAFrame; + } - void computeConeLimitInfo(const btQuaternion& qCone, // in - btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs + const btTransform& getFrameOffsetB() const + { + return m_rbBFrame; + } - void computeTwistLimitInfo(const btQuaternion& qTwist, // in - btScalar& twistAngle, btVector3& vTwistAxis); // all outs - void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const; + ///return the local value of parameter + virtual btScalar getParam(int num, int axis = -1) const; + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btConeTwistConstraintData +{ + btTypedConstraintData m_typeConstraintData; + btTransformFloatData m_rbAFrame; + btTransformFloatData m_rbBFrame; + + //limits + float m_swingSpan1; + float m_swingSpan2; + float m_twistSpan; + float m_limitSoftness; + float m_biasFactor; + float m_relaxationFactor; + + float m_damping; + + char m_pad[4]; + }; + + + +SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const +{ + return sizeof(btConeTwistConstraintData); + +} + + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer; + btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer); + + m_rbAFrame.serializeFloat(cone->m_rbAFrame); + m_rbBFrame.serializeFloat(cone->m_rbBFrame); + + cone->m_swingSpan1 = float(m_swingSpan1); + cone->m_swingSpan2 = float(m_swingSpan2); + cone->m_twistSpan = float(m_twistSpan); + cone->m_limitSoftness = float(m_limitSoftness); + cone->m_biasFactor = float(m_biasFactor); + cone->m_relaxationFactor = float(m_relaxationFactor); + cone->m_damping = float(m_damping); + + return "btConeTwistConstraintData"; +} + #endif //CONETWISTCONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 012c321fd6d..d97096d9f26 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -22,6 +22,52 @@ subject to the following restrictions: #include "LinearMath/btMinMax.h" #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" + + +btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB) +:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB), + m_contactManifold(*contactManifold) +{ + +} + +btContactConstraint::~btContactConstraint() +{ + +} + +void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold) +{ + m_contactManifold = *contactManifold; +} + +void btContactConstraint::getInfo1 (btConstraintInfo1* info) +{ + +} + +void btContactConstraint::getInfo2 (btConstraintInfo2* info) +{ + +} + +void btContactConstraint::buildJacobian() +{ + +} + + + + + +#include "btContactConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btVector3.h" +#include "btJacobianEntry.h" +#include "btContactSolverInfo.h" +#include "LinearMath/btMinMax.h" +#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" + #define ASSERT2 btAssert #define USE_INTERNAL_APPLY_IMPULSE 1 @@ -85,345 +131,4 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, -//response between two dynamic objects with friction -btScalar resolveSingleCollision( - btRigidBody& body1, - btRigidBody& body2, - btManifoldPoint& contactPoint, - const btContactSolverInfo& solverInfo) -{ - - const btVector3& pos1_ = contactPoint.getPositionWorldOnA(); - const btVector3& pos2_ = contactPoint.getPositionWorldOnB(); - const btVector3& normal = contactPoint.m_normalWorldOnB; - - //constant over all iterations - btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition(); - - btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - btScalar rel_vel; - rel_vel = normal.dot(vel); - - btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ; - - // btScalar damping = solverInfo.m_damping ; - btScalar Kerp = solverInfo.m_erp; - btScalar Kcor = Kerp *Kfps; - - btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; - btAssert(cpd); - btScalar distance = cpd->m_penetration; - btScalar positionalError = Kcor *-distance; - btScalar velocityError = cpd->m_restitution - rel_vel;// * damping; - - btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv; - - btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv; - - btScalar normalImpulse = penetrationImpulse+velocityImpulse; - - // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - btScalar oldNormalImpulse = cpd->m_appliedImpulse; - btScalar sum = oldNormalImpulse + normalImpulse; - cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; - - normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse; - -#ifdef USE_INTERNAL_APPLY_IMPULSE - if (body1.getInvMass()) - { - body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse); - } - if (body2.getInvMass()) - { - body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse); - } -#else //USE_INTERNAL_APPLY_IMPULSE - body1.applyImpulse(normal*(normalImpulse), rel_pos1); - body2.applyImpulse(-normal*(normalImpulse), rel_pos2); -#endif //USE_INTERNAL_APPLY_IMPULSE - - return normalImpulse; -} - - -btScalar resolveSingleFriction( - btRigidBody& body1, - btRigidBody& body2, - btManifoldPoint& contactPoint, - const btContactSolverInfo& solverInfo) -{ - - (void)solverInfo; - - const btVector3& pos1 = contactPoint.getPositionWorldOnA(); - const btVector3& pos2 = contactPoint.getPositionWorldOnB(); - - btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); - - btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; - btAssert(cpd); - - btScalar combinedFriction = cpd->m_friction; - - btScalar limit = cpd->m_appliedImpulse * combinedFriction; - - if (cpd->m_appliedImpulse>btScalar(0.)) - //friction - { - //apply friction in the 2 tangential directions - - // 1st tangent - btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - btScalar j1,j2; - - { - - btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel); - - // calculate j that moves us to zero relative velocity - j1 = -vrel * cpd->m_jacDiagABInvTangent0; - btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0; - cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1; - btSetMin(cpd->m_accumulatedTangentImpulse0, limit); - btSetMax(cpd->m_accumulatedTangentImpulse0, -limit); - j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse; - - } - { - // 2nd tangent - - btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel); - - // calculate j that moves us to zero relative velocity - j2 = -vrel * cpd->m_jacDiagABInvTangent1; - btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1; - cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2; - btSetMin(cpd->m_accumulatedTangentImpulse1, limit); - btSetMax(cpd->m_accumulatedTangentImpulse1, -limit); - j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse; - } - -#ifdef USE_INTERNAL_APPLY_IMPULSE - if (body1.getInvMass()) - { - body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1); - body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2); - } - if (body2.getInvMass()) - { - body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1); - body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2); - } -#else //USE_INTERNAL_APPLY_IMPULSE - body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1); - body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2); -#endif //USE_INTERNAL_APPLY_IMPULSE - - - } - return cpd->m_appliedImpulse; -} - - -btScalar resolveSingleFrictionOriginal( - btRigidBody& body1, - btRigidBody& body2, - btManifoldPoint& contactPoint, - const btContactSolverInfo& solverInfo); - -btScalar resolveSingleFrictionOriginal( - btRigidBody& body1, - btRigidBody& body2, - btManifoldPoint& contactPoint, - const btContactSolverInfo& solverInfo) -{ - - (void)solverInfo; - - const btVector3& pos1 = contactPoint.getPositionWorldOnA(); - const btVector3& pos2 = contactPoint.getPositionWorldOnB(); - - btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); - - btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; - btAssert(cpd); - - btScalar combinedFriction = cpd->m_friction; - - btScalar limit = cpd->m_appliedImpulse * combinedFriction; - //if (contactPoint.m_appliedImpulse>btScalar(0.)) - //friction - { - //apply friction in the 2 tangential directions - - { - // 1st tangent - btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel); - - // calculate j that moves us to zero relative velocity - btScalar j = -vrel * cpd->m_jacDiagABInvTangent0; - btScalar total = cpd->m_accumulatedTangentImpulse0 + j; - btSetMin(total, limit); - btSetMax(total, -limit); - j = total - cpd->m_accumulatedTangentImpulse0; - cpd->m_accumulatedTangentImpulse0 = total; - body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1); - body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2); - } - - - { - // 2nd tangent - btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel); - - // calculate j that moves us to zero relative velocity - btScalar j = -vrel * cpd->m_jacDiagABInvTangent1; - btScalar total = cpd->m_accumulatedTangentImpulse1 + j; - btSetMin(total, limit); - btSetMax(total, -limit); - j = total - cpd->m_accumulatedTangentImpulse1; - cpd->m_accumulatedTangentImpulse1 = total; - body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1); - body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2); - } - } - return cpd->m_appliedImpulse; -} - - -//velocity + friction -//response between two dynamic objects with friction -btScalar resolveSingleCollisionCombined( - btRigidBody& body1, - btRigidBody& body2, - btManifoldPoint& contactPoint, - const btContactSolverInfo& solverInfo) -{ - - const btVector3& pos1 = contactPoint.getPositionWorldOnA(); - const btVector3& pos2 = contactPoint.getPositionWorldOnB(); - const btVector3& normal = contactPoint.m_normalWorldOnB; - - btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); - - btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - btScalar rel_vel; - rel_vel = normal.dot(vel); - - btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ; - - //btScalar damping = solverInfo.m_damping ; - btScalar Kerp = solverInfo.m_erp; - btScalar Kcor = Kerp *Kfps; - - btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; - btAssert(cpd); - btScalar distance = cpd->m_penetration; - btScalar positionalError = Kcor *-distance; - btScalar velocityError = cpd->m_restitution - rel_vel;// * damping; - - btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv; - - btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv; - - btScalar normalImpulse = penetrationImpulse+velocityImpulse; - - // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - btScalar oldNormalImpulse = cpd->m_appliedImpulse; - btScalar sum = oldNormalImpulse + normalImpulse; - cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; - - normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse; - - -#ifdef USE_INTERNAL_APPLY_IMPULSE - if (body1.getInvMass()) - { - body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse); - } - if (body2.getInvMass()) - { - body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse); - } -#else //USE_INTERNAL_APPLY_IMPULSE - body1.applyImpulse(normal*(normalImpulse), rel_pos1); - body2.applyImpulse(-normal*(normalImpulse), rel_pos2); -#endif //USE_INTERNAL_APPLY_IMPULSE - - { - //friction - btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - rel_vel = normal.dot(vel); - - - btVector3 lat_vel = vel - normal * rel_vel; - btScalar lat_rel_vel = lat_vel.length(); - - btScalar combinedFriction = cpd->m_friction; - - if (cpd->m_appliedImpulse > 0) - if (lat_rel_vel > SIMD_EPSILON) - { - lat_vel /= lat_rel_vel; - btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel); - btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel); - btScalar friction_impulse = lat_rel_vel / - (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); - btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction; - - btSetMin(friction_impulse, normal_impulse); - btSetMax(friction_impulse, -normal_impulse); - body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1); - body2.applyImpulse(lat_vel * friction_impulse, rel_pos2); - } - } - - - - return normalImpulse; -} - -btScalar resolveSingleFrictionEmpty( - btRigidBody& body1, - btRigidBody& body2, - btManifoldPoint& contactPoint, - const btContactSolverInfo& solverInfo); - -btScalar resolveSingleFrictionEmpty( - btRigidBody& body1, - btRigidBody& body2, - btManifoldPoint& contactPoint, - const btContactSolverInfo& solverInfo) -{ - (void)contactPoint; - (void)body1; - (void)body2; - (void)solverInfo; - - - return btScalar(0.); -} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index e8871f3860b..63c1a417bc1 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -16,107 +16,53 @@ subject to the following restrictions: #ifndef CONTACT_CONSTRAINT_H #define CONTACT_CONSTRAINT_H -///@todo: make into a proper class working with the iterative constraint solver - -class btRigidBody; #include "LinearMath/btVector3.h" -#include "LinearMath/btScalar.h" -struct btContactSolverInfo; -class btManifoldPoint; - -enum { - DEFAULT_CONTACT_SOLVER_TYPE=0, - CONTACT_SOLVER_TYPE1, - CONTACT_SOLVER_TYPE2, - USER_CONTACT_SOLVER_TYPE1, - MAX_CONTACT_SOLVER_TYPES -}; +#include "btJacobianEntry.h" +#include "btTypedConstraint.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" + +///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface +ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint +{ +protected: + btPersistentManifold m_contactManifold; -typedef btScalar (*ContactSolverFunc)(btRigidBody& body1, - btRigidBody& body2, - class btManifoldPoint& contactPoint, - const btContactSolverInfo& info); +public: -///stores some extra information to each contact point. It is not in the contact point, because that want to keep the collision detection independent from the constraint solver. -struct btConstraintPersistentData -{ - inline btConstraintPersistentData() - :m_appliedImpulse(btScalar(0.)), - m_prevAppliedImpulse(btScalar(0.)), - m_accumulatedTangentImpulse0(btScalar(0.)), - m_accumulatedTangentImpulse1(btScalar(0.)), - m_jacDiagABInv(btScalar(0.)), - m_persistentLifeTime(0), - m_restitution(btScalar(0.)), - m_friction(btScalar(0.)), - m_penetration(btScalar(0.)), - m_contactSolverFunc(0), - m_frictionSolverFunc(0) + + btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB); + + void setContactManifold(btPersistentManifold* contactManifold); + + btPersistentManifold* getContactManifold() { + return &m_contactManifold; } - - - /// total applied impulse during most recent frame - btScalar m_appliedImpulse; - btScalar m_prevAppliedImpulse; - btScalar m_accumulatedTangentImpulse0; - btScalar m_accumulatedTangentImpulse1; - - btScalar m_jacDiagABInv; - btScalar m_jacDiagABInvTangent0; - btScalar m_jacDiagABInvTangent1; - int m_persistentLifeTime; - btScalar m_restitution; - btScalar m_friction; - btScalar m_penetration; - btVector3 m_frictionWorldTangential0; - btVector3 m_frictionWorldTangential1; - - btVector3 m_frictionAngularComponent0A; - btVector3 m_frictionAngularComponent0B; - btVector3 m_frictionAngularComponent1A; - btVector3 m_frictionAngularComponent1B; - - //some data doesn't need to be persistent over frames: todo: clean/reuse this - btVector3 m_angularComponentA; - btVector3 m_angularComponentB; - - ContactSolverFunc m_contactSolverFunc; - ContactSolverFunc m_frictionSolverFunc; -}; + const btPersistentManifold* getContactManifold() const + { + return &m_contactManifold; + } -///bilateral constraint between two dynamic objects -///positive distance = separation, negative distance = penetration -void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, - btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep); + virtual ~btContactConstraint(); + + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + ///obsolete methods + virtual void buildJacobian(); -///contact constraint resolution: -///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint -///positive distance = separation, negative distance = penetration -btScalar resolveSingleCollision( - btRigidBody& body1, - btRigidBody& body2, - btManifoldPoint& contactPoint, - const btContactSolverInfo& info); -btScalar resolveSingleFriction( - btRigidBody& body1, - btRigidBody& body2, - btManifoldPoint& contactPoint, - const btContactSolverInfo& solverInfo - ); +}; + +///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects +void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, + btRigidBody& body2, const btVector3& pos2, + btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep); -btScalar resolveSingleCollisionCombined( - btRigidBody& body1, - btRigidBody& body2, - btManifoldPoint& contactPoint, - const btContactSolverInfo& solverInfo - ); #endif //CONTACT_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index a11fc94ea11..db5bb5e4c51 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -35,7 +35,7 @@ struct btContactSolverInfoData btScalar m_tau; - btScalar m_damping; + btScalar m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. btScalar m_friction; btScalar m_timeStep; btScalar m_restitution; @@ -52,6 +52,7 @@ struct btContactSolverInfoData int m_solverMode; int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; }; @@ -77,8 +78,9 @@ struct btContactSolverInfo : public btContactSolverInfoData m_splitImpulsePenetrationThreshold = -0.02f; m_linearSlop = btScalar(0.0); m_warmstartingFactor=btScalar(0.85); - m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_USE_2_FRICTION_DIRECTIONS |SOLVER_SIMD | SOLVER_RANDMIZE_ORDER; + m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER; m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution + m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit } }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 38e81688f02..7c5e4f6e7b2 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -28,14 +28,10 @@ http://gimpact.sf.net #define D6_USE_OBSOLETE_METHOD false +#define D6_USE_FRAME_OFFSET true + -btGeneric6DofConstraint::btGeneric6DofConstraint() -:btTypedConstraint(D6_CONSTRAINT_TYPE), -m_useLinearReferenceFrameA(true), -m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) -{ -} @@ -44,13 +40,31 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& , m_frameInA(frameInA) , m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), +m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), +m_flags(0), m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) { + calculateTransforms(); +} + + +btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) + : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB), + m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameB), + m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), + m_flags(0), + m_useSolveConstraintObsolete(false) +{ + ///not providing rigidbody A means implicitly using worldspace for body A + m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; + calculateTransforms(); } + #define GENERIC_D6_DISABLE_WARMSTARTING 1 @@ -112,7 +126,6 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value) m_currentLimit = 0;//Free from violation return 0; } - if (test_value < m_loLimit) { m_currentLimit = 1;//low limit violation @@ -135,7 +148,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value) btScalar btRotationalLimitMotor::solveAngularLimits( btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, - btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB) + btRigidBody * body0, btRigidBody * body1 ) { if (needApplyTorques()==false) return 0.0f; @@ -145,7 +158,7 @@ btScalar btRotationalLimitMotor::solveAngularLimits( //current error correction if (m_currentLimit!=0) { - target_velocity = -m_ERP*m_currentLimitError/(timeStep); + target_velocity = -m_stopERP*m_currentLimitError/(timeStep); maxMotorForce = m_maxLimitForce; } @@ -154,9 +167,9 @@ btScalar btRotationalLimitMotor::solveAngularLimits( // current velocity difference btVector3 angVelA; - bodyA.getAngularVelocity(angVelA); + body0->internalGetAngularVelocity(angVelA); btVector3 angVelB; - bodyB.getAngularVelocity(angVelB); + body1->internalGetAngularVelocity(angVelB); btVector3 vel_diff; vel_diff = angVelA-angVelB; @@ -207,8 +220,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits( //body0->applyTorqueImpulse(motorImp); //body1->applyTorqueImpulse(-motorImp); - bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); - bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); + body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); + body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); return clippedMotorImpulse; @@ -258,8 +271,8 @@ int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_valu btScalar btTranslationalLimitMotor::solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, - btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA, - btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB, + btRigidBody& body1,const btVector3 &pointInA, + btRigidBody& body2,const btVector3 &pointInB, int limit_index, const btVector3 & axis_normal_on_a, const btVector3 & anchorPos) @@ -272,9 +285,9 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); btVector3 vel1; - bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); btVector3 vel2; - bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); btVector3 vel = vel1 - vel2; btScalar rel_vel = axis_normal_on_a.dot(vel); @@ -332,8 +345,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a); btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a); - bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); - bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); + body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); + body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); @@ -374,14 +387,33 @@ void btGeneric6DofConstraint::calculateAngleInfo() } - - void btGeneric6DofConstraint::calculateTransforms() { - m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; - m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); +} + +void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) +{ + m_calculatedTransformA = transA * m_frameInA; + m_calculatedTransformB = transB * m_frameInB; calculateLinearInfo(); calculateAngleInfo(); + if(m_useOffsetForConstraintFrame) + { // get weight factors depending on masses + btScalar miA = getRigidBodyA().getInvMass(); + btScalar miB = getRigidBodyB().getInvMass(); + m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); + btScalar miS = miA + miB; + if(miS > btScalar(0.f)) + { + m_factA = miB / miS; + } + else + { + m_factA = btScalar(0.5f); + } + m_factB = btScalar(1.0f) - m_factA; + } } @@ -420,6 +452,7 @@ void btGeneric6DofConstraint::buildAngularJacobian( bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) { btScalar angle = m_calculatedAxisAngleDiff[axis_index]; + angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit); m_angularLimits[axis_index].m_currentPosition = angle; //test limits m_angularLimits[axis_index].testLimitValue(angle); @@ -430,6 +463,7 @@ bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) void btGeneric6DofConstraint::buildJacobian() { +#ifndef __SPU__ if (m_useSolveConstraintObsolete) { @@ -441,7 +475,7 @@ void btGeneric6DofConstraint::buildJacobian() m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); } //calculates transform - calculateTransforms(); + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); @@ -484,8 +518,9 @@ void btGeneric6DofConstraint::buildJacobian() } } -} +#endif //__SPU__ +} void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) @@ -497,7 +532,7 @@ void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) } else { //prepare constraint - calculateTransforms(); + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); info->m_numConstraintRows = 0; info->nub = 6; int i; @@ -522,21 +557,76 @@ void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) } } +void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } else + { + //pre-allocate all 6 + info->m_numConstraintRows = 6; + info->nub = 0; + } +} void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) { btAssert(!m_useSolveConstraintObsolete); - int row = setLinearLimits(info); - setAngularLimits(info, row); + + const btTransform& transA = m_rbA.getCenterOfMassTransform(); + const btTransform& transB = m_rbB.getCenterOfMassTransform(); + const btVector3& linVelA = m_rbA.getLinearVelocity(); + const btVector3& linVelB = m_rbB.getLinearVelocity(); + const btVector3& angVelA = m_rbA.getAngularVelocity(); + const btVector3& angVelB = m_rbB.getAngularVelocity(); + + if(m_useOffsetForConstraintFrame) + { // for stability better to solve angular limits first + int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); + setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); + } + else + { // leave old version for compatibility + int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB); + setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB); + } + +} + + +void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) +{ + + btAssert(!m_useSolveConstraintObsolete); + //prepare constraint + calculateTransforms(transA,transB); + + int i; + for (i=0;i<3 ;i++ ) + { + testAngularLimitMotor(i); + } + + if(m_useOffsetForConstraintFrame) + { // for stability better to solve angular limits first + int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); + setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); + } + else + { // leave old version for compatibility + int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB); + setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB); + } } -int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) +int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) { - btGeneric6DofConstraint * d6constraint = this; - int row = 0; +// int row = 0; //solve linear limits btRotationalLimitMotor limot; for (int i=0;i<3 ;i++ ) @@ -549,7 +639,6 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; limot.m_damping = m_linearLimits.m_damping; limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; - limot.m_ERP = m_linearLimits.m_restitution; limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; limot.m_limitSoftness = m_linearLimits.m_limitSoftness; limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; @@ -557,7 +646,25 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); - row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0); + int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT); + limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0]; + limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; + limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp; + if(m_useOffsetForConstraintFrame) + { + int indx1 = (i + 1) % 3; + int indx2 = (i + 2) % 3; + int rotAllowed = 1; // rotations around orthos to current axis + if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit) + { + rotAllowed = 0; + } + row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed); + } + else + { + row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0); + } } } return row; @@ -565,7 +672,7 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) -int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset) +int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) { btGeneric6DofConstraint * d6constraint = this; int row = row_offset; @@ -575,80 +682,30 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) { btVector3 axis = d6constraint->getAxis(i); - row += get_limit_motor_info2( - d6constraint->getRotationalLimitMotor(i), - &m_rbA, - &m_rbB, - info,row,axis,1); - } - } - - return row; -} - - - -void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) -{ - if (m_useSolveConstraintObsolete) - { - - - m_timeStep = timeStep; - - //calculateTransforms(); - - int i; - - // linear - - btVector3 pointInA = m_calculatedTransformA.getOrigin(); - btVector3 pointInB = m_calculatedTransformB.getOrigin(); - - btScalar jacDiagABInv; - btVector3 linear_axis; - for (i=0;i<3;i++) - { - if (m_linearLimits.isLimited(i)) + int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT); + if(!(flags & BT_6DOF_FLAGS_CFM_NORM)) { - jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); - - if (m_useLinearReferenceFrameA) - linear_axis = m_calculatedTransformA.getBasis().getColumn(i); - else - linear_axis = m_calculatedTransformB.getBasis().getColumn(i); - - m_linearLimits.solveLinearAxis( - m_timeStep, - jacDiagABInv, - m_rbA,bodyA,pointInA, - m_rbB,bodyB,pointInB, - i,linear_axis, m_AnchorPos); - + m_angularLimits[i].m_normalCFM = info->cfm[0]; } - } - - // angular - btVector3 angular_axis; - btScalar angularJacDiagABInv; - for (i=0;i<3;i++) - { - if (m_angularLimits[i].needApplyTorques()) + if(!(flags & BT_6DOF_FLAGS_CFM_STOP)) { - - // get axis - angular_axis = getAxis(i); - - angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); - - m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB); + m_angularLimits[i].m_stopCFM = info->cfm[0]; + } + if(!(flags & BT_6DOF_FLAGS_ERP_STOP)) + { + m_angularLimits[i].m_stopERP = info->erp; } + row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i), + transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1); } } + + return row; } + void btGeneric6DofConstraint::updateRHS(btScalar timeStep) { (void)timeStep; @@ -656,6 +713,15 @@ void btGeneric6DofConstraint::updateRHS(btScalar timeStep) } +void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB) +{ + m_frameInA = frameA; + m_frameInB = frameB; + buildJacobian(); + calculateTransforms(); +} + + btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const { @@ -712,8 +778,8 @@ void btGeneric6DofConstraint::calculateLinearInfo() int btGeneric6DofConstraint::get_limit_motor_info2( btRotationalLimitMotor * limot, - btRigidBody * body0, btRigidBody * body1, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational) + const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, + btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed) { int srow = row * info->rowskip; int powered = limot->m_enableMotor; @@ -733,18 +799,51 @@ int btGeneric6DofConstraint::get_limit_motor_info2( } if((!rotational)) { - btVector3 ltd; // Linear Torque Decoupling vector - btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition(); - ltd = c.cross(ax1); - info->m_J1angularAxis[srow+0] = ltd[0]; - info->m_J1angularAxis[srow+1] = ltd[1]; - info->m_J1angularAxis[srow+2] = ltd[2]; - - c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition(); - ltd = -c.cross(ax1); - info->m_J2angularAxis[srow+0] = ltd[0]; - info->m_J2angularAxis[srow+1] = ltd[1]; - info->m_J2angularAxis[srow+2] = ltd[2]; + if (m_useOffsetForConstraintFrame) + { + btVector3 tmpA, tmpB, relA, relB; + // get vector from bodyB to frameB in WCS + relB = m_calculatedTransformB.getOrigin() - transB.getOrigin(); + // get its projection to constraint axis + btVector3 projB = ax1 * relB.dot(ax1); + // get vector directed from bodyB to constraint axis (and orthogonal to it) + btVector3 orthoB = relB - projB; + // same for bodyA + relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); + btVector3 projA = ax1 * relA.dot(ax1); + btVector3 orthoA = relA - projA; + // get desired offset between frames A and B along constraint axis + btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError; + // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis + btVector3 totalDist = projA + ax1 * desiredOffs - projB; + // get offset vectors relA and relB + relA = orthoA + totalDist * m_factA; + relB = orthoB - totalDist * m_factB; + tmpA = relA.cross(ax1); + tmpB = relB.cross(ax1); + if(m_hasStaticBody && (!rotAllowed)) + { + tmpA *= m_factA; + tmpB *= m_factB; + } + int i; + for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i]; + } else + { + btVector3 ltd; // Linear Torque Decoupling vector + btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin(); + ltd = c.cross(ax1); + info->m_J1angularAxis[srow+0] = ltd[0]; + info->m_J1angularAxis[srow+1] = ltd[1]; + info->m_J1angularAxis[srow+2] = ltd[2]; + + c = m_calculatedTransformB.getOrigin() - transB.getOrigin(); + ltd = -c.cross(ax1); + info->m_J2angularAxis[srow+0] = ltd[0]; + info->m_J2angularAxis[srow+1] = ltd[1]; + info->m_J2angularAxis[srow+2] = ltd[2]; + } } // if we're limited low and high simultaneously, the joint motor is // ineffective @@ -752,7 +851,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2( info->m_constraintError[srow] = btScalar(0.f); if (powered) { - info->cfm[srow] = 0.0f; + info->cfm[srow] = limot->m_normalCFM; if(!limit) { btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; @@ -761,7 +860,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2( limot->m_loLimit, limot->m_hiLimit, tag_vel, - info->fps * info->erp); + info->fps * limot->m_stopERP); info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; info->m_lowerLimit[srow] = -limot->m_maxMotorForce; info->m_upperLimit[srow] = limot->m_maxMotorForce; @@ -769,7 +868,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2( } if(limit) { - btScalar k = info->fps * limot->m_ERP; + btScalar k = info->fps * limot->m_stopERP; if(!rotational) { info->m_constraintError[srow] += k * limot->m_currentLimitError; @@ -778,7 +877,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2( { info->m_constraintError[srow] += -k * limot->m_currentLimitError; } - info->cfm[srow] = 0.0f; + info->cfm[srow] = limot->m_stopCFM; if (limot->m_loLimit == limot->m_hiLimit) { // limited low and high simultaneously info->m_lowerLimit[srow] = -SIMD_INFINITY; @@ -803,15 +902,17 @@ int btGeneric6DofConstraint::get_limit_motor_info2( btScalar vel; if (rotational) { - vel = body0->getAngularVelocity().dot(ax1); - if (body1) - vel -= body1->getAngularVelocity().dot(ax1); + vel = angVelA.dot(ax1); +//make sure that if no body -> angVelB == zero vec +// if (body1) + vel -= angVelB.dot(ax1); } else { - vel = body0->getLinearVelocity().dot(ax1); - if (body1) - vel -= body1->getLinearVelocity().dot(ax1); + vel = linVelA.dot(ax1); +//make sure that if no body -> angVelB == zero vec +// if (body1) + vel -= linVelB.dot(ax1); } // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. @@ -844,128 +945,126 @@ int btGeneric6DofConstraint::get_limit_motor_info2( -btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA) - : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA) -{ - for(int i = 0; i < 6; i++) - { - m_springEnabled[i] = false; - m_equilibriumPoint[i] = btScalar(0.f); - m_springStiffness[i] = btScalar(0.f); - m_springDamping[i] = btScalar(1.f); - } -} -void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff) + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///If no axis is provided, it uses the default axis for this constraint. +void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis) { - btAssert((index >= 0) && (index < 6)); - m_springEnabled[index] = onOff; - if(index < 3) - { - m_linearLimits.m_enableMotor[index] = onOff; - } - else + if((axis >= 0) && (axis < 3)) { - m_angularLimits[index - 3].m_enableMotor = onOff; - } -} - - - -void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness) -{ - btAssert((index >= 0) && (index < 6)); - m_springStiffness[index] = stiffness; -} - - -void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping) -{ - btAssert((index >= 0) && (index < 6)); - m_springDamping[index] = damping; -} - - -void btGeneric6DofSpringConstraint::setEquilibriumPoint() -{ - calculateTransforms(); - for(int i = 0; i < 3; i++) - { - m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + m_linearLimits.m_stopERP[axis] = value; + m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); + break; + case BT_CONSTRAINT_STOP_CFM : + m_linearLimits.m_stopCFM[axis] = value; + m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); + break; + case BT_CONSTRAINT_CFM : + m_linearLimits.m_normalCFM[axis] = value; + m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); + break; + default : + btAssertConstrParams(0); + } } - for(int i = 0; i < 3; i++) + else if((axis >=3) && (axis < 6)) { - m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i]; - } -} - - - -void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index) -{ - btAssert((index >= 0) && (index < 6)); - calculateTransforms(); - if(index < 3) - { - m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + m_angularLimits[axis - 3].m_stopERP = value; + m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); + break; + case BT_CONSTRAINT_STOP_CFM : + m_angularLimits[axis - 3].m_stopCFM = value; + m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); + break; + case BT_CONSTRAINT_CFM : + m_angularLimits[axis - 3].m_normalCFM = value; + m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); + break; + default : + btAssertConstrParams(0); + } } else { - m_equilibriumPoint[index + 3] = m_calculatedAxisAngleDiff[index]; + btAssertConstrParams(0); } } - - -void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info) + ///return the local value of parameter +btScalar btGeneric6DofConstraint::getParam(int num, int axis) const { - calculateTransforms(); - // it is assumed that calculateTransforms() have been called before this call - int i; - btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity(); - for(i = 0; i < 3; i++) + btScalar retVal = 0; + if((axis >= 0) && (axis < 3)) { - if(m_springEnabled[i]) + switch(num) { - // get current position of constraint - btScalar currPos = m_calculatedLinearDiff[i]; - // calculate difference - btScalar delta = currPos - m_equilibriumPoint[i]; - // spring force is (delta * m_stiffness) according to Hooke's Law - btScalar force = delta * m_springStiffness[i]; - btScalar velFactor = info->fps * m_springDamping[i]; - m_linearLimits.m_targetVelocity[i] = velFactor * force; - m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps; + case BT_CONSTRAINT_STOP_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_linearLimits.m_stopERP[axis]; + break; + case BT_CONSTRAINT_STOP_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_linearLimits.m_stopCFM[axis]; + break; + case BT_CONSTRAINT_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_linearLimits.m_normalCFM[axis]; + break; + default : + btAssertConstrParams(0); } } - for(i = 0; i < 3; i++) + else if((axis >=3) && (axis < 6)) { - if(m_springEnabled[i + 3]) + switch(num) { - // get current position of constraint - btScalar currPos = m_calculatedAxisAngleDiff[i]; - // calculate difference - btScalar delta = currPos - m_equilibriumPoint[i+3]; - // spring force is (-delta * m_stiffness) according to Hooke's Law - btScalar force = -delta * m_springStiffness[i+3]; - btScalar velFactor = info->fps * m_springDamping[i+3]; - m_angularLimits[i].m_targetVelocity = velFactor * force; - m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps; + case BT_CONSTRAINT_STOP_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_angularLimits[axis - 3].m_stopERP; + break; + case BT_CONSTRAINT_STOP_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_angularLimits[axis - 3].m_stopCFM; + break; + case BT_CONSTRAINT_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_angularLimits[axis - 3].m_normalCFM; + break; + default : + btAssertConstrParams(0); } } + else + { + btAssertConstrParams(0); + } + return retVal; } + -void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info) +void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2) { - // this will be called by constraint solver at the constraint setup stage - // set current motor parameters - internalUpdateSprings(info); - // do the rest of job for constraint setup - btGeneric6DofConstraint::getInfo2(info); -} - - - - + btVector3 zAxis = axis1.normalized(); + btVector3 yAxis = axis2.normalized(); + btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + + btTransform frameInW; + frameInW.setIdentity(); + frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); + + // now get constraint frame in local coordinate systems + m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; + m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; + + calculateTransforms(); +}
\ No newline at end of file diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 8082eb1f132..4fdac113378 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -12,6 +12,10 @@ subject to the following restrictions: 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. */ + +/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev +/// Added support for generic constraint solver through getInfo1/getInfo2 methods + /* 2007-09-09 btGeneric6DofConstraint Refactored by Francisco Le?n @@ -45,7 +49,9 @@ public: btScalar m_maxLimitForce;//!< max force on limit btScalar m_damping;//!< Damping. btScalar m_limitSoftness;//! Relaxation factor - btScalar m_ERP;//!< Error tolerance factor when joint is at limit + btScalar m_normalCFM;//!< Constraint force mixing factor + btScalar m_stopERP;//!< Error tolerance factor when joint is at limit + btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit btScalar m_bounce;//!< restitution factor bool m_enableMotor; @@ -65,9 +71,11 @@ public: m_targetVelocity = 0; m_maxMotorForce = 0.1f; m_maxLimitForce = 300.0f; - m_loLimit = -SIMD_INFINITY; - m_hiLimit = SIMD_INFINITY; - m_ERP = 0.5f; + m_loLimit = 1.0f; + m_hiLimit = -1.0f; + m_normalCFM = 0.f; + m_stopERP = 0.2f; + m_stopCFM = 0.f; m_bounce = 0.0f; m_damping = 1.0f; m_limitSoftness = 0.5f; @@ -83,7 +91,9 @@ public: m_limitSoftness = limot.m_limitSoftness; m_loLimit = limot.m_loLimit; m_hiLimit = limot.m_hiLimit; - m_ERP = limot.m_ERP; + m_normalCFM = limot.m_normalCFM; + m_stopERP = limot.m_stopERP; + m_stopCFM = limot.m_stopCFM; m_bounce = limot.m_bounce; m_currentLimit = limot.m_currentLimit; m_currentLimitError = limot.m_currentLimitError; @@ -113,7 +123,7 @@ public: int testLimitValue(btScalar test_value); //! apply the correction impulses for two bodies - btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB); + btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); }; @@ -130,6 +140,9 @@ public: btScalar m_limitSoftness;//!< Softness for linear limit btScalar m_damping;//!< Damping for linear limit btScalar m_restitution;//! Bounce parameter for linear limit + btVector3 m_normalCFM;//!< Constraint force mixing factor + btVector3 m_stopERP;//!< Error tolerance factor when joint is at limit + btVector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit //!@} bool m_enableMotor[3]; btVector3 m_targetVelocity;//!< target motor velocity @@ -143,6 +156,9 @@ public: m_lowerLimit.setValue(0.f,0.f,0.f); m_upperLimit.setValue(0.f,0.f,0.f); m_accumulatedImpulse.setValue(0.f,0.f,0.f); + m_normalCFM.setValue(0.f, 0.f, 0.f); + m_stopERP.setValue(0.2f, 0.2f, 0.2f); + m_stopCFM.setValue(0.f, 0.f, 0.f); m_limitSoftness = 0.7f; m_damping = btScalar(1.0f); @@ -164,6 +180,10 @@ public: m_limitSoftness = other.m_limitSoftness ; m_damping = other.m_damping; m_restitution = other.m_restitution; + m_normalCFM = other.m_normalCFM; + m_stopERP = other.m_stopERP; + m_stopCFM = other.m_stopCFM; + for(int i=0; i < 3; i++) { m_enableMotor[i] = other.m_enableMotor[i]; @@ -194,8 +214,8 @@ public: btScalar solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, - btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA, - btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB, + btRigidBody& body1,const btVector3 &pointInA, + btRigidBody& body2,const btVector3 &pointInB, int limit_index, const btVector3 & axis_normal_on_a, const btVector3 & anchorPos); @@ -203,6 +223,15 @@ public: }; +enum bt6DofFlags +{ + BT_6DOF_FLAGS_CFM_NORM = 1, + BT_6DOF_FLAGS_CFM_STOP = 2, + BT_6DOF_FLAGS_ERP_STOP = 4 +}; +#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis + + /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space /*! btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'. @@ -217,20 +246,22 @@ This brings support for limit parameters and motors. </li> <li> Angulars limits have these possible ranges: <table border=1 > -<tr - +<tr> <td><b>AXIS</b></td> <td><b>MIN ANGLE</b></td> <td><b>MAX ANGLE</b></td> +</tr><tr> <td>X</td> - <td>-PI</td> - <td>PI</td> + <td>-PI</td> + <td>PI</td> +</tr><tr> <td>Y</td> - <td>-PI/2</td> - <td>PI/2</td> + <td>-PI/2</td> + <td>PI/2</td> +</tr><tr> <td>Z</td> - <td>-PI/2</td> - <td>PI/2</td> + <td>-PI</td> + <td>PI</td> </tr> </table> </li> @@ -274,11 +305,17 @@ protected: btVector3 m_calculatedAxisAngleDiff; btVector3 m_calculatedAxis[3]; btVector3 m_calculatedLinearDiff; + btScalar m_factA; + btScalar m_factB; + bool m_hasStaticBody; btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes bool m_useLinearReferenceFrameA; + bool m_useOffsetForConstraintFrame; + int m_flags; + //!@} btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other) @@ -289,9 +326,9 @@ protected: } - int setAngularLimits(btConstraintInfo2 *info, int row_offset); + int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); - int setLinearLimits(btConstraintInfo2 *info); + int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); void buildLinearJacobian( btJacobianEntry & jacLinear,const btVector3 & normalWorld, @@ -313,15 +350,16 @@ public: bool m_useSolveConstraintObsolete; btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - - btGeneric6DofConstraint(); - + btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); + //! Calcs global transform of the offsets /*! Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo */ - void calculateTransforms(); + void calculateTransforms(const btTransform& transA,const btTransform& transB); + + void calculateTransforms(); //! Gets the global transform of the offset for body A /*! @@ -368,9 +406,12 @@ public: virtual void getInfo1 (btConstraintInfo1* info); + void getInfo1NonVirtual (btConstraintInfo1* info); + virtual void getInfo2 (btConstraintInfo2* info); - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); + void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); + void updateRHS(btScalar timeStep); @@ -392,6 +433,7 @@ public: */ btScalar getRelativePivotPosition(int axis_index) const; + void setFrames(const btTransform & frameA, const btTransform & frameB); //! Test angular limit. /*! @@ -405,25 +447,45 @@ public: m_linearLimits.m_lowerLimit = linearLower; } - void setLinearUpperLimit(const btVector3& linearUpper) - { - m_linearLimits.m_upperLimit = linearUpper; - } + void getLinearLowerLimit(btVector3& linearLower) + { + linearLower = m_linearLimits.m_lowerLimit; + } + + void setLinearUpperLimit(const btVector3& linearUpper) + { + m_linearLimits.m_upperLimit = linearUpper; + } + + void getLinearUpperLimit(btVector3& linearUpper) + { + linearUpper = m_linearLimits.m_upperLimit; + } void setAngularLowerLimit(const btVector3& angularLower) { - m_angularLimits[0].m_loLimit = angularLower.getX(); - m_angularLimits[1].m_loLimit = angularLower.getY(); - m_angularLimits[2].m_loLimit = angularLower.getZ(); + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); } + void getAngularLowerLimit(btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + angularLower[i] = m_angularLimits[i].m_loLimit; + } + void setAngularUpperLimit(const btVector3& angularUpper) { - m_angularLimits[0].m_hiLimit = angularUpper.getX(); - m_angularLimits[1].m_hiLimit = angularUpper.getY(); - m_angularLimits[2].m_hiLimit = angularUpper.getZ(); + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); } + void getAngularUpperLimit(btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + angularUpper[i] = m_angularLimits[i].m_hiLimit; + } + //! Retrieves the angular limit informacion btRotationalLimitMotor * getRotationalLimitMotor(int index) { @@ -446,6 +508,8 @@ public: } else { + lo = btNormalizeAngle(lo); + hi = btNormalizeAngle(hi); m_angularLimits[axis-3].m_loLimit = lo; m_angularLimits[axis-3].m_hiLimit = hi; } @@ -468,52 +532,83 @@ public: return m_angularLimits[limitIndex-3].isLimited(); } - const btRigidBody& getRigidBodyA() const - { - return m_rbA; - } - const btRigidBody& getRigidBodyB() const - { - return m_rbB; - } - virtual void calcAnchorPos(void); // overridable int get_limit_motor_info2( btRotationalLimitMotor * limot, - btRigidBody * body0, btRigidBody * body1, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational); + const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, + btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false); + // access for UseFrameOffset + bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } + void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } -}; + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///If no axis is provided, it uses the default axis for this constraint. + virtual void setParam(int num, btScalar value, int axis = -1); + ///return the local value of parameter + virtual btScalar getParam(int num, int axis = -1) const; + + void setAxis( const btVector3& axis1, const btVector3& axis2); -/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF + virtual int calculateSerializeBufferSize() const; -/// DOF index used in enableSpring() and setStiffness() means: -/// 0 : translation X -/// 1 : translation Y -/// 2 : translation Z -/// 3 : rotation X (3rd Euler rotational around new position of X axis, range [-PI+epsilon, PI-epsilon] ) -/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] ) -/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] ) + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; -class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btGeneric6DofConstraintData { -protected: - bool m_springEnabled[6]; - btScalar m_equilibriumPoint[6]; - btScalar m_springStiffness[6]; - btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping) - void internalUpdateSprings(btConstraintInfo2* info); -public: - btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - void enableSpring(int index, bool onOff); - void setStiffness(int index, btScalar stiffness); - void setDamping(int index, btScalar damping); - void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF - void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF - virtual void getInfo2 (btConstraintInfo2* info); + btTypedConstraintData m_typeConstraintData; + btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTransformFloatData m_rbBFrame; + + btVector3FloatData m_linearUpperLimit; + btVector3FloatData m_linearLowerLimit; + + btVector3FloatData m_angularUpperLimit; + btVector3FloatData m_angularLowerLimit; + + int m_useLinearReferenceFrameA; + int m_useOffsetForConstraintFrame; }; +SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const +{ + return sizeof(btGeneric6DofConstraintData); +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + + btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer; + btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); + + m_frameInA.serializeFloat(dof->m_rbAFrame); + m_frameInB.serializeFloat(dof->m_rbBFrame); + + + int i; + for (i=0;i<3;i++) + { + dof->m_angularLowerLimit.m_floats[i] = float(m_angularLimits[i].m_loLimit); + dof->m_angularUpperLimit.m_floats[i] = float(m_angularLimits[i].m_hiLimit); + dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]); + dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]); + } + + dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0; + dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0; + + return "btGeneric6DofConstraintData"; +} + + + + #endif //GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp new file mode 100644 index 00000000000..d3503456625 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp @@ -0,0 +1,172 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. + +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 "btGeneric6DofSpringConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" + + +btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA) + : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA) +{ + m_objectType = D6_SPRING_CONSTRAINT_TYPE; + + for(int i = 0; i < 6; i++) + { + m_springEnabled[i] = false; + m_equilibriumPoint[i] = btScalar(0.f); + m_springStiffness[i] = btScalar(0.f); + m_springDamping[i] = btScalar(1.f); + } +} + + +void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + m_springEnabled[index] = onOff; + if(index < 3) + { + m_linearLimits.m_enableMotor[index] = onOff; + } + else + { + m_angularLimits[index - 3].m_enableMotor = onOff; + } +} + + + +void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness) +{ + btAssert((index >= 0) && (index < 6)); + m_springStiffness[index] = stiffness; +} + + +void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping) +{ + btAssert((index >= 0) && (index < 6)); + m_springDamping[index] = damping; +} + + +void btGeneric6DofSpringConstraint::setEquilibriumPoint() +{ + calculateTransforms(); + int i; + + for( i = 0; i < 3; i++) + { + m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; + } + for(i = 0; i < 3; i++) + { + m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i]; + } +} + + + +void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index) +{ + btAssert((index >= 0) && (index < 6)); + calculateTransforms(); + if(index < 3) + { + m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; + } + else + { + m_equilibriumPoint[index] = m_calculatedAxisAngleDiff[index - 3]; + } +} + +void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val) +{ + btAssert((index >= 0) && (index < 6)); + m_equilibriumPoint[index] = val; +} + + +void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info) +{ + // it is assumed that calculateTransforms() have been called before this call + int i; + btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity(); + for(i = 0; i < 3; i++) + { + if(m_springEnabled[i]) + { + // get current position of constraint + btScalar currPos = m_calculatedLinearDiff[i]; + // calculate difference + btScalar delta = currPos - m_equilibriumPoint[i]; + // spring force is (delta * m_stiffness) according to Hooke's Law + btScalar force = delta * m_springStiffness[i]; + btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations); + m_linearLimits.m_targetVelocity[i] = velFactor * force; + m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps; + } + } + for(i = 0; i < 3; i++) + { + if(m_springEnabled[i + 3]) + { + // get current position of constraint + btScalar currPos = m_calculatedAxisAngleDiff[i]; + // calculate difference + btScalar delta = currPos - m_equilibriumPoint[i+3]; + // spring force is (-delta * m_stiffness) according to Hooke's Law + btScalar force = -delta * m_springStiffness[i+3]; + btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations); + m_angularLimits[i].m_targetVelocity = velFactor * force; + m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps; + } + } +} + + +void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info) +{ + // this will be called by constraint solver at the constraint setup stage + // set current motor parameters + internalUpdateSprings(info); + // do the rest of job for constraint setup + btGeneric6DofConstraint::getInfo2(info); +} + + +void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1,const btVector3& axis2) +{ + btVector3 zAxis = axis1.normalized(); + btVector3 yAxis = axis2.normalized(); + btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + + btTransform frameInW; + frameInW.setIdentity(); + frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); + + // now get constraint frame in local coordinate systems + m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; + m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; + + calculateTransforms(); +} + + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h new file mode 100644 index 00000000000..16ff973e427 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h @@ -0,0 +1,97 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef GENERIC_6DOF_SPRING_CONSTRAINT_H +#define GENERIC_6DOF_SPRING_CONSTRAINT_H + + +#include "LinearMath/btVector3.h" +#include "btTypedConstraint.h" +#include "btGeneric6DofConstraint.h" + + +/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF + +/// DOF index used in enableSpring() and setStiffness() means: +/// 0 : translation X +/// 1 : translation Y +/// 2 : translation Z +/// 3 : rotation X (3rd Euler rotational around new position of X axis, range [-PI+epsilon, PI-epsilon] ) +/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] ) +/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] ) + +class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint +{ +protected: + bool m_springEnabled[6]; + btScalar m_equilibriumPoint[6]; + btScalar m_springStiffness[6]; + btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping) + void internalUpdateSprings(btConstraintInfo2* info); +public: + btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); + void enableSpring(int index, bool onOff); + void setStiffness(int index, btScalar stiffness); + void setDamping(int index, btScalar damping); + void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF + void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF + void setEquilibriumPoint(int index, btScalar val); + + virtual void setAxis( const btVector3& axis1, const btVector3& axis2); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual int calculateSerializeBufferSize() const; + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + +}; + + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btGeneric6DofSpringConstraintData +{ + btGeneric6DofConstraintData m_6dofData; + + int m_springEnabled[6]; + float m_equilibriumPoint[6]; + float m_springStiffness[6]; + float m_springDamping[6]; +}; + +SIMD_FORCE_INLINE int btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const +{ + return sizeof(btGeneric6DofSpringConstraintData); +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btGeneric6DofSpringConstraintData* dof = (btGeneric6DofSpringConstraintData*)dataBuffer; + btGeneric6DofConstraint::serialize(&dof->m_6dofData,serializer); + + int i; + for (i=0;i<6;i++) + { + dof->m_equilibriumPoint[i] = m_equilibriumPoint[i]; + dof->m_springDamping[i] = m_springDamping[i]; + dof->m_springEnabled[i] = m_springEnabled[i]? 1 : 0; + dof->m_springStiffness[i] = m_springStiffness[i]; + } + return "btGeneric6DofConstraintData"; +} + +#endif // GENERIC_6DOF_SPRING_CONSTRAINT_H + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp new file mode 100644 index 00000000000..29123d526b4 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. + +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 "btHinge2Constraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" + + + +// constructor +// anchor, axis1 and axis2 are in world coordinate system +// axis1 must be orthogonal to axis2 +btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) +: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), + m_anchor(anchor), + m_axis1(axis1), + m_axis2(axis2) +{ + // build frame basis + // 6DOF constraint uses Euler angles and to define limits + // it is assumed that rotational order is : + // Z - first, allowed limits are (-PI,PI); + // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number + // used to prevent constraint from instability on poles; + // new position of X, allowed limits are (-PI,PI); + // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs + // Build the frame in world coordinate system first + btVector3 zAxis = axis1.normalize(); + btVector3 xAxis = axis2.normalize(); + btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system + btTransform frameInW; + frameInW.setIdentity(); + frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); + frameInW.setOrigin(anchor); + // now get constraint frame in local coordinate systems + m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; + m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; + // sei limits + setLinearLowerLimit(btVector3(0.f, 0.f, -1.f)); + setLinearUpperLimit(btVector3(0.f, 0.f, 1.f)); + // like front wheels of a car + setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f)); + setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f)); + // enable suspension + enableSpring(2, true); + setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-) + setDamping(2, 0.01f); + setEquilibriumPoint(); +} + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h new file mode 100644 index 00000000000..15fd4a014cc --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h @@ -0,0 +1,58 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef HINGE2_CONSTRAINT_H +#define HINGE2_CONSTRAINT_H + + + +#include "LinearMath/btVector3.h" +#include "btTypedConstraint.h" +#include "btGeneric6DofSpringConstraint.h" + + + +// Constraint similar to ODE Hinge2 Joint +// has 3 degrees of frredom: +// 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2) +// 1 translational (along axis Z) with suspension spring + +class btHinge2Constraint : public btGeneric6DofSpringConstraint +{ +protected: + btVector3 m_anchor; + btVector3 m_axis1; + btVector3 m_axis2; +public: + // constructor + // anchor, axis1 and axis2 are in world coordinate system + // axis1 must be orthogonal to axis2 + btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2); + // access + const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); } + const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); } + const btVector3& getAxis1() { return m_axis1; } + const btVector3& getAxis2() { return m_axis2; } + btScalar getAngle1() { return getAngle(2); } + btScalar getAngle2() { return getAngle(0); } + // limits + void setUpperLimit(btScalar ang1max) { setAngularUpperLimit(btVector3(-1.f, 0.f, ang1max)); } + void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3( 1.f, 0.f, ang1min)); } +}; + + + +#endif // HINGE2_CONSTRAINT_H + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index b6b34305804..144beef1cd6 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -21,31 +21,31 @@ subject to the following restrictions: #include <new> #include "btSolverBody.h" -//----------------------------------------------------------------------------- + +//#define HINGE_USE_OBSOLETE_SOLVER false #define HINGE_USE_OBSOLETE_SOLVER false -//----------------------------------------------------------------------------- +#define HINGE_USE_FRAME_OFFSET true + +#ifndef __SPU__ + -btHingeConstraint::btHingeConstraint() -: btTypedConstraint (HINGE_CONSTRAINT_TYPE), -m_enableAngularMotor(false), -m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), -m_useReferenceFrameA(false) -{ - m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); -} -//----------------------------------------------------------------------------- btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, - btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA) + const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), - m_useReferenceFrameA(useReferenceFrameA) + m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), + m_useReferenceFrameA(useReferenceFrameA), + m_flags(0) +#ifdef _BT_USE_CENTER_LIMIT_ + ,m_limit() +#endif { m_rbAFrame.getOrigin() = pivotInA; @@ -78,22 +78,29 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); +#ifndef _BT_USE_CENTER_LIMIT_ //start with free - m_lowerLimit = btScalar(1e30); - m_upperLimit = btScalar(-1e30); + m_lowerLimit = btScalar(1.0f); + m_upperLimit = btScalar(-1.0f); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; +#endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } -//----------------------------------------------------------------------------- -btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA) + +btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), -m_useReferenceFrameA(useReferenceFrameA) +m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), +m_useReferenceFrameA(useReferenceFrameA), +m_flags(0) +#ifdef _BT_USE_CENTER_LIMIT_ +,m_limit() +#endif { // since no frame is given, assume this to be zero angle and just pick rb transform axis @@ -118,17 +125,19 @@ m_useReferenceFrameA(useReferenceFrameA) rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); +#ifndef _BT_USE_CENTER_LIMIT_ //start with free - m_lowerLimit = btScalar(1e30); - m_upperLimit = btScalar(-1e30); + m_lowerLimit = btScalar(1.0f); + m_upperLimit = btScalar(-1.0f); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; +#endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } -//----------------------------------------------------------------------------- + btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) @@ -136,48 +145,62 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), -m_useReferenceFrameA(useReferenceFrameA) +m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), +m_useReferenceFrameA(useReferenceFrameA), +m_flags(0) +#ifdef _BT_USE_CENTER_LIMIT_ +,m_limit() +#endif { +#ifndef _BT_USE_CENTER_LIMIT_ //start with free - m_lowerLimit = btScalar(1e30); - m_upperLimit = btScalar(-1e30); + m_lowerLimit = btScalar(1.0f); + m_upperLimit = btScalar(-1.0f); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; +#endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } -//----------------------------------------------------------------------------- + btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), -m_useReferenceFrameA(useReferenceFrameA) +m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), +m_useReferenceFrameA(useReferenceFrameA), +m_flags(0) +#ifdef _BT_USE_CENTER_LIMIT_ +,m_limit() +#endif { ///not providing rigidbody B means implicitly using worldspace for body B m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); - +#ifndef _BT_USE_CENTER_LIMIT_ //start with free - m_lowerLimit = btScalar(1e30); - m_upperLimit = btScalar(-1e30); + m_lowerLimit = btScalar(1.0f); + m_upperLimit = btScalar(-1.0f); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; +#endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } -//----------------------------------------------------------------------------- + void btHingeConstraint::buildJacobian() { if (m_useSolveConstraintObsolete) { m_appliedImpulse = btScalar(0.); + m_accMotorImpulse = btScalar(0.); if (!m_angularOnly) { @@ -221,7 +244,6 @@ void btHingeConstraint::buildJacobian() btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); - getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); @@ -248,7 +270,7 @@ void btHingeConstraint::buildJacobian() m_accLimitImpulse = btScalar(0.); // test angular limit - testLimit(); + testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); //Compute K = J*W*J' for hinge axis btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); @@ -258,7 +280,9 @@ void btHingeConstraint::buildJacobian() } } -//----------------------------------------------------------------------------- + +#endif //__SPU__ + void btHingeConstraint::getInfo1(btConstraintInfo1* info) { @@ -271,53 +295,123 @@ void btHingeConstraint::getInfo1(btConstraintInfo1* info) { info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular info->nub = 1; + //always add the row, to avoid computation (data is not available yet) //prepare constraint - testLimit(); + testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); if(getSolveLimit() || getEnableAngularMotor()) { info->m_numConstraintRows++; // limit 3rd anguar as well info->nub--; } + } -} // btHingeConstraint::getInfo1 () +} -//----------------------------------------------------------------------------- +void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } + else + { + //always add the 'limit' row, to avoid computation (data is not available yet) + info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular + info->nub = 0; + } +} void btHingeConstraint::getInfo2 (btConstraintInfo2* info) { + if(m_useOffsetForConstraintFrame) + { + getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); + } + else + { + getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); + } +} + + +void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) +{ + ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now + testLimit(transA,transB); + + getInfo2Internal(info,transA,transB,angVelA,angVelB); +} + + +void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) +{ + btAssert(!m_useSolveConstraintObsolete); - int i, s = info->rowskip; + int i, skip = info->rowskip; // transforms in world space - btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame; - btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame; + btTransform trA = transA*m_rbAFrame; + btTransform trB = transB*m_rbBFrame; // pivot point btVector3 pivotAInW = trA.getOrigin(); btVector3 pivotBInW = trB.getOrigin(); +#if 0 + if (0) + { + for (i=0;i<6;i++) + { + info->m_J1linearAxis[i*skip]=0; + info->m_J1linearAxis[i*skip+1]=0; + info->m_J1linearAxis[i*skip+2]=0; + + info->m_J1angularAxis[i*skip]=0; + info->m_J1angularAxis[i*skip+1]=0; + info->m_J1angularAxis[i*skip+2]=0; + + info->m_J2angularAxis[i*skip]=0; + info->m_J2angularAxis[i*skip+1]=0; + info->m_J2angularAxis[i*skip+2]=0; + + info->m_constraintError[i*skip]=0.f; + } + } +#endif //#if 0 // linear (all fixed) - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[s + 1] = 1; - info->m_J1linearAxis[2 * s + 2] = 1; - btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin(); + + if (!m_angularOnly) + { + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[skip + 1] = 1; + info->m_J1linearAxis[2 * skip + 2] = 1; + } + + + + + btVector3 a1 = pivotAInW - transA.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip); btVector3 a1neg = -a1; a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); } - btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin(); + btVector3 a2 = pivotBInW - transB.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip); a2.getSkewSymmetricMatrix(angular0,angular1,angular2); } // linear RHS btScalar k = info->fps * info->erp; - for(i = 0; i < 3; i++) - { - info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]); - } + if (!m_angularOnly) + { + for(i = 0; i < 3; i++) + { + info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]); + } + } // make rotations around X and Y equal // the hinge axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to @@ -374,8 +468,13 @@ void btHingeConstraint::getInfo2 (btConstraintInfo2* info) int limit = 0; if(getSolveLimit()) { - limit_err = m_correction * m_referenceSign; - limit = (limit_err > btScalar(0.0)) ? 1 : 2; +#ifdef _BT_USE_CENTER_LIMIT_ + limit_err = m_limit.getCorrection() * m_referenceSign; +#else + limit_err = m_correction * m_referenceSign; +#endif + limit = (limit_err > btScalar(0.0)) ? 1 : 2; + } // if the hinge has joint limits or motor, add in the extra row int powered = 0; @@ -402,19 +501,26 @@ void btHingeConstraint::getInfo2 (btConstraintInfo2* info) powered = 0; } info->m_constraintError[srow] = btScalar(0.0f); + btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; if(powered) { - info->cfm[srow] = btScalar(0.0); - btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp); + if(m_flags & BT_HINGE_FLAGS_CFM_NORM) + { + info->cfm[srow] = m_normalCFM; + } + btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; info->m_lowerLimit[srow] = - m_maxMotorImpulse; info->m_upperLimit[srow] = m_maxMotorImpulse; } if(limit) { - k = info->fps * info->erp; + k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; - info->cfm[srow] = btScalar(0.0); + if(m_flags & BT_HINGE_FLAGS_CFM_STOP) + { + info->cfm[srow] = m_stopCFM; + } if(lostop == histop) { // limited low and high simultaneously @@ -432,11 +538,15 @@ void btHingeConstraint::getInfo2 (btConstraintInfo2* info) info->m_upperLimit[srow] = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) +#ifdef _BT_USE_CENTER_LIMIT_ + btScalar bounce = m_limit.getRelaxationFactor(); +#else btScalar bounce = m_relaxationFactor; +#endif if(bounce > btScalar(0.0)) { - btScalar vel = m_rbA.getAngularVelocity().dot(ax1); - vel -= m_rbB.getAngularVelocity().dot(ax1); + btScalar vel = angVelA.dot(ax1); + vel -= angVelB.dot(ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if(limit == 1) @@ -462,160 +572,23 @@ void btHingeConstraint::getInfo2 (btConstraintInfo2* info) } } } +#ifdef _BT_USE_CENTER_LIMIT_ + info->m_constraintError[srow] *= m_limit.getBiasFactor(); +#else info->m_constraintError[srow] *= m_biasFactor; +#endif } // if(limit) } // if angular limit or powered } -//----------------------------------------------------------------------------- -void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) { - - ///for backwards compatibility during the transition to 'getInfo/getInfo2' - if (m_useSolveConstraintObsolete) - { - - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - - btScalar tau = btScalar(0.3); - - //linear part - if (!m_angularOnly) - { - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 vel1,vel2; - bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); - bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); - btVector3 vel = vel1 - vel2; - - for (int i=0;i<3;i++) - { - const btVector3& normal = m_jac[i].m_linearJointAxis; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - - btScalar rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - btVector3 impulse_vector = normal * impulse; - btVector3 ftorqueAxis1 = rel_pos1.cross(normal); - btVector3 ftorqueAxis2 = rel_pos2.cross(normal); - bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); - bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); - } - } - - - { - ///solve angular part - - // get axes in world space - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); - - btVector3 angVelA; - bodyA.getAngularVelocity(angVelA); - btVector3 angVelB; - bodyB.getAngularVelocity(angVelB); - - btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); - btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); - - btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; - btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; - btVector3 velrelOrthog = angAorthog-angBorthog; - { - - - //solve orthogonal angular velocity correction - btScalar relaxation = btScalar(1.); - btScalar len = velrelOrthog.length(); - if (len > btScalar(0.00001)) - { - btVector3 normal = velrelOrthog.normalized(); - btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + - getRigidBodyB().computeAngularImpulseDenominator(normal); - // scale for mass and relaxation - //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; - - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom)); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom)); - - } - - //solve angular positional correction - btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep); - btScalar len2 = angularError.length(); - if (len2>btScalar(0.00001)) - { - btVector3 normal2 = angularError.normalized(); - btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + - getRigidBodyB().computeAngularImpulseDenominator(normal2); - //angularError *= (btScalar(1.)/denom2) * relaxation; - - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2)); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2)); - - } - - - - - - // solve limit - if (m_solveLimit) - { - btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; - - btScalar impulseMag = amplitude * m_kHinge; - - // Clamp the accumulated impulse - btScalar temp = m_accLimitImpulse; - m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); - impulseMag = m_accLimitImpulse - temp; - - - - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign)); - - } - } - - //apply motor - if (m_enableAngularMotor) - { - //todo: add limits too - btVector3 angularLimit(0,0,0); - - btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; - btScalar projRelVel = velrel.dot(axisA); - - btScalar desiredMotorVel = m_motorTargetVelocity; - btScalar motor_relvel = desiredMotorVel - projRelVel; - - btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; - //todo: should clip against accumulated impulse - btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; - clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; - btVector3 motorImp = clippedMotorImpulse * axisA; - - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse); - - } - } - } - + m_rbAFrame = frameA; + m_rbBFrame = frameB; + buildJacobian(); } -//----------------------------------------------------------------------------- void btHingeConstraint::updateRHS(btScalar timeStep) { @@ -623,28 +596,37 @@ void btHingeConstraint::updateRHS(btScalar timeStep) } -//----------------------------------------------------------------------------- btScalar btHingeConstraint::getHingeAngle() { - const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); - const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); - const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1); - btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); + return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); +} + +btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB) +{ + const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); + const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); + const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1); +// btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); + btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); return m_referenceSign * angle; } -//----------------------------------------------------------------------------- -void btHingeConstraint::testLimit() + +void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB) { // Compute limit information - m_hingeAngle = getHingeAngle(); + m_hingeAngle = getHingeAngle(transA,transB); +#ifdef _BT_USE_CENTER_LIMIT_ + m_limit.test(m_hingeAngle); +#else m_correction = btScalar(0.); m_limitSign = btScalar(0.); m_solveLimit = false; if (m_lowerLimit <= m_upperLimit) { + m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit); if (m_hingeAngle <= m_lowerLimit) { m_correction = (m_lowerLimit - m_hingeAngle); @@ -658,9 +640,394 @@ void btHingeConstraint::testLimit() m_solveLimit = true; } } +#endif return; -} // btHingeConstraint::testLimit() +} + + +static btVector3 vHinge(0, 0, btScalar(1)); + +void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) +{ + // convert target from body to constraint space + btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation(); + qConstraint.normalize(); + + // extract "pure" hinge component + btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize(); + btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge); + btQuaternion qHinge = qNoHinge.inverse() * qConstraint; + qHinge.normalize(); + + // compute angular target, clamped to limits + btScalar targetAngle = qHinge.getAngle(); + if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. + { + qHinge = operator-(qHinge); + targetAngle = qHinge.getAngle(); + } + if (qHinge.getZ() < 0) + targetAngle = -targetAngle; + + setMotorTarget(targetAngle, dt); +} + +void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) +{ +#ifdef _BT_USE_CENTER_LIMIT_ + m_limit.fit(targetAngle); +#else + if (m_lowerLimit < m_upperLimit) + { + if (targetAngle < m_lowerLimit) + targetAngle = m_lowerLimit; + else if (targetAngle > m_upperLimit) + targetAngle = m_upperLimit; + } +#endif + // compute angular velocity + btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + btScalar dAngle = targetAngle - curAngle; + m_motorTargetVelocity = dAngle / dt; +} + + + +void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) +{ + btAssert(!m_useSolveConstraintObsolete); + int i, s = info->rowskip; + // transforms in world space + btTransform trA = transA*m_rbAFrame; + btTransform trB = transB*m_rbBFrame; + // pivot point + btVector3 pivotAInW = trA.getOrigin(); + btVector3 pivotBInW = trB.getOrigin(); +#if 1 + // difference between frames in WCS + btVector3 ofs = trB.getOrigin() - trA.getOrigin(); + // now get weight factors depending on masses + btScalar miA = getRigidBodyA().getInvMass(); + btScalar miB = getRigidBodyB().getInvMass(); + bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); + btScalar miS = miA + miB; + btScalar factA, factB; + if(miS > btScalar(0.f)) + { + factA = miB / miS; + } + else + { + factA = btScalar(0.5f); + } + factB = btScalar(1.0f) - factA; + // get the desired direction of hinge axis + // as weighted sum of Z-orthos of frameA and frameB in WCS + btVector3 ax1A = trA.getBasis().getColumn(2); + btVector3 ax1B = trB.getBasis().getColumn(2); + btVector3 ax1 = ax1A * factA + ax1B * factB; + ax1.normalize(); + // fill first 3 rows + // we want: velA + wA x relA == velB + wB x relB + btTransform bodyA_trans = transA; + btTransform bodyB_trans = transB; + int s0 = 0; + int s1 = s; + int s2 = s * 2; + int nrow = 2; // last filled row + btVector3 tmpA, tmpB, relA, relB, p, q; + // get vector from bodyB to frameB in WCS + relB = trB.getOrigin() - bodyB_trans.getOrigin(); + // get its projection to hinge axis + btVector3 projB = ax1 * relB.dot(ax1); + // get vector directed from bodyB to hinge axis (and orthogonal to it) + btVector3 orthoB = relB - projB; + // same for bodyA + relA = trA.getOrigin() - bodyA_trans.getOrigin(); + btVector3 projA = ax1 * relA.dot(ax1); + btVector3 orthoA = relA - projA; + btVector3 totalDist = projA - projB; + // get offset vectors relA and relB + relA = orthoA + totalDist * factA; + relB = orthoB - totalDist * factB; + // now choose average ortho to hinge axis + p = orthoB * factA + orthoA * factB; + btScalar len2 = p.length2(); + if(len2 > SIMD_EPSILON) + { + p /= btSqrt(len2); + } + else + { + p = trA.getBasis().getColumn(1); + } + // make one more ortho + q = ax1.cross(p); + // fill three rows + tmpA = relA.cross(p); + tmpB = relB.cross(p); + for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i]; + tmpA = relA.cross(q); + tmpB = relB.cross(q); + if(hasStaticBody && getSolveLimit()) + { // to make constraint between static and dynamic objects more rigid + // remove wA (or wB) from equation if angular limit is hit + tmpB *= factB; + tmpA *= factA; + } + for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i]; + tmpA = relA.cross(ax1); + tmpB = relB.cross(ax1); + if(hasStaticBody) + { // to make constraint between static and dynamic objects more rigid + // remove wA (or wB) from equation + tmpB *= factB; + tmpA *= factA; + } + for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; + + btScalar k = info->fps * info->erp; + + if (!m_angularOnly) + { + for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i]; + for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i]; + for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i]; + + // compute three elements of right hand side + + btScalar rhs = k * p.dot(ofs); + info->m_constraintError[s0] = rhs; + rhs = k * q.dot(ofs); + info->m_constraintError[s1] = rhs; + rhs = k * ax1.dot(ofs); + info->m_constraintError[s2] = rhs; + } + // the hinge axis should be the only unconstrained + // rotational axis, the angular velocity of the two bodies perpendicular to + // the hinge axis should be equal. thus the constraint equations are + // p*w1 - p*w2 = 0 + // q*w1 - q*w2 = 0 + // where p and q are unit vectors normal to the hinge axis, and w1 and w2 + // are the angular velocity vectors of the two bodies. + int s3 = 3 * s; + int s4 = 4 * s; + info->m_J1angularAxis[s3 + 0] = p[0]; + info->m_J1angularAxis[s3 + 1] = p[1]; + info->m_J1angularAxis[s3 + 2] = p[2]; + info->m_J1angularAxis[s4 + 0] = q[0]; + info->m_J1angularAxis[s4 + 1] = q[1]; + info->m_J1angularAxis[s4 + 2] = q[2]; + + info->m_J2angularAxis[s3 + 0] = -p[0]; + info->m_J2angularAxis[s3 + 1] = -p[1]; + info->m_J2angularAxis[s3 + 2] = -p[2]; + info->m_J2angularAxis[s4 + 0] = -q[0]; + info->m_J2angularAxis[s4 + 1] = -q[1]; + info->m_J2angularAxis[s4 + 2] = -q[2]; + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the hinge back into alignment. + // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and + // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if "theta" is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + k = info->fps * info->erp; + btVector3 u = ax1A.cross(ax1B); + info->m_constraintError[s3] = k * u.dot(p); + info->m_constraintError[s4] = k * u.dot(q); +#endif + // check angular limits + nrow = 4; // last filled row + int srow; + btScalar limit_err = btScalar(0.0); + int limit = 0; + if(getSolveLimit()) + { +#ifdef _BT_USE_CENTER_LIMIT_ + limit_err = m_limit.getCorrection() * m_referenceSign; +#else + limit_err = m_correction * m_referenceSign; +#endif + limit = (limit_err > btScalar(0.0)) ? 1 : 2; + + } + // if the hinge has joint limits or motor, add in the extra row + int powered = 0; + if(getEnableAngularMotor()) + { + powered = 1; + } + if(limit || powered) + { + nrow++; + srow = nrow * info->rowskip; + info->m_J1angularAxis[srow+0] = ax1[0]; + info->m_J1angularAxis[srow+1] = ax1[1]; + info->m_J1angularAxis[srow+2] = ax1[2]; + + info->m_J2angularAxis[srow+0] = -ax1[0]; + info->m_J2angularAxis[srow+1] = -ax1[1]; + info->m_J2angularAxis[srow+2] = -ax1[2]; + + btScalar lostop = getLowerLimit(); + btScalar histop = getUpperLimit(); + if(limit && (lostop == histop)) + { // the joint motor is ineffective + powered = 0; + } + info->m_constraintError[srow] = btScalar(0.0f); + btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; + if(powered) + { + if(m_flags & BT_HINGE_FLAGS_CFM_NORM) + { + info->cfm[srow] = m_normalCFM; + } + btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); + info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; + info->m_lowerLimit[srow] = - m_maxMotorImpulse; + info->m_upperLimit[srow] = m_maxMotorImpulse; + } + if(limit) + { + k = info->fps * currERP; + info->m_constraintError[srow] += k * limit_err; + if(m_flags & BT_HINGE_FLAGS_CFM_STOP) + { + info->cfm[srow] = m_stopCFM; + } + if(lostop == histop) + { + // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else if(limit == 1) + { // low limit + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { // high limit + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) +#ifdef _BT_USE_CENTER_LIMIT_ + btScalar bounce = m_limit.getRelaxationFactor(); +#else + btScalar bounce = m_relaxationFactor; +#endif + if(bounce > btScalar(0.0)) + { + btScalar vel = angVelA.dot(ax1); + vel -= angVelB.dot(ax1); + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if(limit == 1) + { // low limit + if(vel < 0) + { + btScalar newc = -bounce * vel; + if(newc > info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + else + { // high limit - all those computations are reversed + if(vel > 0) + { + btScalar newc = -bounce * vel; + if(newc < info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + } +#ifdef _BT_USE_CENTER_LIMIT_ + info->m_constraintError[srow] *= m_limit.getBiasFactor(); +#else + info->m_constraintError[srow] *= m_biasFactor; +#endif + } // if(limit) + } // if angular limit or powered +} + + +///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///If no axis is provided, it uses the default axis for this constraint. +void btHingeConstraint::setParam(int num, btScalar value, int axis) +{ + if((axis == -1) || (axis == 5)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + m_stopERP = value; + m_flags |= BT_HINGE_FLAGS_ERP_STOP; + break; + case BT_CONSTRAINT_STOP_CFM : + m_stopCFM = value; + m_flags |= BT_HINGE_FLAGS_CFM_STOP; + break; + case BT_CONSTRAINT_CFM : + m_normalCFM = value; + m_flags |= BT_HINGE_FLAGS_CFM_NORM; + break; + default : + btAssertConstrParams(0); + } + } + else + { + btAssertConstrParams(0); + } +} + +///return the local value of parameter +btScalar btHingeConstraint::getParam(int num, int axis) const +{ + btScalar retVal = 0; + if((axis == -1) || (axis == 5)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP); + retVal = m_stopERP; + break; + case BT_CONSTRAINT_STOP_CFM : + btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP); + retVal = m_stopCFM; + break; + case BT_CONSTRAINT_CFM : + btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM); + retVal = m_normalCFM; + break; + default : + btAssertConstrParams(0); + } + } + else + { + btAssertConstrParams(0); + } + return retVal; +} + -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 0af655f4409..50e3f73cb6d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -17,6 +17,8 @@ subject to the following restrictions: #ifndef HINGECONSTRAINT_H #define HINGECONSTRAINT_H +#define _BT_USE_CENTER_LIMIT_ 1 + #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" @@ -24,9 +26,27 @@ subject to the following restrictions: class btRigidBody; +#ifdef BT_USE_DOUBLE_PRECISION +#define btHingeConstraintData btHingeConstraintDoubleData +#define btHingeConstraintDataName "btHingeConstraintDoubleData" +#else +#define btHingeConstraintData btHingeConstraintFloatData +#define btHingeConstraintDataName "btHingeConstraintFloatData" +#endif //BT_USE_DOUBLE_PRECISION + + + +enum btHingeFlags +{ + BT_HINGE_FLAGS_CFM_STOP = 1, + BT_HINGE_FLAGS_ERP_STOP = 2, + BT_HINGE_FLAGS_CFM_NORM = 4 +}; + + /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space /// axis defines the orientation of the hinge axis -class btHingeConstraint : public btTypedConstraint +ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint { #ifdef IN_PARALLELL_SOLVER public: @@ -40,48 +60,67 @@ public: btScalar m_motorTargetVelocity; btScalar m_maxMotorImpulse; + +#ifdef _BT_USE_CENTER_LIMIT_ + btAngularLimit m_limit; +#else + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_limitSign; + btScalar m_correction; + btScalar m_limitSoftness; btScalar m_biasFactor; - btScalar m_relaxationFactor; + btScalar m_relaxationFactor; + + bool m_solveLimit; +#endif - btScalar m_lowerLimit; - btScalar m_upperLimit; - btScalar m_kHinge; - btScalar m_limitSign; - btScalar m_correction; btScalar m_accLimitImpulse; btScalar m_hingeAngle; - btScalar m_referenceSign; + btScalar m_referenceSign; bool m_angularOnly; bool m_enableAngularMotor; - bool m_solveLimit; bool m_useSolveConstraintObsolete; + bool m_useOffsetForConstraintFrame; bool m_useReferenceFrameA; + btScalar m_accMotorImpulse; + + int m_flags; + btScalar m_normalCFM; + btScalar m_stopCFM; + btScalar m_stopERP; + public: - btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false); + btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false); - btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false); + btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false); btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false); btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false); - btHingeConstraint(); virtual void buildJacobian(); virtual void getInfo1 (btConstraintInfo1* info); + void getInfo1NonVirtual(btConstraintInfo1* info); + virtual void getInfo2 (btConstraintInfo2* info); - - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); + + void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); + + void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); + void getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); + void updateRHS(btScalar timeStep); @@ -102,7 +141,19 @@ public: btRigidBody& getRigidBodyB() { return m_rbB; - } + } + + btTransform& getFrameOffsetA() + { + return m_rbAFrame; + } + + btTransform& getFrameOffsetB() + { + return m_rbBFrame; + } + + void setFrames(const btTransform& frameA, const btTransform& frameB); void setAngularOnly(bool angularOnly) { @@ -116,44 +167,101 @@ public: m_maxMotorImpulse = maxMotorImpulse; } + // extra motor API, including ability to set a target rotation (as opposed to angular velocity) + // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to + // maintain a given angular target. + void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; } + void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; } + void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B. + void setMotorTarget(btScalar targetAngle, btScalar dt); + + void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) { - m_lowerLimit = low; - m_upperLimit = high; - +#ifdef _BT_USE_CENTER_LIMIT_ + m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor); +#else + m_lowerLimit = btNormalizeAngle(low); + m_upperLimit = btNormalizeAngle(high); m_limitSoftness = _softness; m_biasFactor = _biasFactor; m_relaxationFactor = _relaxationFactor; +#endif + } + + void setAxis(btVector3& axisInA) + { + btVector3 rbAxisA1, rbAxisA2; + btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); + btVector3 pivotInA = m_rbAFrame.getOrigin(); +// m_rbAFrame.getOrigin() = pivotInA; + m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), + rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), + rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); + + btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA; + + btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); + btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); + btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); + + m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA)); + + m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), + rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), + rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); + m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis(); } btScalar getLowerLimit() const { - return m_lowerLimit; +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getLow(); +#else + return m_lowerLimit; +#endif } btScalar getUpperLimit() const { - return m_upperLimit; +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getHigh(); +#else + return m_upperLimit; +#endif } btScalar getHingeAngle(); - void testLimit(); + btScalar getHingeAngle(const btTransform& transA,const btTransform& transB); + + void testLimit(const btTransform& transA,const btTransform& transB); + + const btTransform& getAFrame() const { return m_rbAFrame; }; + const btTransform& getBFrame() const { return m_rbBFrame; }; - const btTransform& getAFrame() { return m_rbAFrame; }; - const btTransform& getBFrame() { return m_rbBFrame; }; + btTransform& getAFrame() { return m_rbAFrame; }; + btTransform& getBFrame() { return m_rbBFrame; }; inline int getSolveLimit() { - return m_solveLimit; +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.isLimit(); +#else + return m_solveLimit; +#endif } inline btScalar getLimitSign() { +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getSign(); +#else return m_limitSign; +#endif } inline bool getAngularOnly() @@ -172,7 +280,101 @@ public: { return m_maxMotorImpulse; } + // access for UseFrameOffset + bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } + void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } + + + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///If no axis is provided, it uses the default axis for this constraint. + virtual void setParam(int num, btScalar value, int axis = -1); + ///return the local value of parameter + virtual btScalar getParam(int num, int axis = -1) const; + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btHingeConstraintDoubleData +{ + btTypedConstraintData m_typeConstraintData; + btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTransformDoubleData m_rbBFrame; + int m_useReferenceFrameA; + int m_angularOnly; + int m_enableAngularMotor; + float m_motorTargetVelocity; + float m_maxMotorImpulse; + + float m_lowerLimit; + float m_upperLimit; + float m_limitSoftness; + float m_biasFactor; + float m_relaxationFactor; + +}; +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btHingeConstraintFloatData +{ + btTypedConstraintData m_typeConstraintData; + btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTransformFloatData m_rbBFrame; + int m_useReferenceFrameA; + int m_angularOnly; + + int m_enableAngularMotor; + float m_motorTargetVelocity; + float m_maxMotorImpulse; + + float m_lowerLimit; + float m_upperLimit; + float m_limitSoftness; + float m_biasFactor; + float m_relaxationFactor; }; + + +SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const +{ + return sizeof(btHingeConstraintData); +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer; + btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer); + + m_rbAFrame.serialize(hingeData->m_rbAFrame); + m_rbBFrame.serialize(hingeData->m_rbBFrame); + + hingeData->m_angularOnly = m_angularOnly; + hingeData->m_enableAngularMotor = m_enableAngularMotor; + hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse); + hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity); + hingeData->m_useReferenceFrameA = m_useReferenceFrameA; +#ifdef _BT_USE_CENTER_LIMIT_ + hingeData->m_lowerLimit = float(m_limit.getLow()); + hingeData->m_upperLimit = float(m_limit.getHigh()); + hingeData->m_limitSoftness = float(m_limit.getSoftness()); + hingeData->m_biasFactor = float(m_limit.getBiasFactor()); + hingeData->m_relaxationFactor = float(m_limit.getRelaxationFactor()); +#else + hingeData->m_lowerLimit = float(m_lowerLimit); + hingeData->m_upperLimit = float(m_upperLimit); + hingeData->m_limitSoftness = float(m_limitSoftness); + hingeData->m_biasFactor = float(m_biasFactor); + hingeData->m_relaxationFactor = float(m_relaxationFactor); +#endif + + return btHingeConstraintDataName; +} + #endif //HINGECONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h index bfeb24c2dfb..22a8af66b8e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -28,7 +28,7 @@ subject to the following restrictions: /// Jacobian entry is an abstraction that allows to describe constraints /// it can be used in combination with a constraint solver /// Can be used to relate the effect of an impulse to the constraint error -class btJacobianEntry +ATTRIBUTE_ALIGNED16(class) btJacobianEntry { public: btJacobianEntry() {}; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index 1da749517e8..7e0d93b9765 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -20,14 +20,11 @@ subject to the following restrictions: -btPoint2PointConstraint::btPoint2PointConstraint() -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE), -m_useSolveConstraintObsolete(false) -{ -} + btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB) :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), +m_flags(0), m_useSolveConstraintObsolete(false) { @@ -36,6 +33,7 @@ m_useSolveConstraintObsolete(false) btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), +m_flags(0), m_useSolveConstraintObsolete(false) { @@ -43,6 +41,7 @@ m_useSolveConstraintObsolete(false) void btPoint2PointConstraint::buildJacobian() { + ///we need it for both methods { m_appliedImpulse = btScalar(0.); @@ -66,11 +65,16 @@ void btPoint2PointConstraint::buildJacobian() } } -} +} void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info) { + getInfo1NonVirtual(info); +} + +void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info) +{ if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; @@ -82,22 +86,26 @@ void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info) } } + + + void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info) { + getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); +} + +void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans) +{ btAssert(!m_useSolveConstraintObsolete); //retrieve matrices - btTransform body0_trans; - body0_trans = m_rbA.getCenterOfMassTransform(); - btTransform body1_trans; - body1_trans = m_rbB.getCenterOfMassTransform(); // anchor points in global coordinates with respect to body PORs. // set jacobian info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; + info->m_J1linearAxis[info->rowskip+1] = 1; + info->m_J1linearAxis[2*info->rowskip+2] = 1; btVector3 a1 = body0_trans.getBasis()*getPivotInA(); { @@ -126,14 +134,21 @@ void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info) // set right hand side - btScalar k = info->fps * info->erp; + btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp; + btScalar k = info->fps * currERP; int j; - for (j=0; j<3; j++) { - info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); + info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); } + if(m_flags & BT_P2P_FLAGS_CFM) + { + for (j=0; j<3; j++) + { + info->cfm[j*info->rowskip] = m_cfm; + } + } btScalar impulseClamp = m_setting.m_impulseClamp;// for (j=0; j<3; j++) @@ -144,87 +159,72 @@ void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info) info->m_upperLimit[j*info->rowskip] = impulseClamp; } } + info->m_damping = m_setting.m_damping; } -void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) -{ - if (m_useSolveConstraintObsolete) - { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; +void btPoint2PointConstraint::updateRHS(btScalar timeStep) +{ + (void)timeStep; - btVector3 normal(0,0,0); - - - // btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - // btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); +} - for (int i=0;i<3;i++) - { - normal[i] = 1; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - //this jacobian entry could be re-used for all iterations - - btVector3 vel1,vel2; - bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); - bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); - btVector3 vel = vel1 - vel2; - - btScalar rel_vel; - rel_vel = normal.dot(vel); - - /* - //velocity error (first order error) - btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - */ - - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - - btScalar deltaImpulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; - - btScalar impulseClamp = m_setting.m_impulseClamp; - - const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse; - if (sum < -impulseClamp) - { - deltaImpulse = -impulseClamp-m_appliedImpulse; - m_appliedImpulse = -impulseClamp; - } - else if (sum > impulseClamp) - { - deltaImpulse = impulseClamp-m_appliedImpulse; - m_appliedImpulse = impulseClamp; - } - else - { - m_appliedImpulse = sum; - } - - - btVector3 impulse_vector = normal * deltaImpulse; - - btVector3 ftorqueAxis1 = rel_pos1.cross(normal); - btVector3 ftorqueAxis2 = rel_pos2.cross(normal); - bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse); - bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse); - - - normal[i] = 0; +///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///If no axis is provided, it uses the default axis for this constraint. +void btPoint2PointConstraint::setParam(int num, btScalar value, int axis) +{ + if(axis != -1) + { + btAssertConstrParams(0); + } + else + { + switch(num) + { + case BT_CONSTRAINT_ERP : + case BT_CONSTRAINT_STOP_ERP : + m_erp = value; + m_flags |= BT_P2P_FLAGS_ERP; + break; + case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_STOP_CFM : + m_cfm = value; + m_flags |= BT_P2P_FLAGS_CFM; + break; + default: + btAssertConstrParams(0); } } } -void btPoint2PointConstraint::updateRHS(btScalar timeStep) +///return the local value of parameter +btScalar btPoint2PointConstraint::getParam(int num, int axis) const { - (void)timeStep; - + btScalar retVal(SIMD_INFINITY); + if(axis != -1) + { + btAssertConstrParams(0); + } + else + { + switch(num) + { + case BT_CONSTRAINT_ERP : + case BT_CONSTRAINT_STOP_ERP : + btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP); + retVal = m_erp; + break; + case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_STOP_CFM : + btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM); + retVal = m_cfm; + break; + default: + btAssertConstrParams(0); + } + } + return retVal; } - + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index e2b865cd484..b589ee68254 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -22,6 +22,15 @@ subject to the following restrictions: class btRigidBody; + +#ifdef BT_USE_DOUBLE_PRECISION +#define btPoint2PointConstraintData btPoint2PointConstraintDoubleData +#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData" +#else +#define btPoint2PointConstraintData btPoint2PointConstraintFloatData +#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData" +#endif //BT_USE_DOUBLE_PRECISION + struct btConstraintSetting { btConstraintSetting() : @@ -35,8 +44,14 @@ struct btConstraintSetting btScalar m_impulseClamp; }; +enum btPoint2PointFlags +{ + BT_P2P_FLAGS_ERP = 1, + BT_P2P_FLAGS_CFM = 2 +}; + /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space -class btPoint2PointConstraint : public btTypedConstraint +ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint { #ifdef IN_PARALLELL_SOLVER public: @@ -46,7 +61,9 @@ public: btVector3 m_pivotInA; btVector3 m_pivotInB; - + int m_flags; + btScalar m_erp; + btScalar m_cfm; public: @@ -59,16 +76,16 @@ public: btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA); - btPoint2PointConstraint(); virtual void buildJacobian(); virtual void getInfo1 (btConstraintInfo1* info); - virtual void getInfo2 (btConstraintInfo2* info); + void getInfo1NonVirtual (btConstraintInfo1* info); + virtual void getInfo2 (btConstraintInfo2* info); - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); + void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans); void updateRHS(btScalar timeStep); @@ -92,7 +109,53 @@ public: return m_pivotInB; } + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///If no axis is provided, it uses the default axis for this constraint. + virtual void setParam(int num, btScalar value, int axis = -1); + ///return the local value of parameter + virtual btScalar getParam(int num, int axis = -1) const; + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + }; +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btPoint2PointConstraintFloatData +{ + btTypedConstraintData m_typeConstraintData; + btVector3FloatData m_pivotInA; + btVector3FloatData m_pivotInB; +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btPoint2PointConstraintDoubleData +{ + btTypedConstraintData m_typeConstraintData; + btVector3DoubleData m_pivotInA; + btVector3DoubleData m_pivotInB; +}; + + +SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const +{ + return sizeof(btPoint2PointConstraintData); + +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer; + + btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer); + m_pivotInA.serialize(p2pData->m_pivotInA); + m_pivotInB.serialize(p2pData->m_pivotInB); + + return btPoint2PointConstraintDataName; +} + #endif //POINT2POINTCONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index dbd09b39238..0fc93cd3756 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -34,6 +34,8 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include <string.h> //for memset +int gNumSplitImpulseRecoveries = 0; + btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() :m_btSeed2(0) { @@ -46,24 +48,24 @@ btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() #ifdef USE_SIMD #include <emmintrin.h> -#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) -static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 ) +#define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) +static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) { __m128 result = _mm_mul_ps( vec0, vec1); - return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) ); + return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) ); } #endif//USE_SIMD // Project Gauss Seidel or the equivalent Sequential Impulse -void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); @@ -76,26 +78,26 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); #else resolveSingleConstraintRowGeneric(body1,body2,c); #endif } // Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); + const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); - const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; +// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; @@ -114,19 +116,19 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( { c.m_appliedImpulse = sum; } - body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); - body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); + body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); @@ -136,24 +138,24 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); #else resolveSingleConstraintRowLowerLimit(body1,body2,c); #endif } // Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); + const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; @@ -167,8 +169,73 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( { c.m_appliedImpulse = sum; } - body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); - body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); + body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); +} + + +void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( + btRigidBody& body1, + btRigidBody& body2, + const btSolverConstraint& c) +{ + if (c.m_rhsPenetration) + { + gNumSplitImpulseRecoveries++; + btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); + const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); + + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse; + c.m_appliedPushImpulse = c.m_lowerLimit; + } + else + { + c.m_appliedPushImpulse = sum; + } + body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + } +} + + void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) +{ +#ifdef USE_SIMD + if (!c.m_rhsPenetration) + return; + + gNumSplitImpulseRecoveries++; + + __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); + __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); + btSimdScalar resultLowerLess,resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); + c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); + __m128 impulseMagnitude = deltaImpulse; + body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); +#else + resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); +#endif } @@ -210,29 +277,33 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n) } - +#if 0 void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) { btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; - solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); - solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f); + solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f); if (rb) { - solverBody->m_invMass = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor(); + solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor(); solverBody->m_originalBody = rb; solverBody->m_angularFactor = rb->getAngularFactor(); } else { - solverBody->m_invMass.setValue(0,0,0); + solverBody->internalGetInvMass().setValue(0,0,0); solverBody->m_originalBody = 0; solverBody->m_angularFactor.setValue(1,1,1); } } +#endif + + -int gNumSplitImpulseRecoveries = 0; btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution) { @@ -258,27 +329,23 @@ void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirec } - -btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation) +void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) { btRigidBody* body0=btRigidBody::upcast(colObj0); btRigidBody* body1=btRigidBody::upcast(colObj1); - btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand(); - memset(&solverConstraint,0xff,sizeof(btSolverConstraint)); solverConstraint.m_contactNormal = normalAxis; - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_frictionIndex = frictionIndex; + solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody(); + solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody(); solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_originalContactPoint = 0; solverConstraint.m_appliedImpulse = 0.f; - // solverConstraint.m_appliedPushImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; { btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); @@ -333,21 +400,31 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c rel_vel = vel1Dotn+vel2Dotn; - btScalar positionalError = 0.f; +// btScalar positionalError = 0.f; - btSimdScalar velocityError = - rel_vel; + btSimdScalar velocityError = desiredVelocity - rel_vel; btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_cfm = 0.f; + solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; } +} + + +btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) +{ + btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); + solverConstraint.m_frictionIndex = frictionIndex; + setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); return solverConstraint; } int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body) { +#if 0 int solverBodyIdA = -1; if (body.getCompanionId() >= 0) @@ -369,70 +446,36 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& } } return solverBodyIdA; +#endif + return 0; } #include <stdio.h> - -void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, + btCollisionObject* colObj0, btCollisionObject* colObj1, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btVector3& vel, btScalar& rel_vel, btScalar& relaxation, + btVector3& rel_pos1, btVector3& rel_pos2) { - btCollisionObject* colObj0=0,*colObj1=0; - - colObj0 = (btCollisionObject*)manifold->getBody0(); - colObj1 = (btCollisionObject*)manifold->getBody1(); - - int solverBodyIdA=-1; - int solverBodyIdB=-1; - - if (manifold->getNumContacts()) - { - solverBodyIdA = getOrInitSolverBody(*colObj0); - solverBodyIdB = getOrInitSolverBody(*colObj1); - } - - ///avoid collision response between two static objects - if (!solverBodyIdA && !solverBodyIdB) - return; - - btVector3 rel_pos1; - btVector3 rel_pos2; - btScalar relaxation; - - for (int j=0;j<manifold->getNumContacts();j++) - { - - btManifoldPoint& cp = manifold->getContactPoint(j); - - if (cp.getDistance() <= manifold->getContactProcessingThreshold()) - { + btRigidBody* rb0 = btRigidBody::upcast(colObj0); + btRigidBody* rb1 = btRigidBody::upcast(colObj1); const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos2 = cp.getPositionWorldOnB(); +// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); +// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - relaxation = 1.f; - btScalar rel_vel; - btVector3 vel; - int frictionIndex = m_tmpSolverContactConstraintPool.size(); + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - { - btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand(); - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); - - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - - solverConstraint.m_originalContactPoint = &cp; - - btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); { #ifdef COMPUTE_IMPULSE_DENOM btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); @@ -462,12 +505,12 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB); - btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); - btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - vel = vel1 - vel2; - rel_vel = cp.m_normalWorldOnB.dot(vel); + btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); + btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); + vel = vel1 - vel2; + rel_vel = cp.m_normalWorldOnB.dot(vel); btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; @@ -494,15 +537,15 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m { solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); + rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); } else { solverConstraint.m_appliedImpulse = 0.f; } - // solverConstraint.m_appliedPushImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; { btScalar rel_vel; @@ -514,69 +557,46 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m rel_vel = vel1Dotn+vel2Dotn; btScalar positionalError = 0.f; - positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; btScalar velocityError = restitution - rel_vel;// * damping; + + if (penetration>0) + { + positionalError = 0; + velocityError -= penetration / infoGlobal.m_timeStep; + } else + { + positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; + } + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } solverConstraint.m_cfm = 0.f; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; } - /////setup the friction constraints - - if (1) - { - solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) - { - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) - { - cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); - if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); - cp.m_lateralFrictionDir2.normalize();//?? - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - cp.m_lateralFrictionInitialized = true; - } else - { - //re-calculate friction direction every frame, todo: check if this is really needed - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } +} - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - cp.m_lateralFrictionInitialized = true; - } - - } else - { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } +void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, + btRigidBody* rb0, btRigidBody* rb1, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) +{ if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) { { @@ -585,9 +605,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m { frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); + rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); + rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse); } else { frictionConstraint1.m_appliedImpulse = 0.f; @@ -601,9 +621,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m { frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); + rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); + rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse); } else { frictionConstraint2.m_appliedImpulse = 0.f; @@ -619,16 +639,109 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m frictionConstraint2.m_appliedImpulse = 0.f; } } +} + + + + +void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) +{ + btCollisionObject* colObj0=0,*colObj1=0; + + colObj0 = (btCollisionObject*)manifold->getBody0(); + colObj1 = (btCollisionObject*)manifold->getBody1(); + + + btRigidBody* solverBodyA = btRigidBody::upcast(colObj0); + btRigidBody* solverBodyB = btRigidBody::upcast(colObj1); + + ///avoid collision response between two static objects + if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass())) + return; + + for (int j=0;j<manifold->getNumContacts();j++) + { + + btManifoldPoint& cp = manifold->getContactPoint(j); + + if (cp.getDistance() <= manifold->getContactProcessingThreshold()) + { + btVector3 rel_pos1; + btVector3 rel_pos2; + btScalar relaxation; + btScalar rel_vel; + btVector3 vel; + + int frictionIndex = m_tmpSolverContactConstraintPool.size(); + btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); + btRigidBody* rb0 = btRigidBody::upcast(colObj0); + btRigidBody* rb1 = btRigidBody::upcast(colObj1); + solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody(); + solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody(); + solverConstraint.m_originalContactPoint = &cp; + + setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); + +// const btVector3& pos1 = cp.getPositionWorldOnA(); +// const btVector3& pos2 = cp.getPositionWorldOnB(); + + /////setup the friction constraints + + solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); + + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) + { + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); + if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) + { + cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); + if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); + cp.m_lateralFrictionDir2.normalize();//?? + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + cp.m_lateralFrictionInitialized = true; + } else + { + //re-calculate friction direction every frame, todo: check if this is really needed + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + cp.m_lateralFrictionInitialized = true; } - } + } else + { + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); + } + + setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal); } } } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) { BT_PROFILE("solveGroupCacheFriendlySetup"); (void)stackAlloc; @@ -641,6 +754,33 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol return 0.f; } + if (infoGlobal.m_splitImpulse) + { + for (int i = 0; i < numBodies; i++) + { + btRigidBody* body = btRigidBody::upcast(bodies[i]); + if (body) + { + body->internalGetDeltaLinearVelocity().setZero(); + body->internalGetDeltaAngularVelocity().setZero(); + body->internalGetPushVelocity().setZero(); + body->internalGetTurnVelocity().setZero(); + } + } + } + else + { + for (int i = 0; i < numBodies; i++) + { + btRigidBody* body = btRigidBody::upcast(bodies[i]); + if (body) + { + body->internalGetDeltaLinearVelocity().setZero(); + body->internalGetDeltaAngularVelocity().setZero(); + } + } + } + if (1) { int j; @@ -650,10 +790,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol constraint->buildJacobian(); } } - - btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&fixedBody,0); - //btRigidBody* rb0=0,*rb1=0; //if (1) @@ -662,26 +798,25 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol int totalNumRows = 0; int i; + + m_tmpConstraintSizesPool.resize(numConstraints); //calculate the total number of contraint rows for (i=0;i<numConstraints;i++) { - - btTypedConstraint::btConstraintInfo1 info1; + btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; constraints[i]->getInfo1(&info1); totalNumRows += info1.m_numConstraintRows; } m_tmpSolverNonContactConstraintPool.resize(totalNumRows); - btTypedConstraint::btConstraintInfo1 info1; - info1.m_numConstraintRows = 0; - - + ///setup the btSolverConstraints int currentRow = 0; - for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows) + for (i=0;i<numConstraints;i++) { - constraints[i]->getInfo1(&info1); + const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + if (info1.m_numConstraintRows) { btAssert(currentRow<totalNumRows); @@ -694,12 +829,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btRigidBody& rbA = constraint->getRigidBodyA(); btRigidBody& rbB = constraint->getRigidBodyB(); - int solverBodyIdA = getOrInitSolverBody(rbA); - int solverBodyIdB = getOrInitSolverBody(rbB); - - btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; - + int j; for ( j=0;j<info1.m_numConstraintRows;j++) { @@ -708,14 +838,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol currentConstraintRow[j].m_upperLimit = FLT_MAX; currentConstraintRow[j].m_appliedImpulse = 0.f; currentConstraintRow[j].m_appliedPushImpulse = 0.f; - currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; - currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; + currentConstraintRow[j].m_solverBodyA = &rbA; + currentConstraintRow[j].m_solverBodyB = &rbB; } - bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); - bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); - bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); - bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); @@ -730,15 +860,19 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol ///the size of btSolverConstraint needs be a multiple of btScalar btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); info2.m_constraintError = ¤tConstraintRow->m_rhs; + currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; + info2.m_damping = infoGlobal.m_damping; info2.cfm = ¤tConstraintRow->m_cfm; info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + info2.m_numIterations = infoGlobal.m_numIterations; constraints[i]->getInfo2(&info2); ///finalize the constraint setup for ( j=0;j<info1.m_numConstraintRows;j++) { btSolverConstraint& solverConstraint = currentConstraintRow[j]; + solverConstraint.m_originalContactPoint = constraint; { const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; @@ -775,7 +909,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btScalar restitution = 0.f; btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 - btScalar velocityError = restitution - rel_vel;// * damping; + btScalar velocityError = restitution - rel_vel * info2.m_damping; btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; @@ -784,13 +918,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } } + currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; } } { int i; btPersistentManifold* manifold = 0; - btCollisionObject* colObj0=0,*colObj1=0; +// btCollisionObject* colObj0=0,*colObj1=0; for (i=0;i<numManifolds;i++) @@ -827,152 +962,173 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) +btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) { - BT_PROFILE("solveGroupCacheFriendlyIterations"); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - //should traverse the contacts random order... - int iteration; + int j; + + if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { - for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) - { + if ((iteration & 7) == 0) { + for (j=0; j<numConstraintPool; ++j) { + int tmp = m_orderTmpConstraintPool[j]; + int swapi = btRandInt2(j+1); + m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; + m_orderTmpConstraintPool[swapi] = tmp; + } - int j; - if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) + for (j=0; j<numFrictionPool; ++j) { + int tmp = m_orderFrictionConstraintPool[j]; + int swapi = btRandInt2(j+1); + m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; + m_orderFrictionConstraintPool[swapi] = tmp; + } + } + } + + if (infoGlobal.m_solverMode & SOLVER_SIMD) + { + ///solve all joint constraints, using SIMD, if available + for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) + { + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; + resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); + } + + for (j=0;j<numConstraints;j++) + { + constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + } + + ///solve all contact constraints using SIMD, if available + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (j=0;j<numPoolConstraints;j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); + + } + ///solve all friction constraints, using SIMD, if available + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + for (j=0;j<numFrictionPoolConstraints;j++) + { + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + + if (totalImpulse>btScalar(0)) { - if ((iteration & 7) == 0) { - for (j=0; j<numConstraintPool; ++j) { - int tmp = m_orderTmpConstraintPool[j]; - int swapi = btRandInt2(j+1); - m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; - m_orderTmpConstraintPool[swapi] = tmp; - } + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - for (j=0; j<numFrictionPool; ++j) { - int tmp = m_orderFrictionConstraintPool[j]; - int swapi = btRandInt2(j+1); - m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; - m_orderFrictionConstraintPool[swapi] = tmp; - } - } + resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold); } + } + } else + { - if (infoGlobal.m_solverMode & SOLVER_SIMD) + ///solve all joint constraints + for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) + { + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; + resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); + } + + for (j=0;j<numConstraints;j++) + { + constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + } + ///solve all contact constraints + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (j=0;j<numPoolConstraints;j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); + } + ///solve all friction constraints + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + for (j=0;j<numFrictionPoolConstraints;j++) + { + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + + if (totalImpulse>btScalar(0)) { - ///solve all joint constraints, using SIMD, if available - for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) - { - btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); - } + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - for (j=0;j<numConstraints;j++) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } + resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); + } + } + } + return 0.f; +} - ///solve all contact constraints using SIMD, if available - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - } - ///solve all friction constraints, using SIMD, if available - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;j<numFrictionPoolConstraints;j++) +void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +{ + int iteration; + if (infoGlobal.m_splitImpulse) + { + if (infoGlobal.m_solverMode & SOLVER_SIMD) + { + for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) + { { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - - if (totalImpulse>btScalar(0)) + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int j; + for (j=0;j<numPoolConstraints;j++) { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); } } - } else + } + } + else + { + for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) { - - ///solve all joint constraints - for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) { - btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); - } - - for (j=0;j<numConstraints;j++) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } - - ///solve all contact constraints - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - } - ///solve all friction constraints - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;j<numFrictionPoolConstraints;j++) - { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - - if (totalImpulse>btScalar(0)) + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int j; + for (j=0;j<numPoolConstraints;j++) { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); } } } - - - } } - return 0.f; } - - -/// btSequentialImpulseConstraintSolver Sequentially applies impulses -btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) { + BT_PROFILE("solveGroupCacheFriendlyIterations"); + //should traverse the contacts random order... + int iteration; + { + for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) + { + solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); + } + + solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); + } + return 0.f; +} - BT_PROFILE("solveGroup"); - //we only implement SOLVER_CACHE_FRIENDLY now - //you need to provide at least some bodies - btAssert(bodies); - btAssert(numBodies); - - int i; - - solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) +{ int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; + int i,j; for (j=0;j<numPoolConstraints;j++) { @@ -990,22 +1146,36 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod //do a callback here? } + numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); + for (j=0;j<numPoolConstraints;j++) + { + const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; + btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; + btScalar sum = constr->internalGetAppliedImpulse(); + sum += solverConstr.m_appliedImpulse; + constr->internalSetAppliedImpulse(sum); + } + + if (infoGlobal.m_splitImpulse) { - for ( i=0;i<m_tmpSolverBodyPool.size();i++) + for ( i=0;i<numBodies;i++) { - m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep); + btRigidBody* body = btRigidBody::upcast(bodies[i]); + if (body) + body->internalWritebackVelocity(infoGlobal.m_timeStep); } } else { - for ( i=0;i<m_tmpSolverBodyPool.size();i++) + for ( i=0;i<numBodies;i++) { - m_tmpSolverBodyPool[i].writebackVelocity(); + btRigidBody* body = btRigidBody::upcast(bodies[i]); + if (body) + body->internalWritebackVelocity(); } } - m_tmpSolverBodyPool.resize(0); m_tmpSolverContactConstraintPool.resize(0); m_tmpSolverNonContactConstraintPool.resize(0); m_tmpSolverContactFrictionConstraintPool.resize(0); @@ -1015,15 +1185,33 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod +/// btSequentialImpulseConstraintSolver Sequentially applies impulses +btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) +{ + BT_PROFILE("solveGroup"); + //you need to provide at least some bodies + btAssert(bodies); + btAssert(numBodies); + solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); + solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - + solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); + + return 0.f; +} void btSequentialImpulseConstraintSolver::reset() { m_btSeed2 = 0; } +btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody() +{ + static btRigidBody s_fixed(0, 0,0); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); + return s_fixed; +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 90e7fc8354d..f268ec9ae74 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -21,48 +21,76 @@ class btIDebugDraw; #include "btContactConstraint.h" #include "btSolverBody.h" #include "btSolverConstraint.h" - - +#include "btTypedConstraint.h" +#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. class btSequentialImpulseConstraintSolver : public btConstraintSolver { protected: - btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool; btConstraintArray m_tmpSolverContactConstraintPool; btConstraintArray m_tmpSolverNonContactConstraintPool; btConstraintArray m_tmpSolverContactFrictionConstraintPool; btAlignedObjectArray<int> m_orderTmpConstraintPool; btAlignedObjectArray<int> m_orderFrictionConstraintPool; + btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool; - btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation); + void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyIdB, + btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, + btScalar desiredVelocity=0., btScalar cfmSlip=0.); + + btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); + void setupContactConstraint(btSolverConstraint& solverConstraint, btCollisionObject* colObj0, btCollisionObject* colObj1, btManifoldPoint& cp, + const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation, + btVector3& rel_pos1, btVector3& rel_pos2); + + void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, btRigidBody* rb0, btRigidBody* rb1, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); + ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction unsigned long m_btSeed2; - void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); +// void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); + + void resolveSplitPenetrationSIMD( + btRigidBody& body1, + btRigidBody& body2, + const btSolverConstraint& contactConstraint); + void resolveSplitPenetrationImpulseCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo); + btRigidBody& body1, + btRigidBody& body2, + const btSolverConstraint& contactConstraint); //internal method int getOrInitSolverBody(btCollisionObject& body); - void resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - void resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - void resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); +protected: + static btRigidBody& getFixedBody(); + + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + + public: @@ -71,9 +99,8 @@ public: virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher); - btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + ///clear internal cached data and reset random seed virtual void reset(); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index 133aed7271b..b69f46da1b4 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -25,7 +25,7 @@ April 04, 2008 #include "LinearMath/btTransformUtil.h" #include <new> - +#define USE_OFFSET_FOR_CONSTANT_FRAME true void btSliderConstraint::initParams() { @@ -36,21 +36,27 @@ void btSliderConstraint::initParams() m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; m_dampingDirLin = btScalar(0.); + m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM; m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; m_dampingDirAng = btScalar(0.); + m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM; m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; + m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM; m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; + m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM; m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; + m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM; m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; + m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM; m_poweredLinMotor = false; m_targetLinMotorVelocity = btScalar(0.); @@ -62,43 +68,38 @@ void btSliderConstraint::initParams() m_maxAngMotorForce = btScalar(0.); m_accumulatedAngMotorImpulse = btScalar(0.0); -} - + m_flags = 0; + m_flags = 0; + m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME; -btSliderConstraint::btSliderConstraint() - :btTypedConstraint(SLIDER_CONSTRAINT_TYPE), - m_useLinearReferenceFrameA(true), - m_useSolveConstraintObsolete(false) -// m_useSolveConstraintObsolete(true) -{ - initParams(); + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); } + + btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB) - , m_frameInA(frameInA) - , m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA), - m_useSolveConstraintObsolete(false) -// m_useSolveConstraintObsolete(true) + : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB), + m_useSolveConstraintObsolete(false), + m_frameInA(frameInA), + m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) { initParams(); } -static btRigidBody s_fixed(0, 0, 0); -btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) - : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, s_fixed, rbB) - , - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameB), - m_useSolveConstraintObsolete(false) -// m_useSolveConstraintObsolete(true) + +btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA) + : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB), + m_useSolveConstraintObsolete(false), + m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) { - ///not providing rigidbody B means implicitly using worldspace for body B + ///not providing rigidbody A means implicitly using worldspace for body A + m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; // m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin()); initParams(); @@ -106,117 +107,211 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& fram -void btSliderConstraint::buildJacobian() + + + +void btSliderConstraint::getInfo1(btConstraintInfo1* info) { - if (!m_useSolveConstraintObsolete) - { - return; - } - if(m_useLinearReferenceFrameA) + if (m_useSolveConstraintObsolete) { - buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB); + info->m_numConstraintRows = 0; + info->nub = 0; } else { - buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA); + info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular + info->nub = 2; + //prepare constraint + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + testAngLimits(); + testLinLimits(); + if(getSolveLinLimit() || getPoweredLinMotor()) + { + info->m_numConstraintRows++; // limit 3rd linear as well + info->nub--; + } + if(getSolveAngLimit() || getPoweredAngMotor()) + { + info->m_numConstraintRows++; // limit 3rd angular as well + info->nub--; + } } } +void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info) +{ + + info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used) + info->nub = 0; +} + +void btSliderConstraint::getInfo2(btConstraintInfo2* info) +{ + getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass()); +} + -void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) + + + + +void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) { - //calculate transforms - m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA; - m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB; + if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) + { + m_calculatedTransformA = transA * m_frameInA; + m_calculatedTransformB = transB * m_frameInB; + } + else + { + m_calculatedTransformA = transB * m_frameInB; + m_calculatedTransformB = transA * m_frameInA; + } m_realPivotAInW = m_calculatedTransformA.getOrigin(); m_realPivotBInW = m_calculatedTransformB.getOrigin(); m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X - m_delta = m_realPivotBInW - m_realPivotAInW; + if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete) + { + m_delta = m_realPivotBInW - m_realPivotAInW; + } + else + { + m_delta = m_realPivotAInW - m_realPivotBInW; + } m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition(); - m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition(); btVector3 normalWorld; int i; //linear part for(i = 0; i < 3; i++) { normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - new (&m_jacLin[i]) btJacobianEntry( - rbA.getCenterOfMassTransform().getBasis().transpose(), - rbB.getCenterOfMassTransform().getBasis().transpose(), - m_relPosA, - m_relPosB, - normalWorld, - rbA.getInvInertiaDiagLocal(), - rbA.getInvMass(), - rbB.getInvInertiaDiagLocal(), - rbB.getInvMass() - ); - m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal(); m_depth[i] = m_delta.dot(normalWorld); } - testLinLimits(); - // angular part - for(i = 0; i < 3; i++) - { - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - new (&m_jacAng[i]) btJacobianEntry( - normalWorld, - rbA.getCenterOfMassTransform().getBasis().transpose(), - rbB.getCenterOfMassTransform().getBasis().transpose(), - rbA.getInvInertiaDiagLocal(), - rbB.getInvInertiaDiagLocal() - ); - } - testAngLimits(); - btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); - m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA)); - // clear accumulator for motors - m_accumulatedLinMotorImpulse = btScalar(0.0); - m_accumulatedAngMotorImpulse = btScalar(0.0); } + - -void btSliderConstraint::getInfo1(btConstraintInfo1* info) +void btSliderConstraint::testLinLimits(void) { - if (m_useSolveConstraintObsolete) + m_solveLinLim = false; + m_linPos = m_depth[0]; + if(m_lowerLinLimit <= m_upperLinLimit) { - info->m_numConstraintRows = 0; - info->nub = 0; + if(m_depth[0] > m_upperLinLimit) + { + m_depth[0] -= m_upperLinLimit; + m_solveLinLim = true; + } + else if(m_depth[0] < m_lowerLinLimit) + { + m_depth[0] -= m_lowerLinLimit; + m_solveLinLim = true; + } + else + { + m_depth[0] = btScalar(0.); + } } else { - info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular - info->nub = 2; - //prepare constraint - calculateTransforms(); - testLinLimits(); - if(getSolveLinLimit() || getPoweredLinMotor()) + m_depth[0] = btScalar(0.); + } +} + + + +void btSliderConstraint::testAngLimits(void) +{ + m_angDepth = btScalar(0.); + m_solveAngLim = false; + if(m_lowerAngLimit <= m_upperAngLimit) + { + const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1); + const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); + const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); +// btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); + btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0)); + rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit); + m_angPos = rot; + if(rot < m_lowerAngLimit) { - info->m_numConstraintRows++; // limit 3rd linear as well - info->nub--; - } - testAngLimits(); - if(getSolveAngLimit() || getPoweredAngMotor()) + m_angDepth = rot - m_lowerAngLimit; + m_solveAngLim = true; + } + else if(rot > m_upperAngLimit) { - info->m_numConstraintRows++; // limit 3rd angular as well - info->nub--; + m_angDepth = rot - m_upperAngLimit; + m_solveAngLim = true; } } } +btVector3 btSliderConstraint::getAncorInA(void) +{ + btVector3 ancorInA; + ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis; + ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA; + return ancorInA; +} + -void btSliderConstraint::getInfo2(btConstraintInfo2* info) +btVector3 btSliderConstraint::getAncorInB(void) +{ + btVector3 ancorInB; + ancorInB = m_frameInB.getOrigin(); + return ancorInB; +} + + +void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass ) { - btAssert(!m_useSolveConstraintObsolete); - int i, s = info->rowskip; const btTransform& trA = getCalculatedTransformA(); const btTransform& trB = getCalculatedTransformB(); + + btAssert(!m_useSolveConstraintObsolete); + int i, s = info->rowskip; + btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f); - // make rotations around Y and Z equal + + // difference between frames in WCS + btVector3 ofs = trB.getOrigin() - trA.getOrigin(); + // now get weight factors depending on masses + btScalar miA = rbAinvMass; + btScalar miB = rbBinvMass; + bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); + btScalar miS = miA + miB; + btScalar factA, factB; + if(miS > btScalar(0.f)) + { + factA = miB / miS; + } + else + { + factA = btScalar(0.5f); + } + factB = btScalar(1.0f) - factA; + btVector3 ax1, p, q; + btVector3 ax1A = trA.getBasis().getColumn(0); + btVector3 ax1B = trB.getBasis().getColumn(0); + if(m_useOffsetForConstraintFrame) + { + // get the desired direction of slider axis + // as weighted sum of X-orthos of frameA and frameB in WCS + ax1 = ax1A * factA + ax1B * factB; + ax1.normalize(); + // construct two orthos to slider axis + btPlaneSpace1 (ax1, p, q); + } + else + { // old way - use frameA + ax1 = trA.getBasis().getColumn(0); + // get 2 orthos to slider axis (Y, Z) + p = trA.getBasis().getColumn(1); + q = trA.getBasis().getColumn(2); + } + // make rotations around these orthos equal // the slider axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the slider axis should be equal. thus the constraint equations are @@ -224,12 +319,6 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info) // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the slider axis, and w1 and w2 // are the angular velocity vectors of the two bodies. - // get slider axis (X) - btVector3 ax1 = trA.getBasis().getColumn(0); - // get 2 orthos to slider axis (Y, Z) - btVector3 p = trA.getBasis().getColumn(1); - btVector3 q = trA.getBasis().getColumn(2); - // set the two slider rows info->m_J1angularAxis[0] = p[0]; info->m_J1angularAxis[1] = p[1]; info->m_J1angularAxis[2] = p[2]; @@ -245,8 +334,8 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info) info->m_J2angularAxis[s+2] = -q[2]; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the slider back into alignment. - // if ax1,ax2 are the unit length slider axes as computed from body1 and - // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if ax1A,ax1B are the unit length slider axes as computed from bodyA and + // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). // if "theta" is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize @@ -258,64 +347,126 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info) // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. - btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); - btVector3 ax2 = trB.getBasis().getColumn(0); - btVector3 u = ax1.cross(ax2); +// btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); + btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp; + btScalar k = info->fps * currERP; + + btVector3 u = ax1A.cross(ax1B); info->m_constraintError[0] = k * u.dot(p); info->m_constraintError[s] = k * u.dot(q); - // pull out pos and R for both bodies. also get the connection - // vector c = pos2-pos1. - // next two rows. we want: vel2 = vel1 + w1 x c ... but this would - // result in three equations, so we project along the planespace vectors - // so that sliding along the slider axis is disregarded. for symmetry we - // also consider rotation around center of mass of two bodies (factA and factB). - btTransform bodyA_trans = m_rbA.getCenterOfMassTransform(); - btTransform bodyB_trans = m_rbB.getCenterOfMassTransform(); - int s2 = 2 * s, s3 = 3 * s; - btVector3 c; - btScalar miA = m_rbA.getInvMass(); - btScalar miB = m_rbB.getInvMass(); - btScalar miS = miA + miB; - btScalar factA, factB; - if(miS > btScalar(0.f)) + if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG) { - factA = miB / miS; + info->cfm[0] = m_cfmOrthoAng; + info->cfm[s] = m_cfmOrthoAng; } - else + + int nrow = 1; // last filled row + int srow; + btScalar limit_err; + int limit; + int powered; + + // next two rows. + // we want: velA + wA x relA == velB + wB x relB ... but this would + // result in three equations, so we project along two orthos to the slider axis + + btTransform bodyA_trans = transA; + btTransform bodyB_trans = transB; + nrow++; + int s2 = nrow * s; + nrow++; + int s3 = nrow * s; + btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0); + if(m_useOffsetForConstraintFrame) { - factA = btScalar(0.5f); + // get vector from bodyB to frameB in WCS + relB = trB.getOrigin() - bodyB_trans.getOrigin(); + // get its projection to slider axis + btVector3 projB = ax1 * relB.dot(ax1); + // get vector directed from bodyB to slider axis (and orthogonal to it) + btVector3 orthoB = relB - projB; + // same for bodyA + relA = trA.getOrigin() - bodyA_trans.getOrigin(); + btVector3 projA = ax1 * relA.dot(ax1); + btVector3 orthoA = relA - projA; + // get desired offset between frames A and B along slider axis + btScalar sliderOffs = m_linPos - m_depth[0]; + // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis + btVector3 totalDist = projA + ax1 * sliderOffs - projB; + // get offset vectors relA and relB + relA = orthoA + totalDist * factA; + relB = orthoB - totalDist * factB; + // now choose average ortho to slider axis + p = orthoB * factA + orthoA * factB; + btScalar len2 = p.length2(); + if(len2 > SIMD_EPSILON) + { + p /= btSqrt(len2); + } + else + { + p = trA.getBasis().getColumn(1); + } + // make one more ortho + q = ax1.cross(p); + // fill two rows + tmpA = relA.cross(p); + tmpB = relB.cross(p); + for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; + tmpA = relA.cross(q); + tmpB = relB.cross(q); + if(hasStaticBody && getSolveAngLimit()) + { // to make constraint between static and dynamic objects more rigid + // remove wA (or wB) from equation if angular limit is hit + tmpB *= factB; + tmpA *= factA; + } + for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i]; + for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; + for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; } - if(factA > 0.99f) factA = 0.99f; - if(factA < 0.01f) factA = 0.01f; - factB = btScalar(1.0f) - factA; - c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); - btVector3 tmp = c.cross(p); - for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i]; - tmp = c.cross(q); - for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i]; - - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; - // compute two elements of right hand side. we want to align the offset - // point (in body 2's frame) with the center of body 1. - btVector3 ofs; // offset point in global coordinates - ofs = trB.getOrigin() - trA.getOrigin(); - k = info->fps * info->erp * getSoftnessOrthoLin(); - info->m_constraintError[s2] = k * p.dot(ofs); - info->m_constraintError[s3] = k * q.dot(ofs); - int nrow = 3; // last filled row - int srow; - // check linear limits linear - btScalar limit_err = btScalar(0.0); - int limit = 0; + else + { // old way - maybe incorrect if bodies are not on the slider axis + // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0 + c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); + btVector3 tmp = c.cross(p); + for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i]; + tmp = c.cross(q); + for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i]; + + for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; + for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; + } + // compute two elements of right hand side + + // k = info->fps * info->erp * getSoftnessOrthoLin(); + currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp; + k = info->fps * currERP; + + btScalar rhs = k * p.dot(ofs); + info->m_constraintError[s2] = rhs; + rhs = k * q.dot(ofs); + info->m_constraintError[s3] = rhs; + if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN) + { + info->cfm[s2] = m_cfmOrthoLin; + info->cfm[s3] = m_cfmOrthoLin; + } + + + // check linear limits + limit_err = btScalar(0.0); + limit = 0; if(getSolveLinLimit()) { limit_err = getLinDepth() * signFact; limit = (limit_err > btScalar(0.0)) ? 2 : 1; } - int powered = 0; + powered = 0; if(getPoweredLinMotor()) { powered = 1; @@ -335,16 +486,32 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info) // constraint force is applied at must lie along the same ax1 axis. // a torque couple will result in limited slider-jointed free // bodies from gaining angular momentum. - // the solution used here is to apply the constraint forces at the center of mass of the two bodies - btVector3 ltd; // Linear Torque Decoupling vector (a torque) -// c = btScalar(0.5) * c; - ltd = c.cross(ax1); - info->m_J1angularAxis[srow+0] = factA*ltd[0]; - info->m_J1angularAxis[srow+1] = factA*ltd[1]; - info->m_J1angularAxis[srow+2] = factA*ltd[2]; - info->m_J2angularAxis[srow+0] = factB*ltd[0]; - info->m_J2angularAxis[srow+1] = factB*ltd[1]; - info->m_J2angularAxis[srow+2] = factB*ltd[2]; + if(m_useOffsetForConstraintFrame) + { + // this is needed only when bodyA and bodyB are both dynamic. + if(!hasStaticBody) + { + tmpA = relA.cross(ax1); + tmpB = relB.cross(ax1); + info->m_J1angularAxis[srow+0] = tmpA[0]; + info->m_J1angularAxis[srow+1] = tmpA[1]; + info->m_J1angularAxis[srow+2] = tmpA[2]; + info->m_J2angularAxis[srow+0] = -tmpB[0]; + info->m_J2angularAxis[srow+1] = -tmpB[1]; + info->m_J2angularAxis[srow+2] = -tmpB[2]; + } + } + else + { // The old way. May be incorrect if bodies are not on the slider axis + btVector3 ltd; // Linear Torque Decoupling vector (a torque) + ltd = c.cross(ax1); + info->m_J1angularAxis[srow+0] = factA*ltd[0]; + info->m_J1angularAxis[srow+1] = factA*ltd[1]; + info->m_J1angularAxis[srow+2] = factA*ltd[2]; + info->m_J2angularAxis[srow+0] = factB*ltd[0]; + info->m_J2angularAxis[srow+1] = factB*ltd[1]; + info->m_J2angularAxis[srow+2] = factB*ltd[2]; + } // right-hand part btScalar lostop = getLowerLinLimit(); btScalar histop = getUpperLinLimit(); @@ -355,21 +522,27 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info) info->m_constraintError[srow] = 0.; info->m_lowerLimit[srow] = 0.; info->m_upperLimit[srow] = 0.; + currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp; if(powered) { - info->cfm[nrow] = btScalar(0.0); + if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN) + { + info->cfm[srow] = m_cfmDirLin; + } btScalar tag_vel = getTargetLinMotorVelocity(); - btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp); -// info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity(); + btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP); info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity(); info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps; info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps; } if(limit) { - k = info->fps * info->erp; + k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; - info->cfm[srow] = btScalar(0.0); // stop_cfm; + if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN) + { + info->cfm[srow] = m_cfmLimLin; + } if(lostop == histop) { // limited low and high simultaneously info->m_lowerLimit[srow] = -SIMD_INFINITY; @@ -389,8 +562,8 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info) btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin()); if(bounce > btScalar(0.0)) { - btScalar vel = m_rbA.getLinearVelocity().dot(ax1); - vel -= m_rbB.getLinearVelocity().dot(ax1); + btScalar vel = linVelA.dot(ax1); + vel -= linVelB.dot(ax1); vel *= signFact; // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. @@ -452,19 +625,26 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info) { // the joint motor is ineffective powered = 0; } + currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp; if(powered) { - info->cfm[srow] = btScalar(0.0); - btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp); + if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG) + { + info->cfm[srow] = m_cfmDirAng; + } + btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP); info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity(); info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps; info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps; } if(limit) { - k = info->fps * info->erp; + k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; - info->cfm[srow] = btScalar(0.0); // stop_cfm; + if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG) + { + info->cfm[srow] = m_cfmLimAng; + } if(lostop == histop) { // limited low and high simultaneously @@ -518,316 +698,160 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info) } - -void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///If no axis is provided, it uses the default axis for this constraint. +void btSliderConstraint::setParam(int num, btScalar value, int axis) { - if (m_useSolveConstraintObsolete) + switch(num) { - m_timeStep = timeStep; - if(m_useLinearReferenceFrameA) + case BT_CONSTRAINT_STOP_ERP : + if(axis < 1) + { + m_softnessLimLin = value; + m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN; + } + else if(axis < 3) + { + m_softnessOrthoLin = value; + m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN; + } + else if(axis == 3) { - solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB); + m_softnessLimAng = value; + m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG; + } + else if(axis < 6) + { + m_softnessOrthoAng = value; + m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG; } else { - solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA); + btAssertConstrParams(0); } - } -} - - - -void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB) -{ - int i; - // linear - btVector3 velA; - bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA); - btVector3 velB; - bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB); - btVector3 vel = velA - velB; - for(i = 0; i < 3; i++) - { - const btVector3& normal = m_jacLin[i].m_linearJointAxis; - btScalar rel_vel = normal.dot(vel); - // calculate positional error - btScalar depth = m_depth[i]; - // get parameters - btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); - btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); - btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); - // calcutate and apply impulse - btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; - btVector3 impulse_vector = normal * normalImpulse; - - //rbA.applyImpulse( impulse_vector, m_relPosA); - //rbB.applyImpulse(-impulse_vector, m_relPosB); - { - btVector3 ftorqueAxis1 = m_relPosA.cross(normal); - btVector3 ftorqueAxis2 = m_relPosB.cross(normal); - bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); - bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); - } - - - - if(m_poweredLinMotor && (!i)) - { // apply linear motor - if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) - { - btScalar desiredMotorVel = m_targetLinMotorVelocity; - btScalar motor_relvel = desiredMotorVel + rel_vel; - normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; - // clamp accumulated impulse - btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse); - if(new_acc > m_maxLinMotorForce) - { - new_acc = m_maxLinMotorForce; - } - btScalar del = new_acc - m_accumulatedLinMotorImpulse; - if(normalImpulse < btScalar(0.0)) - { - normalImpulse = -del; - } - else - { - normalImpulse = del; - } - m_accumulatedLinMotorImpulse = new_acc; - // apply clamped impulse - impulse_vector = normal * normalImpulse; - //rbA.applyImpulse( impulse_vector, m_relPosA); - //rbB.applyImpulse(-impulse_vector, m_relPosB); - - { - btVector3 ftorqueAxis1 = m_relPosA.cross(normal); - btVector3 ftorqueAxis2 = m_relPosB.cross(normal); - bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); - bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); - } - - - - } + break; + case BT_CONSTRAINT_CFM : + if(axis < 1) + { + m_cfmDirLin = value; + m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN; } - } - // angular - // get axes in world space - btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); - btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); - - btVector3 angVelA; - bodyA.getAngularVelocity(angVelA); - btVector3 angVelB; - bodyB.getAngularVelocity(angVelB); - - btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); - btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); - - btVector3 angAorthog = angVelA - angVelAroundAxisA; - btVector3 angBorthog = angVelB - angVelAroundAxisB; - btVector3 velrelOrthog = angAorthog-angBorthog; - //solve orthogonal angular velocity correction - btScalar len = velrelOrthog.length(); - btScalar orthorImpulseMag = 0.f; - - if (len > btScalar(0.00001)) - { - btVector3 normal = velrelOrthog.normalized(); - btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); - //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; - orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; - } - //solve angular positional correction - btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); - btVector3 angularAxis = angularError; - btScalar angularImpulseMag = 0; - - btScalar len2 = angularError.length(); - if (len2>btScalar(0.00001)) - { - btVector3 normal2 = angularError.normalized(); - btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); - angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; - angularError *= angularImpulseMag; - } - // apply impulse - //rbA.applyTorqueImpulse(-velrelOrthog+angularError); - //rbB.applyTorqueImpulse(velrelOrthog-angularError); - - bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag); - bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag); - bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag); - bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag); - - - btScalar impulseMag; - //solve angular limits - if(m_solveAngLim) - { - impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep; - impulseMag *= m_kAngle * m_softnessLimAng; - } - else - { - impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep; - impulseMag *= m_kAngle * m_softnessDirAng; - } - btVector3 impulse = axisA * impulseMag; - //rbA.applyTorqueImpulse(impulse); - //rbB.applyTorqueImpulse(-impulse); - - bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag); - bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag); - - - - //apply angular motor - if(m_poweredAngMotor) - { - if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce) + else if(axis == 3) { - btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB; - btScalar projRelVel = velrel.dot(axisA); - - btScalar desiredMotorVel = m_targetAngMotorVelocity; - btScalar motor_relvel = desiredMotorVel - projRelVel; - - btScalar angImpulse = m_kAngle * motor_relvel; - // clamp accumulated impulse - btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse); - if(new_acc > m_maxAngMotorForce) - { - new_acc = m_maxAngMotorForce; - } - btScalar del = new_acc - m_accumulatedAngMotorImpulse; - if(angImpulse < btScalar(0.0)) - { - angImpulse = -del; - } - else - { - angImpulse = del; - } - m_accumulatedAngMotorImpulse = new_acc; - // apply clamped impulse - btVector3 motorImp = angImpulse * axisA; - //rbA.applyTorqueImpulse(motorImp); - //rbB.applyTorqueImpulse(-motorImp); - - bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse); - bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse); + m_cfmDirAng = value; + m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG; } - } -} - - - - - -void btSliderConstraint::calculateTransforms(void){ - if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) - { - m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; - m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; - } - else - { - m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB; - m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA; - } - m_realPivotAInW = m_calculatedTransformA.getOrigin(); - m_realPivotBInW = m_calculatedTransformB.getOrigin(); - m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X - if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete) - { - m_delta = m_realPivotBInW - m_realPivotAInW; - } - else - { - m_delta = m_realPivotAInW - m_realPivotBInW; - } - m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - btVector3 normalWorld; - int i; - //linear part - for(i = 0; i < 3; i++) - { - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - m_depth[i] = m_delta.dot(normalWorld); - } -} - - - -void btSliderConstraint::testLinLimits(void) -{ - m_solveLinLim = false; - m_linPos = m_depth[0]; - if(m_lowerLinLimit <= m_upperLinLimit) - { - if(m_depth[0] > m_upperLinLimit) + else { - m_depth[0] -= m_upperLinLimit; - m_solveLinLim = true; + btAssertConstrParams(0); } - else if(m_depth[0] < m_lowerLinLimit) + break; + case BT_CONSTRAINT_STOP_CFM : + if(axis < 1) { - m_depth[0] -= m_lowerLinLimit; - m_solveLinLim = true; + m_cfmLimLin = value; + m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN; + } + else if(axis < 3) + { + m_cfmOrthoLin = value; + m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN; + } + else if(axis == 3) + { + m_cfmLimAng = value; + m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG; + } + else if(axis < 6) + { + m_cfmOrthoAng = value; + m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG; } else { - m_depth[0] = btScalar(0.); + btAssertConstrParams(0); } - } - else - { - m_depth[0] = btScalar(0.); + break; } } - - -void btSliderConstraint::testAngLimits(void) +///return the local value of parameter +btScalar btSliderConstraint::getParam(int num, int axis) const { - m_angDepth = btScalar(0.); - m_solveAngLim = false; - if(m_lowerAngLimit <= m_upperAngLimit) + btScalar retVal(SIMD_INFINITY); + switch(num) { - const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1); - const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); - const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); - btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); - m_angPos = rot; - if(rot < m_lowerAngLimit) + case BT_CONSTRAINT_STOP_ERP : + if(axis < 1) { - m_angDepth = rot - m_lowerAngLimit; - m_solveAngLim = true; - } - else if(rot > m_upperAngLimit) + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN); + retVal = m_softnessLimLin; + } + else if(axis < 3) { - m_angDepth = rot - m_upperAngLimit; - m_solveAngLim = true; + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN); + retVal = m_softnessOrthoLin; + } + else if(axis == 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG); + retVal = m_softnessLimAng; + } + else if(axis < 6) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG); + retVal = m_softnessOrthoAng; + } + else + { + btAssertConstrParams(0); + } + break; + case BT_CONSTRAINT_CFM : + if(axis < 1) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN); + retVal = m_cfmDirLin; + } + else if(axis == 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG); + retVal = m_cfmDirAng; + } + else + { + btAssertConstrParams(0); + } + break; + case BT_CONSTRAINT_STOP_CFM : + if(axis < 1) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN); + retVal = m_cfmLimLin; + } + else if(axis < 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN); + retVal = m_cfmOrthoLin; } + else if(axis == 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG); + retVal = m_cfmLimAng; + } + else if(axis < 6) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG); + retVal = m_cfmOrthoAng; + } + else + { + btAssertConstrParams(0); + } + break; } + return retVal; } - -btVector3 btSliderConstraint::getAncorInA(void) -{ - btVector3 ancorInA; - ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis; - ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA; - return ancorInA; -} - - -btVector3 btSliderConstraint::getAncorInB(void) -{ - btVector3 ancorInB; - ancorInB = m_frameInB.getOrigin(); - return ancorInB; -} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 01cef59ed31..e9723a7bb40 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -40,14 +40,32 @@ class btRigidBody; #define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0)) #define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0)) #define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7)) +#define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f)) +enum btSliderFlags +{ + BT_SLIDER_FLAGS_CFM_DIRLIN = (1 << 0), + BT_SLIDER_FLAGS_ERP_DIRLIN = (1 << 1), + BT_SLIDER_FLAGS_CFM_DIRANG = (1 << 2), + BT_SLIDER_FLAGS_ERP_DIRANG = (1 << 3), + BT_SLIDER_FLAGS_CFM_ORTLIN = (1 << 4), + BT_SLIDER_FLAGS_ERP_ORTLIN = (1 << 5), + BT_SLIDER_FLAGS_CFM_ORTANG = (1 << 6), + BT_SLIDER_FLAGS_ERP_ORTANG = (1 << 7), + BT_SLIDER_FLAGS_CFM_LIMLIN = (1 << 8), + BT_SLIDER_FLAGS_ERP_LIMLIN = (1 << 9), + BT_SLIDER_FLAGS_CFM_LIMANG = (1 << 10), + BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11) +}; + class btSliderConstraint : public btTypedConstraint { protected: ///for backwards compatibility during the transition to 'getInfo/getInfo2' bool m_useSolveConstraintObsolete; + bool m_useOffsetForConstraintFrame; btTransform m_frameInA; btTransform m_frameInB; // use frameA fo define limits, if true @@ -67,26 +85,39 @@ protected: btScalar m_softnessDirLin; btScalar m_restitutionDirLin; btScalar m_dampingDirLin; + btScalar m_cfmDirLin; + btScalar m_softnessDirAng; btScalar m_restitutionDirAng; btScalar m_dampingDirAng; + btScalar m_cfmDirAng; + btScalar m_softnessLimLin; btScalar m_restitutionLimLin; btScalar m_dampingLimLin; + btScalar m_cfmLimLin; + btScalar m_softnessLimAng; btScalar m_restitutionLimAng; btScalar m_dampingLimAng; + btScalar m_cfmLimAng; + btScalar m_softnessOrthoLin; btScalar m_restitutionOrthoLin; btScalar m_dampingOrthoLin; + btScalar m_cfmOrthoLin; + btScalar m_softnessOrthoAng; btScalar m_restitutionOrthoAng; btScalar m_dampingOrthoAng; + btScalar m_cfmOrthoAng; // for interlal use bool m_solveLinLim; bool m_solveAngLim; + int m_flags; + btJacobianEntry m_jacLin[3]; btScalar m_jacLinDiagABInv[3]; @@ -126,16 +157,18 @@ protected: public: // constructors btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); - btSliderConstraint(); + btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA); + // overrides - virtual void buildJacobian(); + virtual void getInfo1 (btConstraintInfo1* info); + + void getInfo1NonVirtual(btConstraintInfo1* info); virtual void getInfo2 (btConstraintInfo2* info); - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); - + void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass); + // access const btRigidBody& getRigidBodyA() const { return m_rbA; } @@ -151,9 +184,9 @@ public: btScalar getUpperLinLimit() { return m_upperLinLimit; } void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; } btScalar getLowerAngLimit() { return m_lowerAngLimit; } - void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; } + void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); } btScalar getUpperAngLimit() { return m_upperAngLimit; } - void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; } + void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); } bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; } btScalar getSoftnessDirLin() { return m_softnessDirLin; } btScalar getRestitutionDirLin() { return m_restitutionDirLin; } @@ -211,20 +244,87 @@ public: btScalar getLinDepth() { return m_depth[0]; } bool getSolveAngLimit() { return m_solveAngLim; } btScalar getAngDepth() { return m_angDepth; } - // internal - void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB); - void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB); // shared code used by ODE solver - void calculateTransforms(void); - void testLinLimits(void); - void testLinLimits2(btConstraintInfo2* info); - void testAngLimits(void); + void calculateTransforms(const btTransform& transA,const btTransform& transB); + void testLinLimits(); + void testAngLimits(); // access for PE Solver - btVector3 getAncorInA(void); - btVector3 getAncorInB(void); + btVector3 getAncorInA(); + btVector3 getAncorInB(); + // access for UseFrameOffset + bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } + void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } + + void setFrames(const btTransform& frameA, const btTransform& frameB) + { + m_frameInA=frameA; + m_frameInB=frameB; + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + buildJacobian(); + } + + + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///If no axis is provided, it uses the default axis for this constraint. + virtual void setParam(int num, btScalar value, int axis = -1); + ///return the local value of parameter + virtual btScalar getParam(int num, int axis = -1) const; + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btSliderConstraintData +{ + btTypedConstraintData m_typeConstraintData; + btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTransformFloatData m_rbBFrame; + + float m_linearUpperLimit; + float m_linearLowerLimit; + + float m_angularUpperLimit; + float m_angularLowerLimit; + + int m_useLinearReferenceFrameA; + int m_useOffsetForConstraintFrame; + }; +SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const +{ + return sizeof(btSliderConstraintData); +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + + btSliderConstraintData* sliderData = (btSliderConstraintData*) dataBuffer; + btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer); + + m_frameInA.serializeFloat(sliderData->m_rbAFrame); + m_frameInB.serializeFloat(sliderData->m_rbBFrame); + + sliderData->m_linearUpperLimit = float(m_upperLinLimit); + sliderData->m_linearLowerLimit = float(m_lowerLinLimit); + + sliderData->m_angularUpperLimit = float(m_upperAngLimit); + sliderData->m_angularLowerLimit = float(m_lowerAngLimit); + + sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA; + sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame; + + return "btSliderConstraintData"; +} + + #endif //SLIDER_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h index 6b728959162..8de515812ee 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -105,17 +105,16 @@ operator+(const btSimdScalar& v1, const btSimdScalar& v2) #endif ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. -ATTRIBUTE_ALIGNED16 (struct) btSolverBody +ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete { BT_DECLARE_ALIGNED_ALLOCATOR(); btVector3 m_deltaLinearVelocity; btVector3 m_deltaAngularVelocity; btVector3 m_angularFactor; btVector3 m_invMass; - btScalar m_friction; btRigidBody* m_originalBody; btVector3 m_pushVelocity; - //btVector3 m_turnVelocity; + btVector3 m_turnVelocity; SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const @@ -145,12 +144,18 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody } } - -/* + SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + { + if (m_originalBody) + { + m_pushVelocity += linearComponent*impulseMagnitude; + m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } void writebackVelocity() { - if (m_invMass) + if (m_originalBody) { m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); @@ -158,14 +163,21 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody //m_originalBody->setCompanionId(-1); } } - */ - void writebackVelocity(btScalar timeStep=0) + + void writebackVelocity(btScalar timeStep) { + (void) timeStep; if (m_originalBody) { - m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity); + m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); + + //correct the position/orientation based on push/turn recovery + btTransform newTransform; + btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform); + m_originalBody->setWorldTransform(newTransform); + //m_originalBody->setCompanionId(-1); } } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h index 867d62a301d..79e45a4383b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -26,7 +26,7 @@ class btRigidBody; ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. -ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint +ATTRIBUTE_ALIGNED64 (struct) btSolverConstraint { BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -58,13 +58,13 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint }; union { - int m_solverBodyIdA; - btScalar m_unusedPadding2; + btRigidBody* m_solverBodyA; + int m_companionIdA; }; union { - int m_solverBodyIdB; - btScalar m_unusedPadding3; + btRigidBody* m_solverBodyB; + int m_companionIdB; }; union @@ -78,6 +78,8 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint btScalar m_lowerLimit; btScalar m_upperLimit; + btScalar m_rhsPenetration; + enum btSolverConstraintType { BT_SOLVER_CONTACT_1D = 0, diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 3fa98ee4c88..8212c36ff4e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -16,51 +16,38 @@ subject to the following restrictions: #include "btTypedConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btSerializer.h" -static btRigidBody s_fixed(0, 0,0); #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f) -btTypedConstraint::btTypedConstraint(btTypedConstraintType type) -:m_userConstraintType(-1), -m_userConstraintId(-1), -m_constraintType (type), -m_rbA(s_fixed), -m_rbB(s_fixed), -m_appliedImpulse(btScalar(0.)), -m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) -{ - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); -} btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) -:m_userConstraintType(-1), +:btTypedObject(type), +m_userConstraintType(-1), m_userConstraintId(-1), -m_constraintType (type), +m_needsFeedback(false), m_rbA(rbA), -m_rbB(s_fixed), +m_rbB(getFixedBody()), m_appliedImpulse(btScalar(0.)), m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) { - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); - } btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB) -:m_userConstraintType(-1), +:btTypedObject(type), +m_userConstraintType(-1), m_userConstraintId(-1), -m_constraintType (type), +m_needsFeedback(false), m_rbA(rbA), m_rbB(rbB), m_appliedImpulse(btScalar(0.)), m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) { - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); - } -//----------------------------------------------------------------------------- + btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact) { @@ -109,6 +96,115 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal lim_fact = btScalar(0.0f); } return lim_fact; -} // btTypedConstraint::getMotorFactor() +} + +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btTypedConstraintData* tcd = (btTypedConstraintData*) dataBuffer; + + tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA); + tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB); + char* name = (char*) serializer->findNameForPointer(this); + tcd->m_name = (char*)serializer->getUniquePointer(name); + if (tcd->m_name) + { + serializer->serializeName(name); + } + + tcd->m_objectType = m_objectType; + tcd->m_needsFeedback = m_needsFeedback; + tcd->m_userConstraintId =m_userConstraintId; + tcd->m_userConstraintType =m_userConstraintType; + + tcd->m_appliedImpulse = float(m_appliedImpulse); + tcd->m_dbgDrawSize = float(m_dbgDrawSize ); + + tcd->m_disableCollisionsBetweenLinkedBodies = false; + + int i; + for (i=0;i<m_rbA.getNumConstraintRefs();i++) + if (m_rbA.getConstraintRef(i) == this) + tcd->m_disableCollisionsBetweenLinkedBodies = true; + for (i=0;i<m_rbB.getNumConstraintRefs();i++) + if (m_rbB.getConstraintRef(i) == this) + tcd->m_disableCollisionsBetweenLinkedBodies = true; + + return "btTypedConstraintData"; +} + +btRigidBody& btTypedConstraint::getFixedBody() +{ + static btRigidBody s_fixed(0, 0,0); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); + return s_fixed; +} + + +void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor) +{ + m_halfRange = (high - low) / 2.0f; + m_center = btNormalizeAngle(low + m_halfRange); + m_softness = _softness; + m_biasFactor = _biasFactor; + m_relaxationFactor = _relaxationFactor; +} + +void btAngularLimit::test(const btScalar angle) +{ + m_correction = 0.0f; + m_sign = 0.0f; + m_solveLimit = false; + + if (m_halfRange >= 0.0f) + { + btScalar deviation = btNormalizeAngle(angle - m_center); + if (deviation < -m_halfRange) + { + m_solveLimit = true; + m_correction = - (deviation + m_halfRange); + m_sign = +1.0f; + } + else if (deviation > m_halfRange) + { + m_solveLimit = true; + m_correction = m_halfRange - deviation; + m_sign = -1.0f; + } + } +} +btScalar btAngularLimit::getError() const +{ + return m_correction * m_sign; +} + +void btAngularLimit::fit(btScalar& angle) const +{ + if (m_halfRange > 0.0f) + { + btScalar relativeAngle = btNormalizeAngle(angle - m_center); + if (!btEqual(relativeAngle, m_halfRange)) + { + if (relativeAngle > 0.0f) + { + angle = getHigh(); + } + else + { + angle = getLow(); + } + } + } +} + +btScalar btAngularLimit::getLow() const +{ + return btNormalizeAngle(m_center - m_halfRange); +} + +btScalar btAngularLimit::getHigh() const +{ + return btNormalizeAngle(m_center + m_halfRange); +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index 14cbe831b40..65e6d558300 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2010 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. @@ -19,27 +19,50 @@ subject to the following restrictions: class btRigidBody; #include "LinearMath/btScalar.h" #include "btSolverConstraint.h" -struct btSolverBody; - - +class btSerializer; +//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility enum btTypedConstraintType { - POINT2POINT_CONSTRAINT_TYPE, + POINT2POINT_CONSTRAINT_TYPE=3, HINGE_CONSTRAINT_TYPE, CONETWIST_CONSTRAINT_TYPE, D6_CONSTRAINT_TYPE, - SLIDER_CONSTRAINT_TYPE + SLIDER_CONSTRAINT_TYPE, + CONTACT_CONSTRAINT_TYPE, + D6_SPRING_CONSTRAINT_TYPE, + MAX_CONSTRAINT_TYPE +}; + + +enum btConstraintParams +{ + BT_CONSTRAINT_ERP=1, + BT_CONSTRAINT_STOP_ERP, + BT_CONSTRAINT_CFM, + BT_CONSTRAINT_STOP_CFM }; +#if 1 + #define btAssertConstrParams(_par) btAssert(_par) +#else + #define btAssertConstrParams(_par) +#endif + + ///TypedConstraint is the baseclass for Bullet constraints and vehicles -class btTypedConstraint +class btTypedConstraint : public btTypedObject { int m_userConstraintType; - int m_userConstraintId; - btTypedConstraintType m_constraintType; + union + { + int m_userConstraintId; + void* m_userConstraintPtr; + }; + + bool m_needsFeedback; btTypedConstraint& operator=(btTypedConstraint& other) { @@ -54,10 +77,13 @@ protected: btScalar m_appliedImpulse; btScalar m_dbgDrawSize; + ///internal method used by the constraint solver, don't use them directly + btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact); + + static btRigidBody& getFixedBody(); public: - btTypedConstraint(btTypedConstraintType type); virtual ~btTypedConstraint() {}; btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA); btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB); @@ -93,21 +119,45 @@ public: // note that the returned indexes are relative to the first index of // the constraint. int *findex; - }; + // number of solver iterations + int m_numIterations; + //damping of the velocity + btScalar m_damping; + }; - virtual void buildJacobian() = 0; + ///internal method used by the constraint solver, don't use them directly + virtual void buildJacobian() {}; + ///internal method used by the constraint solver, don't use them directly virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep) { + (void)ca; + (void)solverBodyA; + (void)solverBodyB; + (void)timeStep; } + + ///internal method used by the constraint solver, don't use them directly virtual void getInfo1 (btConstraintInfo1* info)=0; + ///internal method used by the constraint solver, don't use them directly virtual void getInfo2 (btConstraintInfo2* info)=0; - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) = 0; + ///internal method used by the constraint solver, don't use them directly + void internalSetAppliedImpulse(btScalar appliedImpulse) + { + m_appliedImpulse = appliedImpulse; + } + ///internal method used by the constraint solver, don't use them directly + btScalar internalGetAppliedImpulse() + { + return m_appliedImpulse; + } + + ///internal method used by the constraint solver, don't use them directly + virtual void solveConstraintObsolete(btRigidBody& /*bodyA*/,btRigidBody& /*bodyB*/,btScalar /*timeStep*/) {}; - btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact); const btRigidBody& getRigidBodyA() const { @@ -147,19 +197,44 @@ public: return m_userConstraintId; } + void setUserConstraintPtr(void* ptr) + { + m_userConstraintPtr = ptr; + } + + void* getUserConstraintPtr() + { + return m_userConstraintPtr; + } + int getUid() const { return m_userConstraintId; } + bool needsFeedback() const + { + return m_needsFeedback; + } + + ///enableFeedback will allow to read the applied linear and angular impulse + ///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information + void enableFeedback(bool needsFeedback) + { + m_needsFeedback = needsFeedback; + } + + ///getAppliedImpulse is an estimated total applied impulse. + ///This feedback could be used to determine breaking constraints or playing sounds. btScalar getAppliedImpulse() const { + btAssert(m_needsFeedback); return m_appliedImpulse; } btTypedConstraintType getConstraintType () const { - return m_constraintType; + return btTypedConstraintType(m_objectType); } void setDbgDrawSize(btScalar dbgDrawSize) @@ -170,7 +245,166 @@ public: { return m_dbgDrawSize; } + + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///If no axis is provided, it uses the default axis for this constraint. + virtual void setParam(int num, btScalar value, int axis = -1) = 0; + + ///return the local value of parameter + virtual btScalar getParam(int num, int axis = -1) const = 0; + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + }; +// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits +// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI]) +SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians) +{ + if(angleLowerLimitInRadians >= angleUpperLimitInRadians) + { + return angleInRadians; + } + else if(angleInRadians < angleLowerLimitInRadians) + { + btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians)); + btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians)); + return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI); + } + else if(angleInRadians > angleUpperLimitInRadians) + { + btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians)); + btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians)); + return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians; + } + else + { + return angleInRadians; + } +} + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btTypedConstraintData +{ + btRigidBodyData *m_rbA; + btRigidBodyData *m_rbB; + char *m_name; + + int m_objectType; + int m_userConstraintType; + int m_userConstraintId; + int m_needsFeedback; + + float m_appliedImpulse; + float m_dbgDrawSize; + + int m_disableCollisionsBetweenLinkedBodies; + char m_pad4[4]; + +}; + +SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const +{ + return sizeof(btTypedConstraintData); +} + + + +class btAngularLimit +{ +private: + btScalar + m_center, + m_halfRange, + m_softness, + m_biasFactor, + m_relaxationFactor, + m_correction, + m_sign; + + bool + m_solveLimit; + +public: + /// Default constructor initializes limit as inactive, allowing free constraint movement + btAngularLimit() + :m_center(0.0f), + m_halfRange(-1.0f), + m_softness(0.9f), + m_biasFactor(0.3f), + m_relaxationFactor(1.0f), + m_correction(0.0f), + m_sign(0.0f), + m_solveLimit(false) + {} + + /// Sets all limit's parameters. + /// When low > high limit becomes inactive. + /// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit + void set(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f); + + /// Checks conastaint angle against limit. If limit is active and the angle violates the limit + /// correction is calculated. + void test(const btScalar angle); + + /// Returns limit's softness + inline btScalar getSoftness() const + { + return m_softness; + } + + /// Returns limit's bias factor + inline btScalar getBiasFactor() const + { + return m_biasFactor; + } + + /// Returns limit's relaxation factor + inline btScalar getRelaxationFactor() const + { + return m_relaxationFactor; + } + + /// Returns correction value evaluated when test() was invoked + inline btScalar getCorrection() const + { + return m_correction; + } + + /// Returns sign value evaluated when test() was invoked + inline btScalar getSign() const + { + return m_sign; + } + + /// Gives half of the distance between min and max limit angle + inline btScalar getHalfRange() const + { + return m_halfRange; + } + + /// Returns true when the last test() invocation recognized limit violation + inline bool isLimit() const + { + return m_solveLimit; + } + + /// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle + /// returned is modified so it equals to the limit closest to given angle. + void fit(btScalar& angle) const; + + /// Returns correction value multiplied by sign value + btScalar getError() const; + + btScalar getLow() const; + + btScalar getHigh() const; + +}; + + + #endif //TYPED_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp new file mode 100644 index 00000000000..40c56e77954 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp @@ -0,0 +1,87 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. + +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 "btUniversalConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" + + + +#define UNIV_EPS btScalar(0.01f) + + +// constructor +// anchor, axis1 and axis2 are in world coordinate system +// axis1 must be orthogonal to axis2 +btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) +: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), + m_anchor(anchor), + m_axis1(axis1), + m_axis2(axis2) +{ + // build frame basis + // 6DOF constraint uses Euler angles and to define limits + // it is assumed that rotational order is : + // Z - first, allowed limits are (-PI,PI); + // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number + // used to prevent constraint from instability on poles; + // new position of X, allowed limits are (-PI,PI); + // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs + // Build the frame in world coordinate system first + btVector3 zAxis = axis1.normalize(); + btVector3 yAxis = axis2.normalize(); + btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + btTransform frameInW; + frameInW.setIdentity(); + frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); + frameInW.setOrigin(anchor); + // now get constraint frame in local coordinate systems + m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; + m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; + // sei limits + setLinearLowerLimit(btVector3(0., 0., 0.)); + setLinearUpperLimit(btVector3(0., 0., 0.)); + setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS)); + setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS)); +} + +void btUniversalConstraint::setAxis(const btVector3& axis1,const btVector3& axis2) +{ + m_axis1 = axis1; + m_axis2 = axis2; + + btVector3 zAxis = axis1.normalized(); + btVector3 yAxis = axis2.normalized(); + btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + + btTransform frameInW; + frameInW.setIdentity(); + frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); + frameInW.setOrigin(m_anchor); + + // now get constraint frame in local coordinate systems + m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; + m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; + + calculateTransforms(); +} + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h new file mode 100644 index 00000000000..a8d404a591c --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h @@ -0,0 +1,62 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef UNIVERSAL_CONSTRAINT_H +#define UNIVERSAL_CONSTRAINT_H + + + +#include "LinearMath/btVector3.h" +#include "btTypedConstraint.h" +#include "btGeneric6DofConstraint.h" + + + +/// Constraint similar to ODE Universal Joint +/// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1) +/// and Y (axis 2) +/// Description from ODE manual : +/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular. +/// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal." + +class btUniversalConstraint : public btGeneric6DofConstraint +{ +protected: + btVector3 m_anchor; + btVector3 m_axis1; + btVector3 m_axis2; +public: + // constructor + // anchor, axis1 and axis2 are in world coordinate system + // axis1 must be orthogonal to axis2 + btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2); + // access + const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); } + const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); } + const btVector3& getAxis1() { return m_axis1; } + const btVector3& getAxis2() { return m_axis2; } + btScalar getAngle1() { return getAngle(2); } + btScalar getAngle2() { return getAngle(1); } + // limits + void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); } + void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); } + + void setAxis( const btVector3& axis1, const btVector3& axis2); +}; + + + +#endif // UNIVERSAL_CONSTRAINT_H + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp index 20d6975832b..bd8e2748383 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp @@ -43,7 +43,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" -#include "LinearMath/btStackAlloc.h" + /* Create and Delete a Physics SDK @@ -181,12 +181,12 @@ plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z) plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height) { //capsule is convex hull of 2 spheres, so use btMultiSphereShape - btVector3 inertiaHalfExtents(radius,height,radius); + const int numSpheres = 2; btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)}; btScalar radi[numSpheres] = {radius,radius}; void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16); - return (plCollisionShapeHandle) new (mem)btMultiSphereShape(inertiaHalfExtents,positions,radi,numSpheres); + return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres); } plCollisionShapeHandle plNewConeShape(plReal radius, plReal height) { @@ -295,6 +295,14 @@ void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation) body->setWorldTransform(worldTrans); } +void plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix) +{ + btRigidBody* body = reinterpret_cast< btRigidBody* >(object); + btAssert(body); + btTransform& worldTrans = body->getWorldTransform(); + worldTrans.setFromOpenGLMatrix(matrix); +} + void plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix) { btRigidBody* body = reinterpret_cast< btRigidBody* >(object); @@ -365,6 +373,7 @@ double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float btPointCollector gjkOutput; btGjkPairDetector::ClosestPointInput input; + btTransform tr; tr.setIdentity(); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h b/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h index 8348795ef70..e1fea3a49c0 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h @@ -20,11 +20,17 @@ class btIDebugDraw; class btCollisionWorld; #include "LinearMath/btScalar.h" +#include "btRigidBody.h" ///Basic interface to allow actions such as vehicles and characters to be updated inside a btDynamicsWorld class btActionInterface { - public: +protected: + + static btRigidBody& getFixedBody(); + + +public: virtual ~btActionInterface() { @@ -36,4 +42,5 @@ class btActionInterface }; -#endif //_BT_ACTION_INTERFACE_H
\ No newline at end of file +#endif //_BT_ACTION_INTERFACE_H + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp index fa0d63d74a1..23501c4435e 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp @@ -48,6 +48,10 @@ void btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) startProfiling(timeStep); + if(0 != m_internalPreTickCallback) { + (*m_internalPreTickCallback)(this, timeStep); + } + ///update aabbs information updateAabbs(); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index b6231a8fda6..e1eada07c82 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -19,6 +19,7 @@ subject to the following restrictions: //collision detection #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" #include "LinearMath/btTransformUtil.h" @@ -35,27 +36,15 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" -//for debug rendering -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionShapes/btCapsuleShape.h" -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "BulletCollision/CollisionShapes/btConeShape.h" -#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btCylinderShape.h" -#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" -#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" #include "LinearMath/btIDebugDraw.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletDynamics/Dynamics/btActionInterface.h" #include "LinearMath/btQuickprof.h" #include "LinearMath/btMotionState.h" - +#include "LinearMath/btSerializer.h" @@ -63,7 +52,8 @@ btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroa :btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), m_constraintSolver(constraintSolver), m_gravity(0,-10,0), -m_localTime(btScalar(1.)/btScalar(60.)), +m_localTime(0), +m_synchronizeAllMotionStates(false), m_profileTimings(0) { if (!m_constraintSolver) @@ -103,47 +93,31 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) { - +///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows +///to switch status _after_ adding kinematic objects to the world +///fix it for Bullet 3.x release for (int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); - if (body) + if (body && body->getActivationState() != ISLAND_SLEEPING) { - if (body->getActivationState() != ISLAND_SLEEPING) - { - if (body->isKinematicObject()) - { - //to calculate velocities next frame - body->saveKinematicState(timeStep); - } - } + if (body->isKinematicObject()) + { + //to calculate velocities next frame + body->saveKinematicState(timeStep); + } } } + } void btDiscreteDynamicsWorld::debugDrawWorld() { BT_PROFILE("debugDrawWorld"); - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) - { - int numManifolds = getDispatcher()->getNumManifolds(); - btVector3 color(0,0,0); - for (int i=0;i<numManifolds;i++) - { - btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i); - //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0()); - //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1()); + btCollisionWorld::debugDrawWorld(); - int numContacts = contactManifold->getNumContacts(); - for (int j=0;j<numContacts;j++) - { - btManifoldPoint& cp = contactManifold->getContactPoint(j); - getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); - } - } - } bool drawConstraints = false; if (getDebugDrawer()) { @@ -168,42 +142,6 @@ void btDiscreteDynamicsWorld::debugDrawWorld() { int i; - for ( i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) - { - btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.)); - switch(colObj->getActivationState()) - { - case ACTIVE_TAG: - color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break; - case ISLAND_SLEEPING: - color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break; - case WANTS_DEACTIVATION: - color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break; - case DISABLE_DEACTIVATION: - color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break; - case DISABLE_SIMULATION: - color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break; - default: - { - color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.)); - } - }; - - debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); - } - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - btVector3 minAabb,maxAabb; - btVector3 colorvec(1,0,0); - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); - } - - } - if (getDebugDrawer() && getDebugDrawer()->getDebugMode()) { for (i=0;i<m_actions.size();i++) @@ -217,15 +155,12 @@ void btDiscreteDynamicsWorld::debugDrawWorld() void btDiscreteDynamicsWorld::clearForces() { ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_nonStaticRigidBodies.size();i++) { - btCollisionObject* colObj = m_collisionObjects[i]; - - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - { - body->clearForces(); - } + btRigidBody* body = m_nonStaticRigidBodies[i]; + //need to check if next line is ok + //it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up + body->clearForces(); } } @@ -233,12 +168,10 @@ void btDiscreteDynamicsWorld::clearForces() void btDiscreteDynamicsWorld::applyGravity() { ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_nonStaticRigidBodies.size();i++) { - btCollisionObject* colObj = m_collisionObjects[i]; - - btRigidBody* body = btRigidBody::upcast(colObj); - if (body && body->isActive()) + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (body->isActive()) { body->applyGravity(); } @@ -269,32 +202,26 @@ void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) void btDiscreteDynamicsWorld::synchronizeMotionStates() { BT_PROFILE("synchronizeMotionStates"); + if (m_synchronizeAllMotionStates) { - //todo: iterate over awake simulation islands! + //iterate over all collision objects for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); if (body) synchronizeSingleMotionState(body); } - } -/* - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) + } else { - for ( int i=0;i<this->m_vehicles.size();i++) + //iterate over all active rigid bodies + for ( int i=0;i<m_nonStaticRigidBodies.size();i++) { - for (int v=0;v<m_vehicles[i]->getNumWheels();v++) - { - //synchronize the wheels with the (interpolated) chassis worldtransform - m_vehicles[i]->updateWheelTransform(v,true); - } + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (body->isActive()) + synchronizeSingleMotionState(body); } } - */ - - } @@ -340,23 +267,25 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, if (numSimulationSubSteps) { - saveKinematicState(fixedTimeStep); + //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; + + saveKinematicState(fixedTimeStep*clampedSimulationSteps); applyGravity(); - //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt - int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; + for (int i=0;i<clampedSimulationSteps;i++) { internalSingleStepSimulation(fixedTimeStep); - //for Blender, no need to synchronize here, it is done in blender anyway - //synchronizeMotionStates(); + synchronizeMotionStates(); } - } - //else - // synchronizeMotionStates(); + } else + { + synchronizeMotionStates(); + } clearForces(); @@ -372,6 +301,10 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) BT_PROFILE("internalSingleStepSimulation"); + if(0 != m_internalPreTickCallback) { + (*m_internalPreTickCallback)(this, timeStep); + } + ///apply gravity, predict motion predictUnconstraintMotion(timeStep); @@ -412,11 +345,10 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) { m_gravity = gravity; - for ( int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_nonStaticRigidBodies.size();i++) { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) { body->setGravity(gravity); } @@ -428,21 +360,44 @@ btVector3 btDiscreteDynamicsWorld::getGravity () const return m_gravity; } +void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) +{ + btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask); +} + +void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +{ + btRigidBody* body = btRigidBody::upcast(collisionObject); + if (body) + removeRigidBody(body); + else + btCollisionWorld::removeCollisionObject(collisionObject); +} void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) { - removeCollisionObject(body); + m_nonStaticRigidBodies.remove(body); + btCollisionWorld::removeCollisionObject(body); } + void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) { - if (!body->isStaticOrKinematicObject()) + if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) { body->setGravity(m_gravity); } if (body->getCollisionShape()) { + if (!body->isStaticObject()) + { + m_nonStaticRigidBodies.push_back(body); + } else + { + body->setActivationState(ISLAND_SLEEPING); + } + bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); @@ -453,13 +408,21 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask) { - if (!body->isStaticOrKinematicObject()) + if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) { body->setGravity(m_gravity); } if (body->getCollisionShape()) { + if (!body->isStaticObject()) + { + m_nonStaticRigidBodies.push_back(body); + } + else + { + body->setActivationState(ISLAND_SLEEPING); + } addCollisionObject(body,group,mask); } } @@ -480,10 +443,9 @@ void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { BT_PROFILE("updateActivationState"); - for ( int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_nonStaticRigidBodies.size();i++) { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); + btRigidBody* body = m_nonStaticRigidBodies[i]; if (body) { body->updateDeactivation(timeStep); @@ -589,7 +551,6 @@ class btSortConstraintOnIslandPredicate - void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { BT_PROFILE("solveConstraints"); @@ -604,6 +565,11 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btIDebugDraw* m_debugDrawer; btStackAlloc* m_stackAlloc; btDispatcher* m_dispatcher; + + btAlignedObjectArray<btCollisionObject*> m_bodies; + btAlignedObjectArray<btPersistentManifold*> m_manifolds; + btAlignedObjectArray<btTypedConstraint*> m_constraints; + InplaceSolverIslandCallback( btContactSolverInfo& solverInfo, @@ -624,6 +590,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } + InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) { btAssert(0); @@ -664,17 +631,48 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } } - ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive - if (numManifolds + numCurConstraints) + if (m_solverInfo.m_minimumSolverBatchSize<=1) { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive + if (numManifolds + numCurConstraints) + { + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + } + } else + { + + for (i=0;i<numBodies;i++) + m_bodies.push_back(bodies[i]); + for (i=0;i<numManifolds;i++) + m_manifolds.push_back(manifolds[i]); + for (i=0;i<numCurConstraints;i++) + m_constraints.push_back(startConstraint[i]); + if ((m_constraints.size()+m_manifolds.size())>m_solverInfo.m_minimumSolverBatchSize) + { + processConstraints(); + } else + { + //printf("deferred\n"); + } } - } } + void processConstraints() + { + if (m_manifolds.size() + m_constraints.size()>0) + { + m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + } + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + + } }; + + //sorted version of all btTypedConstraint, based on islandId btAlignedObjectArray<btTypedConstraint*> sortedConstraints; sortedConstraints.resize( m_constraints.size()); @@ -699,6 +697,8 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) /// solve all the constraints for this island m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback); + solverCallback.processConstraints(); + m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc); } @@ -726,13 +726,9 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() { if (colObj0->isActive() || colObj1->isActive()) { - if ((colObj0)->getIslandTag() != -1 && (colObj1)->getIslandTag() != -1) - { - - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), - (colObj1)->getIslandTag()); - } - + + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), + (colObj1)->getIslandTag()); } } } @@ -745,7 +741,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() } -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" + class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback { @@ -758,8 +754,8 @@ class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConve public: btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : btCollisionWorld::ClosestConvexResultCallback(fromA,toA), - m_allowedPenetration(0.0f), m_me(me), + m_allowedPenetration(0.0f), m_pairCache(pairCache), m_dispatcher(dispatcher) { @@ -833,46 +829,42 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { BT_PROFILE("integrateTransforms"); btTransform predictedTrans; - for ( int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_nonStaticRigidBodies.size();i++) { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) + btRigidBody* body = m_nonStaticRigidBodies[i]; + body->setHitFraction(1.f); + + if (body->isActive() && (!body->isStaticOrKinematicObject())) { - body->setHitFraction(1.f); + body->predictIntegratedTransform(timeStep, predictedTrans); + btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - if (body->isActive() && (!body->isStaticOrKinematicObject())) + if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) { - body->predictIntegratedTransform(timeStep, predictedTrans); - btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - - if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) + BT_PROFILE("CCD motion clamping"); + if (body->getCollisionShape()->isConvex()) { - BT_PROFILE("CCD motion clamping"); - if (body->getCollisionShape()->isConvex()) + gNumClampedCcdMotions++; + + btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); + //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + + sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + + convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults); + if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { - gNumClampedCcdMotions++; - - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); - btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); - btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); - - sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; - - convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults); - if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) - { - body->setHitFraction(sweepResults.m_closestHitFraction); - body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); - body->setHitFraction(0.f); + body->setHitFraction(sweepResults.m_closestHitFraction); + body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); + body->setHitFraction(0.f); // printf("clamped integration to hit fraction = %f\n",fraction); - } } } - - body->proceedToTransform( predictedTrans); } + + body->proceedToTransform( predictedTrans); } } } @@ -884,21 +876,16 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { BT_PROFILE("predictUnconstraintMotion"); - for ( int i=0;i<m_collisionObjects.size();i++) + for ( int i=0;i<m_nonStaticRigidBodies.size();i++) { - btCollisionObject* colObj = m_collisionObjects[i]; - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (!body->isStaticOrKinematicObject()) { - if (!body->isStaticOrKinematicObject()) - { - - body->integrateVelocities( timeStep); - //damping - body->applyDamping(timeStep); + body->integrateVelocities( timeStep); + //damping + body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); - } + body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); } } } @@ -919,281 +906,6 @@ void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) -class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback -{ - btIDebugDraw* m_debugDrawer; - btVector3 m_color; - btTransform m_worldTrans; - -public: - - DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) : - m_debugDrawer(debugDrawer), - m_color(color), - m_worldTrans(worldTrans) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - processTriangle(triangle,partId,triangleIndex); - } - - virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex) - { - (void)partId; - (void)triangleIndex; - - btVector3 wv0,wv1,wv2; - wv0 = m_worldTrans*triangle[0]; - wv1 = m_worldTrans*triangle[1]; - wv2 = m_worldTrans*triangle[2]; - m_debugDrawer->drawLine(wv0,wv1,m_color); - m_debugDrawer->drawLine(wv1,wv2,m_color); - m_debugDrawer->drawLine(wv2,wv0,m_color); - } -}; - -void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color) -{ - btVector3 start = transform.getOrigin(); - - const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0); - const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0); - const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius); - - // XY - getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color); - getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color); - getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color); - getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color); - - // XZ - getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color); - getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color); - getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color); - getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color); - - // YZ - getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color); - getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color); - getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color); - getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color); -} - -void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) -{ - // Draw a small simplex at the center of the object - { - btVector3 start = worldTransform.getOrigin(); - getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0)); - getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0)); - getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1)); - } - - if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) - { - const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape); - for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) - { - btTransform childTrans = compoundShape->getChildTransform(i); - const btCollisionShape* colShape = compoundShape->getChildShape(i); - debugDrawObject(worldTransform*childTrans,colShape,color); - } - - } else - { - switch (shape->getShapeType()) - { - - case SPHERE_SHAPE_PROXYTYPE: - { - const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); - btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin - - debugDrawSphere(radius, worldTransform, color); - break; - } - case MULTI_SPHERE_SHAPE_PROXYTYPE: - { - const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); - - for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) - { - btTransform childTransform = worldTransform; - childTransform.getOrigin() += multiSphereShape->getSpherePosition(i); - debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color); - } - - break; - } - case CAPSULE_SHAPE_PROXYTYPE: - { - const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape); - - btScalar radius = capsuleShape->getRadius(); - btScalar halfHeight = capsuleShape->getHalfHeight(); - - int upAxis = capsuleShape->getUpAxis(); - - - btVector3 capStart(0.f,0.f,0.f); - capStart[upAxis] = -halfHeight; - - btVector3 capEnd(0.f,0.f,0.f); - capEnd[upAxis] = halfHeight; - - // Draw the ends - { - - btTransform childTransform = worldTransform; - childTransform.getOrigin() = worldTransform * capStart; - debugDrawSphere(radius, childTransform, color); - } - - { - btTransform childTransform = worldTransform; - childTransform.getOrigin() = worldTransform * capEnd; - debugDrawSphere(radius, childTransform, color); - } - - // Draw some additional lines - btVector3 start = worldTransform.getOrigin(); - - - capStart[(upAxis+1)%3] = radius; - capEnd[(upAxis+1)%3] = radius; - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); - capStart[(upAxis+1)%3] = -radius; - capEnd[(upAxis+1)%3] = -radius; - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); - - capStart[(upAxis+1)%3] = 0.f; - capEnd[(upAxis+1)%3] = 0.f; - - capStart[(upAxis+2)%3] = radius; - capEnd[(upAxis+2)%3] = radius; - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); - capStart[(upAxis+2)%3] = -radius; - capEnd[(upAxis+2)%3] = -radius; - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); - - - break; - } - case CONE_SHAPE_PROXYTYPE: - { - const btConeShape* coneShape = static_cast<const btConeShape*>(shape); - btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); - btScalar height = coneShape->getHeight();//+coneShape->getMargin(); - btVector3 start = worldTransform.getOrigin(); - - int upAxis= coneShape->getConeUpIndex(); - - - btVector3 offsetHeight(0,0,0); - offsetHeight[upAxis] = height * btScalar(0.5); - btVector3 offsetRadius(0,0,0); - offsetRadius[(upAxis+1)%3] = radius; - btVector3 offset2Radius(0,0,0); - offset2Radius[(upAxis+2)%3] = radius; - - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color); - - - - break; - - } - case CYLINDER_SHAPE_PROXYTYPE: - { - const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); - int upAxis = cylinder->getUpAxis(); - btScalar radius = cylinder->getRadius(); - btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; - btVector3 start = worldTransform.getOrigin(); - btVector3 offsetHeight(0,0,0); - offsetHeight[upAxis] = halfHeight; - btVector3 offsetRadius(0,0,0); - offsetRadius[(upAxis+1)%3] = radius; - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); - break; - } - - case STATIC_PLANE_PROXYTYPE: - { - const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); - btScalar planeConst = staticPlaneShape->getPlaneConstant(); - const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); - btVector3 planeOrigin = planeNormal * planeConst; - btVector3 vec0,vec1; - btPlaneSpace1(planeNormal,vec0,vec1); - btScalar vecLen = 100.f; - btVector3 pt0 = planeOrigin + vec0*vecLen; - btVector3 pt1 = planeOrigin - vec0*vecLen; - btVector3 pt2 = planeOrigin + vec1*vecLen; - btVector3 pt3 = planeOrigin - vec1*vecLen; - getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color); - getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color); - break; - - } - default: - { - - if (shape->isConcave()) - { - btConcaveShape* concaveMesh = (btConcaveShape*) shape; - - ///@todo pass camera, for some culling? no -> we are not a graphics lib - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); - - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); - - } - - if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) - { - btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; - //todo: pass camera for some culling - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); - //DebugDrawcallback drawCallback; - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); - } - - - /// for polyhedral shapes - if (shape->isPolyhedral()) - { - btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; - - int i; - for (i=0;i<polyshape->getNumEdges();i++) - { - btVector3 a,b; - polyshape->getEdge(i,a,b); - btVector3 wa = worldTransform * a; - btVector3 wb = worldTransform * b; - getDebugDrawer()->drawLine(wa,wb,color); - - } - - - } - } - } - } -} - - void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) { bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0; @@ -1297,6 +1009,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) } } break; + case D6_SPRING_CONSTRAINT_TYPE: case D6_CONSTRAINT_TYPE: { btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; @@ -1354,7 +1067,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); if(drawLimits) { - btTransform tr = pSlider->getCalculatedTransformA(); + btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB(); btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); @@ -1371,7 +1084,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) break; } return; -} // btDiscreteDynamicsWorld::debugDrawConstraint() +} @@ -1407,3 +1120,43 @@ const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const } + +void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer) +{ + int i; + //serialize all collision objects + for (i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY) + { + int len = colObj->calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(len,1); + const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj); + } + } + + for (i=0;i<m_constraints.size();i++) + { + btTypedConstraint* constraint = m_constraints[i]; + int size = constraint->calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(size,1); + const char* structType = constraint->serialize(chunk->m_oldPtr,serializer); + serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint); + } +} + + +void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) +{ + + serializer->startSerialization(); + + serializeRigidBodies(serializer); + + serializeCollisionObjects(serializer); + + serializer->finishSerialization(); +} + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 4662cf5052a..df47c29044f 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -13,6 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ + #ifndef BT_DISCRETE_DYNAMICS_WORLD_H #define BT_DISCRETE_DYNAMICS_WORLD_H @@ -41,6 +42,8 @@ protected: btAlignedObjectArray<btTypedConstraint*> m_constraints; + btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies; + btVector3 m_gravity; //for variable timesteps @@ -49,6 +52,7 @@ protected: bool m_ownsIslandManager; bool m_ownsConstraintSolver; + bool m_synchronizeAllMotionStates; btAlignedObjectArray<btActionInterface*> m_actions; @@ -73,8 +77,7 @@ protected: virtual void saveKinematicState(btScalar timeStep); - void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color); - + void serializeRigidBodies(btSerializer* serializer); public: @@ -120,13 +123,17 @@ public: virtual btVector3 getGravity () const; + virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + virtual void addRigidBody(btRigidBody* body); virtual void addRigidBody(btRigidBody* body, short group, short mask); virtual void removeRigidBody(btRigidBody* body); - void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); + ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject + virtual void removeCollisionObject(btCollisionObject* collisionObject); + void debugDrawConstraint(btTypedConstraint* constraint); @@ -174,6 +181,18 @@ public: ///obsolete, use removeAction instead virtual void removeCharacter(btActionInterface* character); + void setSynchronizeAllMotionStates(bool synchronizeAll) + { + m_synchronizeAllMotionStates = synchronizeAll; + } + bool getSynchronizeAllMotionStates() const + { + return m_synchronizeAllMotionStates; + } + + ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo) + virtual void serialize(btSerializer* serializer); + }; #endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index ecf7a2f0c64..105317920ae 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -41,6 +41,7 @@ class btDynamicsWorld : public btCollisionWorld protected: btInternalTickCallback m_internalTickCallback; + btInternalTickCallback m_internalPreTickCallback; void* m_worldUserInfo; btContactSolverInfo m_solverInfo; @@ -49,7 +50,7 @@ public: btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration) - :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0) + :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0) { } @@ -85,6 +86,8 @@ public: virtual void addRigidBody(btRigidBody* body) = 0; + virtual void addRigidBody(btRigidBody* body, short group, short mask) = 0; + virtual void removeRigidBody(btRigidBody* body) = 0; virtual void setConstraintSolver(btConstraintSolver* solver) = 0; @@ -102,9 +105,15 @@ public: virtual void clearForces() = 0; /// Set the callback for when an internal tick (simulation substep) happens, optional user info - void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0) + void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0,bool isPreTick=false) { - m_internalTickCallback = cb; + if (isPreTick) + { + m_internalPreTickCallback = cb; + } else + { + m_internalTickCallback = cb; + } m_worldUserInfo = worldUserInfo; } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index a4d8e1d77f6..aefb26a1be2 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -19,6 +19,7 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" #include "LinearMath/btMotionState.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "LinearMath/btSerializer.h" //'temporarily' global variables btScalar gDeactivationTime = btScalar(2.); @@ -50,8 +51,8 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_linearDamping = btScalar(0.); - m_angularDamping = btScalar(0.5); + setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); + m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold; m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold; m_optionalMotionState = constructionInfo.m_motionState; @@ -83,9 +84,19 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& m_debugBodyId = uniqueId++; setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); - setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); updateInertiaTensor(); + m_rigidbodyFlags = 0; + + + m_deltaLinearVelocity.setZero(); + m_deltaAngularVelocity.setZero(); + m_invMass = m_inverseMass*m_linearFactor; + m_pushVelocity.setZero(); + m_turnVelocity.setZero(); + + + } @@ -136,8 +147,8 @@ void btRigidBody::setGravity(const btVector3& acceleration) void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) { - m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); - m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); + m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); + m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); } @@ -227,11 +238,15 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); m_inverseMass = btScalar(1.0) / mass; } + + //Fg = m * a + m_gravity = mass * m_gravity_acceleration; m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0), inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0), inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0)); + m_invMass = m_linearFactor*m_inverseMass; } @@ -301,6 +316,28 @@ bool btRigidBody::checkCollideWithOverride(btCollisionObject* co) return true; } +void btRigidBody::internalWritebackVelocity(btScalar timeStep) +{ + (void) timeStep; + if (m_inverseMass) + { + setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity); + setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity); + + //correct the position/orientation based on push/turn recovery + btTransform newTransform; + btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform); + setWorldTransform(newTransform); + //m_originalBody->setCompanionId(-1); + } +// m_deltaLinearVelocity.setZero(); +// m_deltaAngularVelocity .setZero(); +// m_pushVelocity.setZero(); +// m_turnVelocity.setZero(); +} + + + void btRigidBody::addConstraintRef(btTypedConstraint* c) { int index = m_constraintRefs.findLinearSearch(c); @@ -315,3 +352,51 @@ void btRigidBody::removeConstraintRef(btTypedConstraint* c) m_constraintRefs.remove(c); m_checkCollideWith = m_constraintRefs.size() > 0; } + +int btRigidBody::calculateSerializeBufferSize() const +{ + int sz = sizeof(btRigidBodyData); + return sz; +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const +{ + btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer; + + btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer); + + m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld); + m_linearVelocity.serialize(rbd->m_linearVelocity); + m_angularVelocity.serialize(rbd->m_angularVelocity); + rbd->m_inverseMass = m_inverseMass; + m_angularFactor.serialize(rbd->m_angularFactor); + m_linearFactor.serialize(rbd->m_linearFactor); + m_gravity.serialize(rbd->m_gravity); + m_gravity_acceleration.serialize(rbd->m_gravity_acceleration); + m_invInertiaLocal.serialize(rbd->m_invInertiaLocal); + m_totalForce.serialize(rbd->m_totalForce); + m_totalTorque.serialize(rbd->m_totalTorque); + rbd->m_linearDamping = m_linearDamping; + rbd->m_angularDamping = m_angularDamping; + rbd->m_additionalDamping = m_additionalDamping; + rbd->m_additionalDampingFactor = m_additionalDampingFactor; + rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr; + rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr; + rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor; + rbd->m_linearSleepingThreshold=m_linearSleepingThreshold; + rbd->m_angularSleepingThreshold = m_angularSleepingThreshold; + + return btRigidBodyDataName; +} + + + +void btRigidBody::serializeSingleObject(class btSerializer* serializer) const +{ + btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1); + const char* structType = serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this); +} + + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index da1fcb78611..5a7ba97ccbc 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -29,6 +29,20 @@ class btTypedConstraint; extern btScalar gDeactivationTime; extern bool gDisableDeactivation; +#ifdef BT_USE_DOUBLE_PRECISION +#define btRigidBodyData btRigidBodyDoubleData +#define btRigidBodyDataName "btRigidBodyDoubleData" +#else +#define btRigidBodyData btRigidBodyFloatData +#define btRigidBodyDataName "btRigidBodyFloatData" +#endif //BT_USE_DOUBLE_PRECISION + + +enum btRigidBodyFlags +{ + BT_DISABLE_WORLD_GRAVITY = 1 +}; + ///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape. ///It is recommended for performance and memory use to share btCollisionShape objects whenever possible. @@ -45,7 +59,6 @@ class btRigidBody : public btCollisionObject btVector3 m_linearVelocity; btVector3 m_angularVelocity; btScalar m_inverseMass; - btVector3 m_angularFactor; btVector3 m_linearFactor; btVector3 m_gravity; @@ -73,6 +86,21 @@ class btRigidBody : public btCollisionObject //keep track of typed constraints referencing this rigid body btAlignedObjectArray<btTypedConstraint*> m_constraintRefs; + int m_rigidbodyFlags; + + int m_debugBodyId; + + +protected: + + ATTRIBUTE_ALIGNED64(btVector3 m_deltaLinearVelocity); + btVector3 m_deltaAngularVelocity; + btVector3 m_angularFactor; + btVector3 m_invMass; + btVector3 m_pushVelocity; + btVector3 m_turnVelocity; + + public: @@ -111,7 +139,6 @@ public: btScalar m_additionalAngularDampingThresholdSqr; btScalar m_additionalAngularDampingFactor; - btRigidBodyConstructionInfo( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)): m_mass(mass), m_motionState(motionState), @@ -161,13 +188,13 @@ public: ///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast static const btRigidBody* upcast(const btCollisionObject* colObj) { - if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY) + if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY) return (const btRigidBody*)colObj; return 0; } static btRigidBody* upcast(btCollisionObject* colObj) { - if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY) + if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY) return (btRigidBody*)colObj; return 0; } @@ -227,6 +254,7 @@ public: void setLinearFactor(const btVector3& linearFactor) { m_linearFactor = linearFactor; + m_invMass = m_linearFactor*m_inverseMass; } btScalar getInvMass() const { return m_inverseMass; } const btMatrix3x3& getInvInertiaTensorWorld() const { @@ -242,12 +270,12 @@ public: m_totalForce += force*m_linearFactor; } - const btVector3& getTotalForce() + const btVector3& getTotalForce() const { return m_totalForce; }; - const btVector3& getTotalTorque() + const btVector3& getTotalTorque() const { return m_totalTorque; }; @@ -301,19 +329,6 @@ public: } } - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) - { - if (m_inverseMass != btScalar(0.)) - { - m_linearVelocity += linearComponent*m_linearFactor*impulseMagnitude; - if (m_angularFactor) - { - m_angularVelocity += angularComponent*m_angularFactor*impulseMagnitude; - } - } - } - void clearForces() { m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); @@ -489,12 +504,185 @@ public: return m_constraintRefs[index]; } - int getNumConstraintRefs() + int getNumConstraintRefs() const { return m_constraintRefs.size(); } - int m_debugBodyId; + void setFlags(int flags) + { + m_rigidbodyFlags = flags; + } + + int getFlags() const + { + return m_rigidbodyFlags; + } + + const btVector3& getDeltaLinearVelocity() const + { + return m_deltaLinearVelocity; + } + + const btVector3& getDeltaAngularVelocity() const + { + return m_deltaAngularVelocity; + } + + const btVector3& getPushVelocity() const + { + return m_pushVelocity; + } + + const btVector3& getTurnVelocity() const + { + return m_turnVelocity; + } + + + //////////////////////////////////////////////// + ///some internal methods, don't use them + + btVector3& internalGetDeltaLinearVelocity() + { + return m_deltaLinearVelocity; + } + + btVector3& internalGetDeltaAngularVelocity() + { + return m_deltaAngularVelocity; + } + + const btVector3& internalGetAngularFactor() const + { + return m_angularFactor; + } + + const btVector3& internalGetInvMass() const + { + return m_invMass; + } + + btVector3& internalGetPushVelocity() + { + return m_pushVelocity; + } + + btVector3& internalGetTurnVelocity() + { + return m_turnVelocity; + } + + SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const + { + velocity = getLinearVelocity()+m_deltaLinearVelocity + (getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos); + } + + SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const + { + angVel = getAngularVelocity()+m_deltaAngularVelocity; + } + + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude) + { + if (m_inverseMass) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + { + if (m_inverseMass) + { + m_pushVelocity += linearComponent*impulseMagnitude; + m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + void internalWritebackVelocity() + { + if (m_inverseMass) + { + setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity); + setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity); + //m_deltaLinearVelocity.setZero(); + //m_deltaAngularVelocity .setZero(); + //m_originalBody->setCompanionId(-1); + } + } + + + void internalWritebackVelocity(btScalar timeStep); + + + + /////////////////////////////////////////////// + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; + + virtual void serializeSingleObject(class btSerializer* serializer) const; + +}; + +//@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btRigidBodyFloatData +{ + btCollisionObjectFloatData m_collisionObjectData; + btMatrix3x3FloatData m_invInertiaTensorWorld; + btVector3FloatData m_linearVelocity; + btVector3FloatData m_angularVelocity; + btVector3FloatData m_angularFactor; + btVector3FloatData m_linearFactor; + btVector3FloatData m_gravity; + btVector3FloatData m_gravity_acceleration; + btVector3FloatData m_invInertiaLocal; + btVector3FloatData m_totalForce; + btVector3FloatData m_totalTorque; + float m_inverseMass; + float m_linearDamping; + float m_angularDamping; + float m_additionalDampingFactor; + float m_additionalLinearDampingThresholdSqr; + float m_additionalAngularDampingThresholdSqr; + float m_additionalAngularDampingFactor; + float m_linearSleepingThreshold; + float m_angularSleepingThreshold; + int m_additionalDamping; +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btRigidBodyDoubleData +{ + btCollisionObjectDoubleData m_collisionObjectData; + btMatrix3x3DoubleData m_invInertiaTensorWorld; + btVector3DoubleData m_linearVelocity; + btVector3DoubleData m_angularVelocity; + btVector3DoubleData m_angularFactor; + btVector3DoubleData m_linearFactor; + btVector3DoubleData m_gravity; + btVector3DoubleData m_gravity_acceleration; + btVector3DoubleData m_invInertiaLocal; + btVector3DoubleData m_totalForce; + btVector3DoubleData m_totalTorque; + double m_inverseMass; + double m_linearDamping; + double m_angularDamping; + double m_additionalDampingFactor; + double m_additionalLinearDampingThresholdSqr; + double m_additionalAngularDampingThresholdSqr; + double m_additionalAngularDampingFactor; + double m_linearSleepingThreshold; + double m_angularSleepingThreshold; + int m_additionalDamping; + char m_padding[4]; }; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 3f6141463c3..5fc2f3cf8f7 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -78,7 +78,7 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b btContactSolverInfo infoGlobal; infoGlobal.m_timeStep = timeStep; m_constraintSolver->prepareSolve(0,numManifolds); - m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1); + m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1); m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc); } @@ -132,9 +132,19 @@ btVector3 btSimpleDynamicsWorld::getGravity () const void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) { - removeCollisionObject(body); + btCollisionWorld::removeCollisionObject(body); } +void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +{ + btRigidBody* body = btRigidBody::upcast(collisionObject); + if (body) + removeRigidBody(body); + else + btCollisionWorld::removeCollisionObject(collisionObject); +} + + void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) { body->setGravity(m_gravity); @@ -145,6 +155,33 @@ void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) } } +void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask) +{ + body->setGravity(m_gravity); + + if (body->getCollisionShape()) + { + addCollisionObject(body,group,mask); + } +} + + +void btSimpleDynamicsWorld::debugDrawWorld() +{ + +} + +void btSimpleDynamicsWorld::addAction(btActionInterface* action) +{ + +} + +void btSimpleDynamicsWorld::removeAction(btActionInterface* action) +{ + +} + + void btSimpleDynamicsWorld::updateAabbs() { btTransform predictedTrans; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index ea1a2a1ccc7..07a727e2efe 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -56,7 +56,18 @@ public: virtual void addRigidBody(btRigidBody* body); + virtual void addRigidBody(btRigidBody* body, short group, short mask); + virtual void removeRigidBody(btRigidBody* body); + + virtual void debugDrawWorld(); + + virtual void addAction(btActionInterface* action); + + virtual void removeAction(btActionInterface* action); + + ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject + virtual void removeCollisionObject(btCollisionObject* collisionObject); virtual void updateAabbs(); diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index 031fcb5b447..345dca6a11c 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -22,7 +22,15 @@ #include "LinearMath/btIDebugDraw.h" #include "BulletDynamics/ConstraintSolver/btContactConstraint.h" -static btRigidBody s_fixedObject( 0,0,0); +#define ROLLING_INFLUENCE_FIX + + +btRigidBody& btActionInterface::getFixedBody() +{ + static btRigidBody s_fixed(0, 0,0); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); + return s_fixed; +} btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ) :m_vehicleRaycaster(raycaster), @@ -70,6 +78,7 @@ btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, con ci.m_frictionSlip = tuning.m_frictionSlip; ci.m_bIsFrontWheel = isFrontWheel; ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm; + ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce; m_wheelInfo.push_back( btWheelInfo(ci)); @@ -186,7 +195,7 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; wheel.m_raycastInfo.m_isInContact = true; - wheel.m_raycastInfo.m_groundObject = &s_fixedObject;///@todo for driving on dynamic/movable objects!; + wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!; //wheel.m_raycastInfo.m_groundObject = object; @@ -301,10 +310,9 @@ void btRaycastVehicle::updateVehicle( btScalar step ) btScalar suspensionForce = wheel.m_wheelsSuspensionForce; - btScalar gMaxSuspensionForce = btScalar(6000.); - if (suspensionForce > gMaxSuspensionForce) + if (suspensionForce > wheel.m_maxSuspensionForce) { - suspensionForce = gMaxSuspensionForce; + suspensionForce = wheel.m_maxSuspensionForce; } btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition(); @@ -689,7 +697,12 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; +#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT. + btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(1); + rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence)); +#else rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence; +#endif m_chassisBody->applyImpulse(sideImp,rel_pos); //apply friction impulse on the ground @@ -708,13 +721,13 @@ void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer) for (int v=0;v<this->getNumWheels();v++) { - btVector3 wheelColor(0,255,255); + btVector3 wheelColor(0,1,1); if (getWheelInfo(v).m_raycastInfo.m_isInContact) { - wheelColor.setValue(0,0,255); + wheelColor.setValue(0,0,1); } else { - wheelColor.setValue(255,0,255); + wheelColor.setValue(1,0,1); } btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin(); diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h index d5a299c606f..ac42f1313be 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -29,12 +29,11 @@ class btRaycastVehicle : public btActionInterface btAlignedObjectArray<btVector3> m_axle; btAlignedObjectArray<btScalar> m_forwardImpulse; btAlignedObjectArray<btScalar> m_sideImpulse; - + + ///backwards compatibility int m_userConstraintType; - int m_userConstraintId; - public: class btVehicleTuning { @@ -45,7 +44,8 @@ public: m_suspensionCompression(btScalar(0.83)), m_suspensionDamping(btScalar(0.88)), m_maxSuspensionTravelCm(btScalar(500.)), - m_frictionSlip(btScalar(10.5)) + m_frictionSlip(btScalar(10.5)), + m_maxSuspensionForce(btScalar(6000.)) { } btScalar m_suspensionStiffness; @@ -53,6 +53,7 @@ public: btScalar m_suspensionDamping; btScalar m_maxSuspensionTravelCm; btScalar m_frictionSlip; + btScalar m_maxSuspensionForce; }; private: @@ -83,6 +84,7 @@ public: ///btActionInterface interface virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step) { + (void) collisionWorld; updateVehicle(step); } @@ -110,7 +112,7 @@ public: void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true ); - void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth); +// void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth); btWheelInfo& addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel); @@ -192,6 +194,8 @@ public: m_indexForwardAxis = forwardIndex; } + + ///backwards compatibility int getUserConstraintType() const { return m_userConstraintType ; @@ -212,7 +216,6 @@ public: return m_userConstraintId; } - }; class btDefaultVehicleRaycaster : public btVehicleRaycaster diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h index ac2729f4fd7..b74f8c80acb 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h @@ -29,6 +29,7 @@ struct btWheelInfoConstructionInfo btScalar m_wheelsDampingCompression; btScalar m_wheelsDampingRelaxation; btScalar m_frictionSlip; + btScalar m_maxSuspensionForce; bool m_bIsFrontWheel; }; @@ -68,6 +69,7 @@ struct btWheelInfo btScalar m_rotation; btScalar m_deltaRotation; btScalar m_rollInfluence; + btScalar m_maxSuspensionForce; btScalar m_engineForce; @@ -99,6 +101,7 @@ struct btWheelInfo m_brake = btScalar(0.); m_rollInfluence = btScalar(0.1); m_bIsFrontWheel = ci.m_bIsFrontWheel; + m_maxSuspensionForce = ci.m_maxSuspensionForce; } diff --git a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp new file mode 100644 index 00000000000..c876ebf1fd6 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp @@ -0,0 +1,151 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" + +#include "btDefaultSoftBodySolver.h" +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" +#include "BulletSoftBody/btSoftBody.h" + + +btDefaultSoftBodySolver::btDefaultSoftBodySolver() +{ + // Initial we will clearly need to update solver constants + // For now this is global for the cloths linked with this solver - we should probably make this body specific + // for performance in future once we understand more clearly when constants need to be updated + m_updateSolverConstants = true; +} + +btDefaultSoftBodySolver::~btDefaultSoftBodySolver() +{ +} + +// In this case the data is already in the soft bodies so there is no need for us to do anything +void btDefaultSoftBodySolver::copyBackToSoftBodies() +{ + +} + +void btDefaultSoftBodySolver::optimize( btAlignedObjectArray< btSoftBody * > &softBodies , bool forceUpdate) +{ + m_softBodySet.copyFromArray( softBodies ); +} + +void btDefaultSoftBodySolver::updateSoftBodies( ) +{ + for ( int i=0; i < m_softBodySet.size(); i++) + { + btSoftBody* psb=(btSoftBody*)m_softBodySet[i]; + if (psb->isActive()) + { + psb->integrateMotion(); + } + } +} // updateSoftBodies + +bool btDefaultSoftBodySolver::checkInitialized() +{ + return true; +} + +void btDefaultSoftBodySolver::solveConstraints( float solverdt ) +{ + // Solve constraints for non-solver softbodies + for(int i=0; i < m_softBodySet.size(); ++i) + { + btSoftBody* psb = static_cast<btSoftBody*>(m_softBodySet[i]); + if (psb->isActive()) + { + psb->solveConstraints(); + } + } +} // btDefaultSoftBodySolver::solveConstraints + + +void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer ) +{ + // Currently only support CPU output buffers + // TODO: check for DX11 buffers. Take all offsets into the same DX11 buffer + // and use them together on a single kernel call if possible by setting up a + // per-cloth target buffer array for the copy kernel. + + if( vertexBuffer->getBufferType() == btVertexBufferDescriptor::CPU_BUFFER ) + { + const btAlignedObjectArray<btSoftBody::Node> &clothVertices( softBody->m_nodes ); + int numVertices = clothVertices.size(); + + const btCPUVertexBufferDescriptor *cpuVertexBuffer = static_cast< btCPUVertexBufferDescriptor* >(vertexBuffer); + float *basePointer = cpuVertexBuffer->getBasePointer(); + + if( vertexBuffer->hasVertexPositions() ) + { + const int vertexOffset = cpuVertexBuffer->getVertexOffset(); + const int vertexStride = cpuVertexBuffer->getVertexStride(); + float *vertexPointer = basePointer + vertexOffset; + + for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) + { + btVector3 position = clothVertices[vertexIndex].m_x; + *(vertexPointer + 0) = position.getX(); + *(vertexPointer + 1) = position.getY(); + *(vertexPointer + 2) = position.getZ(); + vertexPointer += vertexStride; + } + } + if( vertexBuffer->hasNormals() ) + { + const int normalOffset = cpuVertexBuffer->getNormalOffset(); + const int normalStride = cpuVertexBuffer->getNormalStride(); + float *normalPointer = basePointer + normalOffset; + + for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) + { + btVector3 normal = clothVertices[vertexIndex].m_n; + *(normalPointer + 0) = normal.getX(); + *(normalPointer + 1) = normal.getY(); + *(normalPointer + 2) = normal.getZ(); + normalPointer += normalStride; + } + } + } +} // btDefaultSoftBodySolver::copySoftBodyToVertexBuffer + +void btDefaultSoftBodySolver::processCollision( btSoftBody* softBody, btSoftBody* otherSoftBody) +{ + softBody->defaultCollisionHandler( otherSoftBody); +} + +// For the default solver just leave the soft body to do its collision processing +void btDefaultSoftBodySolver::processCollision( btSoftBody *softBody, btCollisionObject* collisionObject ) +{ + softBody->defaultCollisionHandler( collisionObject ); +} // btDefaultSoftBodySolver::processCollision + + +void btDefaultSoftBodySolver::predictMotion( float timeStep ) +{ + for ( int i=0; i < m_softBodySet.size(); ++i) + { + btSoftBody* psb = m_softBodySet[i]; + + if (psb->isActive()) + { + psb->predictMotion(timeStep); + } + } +} + diff --git a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h new file mode 100644 index 00000000000..8e7db3daf98 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h @@ -0,0 +1,63 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOFT_BODY_DEFAULT_SOLVER_H +#define BT_SOFT_BODY_DEFAULT_SOLVER_H + + +#include "BulletSoftBody/btSoftBodySolvers.h" +#include "btSoftBodySolverVertexBuffer.h" + + +class btDefaultSoftBodySolver : public btSoftBodySolver +{ +protected: + /** Variable to define whether we need to update solver constants on the next iteration */ + bool m_updateSolverConstants; + + btAlignedObjectArray< btSoftBody * > m_softBodySet; + + +public: + btDefaultSoftBodySolver(); + + virtual ~btDefaultSoftBodySolver(); + + virtual SolverTypes getSolverType() const + { + return DEFAULT_SOLVER; + } + + virtual bool checkInitialized(); + + virtual void updateSoftBodies( ); + + virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies,bool forceUpdate=false ); + + virtual void copyBackToSoftBodies(); + + virtual void solveConstraints( float solverdt ); + + virtual void predictMotion( float solverdt ); + + virtual void copySoftBodyToVertexBuffer( const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer ); + + virtual void processCollision( btSoftBody *, btCollisionObject* ); + + virtual void processCollision( btSoftBody*, btSoftBody* ); + +}; + +#endif // #ifndef BT_ACCELERATED_SOFT_BODY_CPU_SOLVER_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp index ee810c54082..a90acb99f04 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp @@ -15,12 +15,51 @@ subject to the following restrictions: ///btSoftBody implementation by Nathanael Presson #include "btSoftBodyInternals.h" +#include "BulletSoftBody/btSoftBodySolvers.h" +#include "btSoftBodyData.h" +#include "LinearMath/btSerializer.h" // btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m) -:m_worldInfo(worldInfo) +:m_worldInfo(worldInfo),m_softBodySolver(0) { /* Init */ + initDefaults(); + + /* Default material */ + Material* pm=appendMaterial(); + pm->m_kLST = 1; + pm->m_kAST = 1; + pm->m_kVST = 1; + pm->m_flags = fMaterial::Default; + + /* Nodes */ + const btScalar margin=getCollisionShape()->getMargin(); + m_nodes.resize(node_count); + for(int i=0,ni=node_count;i<ni;++i) + { + Node& n=m_nodes[i]; + ZeroInitialize(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; + n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); + n.m_material= pm; + } + updateBounds(); + +} + +btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo) +:m_worldInfo(worldInfo) +{ + initDefaults(); +} + + +void btSoftBody::initDefaults() +{ m_internalType = CO_SOFT_BODY; m_cfg.aeromodel = eAeroModel::V_Point; m_cfg.kVCF = 1; @@ -61,33 +100,16 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btV m_bounds[1] = btVector3(0,0,0); m_worldTransform.setIdentity(); setSolver(eSolverPresets::Positions); - /* Default material */ - Material* pm=appendMaterial(); - pm->m_kLST = 1; - pm->m_kAST = 1; - pm->m_kVST = 1; - pm->m_flags = fMaterial::Default; + /* Collision shape */ ///for now, create a collision shape internally m_collisionShape = new btSoftBodyCollisionShape(this); m_collisionShape->setMargin(0.25); - /* Nodes */ - const btScalar margin=getCollisionShape()->getMargin(); - m_nodes.resize(node_count); - for(int i=0,ni=node_count;i<ni;++i) - { - Node& n=m_nodes[i]; - ZeroInitialize(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; - n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); - n.m_material= pm; - } - updateBounds(); - + m_initialWorldTransform.setIdentity(); + + m_windVelocity = btVector3(0,0,0); + } // @@ -306,8 +328,44 @@ void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat) } // +void btSoftBody::appendTetra(int model,Material* mat) +{ +Tetra t; +if(model>=0) + t=m_tetras[model]; + else + { ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; } +m_tetras.push_back(t); +} + +// +void btSoftBody::appendTetra(int node0, + int node1, + int node2, + int node3, + Material* mat) +{ + appendTetra(-1,mat); + Tetra& t=m_tetras[m_tetras.size()-1]; + t.m_n[0] = &m_nodes[node0]; + t.m_n[1] = &m_nodes[node1]; + t.m_n[2] = &m_nodes[node2]; + t.m_n[3] = &m_nodes[node3]; + t.m_rv = VolumeOf(t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x); + m_bUpdateRtCst=true; +} + +// + void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies) { + btVector3 local = body->getWorldTransform().inverse()*m_nodes[node].m_x; + appendAnchor(node,body,local,disableCollisionBetweenLinkedBodies); +} + +// +void btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies) +{ if (disableCollisionBetweenLinkedBodies) { if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size()) @@ -319,7 +377,7 @@ void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisio Anchor a; a.m_node = &m_nodes[node]; a.m_body = body; - a.m_local = body->getInterpolationWorldTransform().inverse()*a.m_node->m_x; + a.m_local = localPivot; a.m_node->m_battach = 1; m_anchors.push_back(a); } @@ -491,6 +549,51 @@ void btSoftBody::setTotalDensity(btScalar density) } // +void btSoftBody::setVolumeMass(btScalar mass) +{ +btAlignedObjectArray<btScalar> ranks; +ranks.resize(m_nodes.size(),0); +int i; + +for(i=0;i<m_nodes.size();++i) + { + m_nodes[i].m_im=0; + } +for(i=0;i<m_tetras.size();++i) + { + const Tetra& t=m_tetras[i]; + for(int j=0;j<4;++j) + { + t.m_n[j]->m_im+=btFabs(t.m_rv); + ranks[int(t.m_n[j]-&m_nodes[0])]+=1; + } + } +for( i=0;i<m_nodes.size();++i) + { + if(m_nodes[i].m_im>0) + { + m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im; + } + } +setTotalMass(mass,false); +} + +// +void btSoftBody::setVolumeDensity(btScalar density) +{ +btScalar volume=0; +for(int i=0;i<m_tetras.size();++i) + { + const Tetra& t=m_tetras[i]; + for(int j=0;j<4;++j) + { + volume+=btFabs(t.m_rv); + } + } +setVolumeMass(volume*density/6); +} + +// void btSoftBody::transform(const btTransform& trs) { const btScalar margin=getCollisionShape()->getMargin(); @@ -533,6 +636,7 @@ void btSoftBody::rotate( const btQuaternion& rot) // void btSoftBody::scale(const btVector3& scl) { + const btScalar margin=getCollisionShape()->getMargin(); ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; @@ -611,7 +715,7 @@ btScalar btSoftBody::getVolume() const for(i=0,ni=m_faces.size();i<ni;++i) { const Face& f=m_faces[i]; - vol+=dot(f.m_n[0]->m_x-org,cross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org)); + vol+=btDot(f.m_n[0]->m_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org)); } vol/=(btScalar)6; } @@ -644,14 +748,14 @@ btVector3 btSoftBody::clusterCom(int cluster) const // btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos) { - return(cluster->m_lv+cross(cluster->m_av,rpos)); + return(cluster->m_lv+btCross(cluster->m_av,rpos)); } // void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) { const btVector3 li=cluster->m_imass*impulse; - const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); + const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse); cluster->m_vimpulses[0]+=li;cluster->m_lv+=li; cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; cluster->m_nvimpulses++; @@ -661,7 +765,7 @@ void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) { const btVector3 li=cluster->m_imass*impulse; - const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); + const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse); cluster->m_dimpulses[0]+=li; cluster->m_dimpulses[1]+=ai; cluster->m_ndimpulses++; @@ -704,6 +808,13 @@ void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse) cluster->m_ndimpulses++; } +struct NodeLinks +{ + btAlignedObjectArray<int> m_links; +}; + + + // int btSoftBody::generateBendingConstraints(int distance,Material* mat) { @@ -715,14 +826,21 @@ int btSoftBody::generateBendingConstraints(int distance,Material* mat) 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(j=0;j<n;++j) { for(i=0;i<n;++i) { - if(i!=j) adj[IDX(i,j)]=adj[IDX(j,i)]=inf; + if(i!=j) + { + adj[IDX(i,j)]=adj[IDX(j,i)]=inf; + } else + { adj[IDX(i,j)]=adj[IDX(j,i)]=0; + } } } for( i=0;i<m_links.size();++i) @@ -732,20 +850,71 @@ int btSoftBody::generateBendingConstraints(int distance,Material* mat) adj[IDX(ia,ib)]=1; adj[IDX(ib,ia)]=1; } - for(int k=0;k<n;++k) + + + //special optimized case for distance == 2 + if (distance == 2) + { + + btAlignedObjectArray<NodeLinks> nodeLinks; + + + /* Build node links */ + nodeLinks.resize(m_nodes.size()); + + for( i=0;i<m_links.size();++i) + { + const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]); + const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]); + if (nodeLinks[ia].m_links.findLinearSearch(ib)==nodeLinks[ia].m_links.size()) + nodeLinks[ia].m_links.push_back(ib); + + if (nodeLinks[ib].m_links.findLinearSearch(ia)==nodeLinks[ib].m_links.size()) + nodeLinks[ib].m_links.push_back(ia); + } + for (int ii=0;ii<nodeLinks.size();ii++) + { + int i=ii; + + for (int jj=0;jj<nodeLinks[ii].m_links.size();jj++) + { + int k = nodeLinks[ii].m_links[jj]; + for (int kk=0;kk<nodeLinks[k].m_links.size();kk++) + { + int j = nodeLinks[k].m_links[kk]; + if (i!=j) + { + const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)]; + btAssert(sum==2); + if(adj[IDX(i,j)]>sum) + { + adj[IDX(i,j)]=adj[IDX(j,i)]=sum; + } + } + + } + } + } + } else { - for(j=0;j<n;++j) + ///generic Floyd's algorithm + for(int k=0;k<n;++k) { - for(i=j+1;i<n;++i) + for(j=0;j<n;++j) { - const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)]; - if(adj[IDX(i,j)]>sum) + for(i=j+1;i<n;++i) { - adj[IDX(i,j)]=adj[IDX(j,i)]=sum; + const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)]; + if(adj[IDX(i,j)]>sum) + { + adj[IDX(i,j)]=adj[IDX(j,i)]=sum; + } } } } } + + /* Build links */ int nlinks=0; for(j=0;j<n;++j) @@ -917,10 +1086,50 @@ int btSoftBody::generateClusters(int k,int maxiterations) releaseCluster(i--); } } + } else + { + //create a cluster for each tetrahedron (if tetrahedra exist) or each face + if (m_tetras.size()) + { + m_clusters.resize(m_tetras.size()); + for(i=0;i<m_clusters.size();++i) + { + m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); + m_clusters[i]->m_collide= true; + } + for (i=0;i<m_tetras.size();i++) + { + for (int j=0;j<4;j++) + { + m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]); + } + } + + } else + { + m_clusters.resize(m_faces.size()); + for(i=0;i<m_clusters.size();++i) + { + m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); + m_clusters[i]->m_collide= true; + } + + for(i=0;i<m_faces.size();++i) + { + for(int j=0;j<3;++j) + { + m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]); + } + } + } + } + if (m_clusters.size()) + { initializeClusters(); updateClusters(); + //for self-collision m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size()); { @@ -948,10 +1157,9 @@ int btSoftBody::generateClusters(int k,int maxiterations) } } } - - return(m_clusters.size()); } - return(0); + + return(m_clusters.size()); } // @@ -1311,6 +1519,7 @@ void btSoftBody::setSolver(eSolverPresets::_ preset) // void btSoftBody::predictMotion(btScalar dt) { + int i,ni; /* Update */ @@ -1402,6 +1611,7 @@ void btSoftBody::predictMotion(btScalar dt) // void btSoftBody::solveConstraints() { + /* Apply clusters */ applyClusters(false); /* Prepare links */ @@ -1585,19 +1795,19 @@ btScalar btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayF static const btScalar ceps=-SIMD_EPSILON*10; static const btScalar teps=SIMD_EPSILON*10; - const btVector3 n=cross(b-a,c-a); - const btScalar d=dot(a,n); - const btScalar den=dot(rayNormalizedDirection,n); + const btVector3 n=btCross(b-a,c-a); + const btScalar d=btDot(a,n); + const btScalar den=btDot(rayNormalizedDirection,n); if(!btFuzzyZero(den)) { - const btScalar num=dot(rayFrom,n)-d; + const btScalar num=btDot(rayFrom,n)-d; const btScalar t=-num/den; if((t>teps)&&(t<maxt)) { const btVector3 hit=rayFrom+rayNormalizedDirection*t; - if( (dot(n,cross(a-hit,b-hit))>ceps) && - (dot(n,cross(b-hit,c-hit))>ceps) && - (dot(n,cross(c-hit,a-hit))>ceps)) + if( (btDot(n,btCross(a-hit,b-hit))>ceps) && + (btDot(n,btCross(b-hit,c-hit))>ceps) && + (btDot(n,btCross(c-hit,a-hit))>ceps)) { return(t); } @@ -1771,20 +1981,21 @@ bool btSoftBody::checkContact( btCollisionObject* colObj, btScalar margin, btSoftBody::sCti& cti) const { - btVector3 nrm; - btCollisionShape* shp=colObj->getCollisionShape(); - btRigidBody* tmpRigid = btRigidBody::upcast(colObj); - const btTransform& wtr=tmpRigid? tmpRigid->getInterpolationWorldTransform() : colObj->getWorldTransform(); - btScalar dst=m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), - shp, - nrm, - margin); + btVector3 nrm; + btCollisionShape *shp = colObj->getCollisionShape(); + btRigidBody *tmpRigid = btRigidBody::upcast(colObj); + const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObj->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(x), + shp, + nrm, + margin); if(dst<0) { - cti.m_colObj = colObj; - cti.m_normal = wtr.getBasis()*nrm; - cti.m_offset = -dot( cti.m_normal, - x-cti.m_normal*dst); + cti.m_colObj = colObj; + cti.m_normal = wtr.getBasis()*nrm; + cti.m_offset = -btDot( cti.m_normal, x - cti.m_normal * dst ); return(true); } return(false); @@ -1793,6 +2004,7 @@ bool btSoftBody::checkContact( btCollisionObject* colObj, // void btSoftBody::updateNormals() { + const btVector3 zv(0,0,0); int i,ni; @@ -1803,7 +2015,7 @@ void btSoftBody::updateNormals() for(i=0,ni=m_faces.size();i<ni;++i) { btSoftBody::Face& f=m_faces[i]; - const btVector3 n=cross(f.m_n[1]->m_x-f.m_n[0]->m_x, + const btVector3 n=btCross(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; @@ -1821,29 +2033,42 @@ void btSoftBody::updateNormals() // void btSoftBody::updateBounds() { - if(m_ndbvt.m_root) - { - const btVector3& mins=m_ndbvt.m_root->volume.Mins(); - const btVector3& maxs=m_ndbvt.m_root->volume.Maxs(); - const btScalar csm=getCollisionShape()->getMargin(); - const btVector3 mrg=btVector3( csm, - csm, - csm)*1; // ??? to investigate... - m_bounds[0]=mins-mrg; - m_bounds[1]=maxs+mrg; - if(0!=getBroadphaseHandle()) - { - m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), - m_bounds[0], - m_bounds[1], - m_worldInfo->m_dispatcher); + /*if( m_acceleratedSoftBody ) + { + // If we have an accelerated softbody we need to obtain the bounds correctly + // For now (slightly hackily) just have a very large AABB + // TODO: Write get bounds kernel + // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could + // probably do a test and exchange reasonably efficiently. + + m_bounds[0] = btVector3(-1000, -1000, -1000); + m_bounds[1] = btVector3(1000, 1000, 1000); + + } else {*/ + if(m_ndbvt.m_root) + { + const btVector3& mins=m_ndbvt.m_root->volume.Mins(); + const btVector3& maxs=m_ndbvt.m_root->volume.Maxs(); + const btScalar csm=getCollisionShape()->getMargin(); + const btVector3 mrg=btVector3( csm, + csm, + csm)*1; // ??? to investigate... + m_bounds[0]=mins-mrg; + m_bounds[1]=maxs+mrg; + if(0!=getBroadphaseHandle()) + { + m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); + } } - } - else - { - m_bounds[0]= - m_bounds[1]=btVector3(0,0,0); - } + else + { + m_bounds[0]= + m_bounds[1]=btVector3(0,0,0); + } + //} } @@ -1941,10 +2166,17 @@ void btSoftBody::initializeClusters() c.m_masses.resize(c.m_nodes.size()); for(int j=0;j<c.m_nodes.size();++j) { - c.m_masses[j] = c.m_nodes[j]->m_im>0?1/c.m_nodes[j]->m_im:0; + if (c.m_nodes[j]->m_im==0) + { + c.m_containsAnchor = true; + c.m_masses[j] = BT_LARGE_FLOAT; + } else + { + c.m_masses[j] = btScalar(1.)/c.m_nodes[j]->m_im; + } c.m_imass += c.m_masses[j]; } - c.m_imass = 1/c.m_imass; + c.m_imass = btScalar(1.)/c.m_imass; c.m_com = btSoftBody::clusterCom(&c); c.m_lv = btVector3(0,0,0); c.m_av = btVector3(0,0,0); @@ -1971,7 +2203,9 @@ void btSoftBody::initializeClusters() ii[1][0]=ii[0][1]; ii[2][0]=ii[0][2]; ii[2][1]=ii[1][2]; - ii=ii.inverse(); + + ii = ii.inverse(); + /* Frame */ c.m_framexform.setIdentity(); c.m_framexform.setOrigin(c.m_com); @@ -1996,7 +2230,7 @@ void btSoftBody::updateClusters() { btSoftBody::Cluster& c=*m_clusters[i]; const int n=c.m_nodes.size(); - const btScalar invn=1/(btScalar)n; + //const btScalar invn=1/(btScalar)n; if(n) { /* Frame */ @@ -2058,7 +2292,7 @@ void btSoftBody::updateClusters() { const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i]; c.m_lv += v; - c.m_av += cross(c.m_nodes[i]->m_x-c.m_com,v); + c.m_av += btCross(c.m_nodes[i]->m_x-c.m_com,v); } } c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping); @@ -2141,8 +2375,8 @@ void btSoftBody::solveClusters(btScalar sor) void btSoftBody::applyClusters(bool drift) { BT_PROFILE("ApplyClusters"); - const btScalar f0=m_sst.sdt; - const btScalar f1=f0/2; +// const btScalar f0=m_sst.sdt; + //const btScalar f1=f0/2; btAlignedObjectArray<btVector3> deltas; btAlignedObjectArray<btScalar> weights; deltas.resize(m_nodes.size(),btVector3(0,0,0)); @@ -2174,14 +2408,17 @@ void btSoftBody::applyClusters(bool drift) const int idx=int(c.m_nodes[j]-&m_nodes[0]); const btVector3& x=c.m_nodes[j]->m_x; const btScalar q=c.m_masses[j]; - deltas[idx] += (v+cross(w,x-c.m_com))*q; + deltas[idx] += (v+btCross(w,x-c.m_com))*q; weights[idx] += q; } } } for(i=0;i<deltas.size();++i) { - if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i]; + if(weights[i]>0) + { + m_nodes[i].m_x+=deltas[i]/weights[i]; + } } } @@ -2200,7 +2437,7 @@ void btSoftBody::dampClusters() Node& n=*c.m_nodes[j]; if(n.m_im>0) { - const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com); + const btVector3 vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com); if(vx.length2()<=n.m_v.length2()) { n.m_v += c.m_ndamping*(vx-n.m_v); @@ -2269,8 +2506,8 @@ void btSoftBody::AJoint::Prepare(btScalar dt,int iterations) Joint::Prepare(dt,iterations); m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0]; m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1]; - m_drift = NormalizeAny(cross(m_axis[1],m_axis[0])); - m_drift *= btMin(maxdrift,btAcos(Clamp<btScalar>(dot(m_axis[0],m_axis[1]),-1,+1))); + m_drift = NormalizeAny(btCross(m_axis[1],m_axis[0])); + m_drift *= btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1))); m_drift *= m_erp/dt; m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia()); if(m_split>0) @@ -2287,7 +2524,7 @@ void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor) const btVector3 va=m_bodies[0].angularVelocity(); const btVector3 vb=m_bodies[1].angularVelocity(); const btVector3 vr=va-vb; - const btScalar sp=dot(vr,m_axis[0]); + const btScalar sp=btDot(vr,m_axis[0]); const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp); btSoftBody::Impulse impulse; impulse.m_asVelocity = 1; @@ -2334,7 +2571,7 @@ void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor) const btVector3 va=m_bodies[0].velocity(m_rpos[0]); const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); const btVector3 vrel=va-vb; - const btScalar rvac=dot(vrel,m_normal); + const btScalar rvac=btDot(vrel,m_normal); btSoftBody::Impulse impulse; impulse.m_asVelocity = 1; impulse.m_velocity = m_drift; @@ -2345,8 +2582,29 @@ void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor) impulse.m_velocity += iv+fv*m_friction; } impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor; - m_bodies[0].applyImpulse(-impulse,m_rpos[0]); - m_bodies[1].applyImpulse( impulse,m_rpos[1]); + + if (m_bodies[0].m_soft==m_bodies[1].m_soft) + { + if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&& + (impulse.m_velocity.getZ() ==impulse.m_velocity.getZ())) + { + if (impulse.m_asVelocity) + { + if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse) + { + + } else + { + m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]); + m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]); + } + } + } + } else + { + m_bodies[0].applyImpulse(-impulse,m_rpos[0]); + m_bodies[1].applyImpulse( impulse,m_rpos[1]); + } } // @@ -2364,27 +2622,27 @@ void btSoftBody::applyForces() { BT_PROFILE("SoftBody applyForces"); - const btScalar dt=m_sst.sdt; - const btScalar kLF=m_cfg.kLF; - const btScalar kDG=m_cfg.kDG; - const btScalar kPR=m_cfg.kPR; - const btScalar kVC=m_cfg.kVC; - const bool as_lift=kLF>0; - const bool as_drag=kDG>0; - const bool as_pressure=kPR!=0; - const bool as_volume=kVC>0; - const bool as_aero= as_lift || - as_drag ; - const bool as_vaero= as_aero && - (m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided); - const bool as_faero= as_aero && - (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided); - const bool use_medium= as_aero; - const bool use_volume= as_pressure || + const btScalar dt = m_sst.sdt; + const btScalar kLF = m_cfg.kLF; + const btScalar kDG = m_cfg.kDG; + const btScalar kPR = m_cfg.kPR; + const btScalar kVC = m_cfg.kVC; + const bool as_lift = kLF>0; + const bool as_drag = kDG>0; + const bool as_pressure = kPR!=0; + const bool as_volume = kVC>0; + const bool as_aero = as_lift || + as_drag ; + const bool as_vaero = as_aero && + (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided); + const bool as_faero = as_aero && + (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided); + const bool use_medium = as_aero; + const bool use_volume = as_pressure || as_volume ; - btScalar volume=0; - btScalar ivolumetp=0; - btScalar dvolumetv=0; + btScalar volume = 0; + btScalar ivolumetp = 0; + btScalar dvolumetv = 0; btSoftBody::sMedium medium; if(use_volume) { @@ -2402,33 +2660,41 @@ void btSoftBody::applyForces() { if(use_medium) { - EvaluateMedium(m_worldInfo,n.m_x,medium); + EvaluateMedium(m_worldInfo, n.m_x, medium); + medium.m_velocity = m_windVelocity; + medium.m_density = m_worldInfo->air_density; + /* Aerodynamics */ if(as_vaero) { - const btVector3 rel_v=n.m_v-medium.m_velocity; - const btScalar rel_v2=rel_v.length2(); + const btVector3 rel_v = n.m_v - medium.m_velocity; + const btScalar rel_v2 = rel_v.length2(); if(rel_v2>SIMD_EPSILON) { - btVector3 nrm=n.m_n; + btVector3 nrm = n.m_n; /* Setup normal */ switch(m_cfg.aeromodel) { case btSoftBody::eAeroModel::V_Point: - nrm=NormalizeAny(rel_v);break; + nrm = NormalizeAny(rel_v); + break; case btSoftBody::eAeroModel::V_TwoSided: - nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break; + nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + break; + default: + { + } } - const btScalar dvn=dot(rel_v,nrm); + const btScalar dvn = btDot(rel_v,nrm); /* Compute forces */ if(dvn>0) { btVector3 force(0,0,0); - const btScalar c0 = n.m_area*dvn*rel_v2/2; - const btScalar c1 = c0*medium.m_density; + const btScalar c0 = n.m_area * dvn * rel_v2/2; + const btScalar c1 = c0 * medium.m_density; force += nrm*(-c1*kLF); - force += rel_v.normalized()*(-c1*kDG); - ApplyClampedForce(n,force,dt); + force += rel_v.normalized() * (-c1 * kDG); + ApplyClampedForce(n, force, dt); } } } @@ -2463,9 +2729,12 @@ void btSoftBody::applyForces() switch(m_cfg.aeromodel) { case btSoftBody::eAeroModel::F_TwoSided: - nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break; + nrm*=(btScalar)(btDot(nrm,rel_v)<0?-1:+1);break; + default: + { + } } - const btScalar dvn=dot(rel_v,nrm); + const btScalar dvn=btDot(rel_v,nrm); /* Compute forces */ if(dvn>0) { @@ -2490,7 +2759,7 @@ void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti) for(int i=0,ni=psb->m_anchors.size();i<ni;++i) { const Anchor& a=psb->m_anchors[i]; - const btTransform& t=a.m_body->getInterpolationWorldTransform(); + const btTransform& t=a.m_body->getWorldTransform(); Node& n=*a.m_node; const btVector3 wa=t*a.m_local; const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt; @@ -2503,26 +2772,27 @@ void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti) } // -void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti) +void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) { - const btScalar dt=psb->m_sst.sdt; - const btScalar mrg=psb->getCollisionShape()->getMargin(); + const btScalar dt = psb->m_sst.sdt; + const btScalar mrg = psb->getCollisionShape()->getMargin(); for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i) { - const RContact& c=psb->m_rcontacts[i]; - const sCti& cti=c.m_cti; + const RContact& c = psb->m_rcontacts[i]; + const sCti& cti = c.m_cti; btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj); - const btVector3 va=tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); - const btVector3 vb=c.m_node->m_x-c.m_node->m_q; - const btVector3 vr=vb-va; - const btScalar dn=dot(vr,cti.m_normal); + const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); + const btVector3 vb = c.m_node->m_x-c.m_node->m_q; + const btVector3 vr = vb-va; + const btScalar dn = btDot(vr, cti.m_normal); if(dn<=SIMD_EPSILON) { - const btScalar dp=btMin(dot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg); - const btVector3 fv=vr-cti.m_normal*dn; - const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst); - c.m_node->m_x-=impulse*c.m_c2; + const btScalar dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg ); + const btVector3 fv = vr - (cti.m_normal * dn); + // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient + const btVector3 impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst ); + c.m_node->m_x -= impulse * c.m_c2; if (tmpRigid) tmpRigid->applyImpulse(impulse,c.m_c1); } @@ -2548,9 +2818,10 @@ void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti) c.m_weights); const btVector3 vr=(n.m_x-n.m_q)-(p-q); btVector3 corr(0,0,0); - if(dot(vr,nr)<0) + btScalar dot = btDot(vr,nr); + if(dot<0) { - const btScalar j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p)); + const btScalar j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p)); corr+=c.m_normal*j; } corr -= ProjectOnPlane(vr,nr)*c.m_friction; @@ -2573,10 +2844,12 @@ void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti) Node& b=*l.m_n[1]; const btVector3 del=b.m_x-a.m_x; const btScalar len=del.length2(); - const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst; - const btScalar t=k*a.m_im; - a.m_x-=del*(k*a.m_im); - b.m_x+=del*(k*b.m_im); + if (l.m_c1+len > SIMD_EPSILON) + { + 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); + } } } } @@ -2588,7 +2861,7 @@ void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst) { Link& l=psb->m_links[i]; Node** n=l.m_n; - const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; + const btScalar j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; n[0]->m_v+= l.m_c3*(j*n[0]->m_im); n[1]->m_v-= l.m_c3*(j*n[1]->m_im); } @@ -2599,10 +2872,17 @@ btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver) { switch(solver) { - case ePSolver::Anchors: return(&btSoftBody::PSolve_Anchors); - case ePSolver::Linear: return(&btSoftBody::PSolve_Links); - case ePSolver::RContacts: return(&btSoftBody::PSolve_RContacts); - case ePSolver::SContacts: return(&btSoftBody::PSolve_SContacts); + case ePSolver::Anchors: + return(&btSoftBody::PSolve_Anchors); + case ePSolver::Linear: + return(&btSoftBody::PSolve_Links); + case ePSolver::RContacts: + return(&btSoftBody::PSolve_RContacts); + case ePSolver::SContacts: + return(&btSoftBody::PSolve_SContacts); + default: + { + } } return(0); } @@ -2613,6 +2893,9 @@ btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) switch(solver) { case eVSolver::Linear: return(&btSoftBody::VSolve_Links); + default: + { + } } return(0); } @@ -2620,13 +2903,14 @@ btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) // void btSoftBody::defaultCollisionHandler(btCollisionObject* pco) { + switch(m_cfg.collisions&fCollision::RVSmask) { case fCollision::SDF_RS: { btSoftColliders::CollideSDF_RS docollide; btRigidBody* prb1=btRigidBody::upcast(pco); - btTransform wtr=prb1 ? prb1->getInterpolationWorldTransform() : pco->getWorldTransform(); + btTransform wtr=pco->getWorldTransform(); const btTransform ctr=pco->getWorldTransform(); const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length(); @@ -2634,7 +2918,7 @@ void btSoftBody::defaultCollisionHandler(btCollisionObject* pco) btVector3 mins; btVector3 maxs; ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(), + pco->getCollisionShape()->getAabb( pco->getWorldTransform(), mins, maxs); volume=btDbvtVolume::FromMM(mins,maxs); @@ -2665,8 +2949,14 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) { case fCollision::CL_SS: { - btSoftColliders::CollideCL_SS docollide; - docollide.Process(this,psb); + + //support self-collision if CL_SELF flag set + if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF) + { + btSoftColliders::CollideCL_SS docollide; + docollide.Process(this,psb); + } + } break; case fCollision::VF_SS: @@ -2693,5 +2983,426 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) } } break; + default: + { + + } + } +} + + + +void btSoftBody::setWindVelocity( const btVector3 &velocity ) +{ + m_windVelocity = velocity; +} + + +const btVector3& btSoftBody::getWindVelocity() +{ + return m_windVelocity; +} + + + +int btSoftBody::calculateSerializeBufferSize() const +{ + int sz = sizeof(btSoftBodyData); + return sz; +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const +{ + btSoftBodyData* sbd = (btSoftBodyData*) dataBuffer; + + btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer); + + btHashMap<btHashPtr,int> m_nodeIndexMap; + + sbd->m_numMaterials = m_materials.size(); + sbd->m_materials = sbd->m_numMaterials? (SoftBodyMaterialData**) serializer->getUniquePointer((void*)&m_materials): 0; + + if (sbd->m_materials) + { + int sz = sizeof(SoftBodyMaterialData*); + int numElem = sbd->m_numMaterials; + btChunk* chunk = serializer->allocate(sz,numElem); + //SoftBodyMaterialData** memPtr = chunk->m_oldPtr; + SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + btSoftBody::Material* mat = m_materials[i]; + *memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0; + if (!serializer->findPointer(mat)) + { + //serialize it here + btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData),1); + SoftBodyMaterialData* memPtr = (SoftBodyMaterialData*)chunk->m_oldPtr; + memPtr->m_flags = mat->m_flags; + memPtr->m_angularStiffness = mat->m_kAST; + memPtr->m_linearStiffness = mat->m_kLST; + memPtr->m_volumeStiffness = mat->m_kVST; + serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_SBMATERIAL_CODE,mat); + } + } + serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_ARRAY_CODE,(void*) &m_materials); + } + + + + + sbd->m_numNodes = m_nodes.size(); + sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes): 0; + if (sbd->m_nodes) + { + int sz = sizeof(SoftBodyNodeData); + int numElem = sbd->m_numNodes; + btChunk* chunk = serializer->allocate(sz,numElem); + SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + m_nodes[i].m_f.serializeFloat( memPtr->m_accumulatedForce); + memPtr->m_area = m_nodes[i].m_area; + memPtr->m_attach = m_nodes[i].m_battach; + memPtr->m_inverseMass = m_nodes[i].m_im; + memPtr->m_material = m_nodes[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_nodes[i].m_material):0; + m_nodes[i].m_n.serializeFloat(memPtr->m_normal); + m_nodes[i].m_x.serializeFloat(memPtr->m_position); + m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition); + m_nodes[i].m_v.serializeFloat(memPtr->m_velocity); + m_nodeIndexMap.insert(&m_nodes[i],i); + } + serializer->finalizeChunk(chunk,"SoftBodyNodeData",BT_SBNODE_CODE,(void*) &m_nodes); + } + + sbd->m_numLinks = m_links.size(); + sbd->m_links = sbd->m_numLinks? (SoftBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0; + if (sbd->m_links) + { + int sz = sizeof(SoftBodyLinkData); + int numElem = sbd->m_numLinks; + btChunk* chunk = serializer->allocate(sz,numElem); + SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + memPtr->m_bbending = m_links[i].m_bbending; + memPtr->m_material = m_links[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_links[i].m_material):0; + memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0]: -1; + memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0]: -1; + btAssert(memPtr->m_nodeIndices[0]<m_nodes.size()); + btAssert(memPtr->m_nodeIndices[1]<m_nodes.size()); + memPtr->m_restLength = m_links[i].m_rl; + } + serializer->finalizeChunk(chunk,"SoftBodyLinkData",BT_ARRAY_CODE,(void*) &m_links[0]); + + } + + + sbd->m_numFaces = m_faces.size(); + sbd->m_faces = sbd->m_numFaces? (SoftBodyFaceData*) serializer->getUniquePointer((void*)&m_faces[0]):0; + if (sbd->m_faces) + { + int sz = sizeof(SoftBodyFaceData); + int numElem = sbd->m_numFaces; + btChunk* chunk = serializer->allocate(sz,numElem); + SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*) serializer->getUniquePointer((void*)m_faces[i].m_material): 0; + m_faces[i].m_normal.serializeFloat( memPtr->m_normal); + for (int j=0;j<3;j++) + { + memPtr->m_nodeIndices[j] = m_faces[i].m_n[j]? m_faces[i].m_n[j] - &m_nodes[0]: -1; + } + memPtr->m_restArea = m_faces[i].m_ra; + } + serializer->finalizeChunk(chunk,"SoftBodyFaceData",BT_ARRAY_CODE,(void*) &m_faces[0]); + } + + + sbd->m_numTetrahedra = m_tetras.size(); + sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*) serializer->getUniquePointer((void*)&m_tetras[0]):0; + if (sbd->m_tetrahedra) + { + int sz = sizeof(SoftBodyTetraData); + int numElem = sbd->m_numTetrahedra; + btChunk* chunk = serializer->allocate(sz,numElem); + SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + for (int j=0;j<4;j++) + { + m_tetras[i].m_c0[j].serializeFloat( memPtr->m_c0[j] ); + memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j]? m_tetras[j].m_n[j]-&m_nodes[0] : -1; + } + memPtr->m_c1 = m_tetras[i].m_c1; + memPtr->m_c2 = m_tetras[i].m_c2; + memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_tetras[i].m_material): 0; + memPtr->m_restVolume = m_tetras[i].m_rv; + } + serializer->finalizeChunk(chunk,"SoftBodyTetraData",BT_ARRAY_CODE,(void*) &m_tetras[0]); + } + + sbd->m_numAnchors = m_anchors.size(); + sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*) serializer->getUniquePointer((void*)&m_anchors[0]):0; + if (sbd->m_anchors) + { + int sz = sizeof(SoftRigidAnchorData); + int numElem = sbd->m_numAnchors; + btChunk* chunk = serializer->allocate(sz,numElem); + SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + m_anchors[i].m_c0.serializeFloat(memPtr->m_c0); + m_anchors[i].m_c1.serializeFloat(memPtr->m_c1); + memPtr->m_c2 = m_anchors[i].m_c2; + m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame); + memPtr->m_nodeIndex = m_anchors[i].m_node? m_anchors[i].m_node-&m_nodes[0]: -1; + + memPtr->m_rigidBody = m_anchors[i].m_body? (btRigidBodyData*) serializer->getUniquePointer((void*)m_anchors[i].m_body): 0; + btAssert(memPtr->m_nodeIndex < m_nodes.size()); + } + serializer->finalizeChunk(chunk,"SoftRigidAnchorData",BT_ARRAY_CODE,(void*) &m_anchors[0]); + } + + + sbd->m_config.m_dynamicFriction = m_cfg.kDF; + sbd->m_config.m_baumgarte = m_cfg.kVCF; + sbd->m_config.m_pressure = m_cfg.kPR; + sbd->m_config.m_aeroModel = this->m_cfg.aeromodel; + sbd->m_config.m_lift = m_cfg.kLF; + sbd->m_config.m_drag = m_cfg.kDG; + sbd->m_config.m_positionIterations = m_cfg.piterations; + sbd->m_config.m_driftIterations = m_cfg.diterations; + sbd->m_config.m_clusterIterations = m_cfg.citerations; + sbd->m_config.m_velocityIterations = m_cfg.viterations; + sbd->m_config.m_maxVolume = m_cfg.maxvolume; + sbd->m_config.m_damping = m_cfg.kDP; + sbd->m_config.m_poseMatch = m_cfg.kMT; + sbd->m_config.m_collisionFlags = m_cfg.collisions; + sbd->m_config.m_volume = m_cfg.kVC; + sbd->m_config.m_rigidContactHardness = m_cfg.kCHR; + sbd->m_config.m_kineticContactHardness = m_cfg.kKHR; + sbd->m_config.m_softContactHardness = m_cfg.kSHR; + sbd->m_config.m_anchorHardness = m_cfg.kAHR; + sbd->m_config.m_timeScale = m_cfg.timescale; + sbd->m_config.m_maxVolume = m_cfg.maxvolume; + sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL; + sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL; + sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL; + sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL; + sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL; + sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL; + + //pose for shape matching + { + sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose); + + int sz = sizeof(SoftBodyPoseData); + btChunk* chunk = serializer->allocate(sz,1); + SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr; + + m_pose.m_aqq.serializeFloat(memPtr->m_aqq); + memPtr->m_bframe = m_pose.m_bframe; + memPtr->m_bvolume = m_pose.m_bvolume; + m_pose.m_com.serializeFloat(memPtr->m_com); + + memPtr->m_numPositions = m_pose.m_pos.size(); + memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]): 0; + if (memPtr->m_numPositions) + { + int numElem = memPtr->m_numPositions; + int sz = sizeof(btVector3Data); + btChunk* chunk = serializer->allocate(sz,numElem); + btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + m_pose.m_pos[i].serializeFloat(*memPtr); + } + serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_pose.m_pos[0]); + } + memPtr->m_restVolume = m_pose.m_volume; + m_pose.m_rot.serializeFloat(memPtr->m_rot); + m_pose.m_scl.serializeFloat(memPtr->m_scale); + + memPtr->m_numWeigts = m_pose.m_wgh.size(); + memPtr->m_weights = memPtr->m_numWeigts? (float*) serializer->getUniquePointer((void*) &m_pose.m_wgh[0]) : 0; + if (memPtr->m_numWeigts) + { + + int numElem = memPtr->m_numWeigts; + int sz = sizeof(float); + btChunk* chunk = serializer->allocate(sz,numElem); + float* memPtr = (float*) chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + *memPtr = m_pose.m_wgh[i]; + } + serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_pose.m_wgh[0]); + } + + serializer->finalizeChunk(chunk,"SoftBodyPoseData",BT_ARRAY_CODE,(void*)&m_pose); + } + + //clusters for convex-cluster collision detection + + sbd->m_numClusters = m_clusters.size(); + sbd->m_clusters = sbd->m_numClusters? (SoftBodyClusterData*) serializer->getUniquePointer((void*)m_clusters[0]) : 0; + if (sbd->m_numClusters) + { + int numElem = sbd->m_numClusters; + int sz = sizeof(SoftBodyClusterData); + btChunk* chunk = serializer->allocate(sz,numElem); + SoftBodyClusterData* memPtr = (SoftBodyClusterData*) chunk->m_oldPtr; + for (int i=0;i<numElem;i++,memPtr++) + { + memPtr->m_adamping= m_clusters[i]->m_adamping; + m_clusters[i]->m_av.serializeFloat(memPtr->m_av); + memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex; + memPtr->m_collide = m_clusters[i]->m_collide; + m_clusters[i]->m_com.serializeFloat(memPtr->m_com); + memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor; + m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]); + m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]); + m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform); + memPtr->m_idmass = m_clusters[i]->m_idmass; + memPtr->m_imass = m_clusters[i]->m_imass; + m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi); + memPtr->m_ldamping = m_clusters[i]->m_ldamping; + m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii); + m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv); + memPtr->m_matching = m_clusters[i]->m_matching; + memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse; + memPtr->m_ndamping = m_clusters[i]->m_ndamping; + memPtr->m_ldamping = m_clusters[i]->m_ldamping; + memPtr->m_adamping = m_clusters[i]->m_adamping; + memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor; + + memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size(); + memPtr->m_numMasses = m_clusters[i]->m_masses.size(); + memPtr->m_numNodes = m_clusters[i]->m_nodes.size(); + + memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses; + m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]); + m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]); + memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses; + + + + memPtr->m_framerefs = memPtr->m_numFrameRefs? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0; + if (memPtr->m_framerefs) + { + int numElem = memPtr->m_numFrameRefs; + int sz = sizeof(btVector3FloatData); + btChunk* chunk = serializer->allocate(sz,numElem); + btVector3FloatData* memPtr = (btVector3FloatData*) chunk->m_oldPtr; + for (int j=0;j<numElem;j++,memPtr++) + { + m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr); + } + serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_framerefs[0]); + } + + memPtr->m_masses = memPtr->m_numMasses ? (float*) serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]): 0; + if (memPtr->m_masses) + { + int numElem = memPtr->m_numMasses; + int sz = sizeof(float); + btChunk* chunk = serializer->allocate(sz,numElem); + float* memPtr = (float*) chunk->m_oldPtr; + for (int j=0;j<numElem;j++,memPtr++) + { + *memPtr = m_clusters[i]->m_masses[j]; + } + serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_masses[0]); + } + + memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*) serializer->getUniquePointer((void*) &m_clusters[i]->m_nodes) : 0; + if (memPtr->m_nodeIndices ) + { + int numElem = memPtr->m_numMasses; + int sz = sizeof(int); + btChunk* chunk = serializer->allocate(sz,numElem); + int* memPtr = (int*) chunk->m_oldPtr; + for (int j=0;j<numElem;j++,memPtr++) + { + int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]); + btAssert(indexPtr); + *memPtr = *indexPtr; + } + serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_nodes); + } + } + serializer->finalizeChunk(chunk,"SoftBodyClusterData",BT_ARRAY_CODE,(void*)m_clusters[0]); + + } + + + + sbd->m_numJoints = m_joints.size(); + sbd->m_joints = m_joints.size()? (btSoftBodyJointData*) serializer->getUniquePointer((void*)&m_joints[0]) : 0; + + if (sbd->m_joints) + { + int sz = sizeof(btSoftBodyJointData); + int numElem = m_joints.size(); + btChunk* chunk = serializer->allocate(sz,numElem); + btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr; + + for (int i=0;i<numElem;i++,memPtr++) + { + memPtr->m_jointType = (int)m_joints[i]->Type(); + m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]); + m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]); + memPtr->m_cfm = m_joints[i]->m_cfm; + memPtr->m_erp = m_joints[i]->m_erp; + memPtr->m_split = m_joints[i]->m_split; + memPtr->m_delete = m_joints[i]->m_delete; + + for (int j=0;j<4;j++) + { + memPtr->m_relPosition[0].m_floats[j] = 0.f; + memPtr->m_relPosition[1].m_floats[j] = 0.f; + } + memPtr->m_bodyA = 0; + memPtr->m_bodyB = 0; + if (m_joints[i]->m_bodies[0].m_soft) + { + memPtr->m_bodyAtype = BT_JOINT_SOFT_BODY_CLUSTER; + memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft); + } + if (m_joints[i]->m_bodies[0].m_collisionObject) + { + memPtr->m_bodyAtype = BT_JOINT_COLLISION_OBJECT; + memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject); + } + if (m_joints[i]->m_bodies[0].m_rigid) + { + memPtr->m_bodyAtype = BT_JOINT_RIGID_BODY; + memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid); + } + + if (m_joints[i]->m_bodies[1].m_soft) + { + memPtr->m_bodyBtype = BT_JOINT_SOFT_BODY_CLUSTER; + memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft); + } + if (m_joints[i]->m_bodies[1].m_collisionObject) + { + memPtr->m_bodyBtype = BT_JOINT_COLLISION_OBJECT; + memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject); + } + if (m_joints[i]->m_bodies[1].m_rigid) + { + memPtr->m_bodyBtype = BT_JOINT_RIGID_BODY; + memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid); + } + } + serializer->finalizeChunk(chunk,"btSoftBodyJointData",BT_ARRAY_CODE,(void*) &m_joints[0]); } + + + return btSoftBodyDataName; } + diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.h b/extern/bullet2/src/BulletSoftBody/btSoftBody.h index a62c21883c8..87247c3c1c0 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.h @@ -27,8 +27,17 @@ subject to the following restrictions: #include "btSparseSDF.h" #include "BulletCollision/BroadphaseCollision/btDbvt.h" +//#ifdef BT_USE_DOUBLE_PRECISION +//#define btRigidBodyData btRigidBodyDoubleData +//#define btRigidBodyDataName "btRigidBodyDoubleData" +//#else +#define btSoftBodyData btSoftBodyFloatData +#define btSoftBodyDataName "btSoftBodyFloatData" +//#endif //BT_USE_DOUBLE_PRECISION + class btBroadphaseInterface; class btDispatcher; +class btSoftBodySolver; /* btSoftBodyWorldInfo */ struct btSoftBodyWorldInfo @@ -41,6 +50,17 @@ struct btSoftBodyWorldInfo btDispatcher* m_dispatcher; btVector3 m_gravity; btSparseSdf<3> m_sparsesdf; + + btSoftBodyWorldInfo() + :air_density((btScalar)1.2), + water_density(0), + water_offset(0), + water_normal(0,0,0), + m_broadphase(0), + m_dispatcher(0), + m_gravity(0,-10,0) + { + } }; @@ -51,6 +71,9 @@ class btSoftBody : public btCollisionObject public: btAlignedObjectArray<class btCollisionObject*> m_collisionDisabledObjects; + // The solver object that handles this soft body + btSoftBodySolver *m_softBodySolver; + // // Enumerations // @@ -110,9 +133,10 @@ public: SDF_RS = 0x0001, ///SDF based rigid vs soft CL_RS = 0x0002, ///Cluster vs convex rigid vs soft - SVSmask = 0x00f0, ///Rigid versus soft mask + SVSmask = 0x0030, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling + CL_SELF = 0x0040, ///Cluster soft body self collision /* presets */ Default = SDF_RS, END @@ -181,12 +205,14 @@ public: btScalar m_kAST; // Area/Angular stiffness coefficient [0,1] btScalar m_kVST; // Volume stiffness coefficient [0,1] int m_flags; // Flags + Material() : Element() {} }; /* Feature */ struct Feature : Element { Material* m_material; // Material + Feature() : Element() {} }; /* Node */ struct Node : Feature @@ -200,6 +226,7 @@ public: btScalar m_area; // Area btDbvtNode* m_leaf; // Leaf data int m_battach:1; // Attached + Node() : Feature() {} }; /* Link */ struct Link : Feature @@ -211,6 +238,7 @@ public: btScalar m_c1; // rl^2 btScalar m_c2; // |gradient|^2/c0 btVector3 m_c3; // gradient + Link() : Feature() {} }; /* Face */ struct Face : Feature @@ -219,6 +247,18 @@ public: btVector3 m_normal; // Normal btScalar m_ra; // Rest area btDbvtNode* m_leaf; // Leaf data + Face() : Feature() {} + }; + /* Tetra */ + struct Tetra : Feature + { + Node* m_n[4]; // Node pointers + btScalar m_rv; // Rest volume + btDbvtNode* m_leaf; // Leaf data + btVector3 m_c0[4]; // gradients + btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) + btScalar m_c2; // m_c1/sum(|g0..3|^2) + Tetra() : Feature() {} }; /* RContact */ struct RContact @@ -260,6 +300,7 @@ public: int m_rank; // Rank Node* m_nodes[4]; // Nodes btScalar m_coords[4]; // Coordinates + Note() : Element() {} }; /* Pose */ struct Pose @@ -276,9 +317,9 @@ public: }; /* Cluster */ struct Cluster - { - btAlignedObjectArray<Node*> m_nodes; + { tScalarArray m_masses; + btAlignedObjectArray<Node*> m_nodes; tVector3Array m_framerefs; btTransform m_framexform; btScalar m_idmass; @@ -297,9 +338,16 @@ public: btScalar m_ldamping; /* Linear damping */ btScalar m_adamping; /* Angular damping */ btScalar m_matching; + btScalar m_maxSelfCollisionImpulse; + btScalar m_selfCollisionImpulseFactor; + bool m_containsAnchor; bool m_collide; int m_clusterIndex; - Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {} + Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) + ,m_maxSelfCollisionImpulse(100.f), + m_selfCollisionImpulseFactor(0.01f), + m_containsAnchor(false) + {} }; /* Impulse */ struct Impulse @@ -340,7 +388,11 @@ public: void activate() const { - if(m_rigid) m_rigid->activate(); + if(m_rigid) + m_rigid->activate(); + if (m_collisionObject) + m_collisionObject->activate(); + } const btMatrix3x3& invWorldInertia() const { @@ -358,7 +410,7 @@ public: const btTransform& xform() const { static const btTransform identity=btTransform::getIdentity(); - if(m_collisionObject) return(m_collisionObject->getInterpolationWorldTransform()); + if(m_collisionObject) return(m_collisionObject->getWorldTransform()); if(m_soft) return(m_soft->m_framexform); return(identity); } @@ -370,8 +422,8 @@ public: } btVector3 angularVelocity(const btVector3& rpos) const { - if(m_rigid) return(cross(m_rigid->getAngularVelocity(),rpos)); - if(m_soft) return(cross(m_soft->m_av,rpos)); + if(m_rigid) return(btCross(m_rigid->getAngularVelocity(),rpos)); + if(m_soft) return(btCross(m_soft->m_av,rpos)); return(btVector3(0,0,0)); } btVector3 angularVelocity() const @@ -396,8 +448,16 @@ public: } void applyImpulse(const Impulse& impulse,const btVector3& rpos) const { - if(impulse.m_asVelocity) applyVImpulse(impulse.m_velocity,rpos); - if(impulse.m_asDrift) applyDImpulse(impulse.m_drift,rpos); + if(impulse.m_asVelocity) + { +// printf("impulse.m_velocity = %f,%f,%f\n",impulse.m_velocity.getX(),impulse.m_velocity.getY(),impulse.m_velocity.getZ()); + applyVImpulse(impulse.m_velocity,rpos); + } + if(impulse.m_asDrift) + { +// printf("impulse.m_drift = %f,%f,%f\n",impulse.m_drift.getX(),impulse.m_drift.getY(),impulse.m_drift.getZ()); + applyDImpulse(impulse.m_drift,rpos); + } } void applyVAImpulse(const btVector3& impulse) const { @@ -424,7 +484,7 @@ public: struct Joint { struct eType { enum _ { - Linear, + Linear=0, Angular, Contact };}; @@ -563,7 +623,7 @@ public: }; // - // Typedef's + // Typedefs // typedef void (*psolver_t)(btSoftBody*,btScalar,btScalar); @@ -574,6 +634,7 @@ public: typedef btAlignedObjectArray<btDbvtNode*> tLeafArray; typedef btAlignedObjectArray<Link> tLinkArray; typedef btAlignedObjectArray<Face> tFaceArray; + typedef btAlignedObjectArray<Tetra> tTetraArray; typedef btAlignedObjectArray<Anchor> tAnchorArray; typedef btAlignedObjectArray<RContact> tRContactArray; typedef btAlignedObjectArray<SContact> tSContactArray; @@ -594,6 +655,7 @@ public: tNodeArray m_nodes; // Nodes tLinkArray m_links; // Links tFaceArray m_faces; // Faces + tTetraArray m_tetras; // Tetras tAnchorArray m_anchors; // Anchors tRContactArray m_rcontacts; // Rigid contacts tSContactArray m_scontacts; // Soft contacts @@ -611,14 +673,19 @@ public: btTransform m_initialWorldTransform; + btVector3 m_windVelocity; // // Api // /* ctor */ - btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count, - const btVector3* x, - const btScalar* m); + btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m); + + /* ctor */ + btSoftBody( btSoftBodyWorldInfo* worldInfo); + + void initDefaults(); + /* dtor */ virtual ~btSoftBody(); /* Check for existing link */ @@ -681,9 +748,19 @@ public: int node1, int node2, Material* mat=0); + void appendTetra(int model,Material* mat); + // + void appendTetra(int node0, + int node1, + int node2, + int node3, + Material* mat=0); + + /* Append anchor */ void appendAnchor( int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies=false); + void appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies=false); /* Append linear joint */ void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1); void appendLinearJoint(const LJoint::Specs& specs,Body body=Body()); @@ -718,6 +795,10 @@ public: bool fromfaces=false); /* Set total density */ void setTotalDensity(btScalar density); + /* Set volume mass (using tetrahedrons) */ + void setVolumeMass( btScalar mass); + /* Set volume density (using tetrahedrons) */ + void setVolumeDensity( btScalar density); /* Transform */ void transform( const btTransform& trs); /* Translate */ @@ -755,6 +836,8 @@ public: void releaseCluster(int index); void releaseClusters(); /* Generate clusters (K-mean) */ + ///generateClusters with k=0 will create a convex cluster for each tetrahedron or triangle + ///otherwise an approximation will be used (better performance) int generateClusters(int k,int maxiterations=8192); /* Refine */ void refine(ImplicitFn* ifn,btScalar accurary,bool cut); @@ -784,6 +867,49 @@ public: void defaultCollisionHandler(btCollisionObject* pco); void defaultCollisionHandler(btSoftBody* psb); + + + // + // Functionality to deal with new accelerated solvers. + // + + /** + * Set a wind velocity for interaction with the air. + */ + void setWindVelocity( const btVector3 &velocity ); + + + /** + * Return the wind velocity for interaction with the air. + */ + const btVector3& getWindVelocity(); + + // + // Set the solver that handles this soft body + // Should not be allowed to get out of sync with reality + // Currently called internally on addition to the world + void setSoftBodySolver( btSoftBodySolver *softBodySolver ) + { + m_softBodySolver = softBodySolver; + } + + // + // Return the solver that handles this soft body + // + btSoftBodySolver *getSoftBodySolver() + { + return m_softBodySolver; + } + + // + // Return the solver that handles this soft body + // + btSoftBodySolver *getSoftBodySolver() const + { + return m_softBodySolver; + } + + // // Cast // @@ -841,8 +967,18 @@ public: static psolver_t getSolver(ePSolver::_ solver); static vsolver_t getSolver(eVSolver::_ solver); + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; + + //virtual void serializeSingleObject(class btSerializer* serializer) const; + + }; + #endif //_BT_SOFT_BODY_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp index f334e15e0d3..04ee7ea77cf 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @@ -95,9 +95,9 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, ci.m_dispatcher1 = m_dispatcher; ///debug drawing of the overlapping triangles - if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) + if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe) { - btVector3 color(255,255,0); + btVector3 color(1,1,0); btTransform& tr = ob->getWorldTransform(); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); @@ -168,8 +168,7 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, ///this should use the btDispatcher, so the actual registered algorithm is used // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); - //m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex); - // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); + //m_resultOut->setShapeIdentifiersB(partId,triangleIndex); // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); @@ -219,7 +218,7 @@ void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* b { - btCollisionObject* convexBody = m_isSwapped ? body1 : body0; + //btCollisionObject* convexBody = m_isSwapped ? body1 : body0; btCollisionObject* triBody = m_isSwapped ? body0 : body1; if (triBody->getCollisionShape()->isConcave()) diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyData.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyData.h new file mode 100644 index 00000000000..40dc65c3d8c --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyData.h @@ -0,0 +1,217 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOFTBODY_FLOAT_DATA +#define BT_SOFTBODY_FLOAT_DATA + +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + + + +struct SoftBodyMaterialData +{ + float m_linearStiffness; + float m_angularStiffness; + float m_volumeStiffness; + int m_flags; +}; + +struct SoftBodyNodeData +{ + SoftBodyMaterialData *m_material; + btVector3FloatData m_position; + btVector3FloatData m_previousPosition; + btVector3FloatData m_velocity; + btVector3FloatData m_accumulatedForce; + btVector3FloatData m_normal; + float m_inverseMass; + float m_area; + int m_attach; + int m_pad; +}; + +struct SoftBodyLinkData +{ + SoftBodyMaterialData *m_material; + int m_nodeIndices[2]; // Node pointers + float m_restLength; // Rest length + int m_bbending; // Bending link +}; + +struct SoftBodyFaceData +{ + btVector3FloatData m_normal; // Normal + SoftBodyMaterialData *m_material; + int m_nodeIndices[3]; // Node pointers + float m_restArea; // Rest area +}; + +struct SoftBodyTetraData +{ + btVector3FloatData m_c0[4]; // gradients + SoftBodyMaterialData *m_material; + int m_nodeIndices[4]; // Node pointers + float m_restVolume; // Rest volume + float m_c1; // (4*kVST)/(im0+im1+im2+im3) + float m_c2; // m_c1/sum(|g0..3|^2) + int m_pad; +}; + +struct SoftRigidAnchorData +{ + btMatrix3x3FloatData m_c0; // Impulse matrix + btVector3FloatData m_c1; // Relative anchor + btVector3FloatData m_localFrame; // Anchor position in body space + btRigidBodyData *m_rigidBody; + int m_nodeIndex; // Node pointer + float m_c2; // ima*dt +}; + + + +struct SoftBodyConfigData +{ + int m_aeroModel; // Aerodynamic model (default: V_Point) + float m_baumgarte; // Velocities correction factor (Baumgarte) + float m_damping; // Damping coefficient [0,1] + float m_drag; // Drag coefficient [0,+inf] + float m_lift; // Lift coefficient [0,+inf] + float m_pressure; // Pressure coefficient [-inf,+inf] + float m_volume; // Volume conversation coefficient [0,+inf] + float m_dynamicFriction; // Dynamic friction coefficient [0,1] + float m_poseMatch; // Pose matching coefficient [0,1] + float m_rigidContactHardness; // Rigid contacts hardness [0,1] + float m_kineticContactHardness; // Kinetic contacts hardness [0,1] + float m_softContactHardness; // Soft contacts hardness [0,1] + float m_anchorHardness; // Anchors hardness [0,1] + float m_softRigidClusterHardness; // Soft vs rigid hardness [0,1] (cluster only) + float m_softKineticClusterHardness; // Soft vs kinetic hardness [0,1] (cluster only) + float m_softSoftClusterHardness; // Soft vs soft hardness [0,1] (cluster only) + float m_softRigidClusterImpulseSplit; // Soft vs rigid impulse split [0,1] (cluster only) + float m_softKineticClusterImpulseSplit; // Soft vs rigid impulse split [0,1] (cluster only) + float m_softSoftClusterImpulseSplit; // Soft vs rigid impulse split [0,1] (cluster only) + float m_maxVolume; // Maximum volume ratio for pose + float m_timeScale; // Time scale + int m_velocityIterations; // Velocities solver iterations + int m_positionIterations; // Positions solver iterations + int m_driftIterations; // Drift solver iterations + int m_clusterIterations; // Cluster solver iterations + int m_collisionFlags; // Collisions flags +}; + +struct SoftBodyPoseData +{ + btMatrix3x3FloatData m_rot; // Rotation + btMatrix3x3FloatData m_scale; // Scale + btMatrix3x3FloatData m_aqq; // Base scaling + btVector3FloatData m_com; // COM + + btVector3FloatData *m_positions; // Reference positions + float *m_weights; // Weights + int m_numPositions; + int m_numWeigts; + + int m_bvolume; // Is valid + int m_bframe; // Is frame + float m_restVolume; // Rest volume + int m_pad; +}; + +struct SoftBodyClusterData +{ + btTransformFloatData m_framexform; + btMatrix3x3FloatData m_locii; + btMatrix3x3FloatData m_invwi; + btVector3FloatData m_com; + btVector3FloatData m_vimpulses[2]; + btVector3FloatData m_dimpulses[2]; + btVector3FloatData m_lv; + btVector3FloatData m_av; + + btVector3FloatData *m_framerefs; + int *m_nodeIndices; + float *m_masses; + + int m_numFrameRefs; + int m_numNodes; + int m_numMasses; + + float m_idmass; + float m_imass; + int m_nvimpulses; + int m_ndimpulses; + float m_ndamping; + float m_ldamping; + float m_adamping; + float m_matching; + float m_maxSelfCollisionImpulse; + float m_selfCollisionImpulseFactor; + int m_containsAnchor; + int m_collide; + int m_clusterIndex; +}; + + +enum btSoftJointBodyType +{ + BT_JOINT_SOFT_BODY_CLUSTER=1, + BT_JOINT_RIGID_BODY, + BT_JOINT_COLLISION_OBJECT +}; + +struct btSoftBodyJointData +{ + void *m_bodyA; + void *m_bodyB; + btVector3FloatData m_refs[2]; + float m_cfm; + float m_erp; + float m_split; + int m_delete; + btVector3FloatData m_relPosition[2];//linear + int m_bodyAtype; + int m_bodyBtype; + int m_jointType; + int m_pad; +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btSoftBodyFloatData +{ + btCollisionObjectFloatData m_collisionObjectData; + + SoftBodyPoseData *m_pose; + SoftBodyMaterialData **m_materials; + SoftBodyNodeData *m_nodes; + SoftBodyLinkData *m_links; + SoftBodyFaceData *m_faces; + SoftBodyTetraData *m_tetrahedra; + SoftRigidAnchorData *m_anchors; + SoftBodyClusterData *m_clusters; + btSoftBodyJointData *m_joints; + + int m_numMaterials; + int m_numNodes; + int m_numLinks; + int m_numFaces; + int m_numTetrahedra; + int m_numAnchors; + int m_numClusters; + int m_numJoints; + SoftBodyConfigData m_config; +}; + +#endif //BT_SOFTBODY_FLOAT_DATA + diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp index 61aac5b4ce5..1a271066497 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -130,7 +130,7 @@ static inline btScalar tetravolume(const btVector3& x0, const btVector3 a=x1-x0; const btVector3 b=x2-x0; const btVector3 c=x3-x0; - return(dot(a,cross(b,c))); + return(btDot(a,btCross(b,c))); } // @@ -165,98 +165,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, const btVector3 ccolor=btVector3(1,0,0); int i,j,nj; - /* Nodes */ - if(0!=(drawflags&fDrawFlags::Nodes)) - { - for(i=0;i<psb->m_nodes.size();++i) - { - const btSoftBody::Node& n=psb->m_nodes[i]; - if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - 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(i=0;i<psb->m_links.size();++i) - { - const btSoftBody::Link& l=psb->m_links[i]; - if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor); - } - } - /* Normals */ - if(0!=(drawflags&fDrawFlags::Normals)) - { - for(i=0;i<psb->m_nodes.size();++i) - { - const btSoftBody::Node& n=psb->m_nodes[i]; - if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - 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(i=0;i<psb->m_rcontacts.size();++i) - { - const btSoftBody::RContact& c=psb->m_rcontacts[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(i=0;i<psb->m_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(i=0;i<psb->m_nodes.size();++i) - { - const btSoftBody::Node& n=psb->m_nodes[i]; - if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - 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.8; - const btScalar alp=(btScalar)1; - const btVector3 col(0,(btScalar)0.7,0); - for(i=0;i<psb->m_faces.size();++i) - { - const btSoftBody::Face& f=psb->m_faces[i]; - if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - 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, - col,alp); - } - } - /* Clusters */ + /* Clusters */ if(0!=(drawflags&fDrawFlags::Clusters)) { srand(1806); @@ -299,17 +208,131 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, { const btSoftBody::Cluster& c=psb->m_clusters[i]; const btVector3 r=c.m_nodes[j]->m_x-c.m_com; - const btVector3 v=c.m_lv+cross(c.m_av,r); + const btVector3 v=c.m_lv+btCross(c.m_av,r); idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0)); } #endif /* Frame */ - btSoftBody::Cluster& c=*psb->m_clusters[i]; - idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); - idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); - idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); + // btSoftBody::Cluster& c=*psb->m_clusters[i]; + // idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); + // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); + // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); } } + else + { + /* Nodes */ + if(0!=(drawflags&fDrawFlags::Nodes)) + { + for(i=0;i<psb->m_nodes.size();++i) + { + const btSoftBody::Node& n=psb->m_nodes[i]; + if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; + 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(i=0;i<psb->m_links.size();++i) + { + const btSoftBody::Link& l=psb->m_links[i]; + if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; + idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor); + } + } + /* Normals */ + if(0!=(drawflags&fDrawFlags::Normals)) + { + for(i=0;i<psb->m_nodes.size();++i) + { + const btSoftBody::Node& n=psb->m_nodes[i]; + if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; + 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(i=0;i<psb->m_rcontacts.size();++i) + { + const btSoftBody::RContact& c=psb->m_rcontacts[i]; + const btVector3 o= c.m_node->m_x-c.m_cti.m_normal* + (btDot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset); + const btVector3 x=btCross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized(); + const btVector3 y=btCross(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)); + } + } + /* Faces */ + if(0!=(drawflags&fDrawFlags::Faces)) + { + const btScalar scl=(btScalar)0.8; + const btScalar alp=(btScalar)1; + const btVector3 col(0,(btScalar)0.7,0); + for(i=0;i<psb->m_faces.size();++i) + { + const btSoftBody::Face& f=psb->m_faces[i]; + if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; + 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, + col,alp); + } + } + /* Tetras */ + if(0!=(drawflags&fDrawFlags::Tetras)) + { + const btScalar scl=(btScalar)0.8; + const btScalar alp=(btScalar)1; + const btVector3 col((btScalar)0.7,(btScalar)0.7,(btScalar)0.7); + for(int i=0;i<psb->m_tetras.size();++i) + { + const btSoftBody::Tetra& t=psb->m_tetras[i]; + if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; + const btVector3 x[]={t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x}; + const btVector3 c=(x[0]+x[1]+x[2]+x[3])/4; + idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp); + idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp); + idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp); + idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp); + } + } + } + /* Anchors */ + if(0!=(drawflags&fDrawFlags::Anchors)) + { + for(i=0;i<psb->m_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(i=0;i<psb->m_nodes.size();++i) + { + const btSoftBody::Node& n=psb->m_nodes[i]; + if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; + if(n.m_im<=0) + { + drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0)); + } + } + } + + /* Notes */ if(0!=(drawflags&fDrawFlags::Notes)) { @@ -351,7 +374,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, break; case btSoftBody::Joint::eType::Angular: { - const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; + //const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; @@ -360,7 +383,12 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); + break; + } + default: + { } + } } } @@ -828,10 +856,12 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo #undef IDX psb->appendFace(idx[0],idx[1],idx[2]); } + if (randomizeConstraints) { psb->randomizeConstraints(); } + return(psb); } @@ -863,3 +893,130 @@ btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldI } return(psb); } + + + + +static int nextLine(const char* buffer) +{ + int numBytesRead=0; + + while (*buffer != '\n') + { + buffer++; + numBytesRead++; + } + + + if (buffer[0]==0x0a) + { + buffer++; + numBytesRead++; + } + return numBytesRead; +} + +/* Create from TetGen .ele, .face, .node data */ +btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo, + const char* ele, + const char* face, + const char* node, + bool bfacelinks, + bool btetralinks, + bool bfacesfromtetras) +{ +btAlignedObjectArray<btVector3> pos; +int nnode=0; +int ndims=0; +int nattrb=0; +int hasbounds=0; +int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds); +result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds); +node += nextLine(node); + +pos.resize(nnode); +for(int i=0;i<pos.size();++i) + { + int index=0; + //int bound=0; + float x,y,z; + sscanf(node,"%d %f %f %f",&index,&x,&y,&z); + +// sn>>index; +// sn>>x;sn>>y;sn>>z; + node += nextLine(node); + + //for(int j=0;j<nattrb;++j) + // sn>>a; + + //if(hasbounds) + // sn>>bound; + + pos[index].setX(btScalar(x)); + pos[index].setY(btScalar(y)); + pos[index].setZ(btScalar(z)); + } +btSoftBody* psb=new btSoftBody(&worldInfo,nnode,&pos[0],0); +#if 0 +if(face&&face[0]) + { + int nface=0; + sf>>nface;sf>>hasbounds; + for(int i=0;i<nface;++i) + { + int index=0; + int bound=0; + int ni[3]; + sf>>index; + sf>>ni[0];sf>>ni[1];sf>>ni[2]; + sf>>bound; + psb->appendFace(ni[0],ni[1],ni[2]); + if(btetralinks) + { + psb->appendLink(ni[0],ni[1],0,true); + psb->appendLink(ni[1],ni[2],0,true); + psb->appendLink(ni[2],ni[0],0,true); + } + } + } +#endif + +if(ele&&ele[0]) + { + int ntetra=0; + int ncorner=0; + int neattrb=0; + sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb); + ele += nextLine(ele); + + //se>>ntetra;se>>ncorner;se>>neattrb; + for(int i=0;i<ntetra;++i) + { + int index=0; + int ni[4]; + + //se>>index; + //se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3]; + sscanf(ele,"%d %d %d %d %d",&index,&ni[0],&ni[1],&ni[2],&ni[3]); + ele+=nextLine(ele); + //for(int j=0;j<neattrb;++j) + // se>>a; + psb->appendTetra(ni[0],ni[1],ni[2],ni[3]); + if(btetralinks) + { + psb->appendLink(ni[0],ni[1],0,true); + psb->appendLink(ni[1],ni[2],0,true); + psb->appendLink(ni[2],ni[0],0,true); + psb->appendLink(ni[0],ni[3],0,true); + psb->appendLink(ni[1],ni[3],0,true); + psb->appendLink(ni[2],ni[3],0,true); + } + } + } +printf("Nodes: %u\r\n",psb->m_nodes.size()); +printf("Links: %u\r\n",psb->m_links.size()); +printf("Faces: %u\r\n",psb->m_faces.size()); +printf("Tetras: %u\r\n",psb->m_tetras.size()); +return(psb); +} + diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h index 5eb2ebc0735..49b1f0bbce8 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h @@ -110,12 +110,34 @@ struct btSoftBodyHelpers const btScalar* vertices, const int* triangles, int ntriangles, - bool randomizeConstraints = true); + bool randomizeConstraints = true); /* Create from convex-hull */ static btSoftBody* CreateFromConvexHull( btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, int nvertices, - bool randomizeConstraints = true); + bool randomizeConstraints = true); + + + /* Export TetGen compatible .smesh file */ +// static void ExportAsSMeshFile( btSoftBody* psb, +// const char* filename); + /* Create from TetGen .ele, .face, .node files */ +// static btSoftBody* CreateFromTetGenFile( btSoftBodyWorldInfo& worldInfo, +// const char* ele, +// const char* face, +// const char* node, +// bool bfacelinks, +// bool btetralinks, +// bool bfacesfromtetras); + /* Create from TetGen .ele, .face, .node data */ + static btSoftBody* CreateFromTetGenData( btSoftBodyWorldInfo& worldInfo, + const char* ele, + const char* face, + const char* node, + bool bfacelinks, + bool btetralinks, + bool bfacesfromtetras); + }; #endif //SOFT_BODY_HELPERS_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h index 5f0f7d54318..885571069d0 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h @@ -19,12 +19,13 @@ subject to the following restrictions: #include "btSoftBody.h" + #include "LinearMath/btQuickprof.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionShapes/btConvexInternalShape.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" - +#include <string.h> //for memset // // btSymMatrix // @@ -124,11 +125,11 @@ public: virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { btSoftBody::Node* const * n=&m_cluster->m_nodes[0]; - btScalar d=dot(vec,n[0]->m_x); + btScalar d=btDot(vec,n[0]->m_x); int j=0; for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i) { - const btScalar k=dot(vec,n[i]->m_x); + const btScalar k=btDot(vec,n[i]->m_x); if(k>d) { d=k;j=i; } } return(n[j]->m_x); @@ -171,8 +172,7 @@ public: template <typename T> static inline void ZeroInitialize(T& value) { - static const T zerodummy; - value=zerodummy; + memset(&value,0,sizeof(T)); } // template <typename T> @@ -296,9 +296,9 @@ static inline btMatrix3x3 Mul(const btMatrix3x3& a, // static inline void Orthogonalize(btMatrix3x3& m) { - m[2]=cross(m[0],m[1]).normalized(); - m[1]=cross(m[2],m[0]).normalized(); - m[0]=cross(m[1],m[2]).normalized(); + m[2]=btCross(m[0],m[1]).normalized(); + m[1]=btCross(m[2],m[0]).normalized(); + m[0]=btCross(m[1],m[2]).normalized(); } // static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) @@ -335,7 +335,7 @@ static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, static inline btVector3 ProjectOnAxis( const btVector3& v, const btVector3& a) { - return(a*dot(v,a)); + return(a*btDot(v,a)); } // static inline btVector3 ProjectOnPlane( const btVector3& v, @@ -354,7 +354,7 @@ static inline void ProjectOrigin( const btVector3& a, const btScalar m2=d.length2(); if(m2>SIMD_EPSILON) { - const btScalar t=Clamp<btScalar>(-dot(a,d)/m2,0,1); + const btScalar t=Clamp<btScalar>(-btDot(a,d)/m2,0,1); const btVector3 p=a+d*t; const btScalar l2=p.length2(); if(l2<sqd) @@ -371,19 +371,19 @@ static inline void ProjectOrigin( const btVector3& a, btVector3& prj, btScalar& sqd) { - const btVector3& q=cross(b-a,c-a); + const btVector3& q=btCross(b-a,c-a); const btScalar m2=q.length2(); if(m2>SIMD_EPSILON) { const btVector3 n=q/btSqrt(m2); - const btScalar k=dot(a,n); + const btScalar k=btDot(a,n); const btScalar k2=k*k; if(k2<sqd) { const btVector3 p=n*k; - if( (dot(cross(a-p,b-p),q)>0)&& - (dot(cross(b-p,c-p),q)>0)&& - (dot(cross(c-p,a-p),q)>0)) + if( (btDot(btCross(a-p,b-p),q)>0)&& + (btDot(btCross(b-p,c-p),q)>0)&& + (btDot(btCross(c-p,a-p),q)>0)) { prj=p; sqd=k2; @@ -413,9 +413,9 @@ static inline btVector3 BaryCoord( const btVector3& a, const btVector3& c, const btVector3& p) { - const btScalar w[]={ cross(a-p,b-p).length(), - cross(b-p,c-p).length(), - cross(c-p,a-p).length()}; + const btScalar w[]={ btCross(a-p,b-p).length(), + btCross(b-p,c-p).length(), + btCross(c-p,a-p).length()}; const btScalar isum=1/(w[0]+w[1]+w[2]); return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); } @@ -485,7 +485,7 @@ static inline btScalar AreaOf( const btVector3& x0, { const btVector3 a=x1-x0; const btVector3 b=x2-x0; - const btVector3 cr=cross(a,b); + const btVector3 cr=btCross(a,b); const btScalar area=cr.length(); return(area); } @@ -499,7 +499,7 @@ static inline btScalar VolumeOf( const btVector3& x0, const btVector3 a=x1-x0; const btVector3 b=x2-x0; const btVector3 c=x3-x0; - return(dot(a,cross(b,c))); + return(btDot(a,btCross(b,c))); } // @@ -512,7 +512,7 @@ static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, medium.m_density = wfi->air_density; if(wfi->water_density>0) { - const btScalar depth=-(dot(x,wfi->water_normal)+wfi->water_offset); + const btScalar depth=-(btDot(x,wfi->water_normal)+wfi->water_offset); if(depth>0) { medium.m_density = wfi->water_density; @@ -654,14 +654,14 @@ struct btSoftColliders { btScalar erp; btScalar idt; - btScalar margin; + btScalar m_margin; btScalar friction; btScalar threshold; ClusterBase() { erp =(btScalar)1; idt =0; - margin =0; + m_margin =0; friction =0; threshold =(btScalar)0; } @@ -669,16 +669,21 @@ struct btSoftColliders btSoftBody::Body ba,btSoftBody::Body bb, btSoftBody::CJoint& joint) { - if(res.distance<margin) + if(res.distance<m_margin) { + btVector3 norm = res.normal; + norm.normalize();//is it necessary? + const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin(); const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin(); const btVector3 va=ba.velocity(ra); const btVector3 vb=bb.velocity(rb); const btVector3 vrel=va-vb; - const btScalar rvac=dot(vrel,res.normal); - const btScalar depth=res.distance-margin; - const btVector3 iv=res.normal*rvac; + const btScalar rvac=btDot(vrel,norm); + btScalar depth=res.distance-m_margin; + +// printf("depth=%f\n",depth); + const btVector3 iv=norm*rvac; const btVector3 fv=vrel-iv; joint.m_bodies[0] = ba; joint.m_bodies[1] = bb; @@ -691,12 +696,16 @@ struct btSoftColliders joint.m_life = 0; joint.m_maxlife = 0; joint.m_split = 1; - joint.m_drift = depth*res.normal; - joint.m_normal = res.normal; + + joint.m_drift = depth*norm; + + joint.m_normal = norm; +// printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ()); joint.m_delete = false; joint.m_friction = fv.length2()<(-rvac*friction)?1:friction; joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0], bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]); + return(true); } return(false); @@ -714,10 +723,16 @@ struct btSoftColliders { btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data; btSoftClusterCollisionShape cshape(cluster); + const btConvexShape* rshape=(const btConvexShape*)m_colObj->getCollisionShape(); + + ///don't collide an anchored cluster with a static/kinematic object + if(m_colObj->isStaticOrKinematicObject() && cluster->m_containsAnchor) + return; + btGjkEpaSolver2::sResults res; if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), - rshape,m_colObj->getInterpolationWorldTransform(), + rshape,m_colObj->getWorldTransform(), btVector3(1,0,0),res)) { btSoftBody::CJoint joint; @@ -743,16 +758,16 @@ struct btSoftColliders psb = ps; m_colObj = colOb; idt = ps->m_sst.isdt; - margin = m_colObj->getCollisionShape()->getMargin(); + m_margin = m_colObj->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin(); ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful. friction = btMin(psb->m_cfg.kDF,m_colObj->getFriction()); btVector3 mins; btVector3 maxs; ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - colOb->getCollisionShape()->getAabb(colOb->getInterpolationWorldTransform(),mins,maxs); + colOb->getCollisionShape()->getAabb(colOb->getWorldTransform(),mins,maxs); volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(1,1,1)*margin); + volume.Expand(btVector3(1,1,1)*m_margin); ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this); } }; @@ -803,7 +818,8 @@ struct btSoftColliders void Process(btSoftBody* psa,btSoftBody* psb) { idt = psa->m_sst.isdt; - margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; + //m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; + m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin()); friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); bodies[0] = psa; bodies[1] = psb; @@ -832,14 +848,14 @@ struct btSoftColliders const btScalar ms=ima+imb; if(ms>0) { - const btTransform& wtr=m_rigidBody?m_rigidBody->getInterpolationWorldTransform() : m_colObj1->getWorldTransform(); + const btTransform& wtr=m_rigidBody?m_rigidBody->getWorldTransform() : m_colObj1->getWorldTransform(); static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0); const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; const btVector3 ra=n.m_x-wtr.getOrigin(); const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0); 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 btScalar dn=btDot(vr,c.m_cti.m_normal); const btVector3 fv=vr-c.m_cti.m_normal*dn; const btScalar fc=psb->m_cfg.kDF*m_colObj1->getFriction(); c.m_node = &n; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h b/extern/bullet2/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h new file mode 100644 index 00000000000..c4733d64000 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h @@ -0,0 +1,165 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H +#define BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H + + +class btVertexBufferDescriptor +{ +public: + enum BufferTypes + { + CPU_BUFFER, + DX11_BUFFER, + OPENGL_BUFFER + }; + +protected: + + bool m_hasVertexPositions; + bool m_hasNormals; + + int m_vertexOffset; + int m_vertexStride; + + int m_normalOffset; + int m_normalStride; + +public: + btVertexBufferDescriptor() + { + m_hasVertexPositions = false; + m_hasNormals = false; + m_vertexOffset = 0; + m_vertexStride = 0; + m_normalOffset = 0; + m_normalStride = 0; + } + + virtual ~btVertexBufferDescriptor() + { + + } + + virtual bool hasVertexPositions() const + { + return m_hasVertexPositions; + } + + virtual bool hasNormals() const + { + return m_hasNormals; + } + + /** + * Return the type of the vertex buffer descriptor. + */ + virtual BufferTypes getBufferType() const = 0; + + /** + * Return the vertex offset in floats from the base pointer. + */ + virtual int getVertexOffset() const + { + return m_vertexOffset; + } + + /** + * Return the vertex stride in number of floats between vertices. + */ + virtual int getVertexStride() const + { + return m_vertexStride; + } + + /** + * Return the vertex offset in floats from the base pointer. + */ + virtual int getNormalOffset() const + { + return m_normalOffset; + } + + /** + * Return the vertex stride in number of floats between vertices. + */ + virtual int getNormalStride() const + { + return m_normalStride; + } +}; + + +class btCPUVertexBufferDescriptor : public btVertexBufferDescriptor +{ +protected: + float *m_basePointer; + +public: + /** + * vertexBasePointer is pointer to beginning of the buffer. + * vertexOffset is the offset in floats to the first vertex. + * vertexStride is the stride in floats between vertices. + */ + btCPUVertexBufferDescriptor( float *basePointer, int vertexOffset, int vertexStride ) + { + m_basePointer = basePointer; + m_vertexOffset = vertexOffset; + m_vertexStride = vertexStride; + m_hasVertexPositions = true; + } + + /** + * vertexBasePointer is pointer to beginning of the buffer. + * vertexOffset is the offset in floats to the first vertex. + * vertexStride is the stride in floats between vertices. + */ + btCPUVertexBufferDescriptor( float *basePointer, int vertexOffset, int vertexStride, int normalOffset, int normalStride ) + { + m_basePointer = basePointer; + + m_vertexOffset = vertexOffset; + m_vertexStride = vertexStride; + m_hasVertexPositions = true; + + m_normalOffset = normalOffset; + m_normalStride = normalStride; + m_hasNormals = true; + } + + virtual ~btCPUVertexBufferDescriptor() + { + + } + + /** + * Return the type of the vertex buffer descriptor. + */ + virtual BufferTypes getBufferType() const + { + return CPU_BUFFER; + } + + /** + * Return the base pointer in memory to the first vertex. + */ + virtual float *getBasePointer() const + { + return m_basePointer; + } +}; + +#endif // #ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h b/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h new file mode 100644 index 00000000000..440084c5f7b --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h @@ -0,0 +1,154 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOFT_BODY_SOLVERS_H +#define BT_SOFT_BODY_SOLVERS_H + +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" + + +class btSoftBodyTriangleData; +class btSoftBodyLinkData; +class btSoftBodyVertexData; +class btVertexBufferDescriptor; +class btCollisionObject; +class btSoftBody; + + +class btSoftBodySolver +{ +public: + enum SolverTypes + { + DEFAULT_SOLVER, + CPU_SOLVER, + CL_SOLVER, + CL_SIMD_SOLVER, + DX_SOLVER, + DX_SIMD_SOLVER + }; + + +protected: + int m_numberOfPositionIterations; + int m_numberOfVelocityIterations; + // Simulation timescale + float m_timeScale; + +public: + btSoftBodySolver() : + m_numberOfPositionIterations( 10 ), + m_timeScale( 1 ) + { + m_numberOfVelocityIterations = 0; + m_numberOfPositionIterations = 5; + } + + virtual ~btSoftBodySolver() + { + } + + /** + * Return the type of the solver. + */ + virtual SolverTypes getSolverType() const = 0; + + + /** Ensure that this solver is initialized. */ + virtual bool checkInitialized() = 0; + + /** Optimize soft bodies in this solver. */ + virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies , bool forceUpdate=false) = 0; + + /** Copy necessary data back to the original soft body source objects. */ + virtual void copyBackToSoftBodies() = 0; + + /** Predict motion of soft bodies into next timestep */ + virtual void predictMotion( float solverdt ) = 0; + + /** Solve constraints for a set of soft bodies */ + virtual void solveConstraints( float solverdt ) = 0; + + /** Perform necessary per-step updates of soft bodies such as recomputing normals and bounding boxes */ + virtual void updateSoftBodies() = 0; + + /** Process a collision between one of the world's soft bodies and another collision object */ + virtual void processCollision( btSoftBody *, btCollisionObject* ) = 0; + + /** Process a collision between two soft bodies */ + virtual void processCollision( btSoftBody*, btSoftBody* ) = 0; + + /** Set the number of velocity constraint solver iterations this solver uses. */ + virtual void setNumberOfPositionIterations( int iterations ) + { + m_numberOfPositionIterations = iterations; + } + + /** Get the number of velocity constraint solver iterations this solver uses. */ + virtual int getNumberOfPositionIterations() + { + return m_numberOfPositionIterations; + } + + /** Set the number of velocity constraint solver iterations this solver uses. */ + virtual void setNumberOfVelocityIterations( int iterations ) + { + m_numberOfVelocityIterations = iterations; + } + + /** Get the number of velocity constraint solver iterations this solver uses. */ + virtual int getNumberOfVelocityIterations() + { + return m_numberOfVelocityIterations; + } + + /** Return the timescale that the simulation is using */ + float getTimeScale() + { + return m_timeScale; + } + +#if 0 + /** + * Add a collision object to be used by the indicated softbody. + */ + virtual void addCollisionObjectForSoftBody( int clothIdentifier, btCollisionObject *collisionObject ) = 0; +#endif +}; + +/** + * Class to manage movement of data from a solver to a given target. + * This version is abstract. Subclasses will have custom pairings for different combinations. + */ +class btSoftBodySolverOutput +{ +protected: + +public: + btSoftBodySolverOutput() + { + } + + virtual ~btSoftBodySolverOutput() + { + } + + + /** Output current computed vertex data to the vertex buffers for all cloths in the solver. */ + virtual void copySoftBodyToVertexBuffer( const btSoftBody * const softBody, btVertexBufferDescriptor *vertexBuffer ) = 0; +}; + + +#endif // #ifndef BT_SOFT_BODY_SOLVERS_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp index 11ad9e7daba..bc374c805e7 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp @@ -19,6 +19,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btBoxShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "btSoftBody.h" +#include "BulletSoftBody/btSoftBodySolvers.h" + ///TODO: include all the shapes that the softbody can collide with ///alternatively, implement special case collision algorithms (just like for rigid collision shapes) @@ -61,7 +63,7 @@ void btSoftRigidCollisionAlgorithm::processCollision (btCollisionObject* body0,b if (softBody->m_collisionDisabledObjects.findLinearSearch(rigidCollisionObject)==softBody->m_collisionDisabledObjects.size()) { - softBody->defaultCollisionHandler(rigidCollisionObject); + softBody->getSoftBodySolver()->processCollision(softBody, rigidCollisionObject); } diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index 982ec3e5eb3..1b9b5e392a1 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -20,15 +20,28 @@ subject to the following restrictions: //softbody & helpers #include "btSoftBody.h" #include "btSoftBodyHelpers.h" - - -//#define USE_BRUTEFORCE_RAYBROADPHASE 1 - - - -btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) -:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) +#include "btSoftBodySolvers.h" +#include "btDefaultSoftBodySolver.h" +#include "LinearMath/btSerializer.h" + + +btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld( + btDispatcher* dispatcher, + btBroadphaseInterface* pairCache, + btConstraintSolver* constraintSolver, + btCollisionConfiguration* collisionConfiguration, + btSoftBodySolver *softBodySolver ) : + btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), + m_softBodySolver( softBodySolver ), + m_ownsSolver(false) { + if( !m_softBodySolver ) + { + void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16); + m_softBodySolver = new(ptr) btDefaultSoftBodySolver(); + m_ownsSolver = true; + } + m_drawFlags = fDrawFlags::Std; m_drawNodeTree = true; m_drawFaceTree = false; @@ -38,31 +51,50 @@ btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBr m_sbi.m_sparsesdf.Initialize(); m_sbi.m_sparsesdf.Reset(); + m_sbi.air_density = (btScalar)1.2; + m_sbi.water_density = 0; + m_sbi.water_offset = 0; + m_sbi.water_normal = btVector3(0,0,0); + m_sbi.m_gravity.setValue(0,-10,0); + + m_sbi.m_sparsesdf.Initialize(); + + } btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() { - + if (m_ownsSolver) + { + m_softBodySolver->~btSoftBodySolver(); + btAlignedFree(m_softBodySolver); + } } void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep); - - for ( int i=0;i<m_softBodies.size();++i) + btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep ); { - btSoftBody* psb= m_softBodies[i]; - - psb->predictMotion(timeStep); + BT_PROFILE("predictUnconstraintMotionSoftBody"); + m_softBodySolver->predictMotion( timeStep ); } } -void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) +void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep ) { + + // Let the solver grab the soft bodies and if necessary optimize for it + m_softBodySolver->optimize( getSoftBodyArray() ); + + if( !m_softBodySolver->checkInitialized() ) + { + btAssert( "Solver initialization failed\n" ); + } + btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); ///solve soft bodies constraints - solveSoftBodiesConstraints(); + solveSoftBodiesConstraints( timeStep ); //self collisions for ( int i=0;i<m_softBodies.size();i++) @@ -72,22 +104,14 @@ void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) } ///update soft bodies - updateSoftBodies(); - -} - -void btSoftRigidDynamicsWorld::updateSoftBodies() -{ - BT_PROFILE("updateSoftBodies"); + m_softBodySolver->updateSoftBodies( ); + + // End solver-wise simulation step + // /////////////////////////////// - for ( int i=0;i<m_softBodies.size();i++) - { - btSoftBody* psb=(btSoftBody*)m_softBodies[i]; - psb->integrateMotion(); - } } -void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints() +void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep ) { BT_PROFILE("solveSoftConstraints"); @@ -96,20 +120,22 @@ void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints() btSoftBody::solveClusters(m_softBodies); } - for(int i=0;i<m_softBodies.size();++i) - { - btSoftBody* psb=(btSoftBody*)m_softBodies[i]; - psb->solveConstraints(); - } + // Solve constraints solver-wise + m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() ); + } -void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body) +void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask) { m_softBodies.push_back(body); + // Set the soft body solver that will deal with this body + // to be the world's solver + body->setSoftBodySolver( m_softBodySolver ); + btCollisionWorld::addCollisionObject(body, - btBroadphaseProxy::DefaultFilter, - btBroadphaseProxy::AllFilter); + collisionFilterGroup, + collisionFilterMask); } @@ -120,6 +146,15 @@ void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body) btCollisionWorld::removeCollisionObject(body); } +void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +{ + btSoftBody* body = btSoftBody::upcast(collisionObject); + if (body) + removeSoftBody(body); + else + btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); +} + void btSoftRigidDynamicsWorld::debugDrawWorld() { btDiscreteDynamicsWorld::debugDrawWorld(); @@ -130,8 +165,12 @@ void btSoftRigidDynamicsWorld::debugDrawWorld() for ( i=0;i<this->m_softBodies.size();i++) { btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; - btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); - btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags); + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)) + { + btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); + btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags); + } + if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { if(m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); @@ -143,6 +182,8 @@ void btSoftRigidDynamicsWorld::debugDrawWorld() } + + struct btSoftSingleRayCallback : public btBroadphaseRayCallback { btVector3 m_rayFromWorld; @@ -251,8 +292,10 @@ void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,con btSoftBody::sRayCast softResult; if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) { - if (softResult.fraction<= resultCallback.m_closestHitFraction) + + if (softResult.fraction<= resultCallback.m_closestHitFraction) { + btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = 0; shapeInfo.m_triangleIndex = softResult.index; @@ -260,14 +303,14 @@ void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,con btVector3 normal = softBody->m_faces[softResult.index].m_normal; btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); if (normal.dot(rayDir) > 0) { - // normal must always point toward origin of the ray + // normal always point toward origin of the ray normal = -normal; } btCollisionWorld::LocalRayResult rayResult (collisionObject, - &shapeInfo, - normal, - softResult.fraction); + &shapeInfo, + normal, + softResult.fraction); bool normalInWorldSpace = true; resultCallback.addSingleResult(rayResult,normalInWorldSpace); } @@ -278,3 +321,38 @@ void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,con btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback); } } + + +void btSoftRigidDynamicsWorld::serializeSoftBodies(btSerializer* serializer) +{ + int i; + //serialize all collision objects + for (i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + if (colObj->getInternalType() & btCollisionObject::CO_SOFT_BODY) + { + int len = colObj->calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(len,1); + const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj); + } + } + +} + +void btSoftRigidDynamicsWorld::serialize(btSerializer* serializer) +{ + + serializer->startSerialization(); + + serializeSoftBodies(serializer); + + serializeRigidBodies(serializer); + + serializeCollisionObjects(serializer); + + serializer->finishSerialization(); +} + + diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h index 14220ee7564..7d8d0cb7daa 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h @@ -21,6 +21,8 @@ subject to the following restrictions: typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; +class btSoftBodySolver; + class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld { @@ -30,6 +32,9 @@ class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld bool m_drawFaceTree; bool m_drawClusterTree; btSoftBodyWorldInfo m_sbi; + ///Solver classes that encapsulate multiple soft bodies for solving + btSoftBodySolver *m_softBodySolver; + bool m_ownsSolver; protected: @@ -37,23 +42,25 @@ protected: virtual void internalSingleStepSimulation( btScalar timeStep); - void updateSoftBodies(); - - void solveSoftBodiesConstraints(); + void solveSoftBodiesConstraints( btScalar timeStep ); + void serializeSoftBodies(btSerializer* serializer); public: - btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 ); virtual ~btSoftRigidDynamicsWorld(); virtual void debugDrawWorld(); - void addSoftBody(btSoftBody* body); + void addSoftBody(btSoftBody* body,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); void removeSoftBody(btSoftBody* body); + ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject + virtual void removeCollisionObject(btCollisionObject* collisionObject); + int getDrawFlags() const { return(m_drawFlags); } void setDrawFlags(int f) { m_drawFlags=f; } @@ -77,6 +84,7 @@ public: return m_softBodies; } + virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. @@ -88,6 +96,8 @@ public: const btTransform& colObjWorldTransform, RayResultCallback& resultCallback); + virtual void serialize(btSerializer* serializer); + }; #endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp index 85a727944e0..1b8cfa72371 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp @@ -17,6 +17,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionShapes/btBoxShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletSoftBody/btSoftBodySolvers.h" #include "btSoftBody.h" #define USE_PERSISTENT_CONTACTS 1 @@ -36,7 +37,7 @@ void btSoftSoftCollisionAlgorithm::processCollision (btCollisionObject* body0,bt { btSoftBody* soft0 = (btSoftBody*)body0; btSoftBody* soft1 = (btSoftBody*)body1; - soft0->defaultCollisionHandler(soft1); + soft0->getSoftBodySolver()->processCollision(soft0, soft1); } btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp index a3d790f8abc..189b759d237 100644 --- a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp @@ -160,22 +160,8 @@ void btAlignedFreeInternal (void* ptr,int line,char* filename) void* btAlignedAllocInternal (size_t size, int alignment) { gNumAlignedAllocs++; - void* ptr; -#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) + void* ptr; ptr = sAlignedAllocFunc(size, alignment); -#else - char *real; - unsigned long offset; - - real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1)); - if (real) { - offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1); - ptr = (void *)((real + sizeof(void *)) + offset); - *((void **)(ptr)-1) = (void *)(real); - } else { - ptr = (void *)(real); - } -#endif // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) // printf("btAlignedAllocInternal %d, %x\n",size,ptr); return ptr; } @@ -189,16 +175,7 @@ void btAlignedFreeInternal (void* ptr) gNumAlignedFree++; // printf("btAlignedFreeInternal %x\n",ptr); -#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) sAlignedFreeFunc(ptr); -#else - void* real; - - if (ptr) { - real = *((void **)(ptr)-1); - sFreeFunc(real); - } -#endif // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) } #endif //BT_DEBUG_MEMORY_ALLOCATIONS diff --git a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h index bad1eee1f63..955bb128e83 100644 --- a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h +++ b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h @@ -138,6 +138,16 @@ class btAlignedObjectArray return m_size; } + SIMD_FORCE_INLINE const T& at(int n) const + { + return m_data[n]; + } + + SIMD_FORCE_INLINE T& at(int n) + { + return m_data[n]; + } + SIMD_FORCE_INLINE const T& operator[](int n) const { return m_data[n]; @@ -171,9 +181,9 @@ class btAlignedObjectArray { int curSize = size(); - if (newsize < size()) + if (newsize < curSize) { - for(int i = curSize; i < newsize; i++) + for(int i = newsize; i < curSize; i++) { m_data[i].~T(); } @@ -195,6 +205,18 @@ class btAlignedObjectArray m_size = newsize; } + SIMD_FORCE_INLINE T& expandNonInitializing( ) + { + int sz = size(); + if( sz == capacity() ) + { + reserve( allocSize(size()) ); + } + m_size++; + + return m_data[sz]; + } + SIMD_FORCE_INLINE T& expand( const T& fillValue=T()) { @@ -384,7 +406,7 @@ class btAlignedObjectArray int findBinarySearch(const T& key) const { int first = 0; - int last = size(); + int last = size()-1; //assume sorted array while (first <= last) { @@ -437,6 +459,13 @@ class btAlignedObjectArray m_capacity = capacity; } + void copyFromArray(const btAlignedObjectArray& otherArray) + { + int otherSize = otherArray.size(); + resize (otherSize); + otherArray.copy(0, otherSize, m_data); + } + }; #endif //BT_OBJECT_ARRAY__ diff --git a/extern/bullet2/src/LinearMath/btConvexHull.cpp b/extern/bullet2/src/LinearMath/btConvexHull.cpp index 419c752a1d9..532d76d881f 100644 --- a/extern/bullet2/src/LinearMath/btConvexHull.cpp +++ b/extern/bullet2/src/LinearMath/btConvexHull.cpp @@ -16,9 +16,9 @@ subject to the following restrictions: #include <string.h> #include "btConvexHull.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btMinMax.h" -#include "LinearMath/btVector3.h" +#include "btAlignedObjectArray.h" +#include "btMinMax.h" +#include "btVector3.h" @@ -96,21 +96,21 @@ btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const // returns the point where the line p0-p1 intersects the plane n&d static btVector3 dif; dif = p1-p0; - btScalar dn= dot(plane.normal,dif); - btScalar t = -(plane.dist+dot(plane.normal,p0) )/dn; + btScalar dn= btDot(plane.normal,dif); + btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn; return p0 + (dif*t); } btVector3 PlaneProject(const btPlane &plane, const btVector3 &point) { - return point - plane.normal * (dot(point,plane.normal)+plane.dist); + return point - plane.normal * (btDot(point,plane.normal)+plane.dist); } btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) { // return the normal of the triangle // inscribed by v0, v1, and v2 - btVector3 cp=cross(v1-v0,v2-v1); + btVector3 cp=btCross(v1-v0,v2-v1); btScalar m=cp.length(); if(m==0) return btVector3(1,0,0); return cp*(btScalar(1.0)/m); @@ -120,23 +120,23 @@ btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint) { static btVector3 cp; - cp = cross(udir,vdir).normalized(); + cp = btCross(udir,vdir).normalized(); - btScalar distu = -dot(cp,ustart); - btScalar distv = -dot(cp,vstart); + btScalar distu = -btDot(cp,ustart); + btScalar distv = -btDot(cp,vstart); btScalar dist = (btScalar)fabs(distu-distv); if(upoint) { btPlane plane; - plane.normal = cross(vdir,cp).normalized(); - plane.dist = -dot(plane.normal,vstart); + plane.normal = btCross(vdir,cp).normalized(); + plane.dist = -btDot(plane.normal,vstart); *upoint = PlaneLineIntersection(plane,ustart,ustart+udir); } if(vpoint) { btPlane plane; - plane.normal = cross(udir,cp).normalized(); - plane.dist = -dot(plane.normal,ustart); + plane.normal = btCross(udir,cp).normalized(); + plane.dist = -btDot(plane.normal,ustart); *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); } return dist; @@ -170,7 +170,7 @@ ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size) int PlaneTest(const btPlane &p, const btVector3 &v); int PlaneTest(const btPlane &p, const btVector3 &v) { - btScalar a = dot(v,p.normal)+p.dist; + btScalar a = btDot(v,p.normal)+p.dist; int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR); return flag; } @@ -228,7 +228,7 @@ int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> & for(int i=0;i<count;i++) if(allow[i]) { - if(m==-1 || dot(p[i],dir)>dot(p[m],dir)) + if(m==-1 || btDot(p[i],dir)>btDot(p[m],dir)) m=i; } btAssert(m!=-1); @@ -238,8 +238,8 @@ int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> & btVector3 orth(const btVector3 &v); btVector3 orth(const btVector3 &v) { - btVector3 a=cross(v,btVector3(0,0,1)); - btVector3 b=cross(v,btVector3(0,1,0)); + btVector3 a=btCross(v,btVector3(0,0,1)); + btVector3 b=btCross(v,btVector3(0,1,0)); if (a.length() > b.length()) { return a.normalized(); @@ -258,7 +258,7 @@ int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &al m = maxdirfiltered(p,count,dir,allow); if(allow[m]==3) return m; T u = orth(dir); - T v = cross(u,dir); + T v = btCross(u,dir); int ma=-1; for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0)) { @@ -313,7 +313,7 @@ int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilo int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon) { btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]); - return (dot(n,p-vertices[t[0]]) > epsilon); // EPSILON??? + return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON??? } int hasedge(const int3 &t, int a,int b); int hasedge(const int3 &t, int a,int b) @@ -495,8 +495,8 @@ int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectAr basis[0] = verts[p0]-verts[p1]; if(p0==p1 || basis[0]==btVector3(0,0,0)) return int4(-1,-1,-1,-1); - basis[1] = cross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]); - basis[2] = cross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]); + basis[1] = btCross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]); + basis[2] = btCross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]); if (basis[1].length() > basis[2].length()) { basis[1].normalize(); @@ -512,13 +512,13 @@ int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectAr if(p2 == p0 || p2 == p1) return int4(-1,-1,-1,-1); basis[1] = verts[p2] - verts[p0]; - basis[2] = cross(basis[1],basis[0]).normalized(); + basis[2] = btCross(basis[1],basis[0]).normalized(); int p3 = maxdirsterid(verts,verts_count,basis[2],allow); if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow); if(p3==p0||p3==p1||p3==p2) return int4(-1,-1,-1,-1); btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3)); - if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);} + if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);} return int4(p0,p1,p2,p3); } @@ -564,7 +564,7 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) btAssert(t->vmax<0); btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); t->vmax = maxdirsterid(verts,verts_count,n,allow); - t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); + t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]); } btHullTriangle *te; vlimit-=4; @@ -592,7 +592,7 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) if(!m_tris[j]) continue; if(!hasvert(*m_tris[j],v)) break; int3 nt=*m_tris[j]; - if(above(verts,nt,center,btScalar(0.01)*epsilon) || cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) ) + if(above(verts,nt,center,btScalar(0.01)*epsilon) || btCross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) ) { btHullTriangle *nb = m_tris[m_tris[j]->n[0]]; btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j); @@ -614,7 +614,7 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) } else { - t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); + t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]); } } vlimit --; @@ -876,7 +876,7 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, vcount = 0; - btScalar recip[3]; + btScalar recip[3]={0.f,0.f,0.f}; if ( scale ) { @@ -1011,9 +1011,9 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, btScalar y = v[1]; btScalar z = v[2]; - btScalar dx = fabsf(x - px ); - btScalar dy = fabsf(y - py ); - btScalar dz = fabsf(z - pz ); + btScalar dx = btFabs(x - px ); + btScalar dy = btFabs(y - py ); + btScalar dz = btFabs(z - pz ); if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon ) { @@ -1135,7 +1135,7 @@ void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, b ocount = 0; - for (i=0; i<indexcount; i++) + for (i=0; i<int (indexcount); i++) { unsigned int v = indices[i]; // original array index @@ -1156,7 +1156,7 @@ void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, b for (int k=0;k<m_vertexIndexMapping.size();k++) { - if (tmpIndices[k]==v) + if (tmpIndices[k]==int(v)) m_vertexIndexMapping[k]=ocount; } diff --git a/extern/bullet2/src/LinearMath/btConvexHull.h b/extern/bullet2/src/LinearMath/btConvexHull.h index 92560bddb9c..a23fa4d550f 100644 --- a/extern/bullet2/src/LinearMath/btConvexHull.h +++ b/extern/bullet2/src/LinearMath/btConvexHull.h @@ -19,8 +19,8 @@ subject to the following restrictions: #ifndef CD_HULL_H #define CD_HULL_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btAlignedObjectArray.h" +#include "btVector3.h" +#include "btAlignedObjectArray.h" typedef btAlignedObjectArray<unsigned int> TUIntArray; diff --git a/extern/bullet2/src/LinearMath/btDefaultMotionState.h b/extern/bullet2/src/LinearMath/btDefaultMotionState.h index d758f77ed81..7858a10d219 100644 --- a/extern/bullet2/src/LinearMath/btDefaultMotionState.h +++ b/extern/bullet2/src/LinearMath/btDefaultMotionState.h @@ -1,6 +1,8 @@ #ifndef DEFAULT_MOTION_STATE_H #define DEFAULT_MOTION_STATE_H +#include "btMotionState.h" + ///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets. struct btDefaultMotionState : public btMotionState { diff --git a/extern/bullet2/src/LinearMath/btHashMap.h b/extern/bullet2/src/LinearMath/btHashMap.h index f883e0e489a..e3302b5e9ff 100644 --- a/extern/bullet2/src/LinearMath/btHashMap.h +++ b/extern/bullet2/src/LinearMath/btHashMap.h @@ -3,94 +3,215 @@ #include "btAlignedObjectArray.h" +///very basic hashable string implementation, compatible with btHashMap +struct btHashString +{ + const char* m_string; + unsigned int m_hash; + + SIMD_FORCE_INLINE unsigned int getHash()const + { + return m_hash; + } + + btHashString(const char* name) + :m_string(name) + { + /* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */ + static const unsigned int InitialFNV = 2166136261u; + static const unsigned int FNVMultiple = 16777619u; + + /* Fowler / Noll / Vo (FNV) Hash */ + unsigned int hash = InitialFNV; + + for(int i = 0; m_string[i]; i++) + { + hash = hash ^ (m_string[i]); /* xor the low 8 bits */ + hash = hash * FNVMultiple; /* multiply by the magic number */ + } + m_hash = hash; + } + + int portableStringCompare(const char* src, const char* dst) const + { + int ret = 0 ; + + while( ! (ret = *(unsigned char *)src - *(unsigned char *)dst) && *dst) + ++src, ++dst; + + if ( ret < 0 ) + ret = -1 ; + else if ( ret > 0 ) + ret = 1 ; + + return( ret ); + } + + bool equals(const btHashString& other) const + { + return (m_string == other.m_string) || + (0==portableStringCompare(m_string,other.m_string)); + + } + +}; + const int BT_HASH_NULL=0xffffffff; -template <class Value> -class btHashKey + +class btHashInt { int m_uid; public: - - btHashKey(int uid) - :m_uid(uid) + btHashInt(int uid) :m_uid(uid) { } - int getUid() const + int getUid1() const { return m_uid; } + void setUid1(int uid) + { + m_uid = uid; + } + + bool equals(const btHashInt& other) const + { + return getUid1() == other.getUid1(); + } //to our success SIMD_FORCE_INLINE unsigned int getHash()const { int key = m_uid; // Thomas Wang's hash - key += ~(key << 15); - key ^= (key >> 10); - key += (key << 3); - key ^= (key >> 6); - key += ~(key << 11); - key ^= (key >> 16); + key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); return key; } +}; - btHashKey getKey(const Value& value) const + + +class btHashPtr +{ + + union { - return btHashKey(value.getUid()); + const void* m_pointer; + int m_hashValues[2]; + }; + +public: + + btHashPtr(const void* ptr) + :m_pointer(ptr) + { + } + + const void* getPointer() const + { + return m_pointer; + } + + bool equals(const btHashPtr& other) const + { + return getPointer() == other.getPointer(); + } + + //to our success + SIMD_FORCE_INLINE unsigned int getHash()const + { + const bool VOID_IS_8 = ((sizeof(void*)==8)); + + int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0]; + + // Thomas Wang's hash + key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); + return key; } + + }; template <class Value> class btHashKeyPtr { + int m_uid; +public: + + btHashKeyPtr(int uid) :m_uid(uid) + { + } + + int getUid1() const + { + return m_uid; + } + + bool equals(const btHashKeyPtr<Value>& other) const + { + return getUid1() == other.getUid1(); + } + + //to our success + SIMD_FORCE_INLINE unsigned int getHash()const + { + int key = m_uid; + // Thomas Wang's hash + key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); + return key; + } + + +}; + + +template <class Value> +class btHashKey +{ int m_uid; public: - btHashKeyPtr(int uid) - :m_uid(uid) + btHashKey(int uid) :m_uid(uid) { } - int getUid() const + int getUid1() const { return m_uid; } + bool equals(const btHashKey<Value>& other) const + { + return getUid1() == other.getUid1(); + } //to our success SIMD_FORCE_INLINE unsigned int getHash()const { int key = m_uid; // Thomas Wang's hash - key += ~(key << 15); - key ^= (key >> 10); - key += (key << 3); - key ^= (key >> 6); - key += ~(key << 11); - key ^= (key >> 16); + key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); return key; } - - btHashKeyPtr getKey(const Value& value) const - { - return btHashKeyPtr(value->getUid()); - } }; + ///The btHashMap template class implements a generic and lightweight hashmap. ///A basic sample of how to use btHashMap is located in Demos\BasicDemo\main.cpp template <class Key, class Value> class btHashMap { +protected: btAlignedObjectArray<int> m_hashTable; btAlignedObjectArray<int> m_next; + btAlignedObjectArray<Value> m_valueArray; + btAlignedObjectArray<Key> m_keyArray; - - - void growTables(const Key& key) + void growTables(const Key& /*key*/) { int newCapacity = m_valueArray.capacity(); @@ -115,9 +236,10 @@ class btHashMap for(i=0;i<curHashtableSize;i++) { - const Value& value = m_valueArray[i]; + //const Value& value = m_valueArray[i]; + //const Key& key = m_keyArray[i]; - int hashValue = key.getKey(value).getHash() & (m_valueArray.capacity()-1); // New hash value with new mask + int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity()-1); // New hash value with new mask m_next[i] = m_hashTable[hashValue]; m_hashTable[hashValue] = i; } @@ -130,14 +252,20 @@ class btHashMap void insert(const Key& key, const Value& value) { int hash = key.getHash() & (m_valueArray.capacity()-1); - //don't add it if it is already there - if (find(key)) + + //replace value if the key is already there + int index = findIndex(key); + if (index != BT_HASH_NULL) { + m_valueArray[index]=value; return; } + int count = m_valueArray.size(); int oldCapacity = m_valueArray.capacity(); m_valueArray.push_back(value); + m_keyArray.push_back(key); + int newCapacity = m_valueArray.capacity(); if (oldCapacity < newCapacity) { @@ -191,12 +319,12 @@ class btHashMap if (lastPairIndex == pairIndex) { m_valueArray.pop_back(); + m_keyArray.pop_back(); return; } // Remove the last pair from the hash table. - const Value* lastValue = &m_valueArray[lastPairIndex]; - int lastHash = key.getKey(*lastValue).getHash() & (m_valueArray.capacity()-1); + int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity()-1); index = m_hashTable[lastHash]; btAssert(index != BT_HASH_NULL); @@ -220,12 +348,14 @@ class btHashMap // Copy the last pair into the remove pair's spot. m_valueArray[pairIndex] = m_valueArray[lastPairIndex]; + m_keyArray[pairIndex] = m_keyArray[lastPairIndex]; // Insert the last pair into the hash table m_next[pairIndex] = m_hashTable[lastHash]; m_hashTable[lastHash] = pairIndex; m_valueArray.pop_back(); + m_keyArray.pop_back(); } @@ -276,15 +406,15 @@ class btHashMap int findIndex(const Key& key) const { - int hash = key.getHash() & (m_valueArray.capacity()-1); + unsigned int hash = key.getHash() & (m_valueArray.capacity()-1); - if (hash >= m_hashTable.size()) + if (hash >= (unsigned int)m_hashTable.size()) { return BT_HASH_NULL; } int index = m_hashTable[hash]; - while ((index != BT_HASH_NULL) && (key.getUid() == key.getKey(m_valueArray[index]).getUid()) == false) + while ((index != BT_HASH_NULL) && key.equals(m_keyArray[index]) == false) { index = m_next[index]; } @@ -296,6 +426,7 @@ class btHashMap m_hashTable.clear(); m_next.clear(); m_valueArray.clear(); + m_keyArray.clear(); } }; diff --git a/extern/bullet2/src/LinearMath/btIDebugDraw.h b/extern/bullet2/src/LinearMath/btIDebugDraw.h index e5a0061b779..48c15806a7d 100644 --- a/extern/bullet2/src/LinearMath/btIDebugDraw.h +++ b/extern/bullet2/src/LinearMath/btIDebugDraw.h @@ -1,27 +1,16 @@ /* -Copyright (c) 2005 Gino van den Bergen / Erwin Coumans http://continuousphysics.com - -Permission is hereby granted, free of charge, to any person or organization -obtaining a copy of the software and accompanying documentation covered by -this license (the "Software") to use, reproduce, display, distribute, -execute, and transmit the Software, and to prepare derivative works of the -Software, and to permit third-parties to whom the Software is furnished to -do so, all subject to the following: - -The copyright notices in the Software and this entire statement, including -the above license grant, this restriction and the following disclaimer, -must be included in all copies of the Software, in whole or in part, and -all derivative works of the Software, unless such copies or derivative -works are solely in the form of machine-executable object code generated by -a source language processor. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT -SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE -FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, -ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -DEALINGS IN THE SOFTWARE. +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. */ @@ -35,6 +24,7 @@ DEALINGS IN THE SOFTWARE. ///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations. ///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld. ///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum. +///For color arguments the X,Y,Z components refer to Red, Green and Blue each in the range [0..1] class btIDebugDraw { public: @@ -55,25 +45,54 @@ class btIDebugDraw DBG_EnableCCD = 1024, DBG_DrawConstraints = (1 << 11), DBG_DrawConstraintLimits = (1 << 12), + DBG_FastWireframe = (1<<13), DBG_MAX_DEBUG_DRAW_MODE }; virtual ~btIDebugDraw() {}; + virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0; + virtual void drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor) { + (void) toColor; drawLine (from, to, fromColor); } - virtual void drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha) + virtual void drawSphere(btScalar radius, const btTransform& transform, const btVector3& color) { - } + btVector3 start = transform.getOrigin(); + + const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0); + const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0); + const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius); + // XY + drawLine(start-xoffs, start+yoffs, color); + drawLine(start+yoffs, start+xoffs, color); + drawLine(start+xoffs, start-yoffs, color); + drawLine(start-yoffs, start-xoffs, color); + + // XZ + drawLine(start-xoffs, start+zoffs, color); + drawLine(start+zoffs, start+xoffs, color); + drawLine(start+xoffs, start-zoffs, color); + drawLine(start-zoffs, start-xoffs, color); + + // YZ + drawLine(start-yoffs, start+zoffs, color); + drawLine(start+zoffs, start+yoffs, color); + drawLine(start+yoffs, start-zoffs, color); + drawLine(start-zoffs, start-yoffs, color); + } + virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color) { + btTransform tr; + tr.setIdentity(); + tr.setOrigin(p); + drawSphere(radius,tr,color); } - - 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) { @@ -96,7 +115,7 @@ class btIDebugDraw virtual int getDebugMode() const = 0; - inline void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color) + virtual void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color) { btVector3 halfExtents = (to-from)* 0.5f; @@ -125,7 +144,7 @@ class btIDebugDraw edgecoord[i]*=-1.f; } } - void drawTransform(const btTransform& transform, btScalar orthoLen) + virtual void drawTransform(const btTransform& transform, btScalar orthoLen) { btVector3 start = transform.getOrigin(); drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0)); @@ -133,7 +152,7 @@ class btIDebugDraw drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f)); } - void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle, + virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle, const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f)) { const btVector3& vx = axis; @@ -158,7 +177,7 @@ class btIDebugDraw drawLine(center, prev, color); } } - void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius, + virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius, btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f)) { btVector3 vA[74]; @@ -260,7 +279,7 @@ class btIDebugDraw } } - void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color) + virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color) { drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color); drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMin[2]), color); @@ -275,7 +294,7 @@ class btIDebugDraw drawLine(btVector3(bbMax[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color); drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color); } - void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color) + virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color) { drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color); drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), color); @@ -290,6 +309,107 @@ class btIDebugDraw drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color); drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color); } + + virtual void drawCapsule(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color) + { + btVector3 capStart(0.f,0.f,0.f); + capStart[upAxis] = -halfHeight; + + btVector3 capEnd(0.f,0.f,0.f); + capEnd[upAxis] = halfHeight; + + // Draw the ends + { + + btTransform childTransform = transform; + childTransform.getOrigin() = transform * capStart; + drawSphere(radius, childTransform, color); + } + + { + btTransform childTransform = transform; + childTransform.getOrigin() = transform * capEnd; + drawSphere(radius, childTransform, color); + } + + // Draw some additional lines + btVector3 start = transform.getOrigin(); + + capStart[(upAxis+1)%3] = radius; + capEnd[(upAxis+1)%3] = radius; + drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); + capStart[(upAxis+1)%3] = -radius; + capEnd[(upAxis+1)%3] = -radius; + drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); + + capStart[(upAxis+1)%3] = 0.f; + capEnd[(upAxis+1)%3] = 0.f; + + capStart[(upAxis+2)%3] = radius; + capEnd[(upAxis+2)%3] = radius; + drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); + capStart[(upAxis+2)%3] = -radius; + capEnd[(upAxis+2)%3] = -radius; + drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); + } + + virtual void drawCylinder(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color) + { + btVector3 start = transform.getOrigin(); + btVector3 offsetHeight(0,0,0); + offsetHeight[upAxis] = halfHeight; + btVector3 offsetRadius(0,0,0); + offsetRadius[(upAxis+1)%3] = radius; + drawLine(start+transform.getBasis() * (offsetHeight+offsetRadius),start+transform.getBasis() * (-offsetHeight+offsetRadius),color); + drawLine(start+transform.getBasis() * (offsetHeight-offsetRadius),start+transform.getBasis() * (-offsetHeight-offsetRadius),color); + + // Drawing top and bottom caps of the cylinder + btVector3 yaxis(0,0,0); + yaxis[upAxis] = btScalar(1.0); + btVector3 xaxis(0,0,0); + xaxis[(upAxis+1)%3] = btScalar(1.0); + drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0)); + drawArc(start+transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0)); + } + + virtual void drawCone(btScalar radius, btScalar height, int upAxis, const btTransform& transform, const btVector3& color) + { + + btVector3 start = transform.getOrigin(); + + btVector3 offsetHeight(0,0,0); + offsetHeight[upAxis] = height * btScalar(0.5); + btVector3 offsetRadius(0,0,0); + offsetRadius[(upAxis+1)%3] = radius; + btVector3 offset2Radius(0,0,0); + offset2Radius[(upAxis+2)%3] = radius; + + drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offsetRadius),color); + drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offsetRadius),color); + drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offset2Radius),color); + drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offset2Radius),color); + + // Drawing the base of the cone + btVector3 yaxis(0,0,0); + yaxis[upAxis] = btScalar(1.0); + btVector3 xaxis(0,0,0); + xaxis[(upAxis+1)%3] = btScalar(1.0); + drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,10.0); + } + + virtual void drawPlane(const btVector3& planeNormal, btScalar planeConst, const btTransform& transform, const btVector3& color) + { + btVector3 planeOrigin = planeNormal * planeConst; + btVector3 vec0,vec1; + btPlaneSpace1(planeNormal,vec0,vec1); + btScalar vecLen = 100.f; + btVector3 pt0 = planeOrigin + vec0*vecLen; + btVector3 pt1 = planeOrigin - vec0*vecLen; + btVector3 pt2 = planeOrigin + vec1*vecLen; + btVector3 pt3 = planeOrigin - vec1*vecLen; + drawLine(transform*pt0,transform*pt1,color); + drawLine(transform*pt2,transform*pt3,color); + } }; diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet2/src/LinearMath/btMatrix3x3.h index e45afc3c055..d0234a04369 100644 --- a/extern/bullet2/src/LinearMath/btMatrix3x3.h +++ b/extern/bullet2/src/LinearMath/btMatrix3x3.h @@ -13,162 +13,179 @@ subject to the following restrictions: */ -#ifndef btMatrix3x3_H -#define btMatrix3x3_H - -#include "btScalar.h" +#ifndef BT_MATRIX3x3_H +#define BT_MATRIX3x3_H #include "btVector3.h" #include "btQuaternion.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define btMatrix3x3Data btMatrix3x3DoubleData +#else +#define btMatrix3x3Data btMatrix3x3FloatData +#endif //BT_USE_DOUBLE_PRECISION /**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3. - * Make sure to only include a pure orthogonal matrix without scaling. */ +* Make sure to only include a pure orthogonal matrix without scaling. */ class btMatrix3x3 { - public: - /** @brief No initializaion constructor */ - btMatrix3x3 () {} - -// explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); } - - /**@brief Constructor from Quaternion */ - explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); } - /* - template <typename btScalar> - Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) - { - setEulerYPR(yaw, pitch, roll); - } - */ - /** @brief Constructor with row major formatting */ - btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz, - const btScalar& yx, const btScalar& yy, const btScalar& yz, - const btScalar& zx, const btScalar& zy, const btScalar& zz) - { - setValue(xx, xy, xz, - yx, yy, yz, - zx, zy, zz); - } - /** @brief Copy constructor */ - SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other) - { - m_el[0] = other.m_el[0]; - m_el[1] = other.m_el[1]; - m_el[2] = other.m_el[2]; - } - /** @brief Assignment Operator */ - SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other) - { - m_el[0] = other.m_el[0]; - m_el[1] = other.m_el[1]; - m_el[2] = other.m_el[2]; - return *this; - } - /** @brief Get a column of the matrix as a vector - * @param i Column number 0 indexed */ - SIMD_FORCE_INLINE btVector3 getColumn(int i) const - { - return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); - } - + ///Data storage for the matrix, each vector is a row of the matrix + btVector3 m_el[3]; - /** @brief Get a row of the matrix as a vector - * @param i Row number 0 indexed */ - SIMD_FORCE_INLINE const btVector3& getRow(int i) const - { - btFullAssert(0 <= i && i < 3); - return m_el[i]; - } +public: + /** @brief No initializaion constructor */ + btMatrix3x3 () {} - /** @brief Get a mutable reference to a row of the matrix as a vector - * @param i Row number 0 indexed */ - SIMD_FORCE_INLINE btVector3& operator[](int i) - { - btFullAssert(0 <= i && i < 3); - return m_el[i]; - } - - /** @brief Get a const reference to a row of the matrix as a vector - * @param i Row number 0 indexed */ - SIMD_FORCE_INLINE const btVector3& operator[](int i) const - { - btFullAssert(0 <= i && i < 3); - return m_el[i]; - } - - /** @brief Multiply by the target matrix on the right - * @param m Rotation matrix to be applied - * Equivilant to this = this * m */ - btMatrix3x3& operator*=(const btMatrix3x3& m); - - /** @brief Set from a carray of btScalars - * @param m A pointer to the beginning of an array of 9 btScalars */ + // explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); } + + /**@brief Constructor from Quaternion */ + explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); } + /* + template <typename btScalar> + Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) + { + setEulerYPR(yaw, pitch, roll); + } + */ + /** @brief Constructor with row major formatting */ + btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz, + const btScalar& yx, const btScalar& yy, const btScalar& yz, + const btScalar& zx, const btScalar& zy, const btScalar& zz) + { + setValue(xx, xy, xz, + yx, yy, yz, + zx, zy, zz); + } + /** @brief Copy constructor */ + SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other) + { + m_el[0] = other.m_el[0]; + m_el[1] = other.m_el[1]; + m_el[2] = other.m_el[2]; + } + /** @brief Assignment Operator */ + SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other) + { + m_el[0] = other.m_el[0]; + m_el[1] = other.m_el[1]; + m_el[2] = other.m_el[2]; + return *this; + } + + /** @brief Get a column of the matrix as a vector + * @param i Column number 0 indexed */ + SIMD_FORCE_INLINE btVector3 getColumn(int i) const + { + return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); + } + + + /** @brief Get a row of the matrix as a vector + * @param i Row number 0 indexed */ + SIMD_FORCE_INLINE const btVector3& getRow(int i) const + { + btFullAssert(0 <= i && i < 3); + return m_el[i]; + } + + /** @brief Get a mutable reference to a row of the matrix as a vector + * @param i Row number 0 indexed */ + SIMD_FORCE_INLINE btVector3& operator[](int i) + { + btFullAssert(0 <= i && i < 3); + return m_el[i]; + } + + /** @brief Get a const reference to a row of the matrix as a vector + * @param i Row number 0 indexed */ + SIMD_FORCE_INLINE const btVector3& operator[](int i) const + { + btFullAssert(0 <= i && i < 3); + return m_el[i]; + } + + /** @brief Multiply by the target matrix on the right + * @param m Rotation matrix to be applied + * Equivilant to this = this * m */ + btMatrix3x3& operator*=(const btMatrix3x3& m); + + /** @brief Adds by the target matrix on the right + * @param m matrix to be applied + * Equivilant to this = this + m */ + btMatrix3x3& operator+=(const btMatrix3x3& m); + + /** @brief Substractss by the target matrix on the right + * @param m matrix to be applied + * Equivilant to this = this - m */ + btMatrix3x3& operator-=(const btMatrix3x3& m); + + /** @brief Set from the rotational part of a 4x4 OpenGL matrix + * @param m A pointer to the beginning of the array of scalars*/ void setFromOpenGLSubMatrix(const btScalar *m) - { - m_el[0].setValue(m[0],m[4],m[8]); - m_el[1].setValue(m[1],m[5],m[9]); - m_el[2].setValue(m[2],m[6],m[10]); + { + m_el[0].setValue(m[0],m[4],m[8]); + m_el[1].setValue(m[1],m[5],m[9]); + m_el[2].setValue(m[2],m[6],m[10]); - } - /** @brief Set the values of the matrix explicitly (row major) - * @param xx Top left - * @param xy Top Middle - * @param xz Top Right - * @param yx Middle Left - * @param yy Middle Middle - * @param yz Middle Right - * @param zx Bottom Left - * @param zy Bottom Middle - * @param zz Bottom Right*/ - void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, - const btScalar& yx, const btScalar& yy, const btScalar& yz, - const btScalar& zx, const btScalar& zy, const btScalar& zz) - { - m_el[0].setValue(xx,xy,xz); - m_el[1].setValue(yx,yy,yz); - m_el[2].setValue(zx,zy,zz); - } + } + /** @brief Set the values of the matrix explicitly (row major) + * @param xx Top left + * @param xy Top Middle + * @param xz Top Right + * @param yx Middle Left + * @param yy Middle Middle + * @param yz Middle Right + * @param zx Bottom Left + * @param zy Bottom Middle + * @param zz Bottom Right*/ + void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, + const btScalar& yx, const btScalar& yy, const btScalar& yz, + const btScalar& zx, const btScalar& zy, const btScalar& zz) + { + m_el[0].setValue(xx,xy,xz); + m_el[1].setValue(yx,yy,yz); + m_el[2].setValue(zx,zy,zz); + } + + /** @brief Set the matrix from a quaternion + * @param q The Quaternion to match */ + void setRotation(const btQuaternion& q) + { + btScalar d = q.length2(); + btFullAssert(d != btScalar(0.0)); + btScalar s = btScalar(2.0) / d; + btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; + btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; + btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; + btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; + setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy, + xy + wz, btScalar(1.0) - (xx + zz), yz - wx, + xz - wy, yz + wx, btScalar(1.0) - (xx + yy)); + } - /** @brief Set the matrix from a quaternion - * @param q The Quaternion to match */ - void setRotation(const btQuaternion& q) - { - btScalar d = q.length2(); - btFullAssert(d != btScalar(0.0)); - btScalar s = btScalar(2.0) / d; - btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; - btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; - btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; - btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; - setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy, - xy + wz, btScalar(1.0) - (xx + zz), yz - wx, - xz - wy, yz + wx, btScalar(1.0) - (xx + yy)); - } - - - /** @brief Set the matrix from euler angles using YPR around YXZ respectively - * @param yaw Yaw about Y axis - * @param pitch Pitch about X axis - * @param roll Roll about Z axis - */ - void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) - { - setEulerZYX(roll, pitch, yaw); - } + + /** @brief Set the matrix from euler angles using YPR around YXZ respectively + * @param yaw Yaw about Y axis + * @param pitch Pitch about X axis + * @param roll Roll about Z axis + */ + void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) + { + setEulerZYX(roll, pitch, yaw); + } /** @brief Set the matrix from euler angles YPR around ZYX axes - * @param eulerX Roll about X axis - * @param eulerY Pitch around Y axis - * @param eulerZ Yaw aboud Z axis - * - * These angles are used to produce a rotation matrix. The euler - * angles are applied in ZYX order. I.e a vector is first rotated - * about X then Y and then Z - **/ + * @param eulerX Roll about X axis + * @param eulerY Pitch around Y axis + * @param eulerZ Yaw aboud Z axis + * + * These angles are used to produce a rotation matrix. The euler + * angles are applied in ZYX order. I.e a vector is first rotated + * about X then Y and then Z + **/ void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { - ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code + ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code btScalar ci ( btCos(eulerX)); btScalar cj ( btCos(eulerY)); btScalar ch ( btCos(eulerZ)); @@ -179,227 +196,233 @@ class btMatrix3x3 { btScalar cs = ci * sh; btScalar sc = si * ch; btScalar ss = si * sh; - + setValue(cj * ch, sj * sc - cs, sj * cc + ss, - cj * sh, sj * ss + cc, sj * cs - sc, - -sj, cj * si, cj * ci); + cj * sh, sj * ss + cc, sj * cs - sc, + -sj, cj * si, cj * ci); } - /**@brief Set the matrix to the identity */ - void setIdentity() - { - setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), - btScalar(0.0), btScalar(1.0), btScalar(0.0), - btScalar(0.0), btScalar(0.0), btScalar(1.0)); - } + /**@brief Set the matrix to the identity */ + void setIdentity() + { + setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), + btScalar(0.0), btScalar(1.0), btScalar(0.0), + btScalar(0.0), btScalar(0.0), btScalar(1.0)); + } + + static const btMatrix3x3& getIdentity() + { + static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0), + btScalar(0.0), btScalar(1.0), btScalar(0.0), + btScalar(0.0), btScalar(0.0), btScalar(1.0)); + return identityMatrix; + } + + /**@brief Fill the rotational part of an OpenGL matrix and clear the shear/perspective + * @param m The array to be filled */ + void getOpenGLSubMatrix(btScalar *m) const + { + m[0] = btScalar(m_el[0].x()); + m[1] = btScalar(m_el[1].x()); + m[2] = btScalar(m_el[2].x()); + m[3] = btScalar(0.0); + m[4] = btScalar(m_el[0].y()); + m[5] = btScalar(m_el[1].y()); + m[6] = btScalar(m_el[2].y()); + m[7] = btScalar(0.0); + m[8] = btScalar(m_el[0].z()); + m[9] = btScalar(m_el[1].z()); + m[10] = btScalar(m_el[2].z()); + m[11] = btScalar(0.0); + } + + /**@brief Get the matrix represented as a quaternion + * @param q The quaternion which will be set */ + void getRotation(btQuaternion& q) const + { + btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); + btScalar temp[4]; - static const btMatrix3x3& getIdentity() + if (trace > btScalar(0.0)) + { + btScalar s = btSqrt(trace + btScalar(1.0)); + temp[3]=(s * btScalar(0.5)); + s = btScalar(0.5) / s; + + temp[0]=((m_el[2].y() - m_el[1].z()) * s); + temp[1]=((m_el[0].z() - m_el[2].x()) * s); + temp[2]=((m_el[1].x() - m_el[0].y()) * s); + } + else { - static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0), - btScalar(0.0), btScalar(1.0), btScalar(0.0), - btScalar(0.0), btScalar(0.0), btScalar(1.0)); - return identityMatrix; + int i = m_el[0].x() < m_el[1].y() ? + (m_el[1].y() < m_el[2].z() ? 2 : 1) : + (m_el[0].x() < m_el[2].z() ? 2 : 0); + int j = (i + 1) % 3; + int k = (i + 2) % 3; + + btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0)); + temp[i] = s * btScalar(0.5); + s = btScalar(0.5) / s; + + temp[3] = (m_el[k][j] - m_el[j][k]) * s; + temp[j] = (m_el[j][i] + m_el[i][j]) * s; + temp[k] = (m_el[k][i] + m_el[i][k]) * s; } + q.setValue(temp[0],temp[1],temp[2],temp[3]); + } + + /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR + * @param yaw Yaw around Y axis + * @param pitch Pitch around X axis + * @param roll around Z axis */ + void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const + { + + // first use the normal calculus + yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); + pitch = btScalar(btAsin(-m_el[2].x())); + roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); - /**@brief Fill the values of the matrix into a 9 element array - * @param m The array to be filled */ - void getOpenGLSubMatrix(btScalar *m) const + // on pitch = +/-HalfPI + if (btFabs(pitch)==SIMD_HALF_PI) { - m[0] = btScalar(m_el[0].x()); - m[1] = btScalar(m_el[1].x()); - m[2] = btScalar(m_el[2].x()); - m[3] = btScalar(0.0); - m[4] = btScalar(m_el[0].y()); - m[5] = btScalar(m_el[1].y()); - m[6] = btScalar(m_el[2].y()); - m[7] = btScalar(0.0); - m[8] = btScalar(m_el[0].z()); - m[9] = btScalar(m_el[1].z()); - m[10] = btScalar(m_el[2].z()); - m[11] = btScalar(0.0); + if (yaw>0) + yaw-=SIMD_PI; + else + yaw+=SIMD_PI; + + if (roll>0) + roll-=SIMD_PI; + else + roll+=SIMD_PI; } + }; - /**@brief Get the matrix represented as a quaternion - * @param q The quaternion which will be set */ - void getRotation(btQuaternion& q) const + + /**@brief Get the matrix represented as euler angles around ZYX + * @param yaw Yaw around X axis + * @param pitch Pitch around Y axis + * @param roll around X axis + * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ + void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const + { + struct Euler { - btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); - btScalar temp[4]; - - if (trace > btScalar(0.0)) + btScalar yaw; + btScalar pitch; + btScalar roll; + }; + + Euler euler_out; + Euler euler_out2; //second solution + //get the pointer to the raw data + + // Check that pitch is not at a singularity + if (btFabs(m_el[2].x()) >= 1) + { + euler_out.yaw = 0; + euler_out2.yaw = 0; + + // From difference of angles formula + btScalar delta = btAtan2(m_el[0].x(),m_el[0].z()); + if (m_el[2].x() > 0) //gimbal locked up { - btScalar s = btSqrt(trace + btScalar(1.0)); - temp[3]=(s * btScalar(0.5)); - s = btScalar(0.5) / s; - - temp[0]=((m_el[2].y() - m_el[1].z()) * s); - temp[1]=((m_el[0].z() - m_el[2].x()) * s); - temp[2]=((m_el[1].x() - m_el[0].y()) * s); - } - else + euler_out.pitch = SIMD_PI / btScalar(2.0); + euler_out2.pitch = SIMD_PI / btScalar(2.0); + euler_out.roll = euler_out.pitch + delta; + euler_out2.roll = euler_out.pitch + delta; + } + else // gimbal locked down { - int i = m_el[0].x() < m_el[1].y() ? - (m_el[1].y() < m_el[2].z() ? 2 : 1) : - (m_el[0].x() < m_el[2].z() ? 2 : 0); - int j = (i + 1) % 3; - int k = (i + 2) % 3; - - btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0)); - temp[i] = s * btScalar(0.5); - s = btScalar(0.5) / s; - - temp[3] = (m_el[k][j] - m_el[j][k]) * s; - temp[j] = (m_el[j][i] + m_el[i][j]) * s; - temp[k] = (m_el[k][i] + m_el[i][k]) * s; + euler_out.pitch = -SIMD_PI / btScalar(2.0); + euler_out2.pitch = -SIMD_PI / btScalar(2.0); + euler_out.roll = -euler_out.pitch + delta; + euler_out2.roll = -euler_out.pitch + delta; } - q.setValue(temp[0],temp[1],temp[2],temp[3]); } - - /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR - * @param yaw Yaw around Y axis - * @param pitch Pitch around X axis - * @param roll around Z axis */ - void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const + else { - - // first use the normal calculus - yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); - pitch = btScalar(btAsin(-m_el[2].x())); - roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); - - // on pitch = +/-HalfPI - if (btFabs(pitch)==SIMD_HALF_PI) - { - if (yaw>0) - yaw-=SIMD_PI; - else - yaw+=SIMD_PI; - - if (roll>0) - roll-=SIMD_PI; - else - roll+=SIMD_PI; - } - }; - + euler_out.pitch = - btAsin(m_el[2].x()); + euler_out2.pitch = SIMD_PI - euler_out.pitch; - /**@brief Get the matrix represented as euler angles around ZYX - * @param yaw Yaw around X axis - * @param pitch Pitch around Y axis - * @param roll around X axis - * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ - void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const - { - struct Euler{btScalar yaw, pitch, roll;}; - Euler euler_out; - Euler euler_out2; //second solution - //get the pointer to the raw data - - // Check that pitch is not at a singularity - if (btFabs(m_el[2].x()) >= 1) - { - euler_out.yaw = 0; - euler_out2.yaw = 0; - - // From difference of angles formula - btScalar delta = btAtan2(m_el[0].x(),m_el[0].z()); - if (m_el[2].x() > 0) //gimbal locked up - { - euler_out.pitch = SIMD_PI / btScalar(2.0); - euler_out2.pitch = SIMD_PI / btScalar(2.0); - euler_out.roll = euler_out.pitch + delta; - euler_out2.roll = euler_out.pitch + delta; - } - else // gimbal locked down - { - euler_out.pitch = -SIMD_PI / btScalar(2.0); - euler_out2.pitch = -SIMD_PI / btScalar(2.0); - euler_out.roll = -euler_out.pitch + delta; - euler_out2.roll = -euler_out.pitch + delta; - } - } - else - { - euler_out.pitch = - btAsin(m_el[2].x()); - euler_out2.pitch = SIMD_PI - euler_out.pitch; - - euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), - m_el[2].z()/btCos(euler_out.pitch)); - euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), + euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), + m_el[2].z()/btCos(euler_out.pitch)); + euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), m_el[2].z()/btCos(euler_out2.pitch)); - - euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), - m_el[0].x()/btCos(euler_out.pitch)); - euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), - m_el[0].x()/btCos(euler_out2.pitch)); - } - - if (solution_number == 1) - { - yaw = euler_out.yaw; - pitch = euler_out.pitch; - roll = euler_out.roll; - } - else - { - yaw = euler_out2.yaw; - pitch = euler_out2.pitch; - roll = euler_out2.roll; - } - } - - /**@brief Create a scaled copy of the matrix - * @param s Scaling vector The elements of the vector will scale each column */ - - btMatrix3x3 scaled(const btVector3& s) const - { - return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), - m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), - m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); - } - /**@brief Return the determinant of the matrix */ - btScalar determinant() const; - /**@brief Return the adjoint of the matrix */ - btMatrix3x3 adjoint() const; - /**@brief Return the matrix with all values non negative */ - btMatrix3x3 absolute() const; - /**@brief Return the transpose of the matrix */ - btMatrix3x3 transpose() const; - /**@brief Return the inverse of the matrix */ - btMatrix3x3 inverse() const; - - btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; - btMatrix3x3 timesTranspose(const btMatrix3x3& m) const; - - SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const - { - return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); + euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), + m_el[0].x()/btCos(euler_out.pitch)); + euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), + m_el[0].x()/btCos(euler_out2.pitch)); } - SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const - { - return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); + + if (solution_number == 1) + { + yaw = euler_out.yaw; + pitch = euler_out.pitch; + roll = euler_out.roll; } - SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const - { - return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); + else + { + yaw = euler_out2.yaw; + pitch = euler_out2.pitch; + roll = euler_out2.roll; } - - - /**@brief diagonalizes this matrix by the Jacobi method. - * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original - * coordinate system, i.e., old_this = rot * new_this * rot^T. - * @param threshold See iteration - * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied - * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. - * - * Note that this matrix is assumed to be symmetric. - */ - void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) + } + + /**@brief Create a scaled copy of the matrix + * @param s Scaling vector The elements of the vector will scale each column */ + + btMatrix3x3 scaled(const btVector3& s) const + { + return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), + m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), + m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); + } + + /**@brief Return the determinant of the matrix */ + btScalar determinant() const; + /**@brief Return the adjoint of the matrix */ + btMatrix3x3 adjoint() const; + /**@brief Return the matrix with all values non negative */ + btMatrix3x3 absolute() const; + /**@brief Return the transpose of the matrix */ + btMatrix3x3 transpose() const; + /**@brief Return the inverse of the matrix */ + btMatrix3x3 inverse() const; + + btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; + btMatrix3x3 timesTranspose(const btMatrix3x3& m) const; + + SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const + { + return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); + } + SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const + { + return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); + } + SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const + { + return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); + } + + + /**@brief diagonalizes this matrix by the Jacobi method. + * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original + * coordinate system, i.e., old_this = rot * new_this * rot^T. + * @param threshold See iteration + * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied + * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. + * + * Note that this matrix is assumed to be symmetric. + */ + void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) + { + rot.setIdentity(); + for (int step = maxSteps; step > 0; step--) { - rot.setIdentity(); - for (int step = maxSteps; step > 0; step--) - { // find off-diagonal element [p][q] with largest magnitude int p = 0; int q = 1; @@ -408,27 +431,27 @@ class btMatrix3x3 { btScalar v = btFabs(m_el[0][2]); if (v > max) { - q = 2; - r = 1; - max = v; + q = 2; + r = 1; + max = v; } v = btFabs(m_el[1][2]); if (v > max) { - p = 1; - q = 2; - r = 0; - max = v; + p = 1; + q = 2; + r = 0; + max = v; } btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2])); if (max <= t) { - if (max <= SIMD_EPSILON * t) - { - return; - } - step = 1; + if (max <= SIMD_EPSILON * t) + { + return; + } + step = 1; } // compute Jacobi rotation J which leads to a zero for element [p][q] @@ -439,17 +462,17 @@ class btMatrix3x3 { btScalar sin; if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON)) { - t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2)) - : 1 / (theta - btSqrt(1 + theta2)); - cos = 1 / btSqrt(1 + t * t); - sin = cos * t; + t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2)) + : 1 / (theta - btSqrt(1 + theta2)); + cos = 1 / btSqrt(1 + t * t); + sin = cos * t; } else { - // approximation for large theta-value, i.e., a nearly diagonal matrix - t = 1 / (theta * (2 + btScalar(0.5) / theta2)); - cos = 1 - btScalar(0.5) * t * t; - sin = cos * t; + // approximation for large theta-value, i.e., a nearly diagonal matrix + t = 1 / (theta * (2 + btScalar(0.5) / theta2)); + cos = 1 - btScalar(0.5) * t * t; + sin = cos * t; } // apply rotation to matrix (this = J^T * this * J) @@ -464,155 +487,285 @@ class btMatrix3x3 { // apply rotation to rot (rot = rot * J) for (int i = 0; i < 3; i++) { - btVector3& row = rot[i]; - mrp = row[p]; - mrq = row[q]; - row[p] = cos * mrp - sin * mrq; - row[q] = cos * mrq + sin * mrp; + btVector3& row = rot[i]; + mrp = row[p]; + mrq = row[q]; + row[p] = cos * mrp - sin * mrq; + row[q] = cos * mrq + sin * mrp; } - } } + } - - protected: - /**@brief Calculate the matrix cofactor - * @param r1 The first row to use for calculating the cofactor - * @param c1 The first column to use for calculating the cofactor - * @param r1 The second row to use for calculating the cofactor - * @param c1 The second column to use for calculating the cofactor - * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details - */ - btScalar cofac(int r1, int c1, int r2, int c2) const - { - return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; - } - ///Data storage for the matrix, each vector is a row of the matrix - btVector3 m_el[3]; - }; - - SIMD_FORCE_INLINE btMatrix3x3& - btMatrix3x3::operator*=(const btMatrix3x3& m) - { - setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), - m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), - m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); - return *this; - } - - SIMD_FORCE_INLINE btScalar - btMatrix3x3::determinant() const - { - return triple((*this)[0], (*this)[1], (*this)[2]); - } - - SIMD_FORCE_INLINE btMatrix3x3 - btMatrix3x3::absolute() const - { - return btMatrix3x3( - btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()), - btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()), - btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z())); - } - SIMD_FORCE_INLINE btMatrix3x3 - btMatrix3x3::transpose() const + /**@brief Calculate the matrix cofactor + * @param r1 The first row to use for calculating the cofactor + * @param c1 The first column to use for calculating the cofactor + * @param r1 The second row to use for calculating the cofactor + * @param c1 The second column to use for calculating the cofactor + * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details + */ + btScalar cofac(int r1, int c1, int r2, int c2) const { - return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), - m_el[0].y(), m_el[1].y(), m_el[2].y(), - m_el[0].z(), m_el[1].z(), m_el[2].z()); - } - - SIMD_FORCE_INLINE btMatrix3x3 - btMatrix3x3::adjoint() const - { - return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2), - cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0), - cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1)); - } - - SIMD_FORCE_INLINE btMatrix3x3 - btMatrix3x3::inverse() const - { - btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); - btScalar det = (*this)[0].dot(co); - btFullAssert(det != btScalar(0.0)); - btScalar s = btScalar(1.0) / det; - return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, - co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, - co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); - } - - SIMD_FORCE_INLINE btMatrix3x3 - btMatrix3x3::transposeTimes(const btMatrix3x3& m) const - { - return btMatrix3x3( - m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), - m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), - m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), - m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), - m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), - m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), - m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), - m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), - m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); - } - - SIMD_FORCE_INLINE btMatrix3x3 - btMatrix3x3::timesTranspose(const btMatrix3x3& m) const - { - return btMatrix3x3( - m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]), - m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]), - m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2])); - + return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; } - SIMD_FORCE_INLINE btVector3 - operator*(const btMatrix3x3& m, const btVector3& v) - { - return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); - } - + void serialize(struct btMatrix3x3Data& dataOut) const; - SIMD_FORCE_INLINE btVector3 - operator*(const btVector3& v, const btMatrix3x3& m) - { - return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); - } + void serializeFloat(struct btMatrix3x3FloatData& dataOut) const; - SIMD_FORCE_INLINE btMatrix3x3 - operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) - { - return btMatrix3x3( - m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]), - m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]), - m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2])); - } + void deSerialize(const struct btMatrix3x3Data& dataIn); + + void deSerializeFloat(const struct btMatrix3x3FloatData& dataIn); + + void deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn); + +}; + + +SIMD_FORCE_INLINE btMatrix3x3& +btMatrix3x3::operator*=(const btMatrix3x3& m) +{ + setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), + m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), + m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); + return *this; +} + +SIMD_FORCE_INLINE btMatrix3x3& +btMatrix3x3::operator+=(const btMatrix3x3& m) +{ + setValue( + m_el[0][0]+m.m_el[0][0], + m_el[0][1]+m.m_el[0][1], + m_el[0][2]+m.m_el[0][2], + m_el[1][0]+m.m_el[1][0], + m_el[1][1]+m.m_el[1][1], + m_el[1][2]+m.m_el[1][2], + m_el[2][0]+m.m_el[2][0], + m_el[2][1]+m.m_el[2][1], + m_el[2][2]+m.m_el[2][2]); + return *this; +} + +SIMD_FORCE_INLINE btMatrix3x3 +operator*(const btMatrix3x3& m, const btScalar & k) +{ + return btMatrix3x3( + m[0].x()*k,m[0].y()*k,m[0].z()*k, + m[1].x()*k,m[1].y()*k,m[1].z()*k, + m[2].x()*k,m[2].y()*k,m[2].z()*k); +} + + SIMD_FORCE_INLINE btMatrix3x3 +operator+(const btMatrix3x3& m1, const btMatrix3x3& m2) +{ + return btMatrix3x3( + m1[0][0]+m2[0][0], + m1[0][1]+m2[0][1], + m1[0][2]+m2[0][2], + m1[1][0]+m2[1][0], + m1[1][1]+m2[1][1], + m1[1][2]+m2[1][2], + m1[2][0]+m2[2][0], + m1[2][1]+m2[2][1], + m1[2][2]+m2[2][2]); +} + +SIMD_FORCE_INLINE btMatrix3x3 +operator-(const btMatrix3x3& m1, const btMatrix3x3& m2) +{ + return btMatrix3x3( + m1[0][0]-m2[0][0], + m1[0][1]-m2[0][1], + m1[0][2]-m2[0][2], + m1[1][0]-m2[1][0], + m1[1][1]-m2[1][1], + m1[1][2]-m2[1][2], + m1[2][0]-m2[2][0], + m1[2][1]-m2[2][1], + m1[2][2]-m2[2][2]); +} + + +SIMD_FORCE_INLINE btMatrix3x3& +btMatrix3x3::operator-=(const btMatrix3x3& m) +{ + setValue( + m_el[0][0]-m.m_el[0][0], + m_el[0][1]-m.m_el[0][1], + m_el[0][2]-m.m_el[0][2], + m_el[1][0]-m.m_el[1][0], + m_el[1][1]-m.m_el[1][1], + m_el[1][2]-m.m_el[1][2], + m_el[2][0]-m.m_el[2][0], + m_el[2][1]-m.m_el[2][1], + m_el[2][2]-m.m_el[2][2]); + return *this; +} + + +SIMD_FORCE_INLINE btScalar +btMatrix3x3::determinant() const +{ + return btTriple((*this)[0], (*this)[1], (*this)[2]); +} + + +SIMD_FORCE_INLINE btMatrix3x3 +btMatrix3x3::absolute() const +{ + return btMatrix3x3( + btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()), + btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()), + btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z())); +} + +SIMD_FORCE_INLINE btMatrix3x3 +btMatrix3x3::transpose() const +{ + return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), + m_el[0].y(), m_el[1].y(), m_el[2].y(), + m_el[0].z(), m_el[1].z(), m_el[2].z()); +} + +SIMD_FORCE_INLINE btMatrix3x3 +btMatrix3x3::adjoint() const +{ + return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2), + cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0), + cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1)); +} + +SIMD_FORCE_INLINE btMatrix3x3 +btMatrix3x3::inverse() const +{ + btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); + btScalar det = (*this)[0].dot(co); + btFullAssert(det != btScalar(0.0)); + btScalar s = btScalar(1.0) / det; + return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, + co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, + co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); +} + +SIMD_FORCE_INLINE btMatrix3x3 +btMatrix3x3::transposeTimes(const btMatrix3x3& m) const +{ + return btMatrix3x3( + m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), + m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), + m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), + m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), + m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), + m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), + m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), + m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), + m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); +} + +SIMD_FORCE_INLINE btMatrix3x3 +btMatrix3x3::timesTranspose(const btMatrix3x3& m) const +{ + return btMatrix3x3( + m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]), + m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]), + m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2])); + +} + +SIMD_FORCE_INLINE btVector3 +operator*(const btMatrix3x3& m, const btVector3& v) +{ + return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); +} + + +SIMD_FORCE_INLINE btVector3 +operator*(const btVector3& v, const btMatrix3x3& m) +{ + return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); +} + +SIMD_FORCE_INLINE btMatrix3x3 +operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) +{ + return btMatrix3x3( + m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]), + m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]), + m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2])); +} /* - SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) { - return btMatrix3x3( - m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], - m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1], - m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2], - m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], - m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], - m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], - m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], - m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], - m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); +SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) { +return btMatrix3x3( +m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], +m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1], +m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2], +m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], +m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], +m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], +m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], +m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], +m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); } */ /**@brief Equality operator between two matrices - * It will test all elements are equal. */ +* It will test all elements are equal. */ SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2) { - return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && - m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && - m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); + return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && + m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && + m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); +} + +///for serialization +struct btMatrix3x3FloatData +{ + btVector3FloatData m_el[3]; +}; + +///for serialization +struct btMatrix3x3DoubleData +{ + btVector3DoubleData m_el[3]; +}; + + + + +SIMD_FORCE_INLINE void btMatrix3x3::serialize(struct btMatrix3x3Data& dataOut) const +{ + for (int i=0;i<3;i++) + m_el[i].serialize(dataOut.m_el[i]); } -#endif +SIMD_FORCE_INLINE void btMatrix3x3::serializeFloat(struct btMatrix3x3FloatData& dataOut) const +{ + for (int i=0;i<3;i++) + m_el[i].serializeFloat(dataOut.m_el[i]); +} + + +SIMD_FORCE_INLINE void btMatrix3x3::deSerialize(const struct btMatrix3x3Data& dataIn) +{ + for (int i=0;i<3;i++) + m_el[i].deSerialize(dataIn.m_el[i]); +} + +SIMD_FORCE_INLINE void btMatrix3x3::deSerializeFloat(const struct btMatrix3x3FloatData& dataIn) +{ + for (int i=0;i<3;i++) + m_el[i].deSerializeFloat(dataIn.m_el[i]); +} + +SIMD_FORCE_INLINE void btMatrix3x3::deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn) +{ + for (int i=0;i<3;i++) + m_el[i].deSerializeDouble(dataIn.m_el[i]); +} + +#endif //BT_MATRIX3x3_H + diff --git a/extern/bullet2/src/LinearMath/btMinMax.h b/extern/bullet2/src/LinearMath/btMinMax.h index 5e27d62a4a4..80601c1e232 100644 --- a/extern/bullet2/src/LinearMath/btMinMax.h +++ b/extern/bullet2/src/LinearMath/btMinMax.h @@ -17,6 +17,8 @@ subject to the following restrictions: #ifndef GEN_MINMAX_H #define GEN_MINMAX_H +#include "LinearMath/btScalar.h" + template <class T> SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) { @@ -30,7 +32,7 @@ SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b) } template <class T> -SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub) +SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub) { return a < lb ? lb : (ub < a ? ub : a); } @@ -54,7 +56,7 @@ SIMD_FORCE_INLINE void btSetMax(T& a, const T& b) } template <class T> -SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub) +SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub) { if (a < lb) { diff --git a/extern/bullet2/src/LinearMath/btPoolAllocator.h b/extern/bullet2/src/LinearMath/btPoolAllocator.h index 39d2559c747..e3b4be9b155 100644 --- a/extern/bullet2/src/LinearMath/btPoolAllocator.h +++ b/extern/bullet2/src/LinearMath/btPoolAllocator.h @@ -57,6 +57,11 @@ public: return m_freeCount; } + int getUsedCount() const + { + return m_maxElements - m_freeCount; + } + void* allocate(int size) { // release mode fix @@ -96,6 +101,15 @@ public: return m_elemSize; } + unsigned char* getPoolAddress() + { + return m_pool; + } + + const unsigned char* getPoolAddress() const + { + return m_pool; + } }; diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet2/src/LinearMath/btQuaternion.h index cbeca2681cc..15cf5f868d9 100644 --- a/extern/bullet2/src/LinearMath/btQuaternion.h +++ b/extern/bullet2/src/LinearMath/btQuaternion.h @@ -209,8 +209,17 @@ public: return s; } + /**@brief Return the axis of the rotation represented by this quaternion */ + btVector3 getAxis() const + { + btScalar s_squared = btScalar(1.) - btPow(m_floats[3], btScalar(2.)); + if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero + return btVector3(1.0, 0.0, 0.0); // Arbitrary + btScalar s = btSqrt(s_squared); + return btVector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s); + } - /**@brief Return the inverse of this quaternion */ + /**@brief Return the inverse of this quaternion */ btQuaternion inverse() const { return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); @@ -252,6 +261,18 @@ public: return (-qd); } + /**@todo document this and it's use */ + SIMD_FORCE_INLINE btQuaternion nearest( const btQuaternion& qd) const + { + btQuaternion diff,sum; + diff = *this - qd; + sum = *this + qd; + if( diff.dot(diff) < sum.dot(sum) ) + return qd; + return (-qd); + } + + /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion * @param q The other quaternion to interpolate with * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. @@ -264,10 +285,17 @@ public: btScalar d = btScalar(1.0) / btSin(theta); btScalar s0 = btSin((btScalar(1.0) - t) * theta); btScalar s1 = btSin(t * theta); - return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d, - (m_floats[1] * s0 + q.y() * s1) * d, - (m_floats[2] * s0 + q.z() * s1) * d, - (m_floats[3] * s0 + q.m_floats[3] * s1) * d); + if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp + return btQuaternion((m_floats[0] * s0 + -q.x() * s1) * d, + (m_floats[1] * s0 + -q.y() * s1) * d, + (m_floats[2] * s0 + -q.z() * s1) * d, + (m_floats[3] * s0 + -q.m_floats[3] * s1) * d); + else + return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d, + (m_floats[1] * s0 + q.y() * s1) * d, + (m_floats[2] * s0 + q.z() * s1) * d, + (m_floats[3] * s0 + q.m_floats[3] * s1) * d); + } else { @@ -378,7 +406,11 @@ shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Ge btScalar d = v0.dot(v1); if (d < -1.0 + SIMD_EPSILON) - return btQuaternion(0.0f,1.0f,0.0f,0.0f); // just pick any vector + { + btVector3 n,unused; + btPlaneSpace1(v0,n,unused); + return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0 + } btScalar s = btSqrt((1.0f + d) * 2.0f); btScalar rs = 1.0f / s; diff --git a/extern/bullet2/src/LinearMath/btQuickprof.cpp b/extern/bullet2/src/LinearMath/btQuickprof.cpp index 621d50427d6..14534068a0d 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.cpp +++ b/extern/bullet2/src/LinearMath/btQuickprof.cpp @@ -1,6 +1,6 @@ +/* - -/*************************************************************************************************** +*************************************************************************************************** ** ** profile.cpp ** @@ -13,13 +13,232 @@ // Credits: The Clock class was inspired by the Timer classes in // Ogre (www.ogre3d.org). -#include "LinearMath/btQuickprof.h" +#include "btQuickprof.h" +#ifndef BT_NO_PROFILE -#ifdef USE_BT_CLOCK static btClock gProfileClock; + +#ifdef __CELLOS_LV2__ +#include <sys/sys_time.h> +#include <sys/time_util.h> +#include <stdio.h> +#endif + +#if defined (SUNOS) || defined (__SUNOS__) +#include <stdio.h> +#endif + +#if defined(WIN32) || defined(_WIN32) + +#define BT_USE_WINDOWS_TIMERS +#define WIN32_LEAN_AND_MEAN +#define NOWINRES +#define NOMCX +#define NOIME + +#ifdef _XBOX + #include <Xtl.h> +#else //_XBOX + #include <windows.h> +#endif //_XBOX + +#include <time.h> + + +#else //_WIN32 +#include <sys/time.h> +#endif //_WIN32 + +#define mymin(a,b) (a > b ? a : b) + +struct btClockData +{ + +#ifdef BT_USE_WINDOWS_TIMERS + LARGE_INTEGER mClockFrequency; + DWORD mStartTick; + LONGLONG mPrevElapsedTime; + LARGE_INTEGER mStartTime; +#else +#ifdef __CELLOS_LV2__ + uint64_t mStartTime; +#else + struct timeval mStartTime; +#endif +#endif //__CELLOS_LV2__ + +}; + +///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling. +btClock::btClock() +{ + m_data = new btClockData; +#ifdef BT_USE_WINDOWS_TIMERS + QueryPerformanceFrequency(&m_data->mClockFrequency); +#endif + reset(); +} + +btClock::~btClock() +{ + delete m_data; +} + +btClock::btClock(const btClock& other) +{ + m_data = new btClockData; + *m_data = *other.m_data; +} + +btClock& btClock::operator=(const btClock& other) +{ + *m_data = *other.m_data; + return *this; +} + + + /// Resets the initial reference time. +void btClock::reset() +{ +#ifdef BT_USE_WINDOWS_TIMERS + QueryPerformanceCounter(&m_data->mStartTime); + m_data->mStartTick = GetTickCount(); + m_data->mPrevElapsedTime = 0; +#else +#ifdef __CELLOS_LV2__ + + typedef uint64_t ClockSize; + ClockSize newTime; + //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); + SYS_TIMEBASE_GET( newTime ); + m_data->mStartTime = newTime; +#else + gettimeofday(&m_data->mStartTime, 0); +#endif +#endif +} + +/// Returns the time in ms since the last call to reset or since +/// the btClock was created. +unsigned long int btClock::getTimeMilliseconds() +{ +#ifdef BT_USE_WINDOWS_TIMERS + LARGE_INTEGER currentTime; + QueryPerformanceCounter(¤tTime); + LONGLONG elapsedTime = currentTime.QuadPart - + m_data->mStartTime.QuadPart; + // Compute the number of millisecond ticks elapsed. + unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / + m_data->mClockFrequency.QuadPart); + // Check for unexpected leaps in the Win32 performance counter. + // (This is caused by unexpected data across the PCI to ISA + // bridge, aka south bridge. See Microsoft KB274323.) + unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick; + signed long msecOff = (signed long)(msecTicks - elapsedTicks); + if (msecOff < -100 || msecOff > 100) + { + // Adjust the starting time forwards. + LONGLONG msecAdjustment = mymin(msecOff * + m_data->mClockFrequency.QuadPart / 1000, elapsedTime - + m_data->mPrevElapsedTime); + m_data->mStartTime.QuadPart += msecAdjustment; + elapsedTime -= msecAdjustment; + + // Recompute the number of millisecond ticks elapsed. + msecTicks = (unsigned long)(1000 * elapsedTime / + m_data->mClockFrequency.QuadPart); + } + + // Store the current elapsed time for adjustments next time. + m_data->mPrevElapsedTime = elapsedTime; + + return msecTicks; +#else + +#ifdef __CELLOS_LV2__ + uint64_t freq=sys_time_get_timebase_frequency(); + double dFreq=((double) freq) / 1000.0; + typedef uint64_t ClockSize; + ClockSize newTime; + SYS_TIMEBASE_GET( newTime ); + //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); + + return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq); +#else + + struct timeval currentTime; + gettimeofday(¤tTime, 0); + return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 + + (currentTime.tv_usec - m_data->mStartTime.tv_usec) / 1000; +#endif //__CELLOS_LV2__ +#endif +} + + /// Returns the time in us since the last call to reset or since + /// the Clock was created. +unsigned long int btClock::getTimeMicroseconds() +{ +#ifdef BT_USE_WINDOWS_TIMERS + LARGE_INTEGER currentTime; + QueryPerformanceCounter(¤tTime); + LONGLONG elapsedTime = currentTime.QuadPart - + m_data->mStartTime.QuadPart; + + // Compute the number of millisecond ticks elapsed. + unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / + m_data->mClockFrequency.QuadPart); + + // Check for unexpected leaps in the Win32 performance counter. + // (This is caused by unexpected data across the PCI to ISA + // bridge, aka south bridge. See Microsoft KB274323.) + unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick; + signed long msecOff = (signed long)(msecTicks - elapsedTicks); + if (msecOff < -100 || msecOff > 100) + { + // Adjust the starting time forwards. + LONGLONG msecAdjustment = mymin(msecOff * + m_data->mClockFrequency.QuadPart / 1000, elapsedTime - + m_data->mPrevElapsedTime); + m_data->mStartTime.QuadPart += msecAdjustment; + elapsedTime -= msecAdjustment; + } + + // Store the current elapsed time for adjustments next time. + m_data->mPrevElapsedTime = elapsedTime; + + // Convert to microseconds. + unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / + m_data->mClockFrequency.QuadPart); + + return usecTicks; +#else + +#ifdef __CELLOS_LV2__ + uint64_t freq=sys_time_get_timebase_frequency(); + double dFreq=((double) freq)/ 1000000.0; + typedef uint64_t ClockSize; + ClockSize newTime; + //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); + SYS_TIMEBASE_GET( newTime ); + + return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq); +#else + + struct timeval currentTime; + gettimeofday(¤tTime, 0); + return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 + + (currentTime.tv_usec - m_data->mStartTime.tv_usec); +#endif//__CELLOS_LV2__ +#endif +} + + + + + inline void Profile_Get_Ticks(unsigned long int * ticks) { *ticks = gProfileClock.getTimeMicroseconds(); @@ -342,5 +561,5 @@ void CProfileManager::dumpAll() -#endif //USE_BT_CLOCK +#endif //BT_NO_PROFILE diff --git a/extern/bullet2/src/LinearMath/btQuickprof.h b/extern/bullet2/src/LinearMath/btQuickprof.h index f8d47c36861..adfbbe6149c 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.h +++ b/extern/bullet2/src/LinearMath/btQuickprof.h @@ -18,216 +18,42 @@ //To disable built-in profiling, please comment out next line //#define BT_NO_PROFILE 1 #ifndef BT_NO_PROFILE - +#include <stdio.h>//@todo remove this, backwards compatibility #include "btScalar.h" -#include "LinearMath/btAlignedAllocator.h" +#include "btAlignedAllocator.h" #include <new> -//if you don't need btClock, you can comment next line + #define USE_BT_CLOCK 1 #ifdef USE_BT_CLOCK -#ifdef __CELLOS_LV2__ -#include <sys/sys_time.h> -#include <sys/time_util.h> -#include <stdio.h> -#endif - -#if defined (SUNOS) || defined (__SUNOS__) -#include <stdio.h> -#endif - -#if defined(WIN32) || defined(_WIN32) - -#define USE_WINDOWS_TIMERS -#define WIN32_LEAN_AND_MEAN -#define NOWINRES -#define NOMCX -#define NOIME -#ifdef _XBOX -#include <Xtl.h> -#else -#include <windows.h> -#endif -#include <time.h> - -#else -#include <sys/time.h> -#endif - -#define mymin(a,b) (a > b ? a : b) ///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling. class btClock { public: - btClock() - { -#ifdef USE_WINDOWS_TIMERS - QueryPerformanceFrequency(&mClockFrequency); -#endif - reset(); - } - - ~btClock() - { - } + btClock(); - /// Resets the initial reference time. - void reset() - { -#ifdef USE_WINDOWS_TIMERS - QueryPerformanceCounter(&mStartTime); - mStartTick = GetTickCount(); - mPrevElapsedTime = 0; -#else -#ifdef __CELLOS_LV2__ + btClock(const btClock& other); + btClock& operator=(const btClock& other); - typedef uint64_t ClockSize; - ClockSize newTime; - //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); - SYS_TIMEBASE_GET( newTime ); - mStartTime = newTime; -#else - gettimeofday(&mStartTime, 0); -#endif + ~btClock(); -#endif - } + /// Resets the initial reference time. + void reset(); /// Returns the time in ms since the last call to reset or since /// the btClock was created. - unsigned long int getTimeMilliseconds() - { -#ifdef USE_WINDOWS_TIMERS - LARGE_INTEGER currentTime; - QueryPerformanceCounter(¤tTime); - LONGLONG elapsedTime = currentTime.QuadPart - - mStartTime.QuadPart; - - // Compute the number of millisecond ticks elapsed. - unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / - mClockFrequency.QuadPart); - - // Check for unexpected leaps in the Win32 performance counter. - // (This is caused by unexpected data across the PCI to ISA - // bridge, aka south bridge. See Microsoft KB274323.) - unsigned long elapsedTicks = GetTickCount() - mStartTick; - signed long msecOff = (signed long)(msecTicks - elapsedTicks); - if (msecOff < -100 || msecOff > 100) - { - // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - mClockFrequency.QuadPart / 1000, elapsedTime - - mPrevElapsedTime); - mStartTime.QuadPart += msecAdjustment; - elapsedTime -= msecAdjustment; - - // Recompute the number of millisecond ticks elapsed. - msecTicks = (unsigned long)(1000 * elapsedTime / - mClockFrequency.QuadPart); - } - - // Store the current elapsed time for adjustments next time. - mPrevElapsedTime = elapsedTime; - - return msecTicks; -#else - -#ifdef __CELLOS_LV2__ - uint64_t freq=sys_time_get_timebase_frequency(); - double dFreq=((double) freq) / 1000.0; - typedef uint64_t ClockSize; - ClockSize newTime; - SYS_TIMEBASE_GET( newTime ); - //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); - - return (unsigned long int)((double(newTime-mStartTime)) / dFreq); -#else - - struct timeval currentTime; - gettimeofday(¤tTime, 0); - return (currentTime.tv_sec - mStartTime.tv_sec) * 1000 + - (currentTime.tv_usec - mStartTime.tv_usec) / 1000; -#endif //__CELLOS_LV2__ -#endif - } + unsigned long int getTimeMilliseconds(); /// Returns the time in us since the last call to reset or since /// the Clock was created. - unsigned long int getTimeMicroseconds() - { -#ifdef USE_WINDOWS_TIMERS - LARGE_INTEGER currentTime; - QueryPerformanceCounter(¤tTime); - LONGLONG elapsedTime = currentTime.QuadPart - - mStartTime.QuadPart; - - // Compute the number of millisecond ticks elapsed. - unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / - mClockFrequency.QuadPart); - - // Check for unexpected leaps in the Win32 performance counter. - // (This is caused by unexpected data across the PCI to ISA - // bridge, aka south bridge. See Microsoft KB274323.) - unsigned long elapsedTicks = GetTickCount() - mStartTick; - signed long msecOff = (signed long)(msecTicks - elapsedTicks); - if (msecOff < -100 || msecOff > 100) - { - // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - mClockFrequency.QuadPart / 1000, elapsedTime - - mPrevElapsedTime); - mStartTime.QuadPart += msecAdjustment; - elapsedTime -= msecAdjustment; - } - - // Store the current elapsed time for adjustments next time. - mPrevElapsedTime = elapsedTime; - - // Convert to microseconds. - unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / - mClockFrequency.QuadPart); - - return usecTicks; -#else - -#ifdef __CELLOS_LV2__ - uint64_t freq=sys_time_get_timebase_frequency(); - double dFreq=((double) freq)/ 1000000.0; - typedef uint64_t ClockSize; - ClockSize newTime; - //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); - SYS_TIMEBASE_GET( newTime ); - - return (unsigned long int)((double(newTime-mStartTime)) / dFreq); -#else - - struct timeval currentTime; - gettimeofday(¤tTime, 0); - return (currentTime.tv_sec - mStartTime.tv_sec) * 1000000 + - (currentTime.tv_usec - mStartTime.tv_usec); -#endif//__CELLOS_LV2__ -#endif - } - + unsigned long int getTimeMicroseconds(); private: -#ifdef USE_WINDOWS_TIMERS - LARGE_INTEGER mClockFrequency; - DWORD mStartTick; - LONGLONG mPrevElapsedTime; - LARGE_INTEGER mStartTime; -#else -#ifdef __CELLOS_LV2__ - uint64_t mStartTime; -#else - struct timeval mStartTime; -#endif -#endif //__CELLOS_LV2__ - + struct btClockData* m_data; }; #endif //USE_BT_CLOCK diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index 08b2dee8af3..165626d530f 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2009 Erwin Coumans http://bullet.googlecode.com 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. @@ -17,15 +17,20 @@ subject to the following restrictions: #ifndef SIMD___SCALAR_H #define SIMD___SCALAR_H -#include <math.h> +#ifdef BT_MANAGED_CODE +//Aligned data types not supported in managed code +#pragma unmanaged +#endif -#include <stdlib.h>//size_t for MSVC 6.0 +#include <math.h> +#include <stdlib.h>//size_t for MSVC 6.0 #include <cstdlib> #include <cfloat> #include <float.h> -#define BT_BULLET_VERSION 274 +/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ +#define BT_BULLET_VERSION 278 inline int btGetVersion() { @@ -37,12 +42,13 @@ inline int btGetVersion() #endif -#ifdef WIN32 +#ifdef _WIN32 #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) #define SIMD_FORCE_INLINE inline #define ATTRIBUTE_ALIGNED16(a) a + #define ATTRIBUTE_ALIGNED64(a) a #define ATTRIBUTE_ALIGNED128(a) a #else //#define BT_HAS_ALIGNED_ALLOCATOR @@ -53,6 +59,7 @@ inline int btGetVersion() #define SIMD_FORCE_INLINE __forceinline #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a + #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a #ifdef _XBOX #define BT_USE_VMX128 @@ -62,7 +69,7 @@ inline int btGetVersion() #define btFsel(a,b,c) __fsel((a),(b),(c)) #else -#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION)) +#if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION)) #define BT_USE_SSE #include <emmintrin.h> #endif @@ -86,14 +93,22 @@ inline int btGetVersion() #else #if defined (__CELLOS_LV2__) - #define SIMD_FORCE_INLINE inline + #define SIMD_FORCE_INLINE inline __attribute__((always_inline)) #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #ifndef assert #include <assert.h> #endif #ifdef BT_DEBUG - #define btAssert assert +#ifdef __SPU__ +#include <spu_printf.h> +#define printf spu_printf + #define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}} +#else + #define btAssert assert +#endif + #else #define btAssert(x) #endif @@ -109,6 +124,7 @@ inline int btGetVersion() #define SIMD_FORCE_INLINE __inline #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #ifndef assert #include <assert.h> @@ -129,11 +145,39 @@ inline int btGetVersion() #else //non-windows systems +#if (defined (__APPLE__) && defined (__i386__) && (!defined (BT_USE_DOUBLE_PRECISION))) + #define BT_USE_SSE + #include <emmintrin.h> + + #define SIMD_FORCE_INLINE inline +///@todo: check out alignment methods for other platforms/compilers + #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #ifndef assert + #include <assert.h> + #endif + + #if defined(DEBUG) || defined (_DEBUG) + #define btAssert assert + #else + #define btAssert(x) + #endif + + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) + #define btLikely(_c) _c + #define btUnlikely(_c) _c + +#else + #define SIMD_FORCE_INLINE inline ///@todo: check out alignment methods for other platforms/compilers ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #define ATTRIBUTE_ALIGNED16(a) a + #define ATTRIBUTE_ALIGNED64(a) a #define ATTRIBUTE_ALIGNED128(a) a #ifndef assert #include <assert.h> @@ -149,21 +193,13 @@ inline int btGetVersion() #define btFullAssert(x) #define btLikely(_c) _c #define btUnlikely(_c) _c - +#endif //__APPLE__ #endif // LIBSPE2 #endif //__CELLOS_LV2__ #endif -/// older compilers (gcc 3.x) and Sun needs double version of sqrt etc. -/// exclude Apple Intel (i's assumed to be a Macbook or new Intel Dual Core Processor) -#if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__)) -//use slow double float precision operation on those platforms -#ifndef BT_USE_DOUBLE_PRECISION -#define BT_FORCE_DOUBLE_FUNCTIONS -#endif -#endif ///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision. #if defined(BT_USE_DOUBLE_PRECISION) @@ -197,13 +233,14 @@ SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); } SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); } SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); } SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); } -SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acos(x); } -SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asin(x); } +SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { if (x<btScalar(-1)) x=btScalar(-1); if (x>btScalar(1)) x=btScalar(1); return acos(x); } +SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { if (x<btScalar(-1)) x=btScalar(-1); if (x>btScalar(1)) x=btScalar(1); return asin(x); } SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); } SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); } SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); } SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); } SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); } +SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); } #else @@ -216,7 +253,7 @@ SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) tempf = y; *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ x = tempf; - z = y*btScalar(0.5); /* hoist out the “/2” */ + z = y*btScalar(0.5); x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */ x = (btScalar(1.5)*x)-(x*x)*(x*z); x = (btScalar(1.5)*x)-(x*x)*(x*z); @@ -232,15 +269,25 @@ SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); } SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); } SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); } SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { - btAssert(x <= btScalar(1.)); + if (x<btScalar(-1)) + x=btScalar(-1); + if (x>btScalar(1)) + x=btScalar(1); return acosf(x); } -SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); } +SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { + if (x<btScalar(-1)) + x=btScalar(-1); + if (x>btScalar(1)) + x=btScalar(1); + return asinf(x); +} SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); } SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); } SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); } +SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); } #endif @@ -249,6 +296,10 @@ SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); } #define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25)) #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0)) #define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI) +#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) + +#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */ + #ifdef BT_USE_DOUBLE_PRECISION #define SIMD_EPSILON DBL_EPSILON @@ -439,5 +490,35 @@ SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) return d; } +// returns normalized value in range [-SIMD_PI, SIMD_PI] +SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) +{ + angleInRadians = btFmod(angleInRadians, SIMD_2_PI); + if(angleInRadians < -SIMD_PI) + { + return angleInRadians + SIMD_2_PI; + } + else if(angleInRadians > SIMD_PI) + { + return angleInRadians - SIMD_2_PI; + } + else + { + return angleInRadians; + } +} +///rudimentary class to provide type info +struct btTypedObject +{ + btTypedObject(int objectType) + :m_objectType(objectType) + { + } + int m_objectType; + inline int getObjectType() const + { + return m_objectType; + } +}; #endif //SIMD___SCALAR_H diff --git a/extern/bullet2/src/LinearMath/btSerializer.cpp b/extern/bullet2/src/LinearMath/btSerializer.cpp new file mode 100644 index 00000000000..c6d387e6133 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btSerializer.cpp @@ -0,0 +1,832 @@ +unsigned char sBulletDNAstr[]= { +83,68,78,65,78,65,77,69,42,1,0,0,109,95,115,105,122,101,0,109, +95,99,97,112,97,99,105,116,121,0,42,109,95,100,97,116,97,0,109,95, +99,111,108,108,105,115,105,111,110,83,104,97,112,101,115,0,109,95,99,111, +108,108,105,115,105,111,110,79,98,106,101,99,116,115,0,109,95,99,111,110, +115,116,114,97,105,110,116,115,0,42,102,105,114,115,116,0,42,108,97,115, +116,0,109,95,102,108,111,97,116,115,91,52,93,0,109,95,101,108,91,51, +93,0,109,95,98,97,115,105,115,0,109,95,111,114,105,103,105,110,0,109, +95,114,111,111,116,78,111,100,101,73,110,100,101,120,0,109,95,115,117,98, +116,114,101,101,83,105,122,101,0,109,95,113,117,97,110,116,105,122,101,100, +65,97,98,98,77,105,110,91,51,93,0,109,95,113,117,97,110,116,105,122, +101,100,65,97,98,98,77,97,120,91,51,93,0,109,95,97,97,98,98,77, +105,110,79,114,103,0,109,95,97,97,98,98,77,97,120,79,114,103,0,109, +95,101,115,99,97,112,101,73,110,100,101,120,0,109,95,115,117,98,80,97, +114,116,0,109,95,116,114,105,97,110,103,108,101,73,110,100,101,120,0,109, +95,112,97,100,91,52,93,0,109,95,101,115,99,97,112,101,73,110,100,101, +120,79,114,84,114,105,97,110,103,108,101,73,110,100,101,120,0,109,95,98, +118,104,65,97,98,98,77,105,110,0,109,95,98,118,104,65,97,98,98,77, +97,120,0,109,95,98,118,104,81,117,97,110,116,105,122,97,116,105,111,110, +0,109,95,99,117,114,78,111,100,101,73,110,100,101,120,0,109,95,117,115, +101,81,117,97,110,116,105,122,97,116,105,111,110,0,109,95,110,117,109,67, +111,110,116,105,103,117,111,117,115,76,101,97,102,78,111,100,101,115,0,109, +95,110,117,109,81,117,97,110,116,105,122,101,100,67,111,110,116,105,103,117, +111,117,115,78,111,100,101,115,0,42,109,95,99,111,110,116,105,103,117,111, +117,115,78,111,100,101,115,80,116,114,0,42,109,95,113,117,97,110,116,105, +122,101,100,67,111,110,116,105,103,117,111,117,115,78,111,100,101,115,80,116, +114,0,42,109,95,115,117,98,84,114,101,101,73,110,102,111,80,116,114,0, +109,95,116,114,97,118,101,114,115,97,108,77,111,100,101,0,109,95,110,117, +109,83,117,98,116,114,101,101,72,101,97,100,101,114,115,0,42,109,95,110, +97,109,101,0,109,95,115,104,97,112,101,84,121,112,101,0,109,95,112,97, +100,100,105,110,103,91,52,93,0,109,95,99,111,108,108,105,115,105,111,110, +83,104,97,112,101,68,97,116,97,0,109,95,108,111,99,97,108,83,99,97, +108,105,110,103,0,109,95,112,108,97,110,101,78,111,114,109,97,108,0,109, +95,112,108,97,110,101,67,111,110,115,116,97,110,116,0,109,95,105,109,112, +108,105,99,105,116,83,104,97,112,101,68,105,109,101,110,115,105,111,110,115, +0,109,95,99,111,108,108,105,115,105,111,110,77,97,114,103,105,110,0,109, +95,112,97,100,100,105,110,103,0,109,95,112,111,115,0,109,95,114,97,100, +105,117,115,0,109,95,99,111,110,118,101,120,73,110,116,101,114,110,97,108, +83,104,97,112,101,68,97,116,97,0,42,109,95,108,111,99,97,108,80,111, +115,105,116,105,111,110,65,114,114,97,121,80,116,114,0,109,95,108,111,99, +97,108,80,111,115,105,116,105,111,110,65,114,114,97,121,83,105,122,101,0, +109,95,118,97,108,117,101,0,109,95,112,97,100,91,50,93,0,109,95,118, +97,108,117,101,115,91,51,93,0,109,95,112,97,100,0,42,109,95,118,101, +114,116,105,99,101,115,51,102,0,42,109,95,118,101,114,116,105,99,101,115, +51,100,0,42,109,95,105,110,100,105,99,101,115,51,50,0,42,109,95,51, +105,110,100,105,99,101,115,49,54,0,42,109,95,51,105,110,100,105,99,101, +115,56,0,42,109,95,105,110,100,105,99,101,115,49,54,0,109,95,110,117, +109,84,114,105,97,110,103,108,101,115,0,109,95,110,117,109,86,101,114,116, +105,99,101,115,0,42,109,95,109,101,115,104,80,97,114,116,115,80,116,114, +0,109,95,115,99,97,108,105,110,103,0,109,95,110,117,109,77,101,115,104, +80,97,114,116,115,0,109,95,109,101,115,104,73,110,116,101,114,102,97,99, +101,0,42,109,95,113,117,97,110,116,105,122,101,100,70,108,111,97,116,66, +118,104,0,42,109,95,113,117,97,110,116,105,122,101,100,68,111,117,98,108, +101,66,118,104,0,42,109,95,116,114,105,97,110,103,108,101,73,110,102,111, +77,97,112,0,109,95,112,97,100,51,91,52,93,0,109,95,116,114,105,109, +101,115,104,83,104,97,112,101,68,97,116,97,0,109,95,116,114,97,110,115, +102,111,114,109,0,42,109,95,99,104,105,108,100,83,104,97,112,101,0,109, +95,99,104,105,108,100,83,104,97,112,101,84,121,112,101,0,109,95,99,104, +105,108,100,77,97,114,103,105,110,0,42,109,95,99,104,105,108,100,83,104, +97,112,101,80,116,114,0,109,95,110,117,109,67,104,105,108,100,83,104,97, +112,101,115,0,109,95,117,112,65,120,105,115,0,109,95,102,108,97,103,115, +0,109,95,101,100,103,101,86,48,86,49,65,110,103,108,101,0,109,95,101, +100,103,101,86,49,86,50,65,110,103,108,101,0,109,95,101,100,103,101,86, +50,86,48,65,110,103,108,101,0,42,109,95,104,97,115,104,84,97,98,108, +101,80,116,114,0,42,109,95,110,101,120,116,80,116,114,0,42,109,95,118, +97,108,117,101,65,114,114,97,121,80,116,114,0,42,109,95,107,101,121,65, +114,114,97,121,80,116,114,0,109,95,99,111,110,118,101,120,69,112,115,105, +108,111,110,0,109,95,112,108,97,110,97,114,69,112,115,105,108,111,110,0, +109,95,101,113,117,97,108,86,101,114,116,101,120,84,104,114,101,115,104,111, +108,100,0,109,95,101,100,103,101,68,105,115,116,97,110,99,101,84,104,114, +101,115,104,111,108,100,0,109,95,122,101,114,111,65,114,101,97,84,104,114, +101,115,104,111,108,100,0,109,95,110,101,120,116,83,105,122,101,0,109,95, +104,97,115,104,84,97,98,108,101,83,105,122,101,0,109,95,110,117,109,86, +97,108,117,101,115,0,109,95,110,117,109,75,101,121,115,0,109,95,103,105, +109,112,97,99,116,83,117,98,84,121,112,101,0,42,109,95,117,110,115,99, +97,108,101,100,80,111,105,110,116,115,70,108,111,97,116,80,116,114,0,42, +109,95,117,110,115,99,97,108,101,100,80,111,105,110,116,115,68,111,117,98, +108,101,80,116,114,0,109,95,110,117,109,85,110,115,99,97,108,101,100,80, +111,105,110,116,115,0,109,95,112,97,100,100,105,110,103,51,91,52,93,0, +42,109,95,98,114,111,97,100,112,104,97,115,101,72,97,110,100,108,101,0, +42,109,95,99,111,108,108,105,115,105,111,110,83,104,97,112,101,0,42,109, +95,114,111,111,116,67,111,108,108,105,115,105,111,110,83,104,97,112,101,0, +109,95,119,111,114,108,100,84,114,97,110,115,102,111,114,109,0,109,95,105, +110,116,101,114,112,111,108,97,116,105,111,110,87,111,114,108,100,84,114,97, +110,115,102,111,114,109,0,109,95,105,110,116,101,114,112,111,108,97,116,105, +111,110,76,105,110,101,97,114,86,101,108,111,99,105,116,121,0,109,95,105, +110,116,101,114,112,111,108,97,116,105,111,110,65,110,103,117,108,97,114,86, +101,108,111,99,105,116,121,0,109,95,97,110,105,115,111,116,114,111,112,105, +99,70,114,105,99,116,105,111,110,0,109,95,99,111,110,116,97,99,116,80, +114,111,99,101,115,115,105,110,103,84,104,114,101,115,104,111,108,100,0,109, +95,100,101,97,99,116,105,118,97,116,105,111,110,84,105,109,101,0,109,95, +102,114,105,99,116,105,111,110,0,109,95,114,101,115,116,105,116,117,116,105, +111,110,0,109,95,104,105,116,70,114,97,99,116,105,111,110,0,109,95,99, +99,100,83,119,101,112,116,83,112,104,101,114,101,82,97,100,105,117,115,0, +109,95,99,99,100,77,111,116,105,111,110,84,104,114,101,115,104,111,108,100, +0,109,95,104,97,115,65,110,105,115,111,116,114,111,112,105,99,70,114,105, +99,116,105,111,110,0,109,95,99,111,108,108,105,115,105,111,110,70,108,97, +103,115,0,109,95,105,115,108,97,110,100,84,97,103,49,0,109,95,99,111, +109,112,97,110,105,111,110,73,100,0,109,95,97,99,116,105,118,97,116,105, +111,110,83,116,97,116,101,49,0,109,95,105,110,116,101,114,110,97,108,84, +121,112,101,0,109,95,99,104,101,99,107,67,111,108,108,105,100,101,87,105, +116,104,0,109,95,99,111,108,108,105,115,105,111,110,79,98,106,101,99,116, +68,97,116,97,0,109,95,105,110,118,73,110,101,114,116,105,97,84,101,110, +115,111,114,87,111,114,108,100,0,109,95,108,105,110,101,97,114,86,101,108, +111,99,105,116,121,0,109,95,97,110,103,117,108,97,114,86,101,108,111,99, +105,116,121,0,109,95,97,110,103,117,108,97,114,70,97,99,116,111,114,0, +109,95,108,105,110,101,97,114,70,97,99,116,111,114,0,109,95,103,114,97, +118,105,116,121,0,109,95,103,114,97,118,105,116,121,95,97,99,99,101,108, +101,114,97,116,105,111,110,0,109,95,105,110,118,73,110,101,114,116,105,97, +76,111,99,97,108,0,109,95,116,111,116,97,108,70,111,114,99,101,0,109, +95,116,111,116,97,108,84,111,114,113,117,101,0,109,95,105,110,118,101,114, +115,101,77,97,115,115,0,109,95,108,105,110,101,97,114,68,97,109,112,105, +110,103,0,109,95,97,110,103,117,108,97,114,68,97,109,112,105,110,103,0, +109,95,97,100,100,105,116,105,111,110,97,108,68,97,109,112,105,110,103,70, +97,99,116,111,114,0,109,95,97,100,100,105,116,105,111,110,97,108,76,105, +110,101,97,114,68,97,109,112,105,110,103,84,104,114,101,115,104,111,108,100, +83,113,114,0,109,95,97,100,100,105,116,105,111,110,97,108,65,110,103,117, +108,97,114,68,97,109,112,105,110,103,84,104,114,101,115,104,111,108,100,83, +113,114,0,109,95,97,100,100,105,116,105,111,110,97,108,65,110,103,117,108, +97,114,68,97,109,112,105,110,103,70,97,99,116,111,114,0,109,95,108,105, +110,101,97,114,83,108,101,101,112,105,110,103,84,104,114,101,115,104,111,108, +100,0,109,95,97,110,103,117,108,97,114,83,108,101,101,112,105,110,103,84, +104,114,101,115,104,111,108,100,0,109,95,97,100,100,105,116,105,111,110,97, +108,68,97,109,112,105,110,103,0,109,95,110,117,109,67,111,110,115,116,114, +97,105,110,116,82,111,119,115,0,110,117,98,0,42,109,95,114,98,65,0, +42,109,95,114,98,66,0,109,95,111,98,106,101,99,116,84,121,112,101,0, +109,95,117,115,101,114,67,111,110,115,116,114,97,105,110,116,84,121,112,101, +0,109,95,117,115,101,114,67,111,110,115,116,114,97,105,110,116,73,100,0, +109,95,110,101,101,100,115,70,101,101,100,98,97,99,107,0,109,95,97,112, +112,108,105,101,100,73,109,112,117,108,115,101,0,109,95,100,98,103,68,114, +97,119,83,105,122,101,0,109,95,100,105,115,97,98,108,101,67,111,108,108, +105,115,105,111,110,115,66,101,116,119,101,101,110,76,105,110,107,101,100,66, +111,100,105,101,115,0,109,95,112,97,100,52,91,52,93,0,109,95,116,121, +112,101,67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,109,95,112, +105,118,111,116,73,110,65,0,109,95,112,105,118,111,116,73,110,66,0,109, +95,114,98,65,70,114,97,109,101,0,109,95,114,98,66,70,114,97,109,101, +0,109,95,117,115,101,82,101,102,101,114,101,110,99,101,70,114,97,109,101, +65,0,109,95,97,110,103,117,108,97,114,79,110,108,121,0,109,95,101,110, +97,98,108,101,65,110,103,117,108,97,114,77,111,116,111,114,0,109,95,109, +111,116,111,114,84,97,114,103,101,116,86,101,108,111,99,105,116,121,0,109, +95,109,97,120,77,111,116,111,114,73,109,112,117,108,115,101,0,109,95,108, +111,119,101,114,76,105,109,105,116,0,109,95,117,112,112,101,114,76,105,109, +105,116,0,109,95,108,105,109,105,116,83,111,102,116,110,101,115,115,0,109, +95,98,105,97,115,70,97,99,116,111,114,0,109,95,114,101,108,97,120,97, +116,105,111,110,70,97,99,116,111,114,0,109,95,115,119,105,110,103,83,112, +97,110,49,0,109,95,115,119,105,110,103,83,112,97,110,50,0,109,95,116, +119,105,115,116,83,112,97,110,0,109,95,100,97,109,112,105,110,103,0,109, +95,108,105,110,101,97,114,85,112,112,101,114,76,105,109,105,116,0,109,95, +108,105,110,101,97,114,76,111,119,101,114,76,105,109,105,116,0,109,95,97, +110,103,117,108,97,114,85,112,112,101,114,76,105,109,105,116,0,109,95,97, +110,103,117,108,97,114,76,111,119,101,114,76,105,109,105,116,0,109,95,117, +115,101,76,105,110,101,97,114,82,101,102,101,114,101,110,99,101,70,114,97, +109,101,65,0,109,95,117,115,101,79,102,102,115,101,116,70,111,114,67,111, +110,115,116,114,97,105,110,116,70,114,97,109,101,0,109,95,54,100,111,102, +68,97,116,97,0,109,95,115,112,114,105,110,103,69,110,97,98,108,101,100, +91,54,93,0,109,95,101,113,117,105,108,105,98,114,105,117,109,80,111,105, +110,116,91,54,93,0,109,95,115,112,114,105,110,103,83,116,105,102,102,110, +101,115,115,91,54,93,0,109,95,115,112,114,105,110,103,68,97,109,112,105, +110,103,91,54,93,0,109,95,108,105,110,101,97,114,83,116,105,102,102,110, +101,115,115,0,109,95,97,110,103,117,108,97,114,83,116,105,102,102,110,101, +115,115,0,109,95,118,111,108,117,109,101,83,116,105,102,102,110,101,115,115, +0,42,109,95,109,97,116,101,114,105,97,108,0,109,95,112,111,115,105,116, +105,111,110,0,109,95,112,114,101,118,105,111,117,115,80,111,115,105,116,105, +111,110,0,109,95,118,101,108,111,99,105,116,121,0,109,95,97,99,99,117, +109,117,108,97,116,101,100,70,111,114,99,101,0,109,95,110,111,114,109,97, +108,0,109,95,97,114,101,97,0,109,95,97,116,116,97,99,104,0,109,95, +110,111,100,101,73,110,100,105,99,101,115,91,50,93,0,109,95,114,101,115, +116,76,101,110,103,116,104,0,109,95,98,98,101,110,100,105,110,103,0,109, +95,110,111,100,101,73,110,100,105,99,101,115,91,51,93,0,109,95,114,101, +115,116,65,114,101,97,0,109,95,99,48,91,52,93,0,109,95,110,111,100, +101,73,110,100,105,99,101,115,91,52,93,0,109,95,114,101,115,116,86,111, +108,117,109,101,0,109,95,99,49,0,109,95,99,50,0,109,95,99,48,0, +109,95,108,111,99,97,108,70,114,97,109,101,0,42,109,95,114,105,103,105, +100,66,111,100,121,0,109,95,110,111,100,101,73,110,100,101,120,0,109,95, +97,101,114,111,77,111,100,101,108,0,109,95,98,97,117,109,103,97,114,116, +101,0,109,95,100,114,97,103,0,109,95,108,105,102,116,0,109,95,112,114, +101,115,115,117,114,101,0,109,95,118,111,108,117,109,101,0,109,95,100,121, +110,97,109,105,99,70,114,105,99,116,105,111,110,0,109,95,112,111,115,101, +77,97,116,99,104,0,109,95,114,105,103,105,100,67,111,110,116,97,99,116, +72,97,114,100,110,101,115,115,0,109,95,107,105,110,101,116,105,99,67,111, +110,116,97,99,116,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116, +67,111,110,116,97,99,116,72,97,114,100,110,101,115,115,0,109,95,97,110, +99,104,111,114,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116,82, +105,103,105,100,67,108,117,115,116,101,114,72,97,114,100,110,101,115,115,0, +109,95,115,111,102,116,75,105,110,101,116,105,99,67,108,117,115,116,101,114, +72,97,114,100,110,101,115,115,0,109,95,115,111,102,116,83,111,102,116,67, +108,117,115,116,101,114,72,97,114,100,110,101,115,115,0,109,95,115,111,102, +116,82,105,103,105,100,67,108,117,115,116,101,114,73,109,112,117,108,115,101, +83,112,108,105,116,0,109,95,115,111,102,116,75,105,110,101,116,105,99,67, +108,117,115,116,101,114,73,109,112,117,108,115,101,83,112,108,105,116,0,109, +95,115,111,102,116,83,111,102,116,67,108,117,115,116,101,114,73,109,112,117, +108,115,101,83,112,108,105,116,0,109,95,109,97,120,86,111,108,117,109,101, +0,109,95,116,105,109,101,83,99,97,108,101,0,109,95,118,101,108,111,99, +105,116,121,73,116,101,114,97,116,105,111,110,115,0,109,95,112,111,115,105, +116,105,111,110,73,116,101,114,97,116,105,111,110,115,0,109,95,100,114,105, +102,116,73,116,101,114,97,116,105,111,110,115,0,109,95,99,108,117,115,116, +101,114,73,116,101,114,97,116,105,111,110,115,0,109,95,114,111,116,0,109, +95,115,99,97,108,101,0,109,95,97,113,113,0,109,95,99,111,109,0,42, +109,95,112,111,115,105,116,105,111,110,115,0,42,109,95,119,101,105,103,104, +116,115,0,109,95,110,117,109,80,111,115,105,116,105,111,110,115,0,109,95, +110,117,109,87,101,105,103,116,115,0,109,95,98,118,111,108,117,109,101,0, +109,95,98,102,114,97,109,101,0,109,95,102,114,97,109,101,120,102,111,114, +109,0,109,95,108,111,99,105,105,0,109,95,105,110,118,119,105,0,109,95, +118,105,109,112,117,108,115,101,115,91,50,93,0,109,95,100,105,109,112,117, +108,115,101,115,91,50,93,0,109,95,108,118,0,109,95,97,118,0,42,109, +95,102,114,97,109,101,114,101,102,115,0,42,109,95,110,111,100,101,73,110, +100,105,99,101,115,0,42,109,95,109,97,115,115,101,115,0,109,95,110,117, +109,70,114,97,109,101,82,101,102,115,0,109,95,110,117,109,78,111,100,101, +115,0,109,95,110,117,109,77,97,115,115,101,115,0,109,95,105,100,109,97, +115,115,0,109,95,105,109,97,115,115,0,109,95,110,118,105,109,112,117,108, +115,101,115,0,109,95,110,100,105,109,112,117,108,115,101,115,0,109,95,110, +100,97,109,112,105,110,103,0,109,95,108,100,97,109,112,105,110,103,0,109, +95,97,100,97,109,112,105,110,103,0,109,95,109,97,116,99,104,105,110,103, +0,109,95,109,97,120,83,101,108,102,67,111,108,108,105,115,105,111,110,73, +109,112,117,108,115,101,0,109,95,115,101,108,102,67,111,108,108,105,115,105, +111,110,73,109,112,117,108,115,101,70,97,99,116,111,114,0,109,95,99,111, +110,116,97,105,110,115,65,110,99,104,111,114,0,109,95,99,111,108,108,105, +100,101,0,109,95,99,108,117,115,116,101,114,73,110,100,101,120,0,42,109, +95,98,111,100,121,65,0,42,109,95,98,111,100,121,66,0,109,95,114,101, +102,115,91,50,93,0,109,95,99,102,109,0,109,95,101,114,112,0,109,95, +115,112,108,105,116,0,109,95,100,101,108,101,116,101,0,109,95,114,101,108, +80,111,115,105,116,105,111,110,91,50,93,0,109,95,98,111,100,121,65,116, +121,112,101,0,109,95,98,111,100,121,66,116,121,112,101,0,109,95,106,111, +105,110,116,84,121,112,101,0,42,109,95,112,111,115,101,0,42,42,109,95, +109,97,116,101,114,105,97,108,115,0,42,109,95,110,111,100,101,115,0,42, +109,95,108,105,110,107,115,0,42,109,95,102,97,99,101,115,0,42,109,95, +116,101,116,114,97,104,101,100,114,97,0,42,109,95,97,110,99,104,111,114, +115,0,42,109,95,99,108,117,115,116,101,114,115,0,42,109,95,106,111,105, +110,116,115,0,109,95,110,117,109,77,97,116,101,114,105,97,108,115,0,109, +95,110,117,109,76,105,110,107,115,0,109,95,110,117,109,70,97,99,101,115, +0,109,95,110,117,109,84,101,116,114,97,104,101,100,114,97,0,109,95,110, +117,109,65,110,99,104,111,114,115,0,109,95,110,117,109,67,108,117,115,116, +101,114,115,0,109,95,110,117,109,74,111,105,110,116,115,0,109,95,99,111, +110,102,105,103,0,0,0,0,84,89,80,69,72,0,0,0,99,104,97,114, +0,117,99,104,97,114,0,115,104,111,114,116,0,117,115,104,111,114,116,0, +105,110,116,0,108,111,110,103,0,117,108,111,110,103,0,102,108,111,97,116, +0,100,111,117,98,108,101,0,118,111,105,100,0,80,111,105,110,116,101,114, +65,114,114,97,121,0,98,116,80,104,121,115,105,99,115,83,121,115,116,101, +109,0,76,105,115,116,66,97,115,101,0,98,116,86,101,99,116,111,114,51, +70,108,111,97,116,68,97,116,97,0,98,116,86,101,99,116,111,114,51,68, +111,117,98,108,101,68,97,116,97,0,98,116,77,97,116,114,105,120,51,120, +51,70,108,111,97,116,68,97,116,97,0,98,116,77,97,116,114,105,120,51, +120,51,68,111,117,98,108,101,68,97,116,97,0,98,116,84,114,97,110,115, +102,111,114,109,70,108,111,97,116,68,97,116,97,0,98,116,84,114,97,110, +115,102,111,114,109,68,111,117,98,108,101,68,97,116,97,0,98,116,66,118, +104,83,117,98,116,114,101,101,73,110,102,111,68,97,116,97,0,98,116,79, +112,116,105,109,105,122,101,100,66,118,104,78,111,100,101,70,108,111,97,116, +68,97,116,97,0,98,116,79,112,116,105,109,105,122,101,100,66,118,104,78, +111,100,101,68,111,117,98,108,101,68,97,116,97,0,98,116,81,117,97,110, +116,105,122,101,100,66,118,104,78,111,100,101,68,97,116,97,0,98,116,81, +117,97,110,116,105,122,101,100,66,118,104,70,108,111,97,116,68,97,116,97, +0,98,116,81,117,97,110,116,105,122,101,100,66,118,104,68,111,117,98,108, +101,68,97,116,97,0,98,116,67,111,108,108,105,115,105,111,110,83,104,97, +112,101,68,97,116,97,0,98,116,83,116,97,116,105,99,80,108,97,110,101, +83,104,97,112,101,68,97,116,97,0,98,116,67,111,110,118,101,120,73,110, +116,101,114,110,97,108,83,104,97,112,101,68,97,116,97,0,98,116,80,111, +115,105,116,105,111,110,65,110,100,82,97,100,105,117,115,0,98,116,77,117, +108,116,105,83,112,104,101,114,101,83,104,97,112,101,68,97,116,97,0,98, +116,73,110,116,73,110,100,101,120,68,97,116,97,0,98,116,83,104,111,114, +116,73,110,116,73,110,100,101,120,68,97,116,97,0,98,116,83,104,111,114, +116,73,110,116,73,110,100,101,120,84,114,105,112,108,101,116,68,97,116,97, +0,98,116,67,104,97,114,73,110,100,101,120,84,114,105,112,108,101,116,68, +97,116,97,0,98,116,77,101,115,104,80,97,114,116,68,97,116,97,0,98, +116,83,116,114,105,100,105,110,103,77,101,115,104,73,110,116,101,114,102,97, +99,101,68,97,116,97,0,98,116,84,114,105,97,110,103,108,101,77,101,115, +104,83,104,97,112,101,68,97,116,97,0,98,116,84,114,105,97,110,103,108, +101,73,110,102,111,77,97,112,68,97,116,97,0,98,116,83,99,97,108,101, +100,84,114,105,97,110,103,108,101,77,101,115,104,83,104,97,112,101,68,97, +116,97,0,98,116,67,111,109,112,111,117,110,100,83,104,97,112,101,67,104, +105,108,100,68,97,116,97,0,98,116,67,111,109,112,111,117,110,100,83,104, +97,112,101,68,97,116,97,0,98,116,67,121,108,105,110,100,101,114,83,104, +97,112,101,68,97,116,97,0,98,116,67,97,112,115,117,108,101,83,104,97, +112,101,68,97,116,97,0,98,116,84,114,105,97,110,103,108,101,73,110,102, +111,68,97,116,97,0,98,116,71,73,109,112,97,99,116,77,101,115,104,83, +104,97,112,101,68,97,116,97,0,98,116,67,111,110,118,101,120,72,117,108, +108,83,104,97,112,101,68,97,116,97,0,98,116,67,111,108,108,105,115,105, +111,110,79,98,106,101,99,116,68,111,117,98,108,101,68,97,116,97,0,98, +116,67,111,108,108,105,115,105,111,110,79,98,106,101,99,116,70,108,111,97, +116,68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121,70,108,111, +97,116,68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121,68,111, +117,98,108,101,68,97,116,97,0,98,116,67,111,110,115,116,114,97,105,110, +116,73,110,102,111,49,0,98,116,84,121,112,101,100,67,111,110,115,116,114, +97,105,110,116,68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121, +68,97,116,97,0,98,116,80,111,105,110,116,50,80,111,105,110,116,67,111, +110,115,116,114,97,105,110,116,70,108,111,97,116,68,97,116,97,0,98,116, +80,111,105,110,116,50,80,111,105,110,116,67,111,110,115,116,114,97,105,110, +116,68,111,117,98,108,101,68,97,116,97,0,98,116,72,105,110,103,101,67, +111,110,115,116,114,97,105,110,116,68,111,117,98,108,101,68,97,116,97,0, +98,116,72,105,110,103,101,67,111,110,115,116,114,97,105,110,116,70,108,111, +97,116,68,97,116,97,0,98,116,67,111,110,101,84,119,105,115,116,67,111, +110,115,116,114,97,105,110,116,68,97,116,97,0,98,116,71,101,110,101,114, +105,99,54,68,111,102,67,111,110,115,116,114,97,105,110,116,68,97,116,97, +0,98,116,71,101,110,101,114,105,99,54,68,111,102,83,112,114,105,110,103, +67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,98,116,83,108,105, +100,101,114,67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,83,111, +102,116,66,111,100,121,77,97,116,101,114,105,97,108,68,97,116,97,0,83, +111,102,116,66,111,100,121,78,111,100,101,68,97,116,97,0,83,111,102,116, +66,111,100,121,76,105,110,107,68,97,116,97,0,83,111,102,116,66,111,100, +121,70,97,99,101,68,97,116,97,0,83,111,102,116,66,111,100,121,84,101, +116,114,97,68,97,116,97,0,83,111,102,116,82,105,103,105,100,65,110,99, +104,111,114,68,97,116,97,0,83,111,102,116,66,111,100,121,67,111,110,102, +105,103,68,97,116,97,0,83,111,102,116,66,111,100,121,80,111,115,101,68, +97,116,97,0,83,111,102,116,66,111,100,121,67,108,117,115,116,101,114,68, +97,116,97,0,98,116,83,111,102,116,66,111,100,121,74,111,105,110,116,68, +97,116,97,0,98,116,83,111,102,116,66,111,100,121,70,108,111,97,116,68, +97,116,97,0,84,76,69,78,1,0,1,0,2,0,2,0,4,0,4,0, +4,0,4,0,8,0,0,0,12,0,36,0,8,0,16,0,32,0,48,0, +96,0,64,0,-128,0,20,0,48,0,80,0,16,0,84,0,-124,0,12,0, +52,0,52,0,20,0,64,0,4,0,4,0,8,0,4,0,32,0,28,0, +60,0,56,0,76,0,76,0,24,0,60,0,60,0,16,0,64,0,68,0, +-56,1,-8,0,-32,1,-104,3,8,0,44,0,0,0,76,0,108,0,84,1, +-44,0,-52,0,-12,0,84,1,-60,0,16,0,100,0,20,0,36,0,100,0, +92,0,104,0,-64,0,92,1,104,0,-92,1,83,84,82,67,61,0,0,0, +10,0,3,0,4,0,0,0,4,0,1,0,9,0,2,0,11,0,3,0, +10,0,3,0,10,0,4,0,10,0,5,0,12,0,2,0,9,0,6,0, +9,0,7,0,13,0,1,0,7,0,8,0,14,0,1,0,8,0,8,0, +15,0,1,0,13,0,9,0,16,0,1,0,14,0,9,0,17,0,2,0, +15,0,10,0,13,0,11,0,18,0,2,0,16,0,10,0,14,0,11,0, +19,0,4,0,4,0,12,0,4,0,13,0,2,0,14,0,2,0,15,0, +20,0,6,0,13,0,16,0,13,0,17,0,4,0,18,0,4,0,19,0, +4,0,20,0,0,0,21,0,21,0,6,0,14,0,16,0,14,0,17,0, +4,0,18,0,4,0,19,0,4,0,20,0,0,0,21,0,22,0,3,0, +2,0,14,0,2,0,15,0,4,0,22,0,23,0,12,0,13,0,23,0, +13,0,24,0,13,0,25,0,4,0,26,0,4,0,27,0,4,0,28,0, +4,0,29,0,20,0,30,0,22,0,31,0,19,0,32,0,4,0,33,0, +4,0,34,0,24,0,12,0,14,0,23,0,14,0,24,0,14,0,25,0, +4,0,26,0,4,0,27,0,4,0,28,0,4,0,29,0,21,0,30,0, +22,0,31,0,4,0,33,0,4,0,34,0,19,0,32,0,25,0,3,0, +0,0,35,0,4,0,36,0,0,0,37,0,26,0,5,0,25,0,38,0, +13,0,39,0,13,0,40,0,7,0,41,0,0,0,21,0,27,0,5,0, +25,0,38,0,13,0,39,0,13,0,42,0,7,0,43,0,4,0,44,0, +28,0,2,0,13,0,45,0,7,0,46,0,29,0,4,0,27,0,47,0, +28,0,48,0,4,0,49,0,0,0,37,0,30,0,1,0,4,0,50,0, +31,0,2,0,2,0,50,0,0,0,51,0,32,0,2,0,2,0,52,0, +0,0,51,0,33,0,2,0,0,0,52,0,0,0,53,0,34,0,8,0, +13,0,54,0,14,0,55,0,30,0,56,0,32,0,57,0,33,0,58,0, +31,0,59,0,4,0,60,0,4,0,61,0,35,0,4,0,34,0,62,0, +13,0,63,0,4,0,64,0,0,0,37,0,36,0,7,0,25,0,38,0, +35,0,65,0,23,0,66,0,24,0,67,0,37,0,68,0,7,0,43,0, +0,0,69,0,38,0,2,0,36,0,70,0,13,0,39,0,39,0,4,0, +17,0,71,0,25,0,72,0,4,0,73,0,7,0,74,0,40,0,4,0, +25,0,38,0,39,0,75,0,4,0,76,0,7,0,43,0,41,0,3,0, +27,0,47,0,4,0,77,0,0,0,37,0,42,0,3,0,27,0,47,0, +4,0,77,0,0,0,37,0,43,0,4,0,4,0,78,0,7,0,79,0, +7,0,80,0,7,0,81,0,37,0,14,0,4,0,82,0,4,0,83,0, +43,0,84,0,4,0,85,0,7,0,86,0,7,0,87,0,7,0,88,0, +7,0,89,0,7,0,90,0,4,0,91,0,4,0,92,0,4,0,93,0, +4,0,94,0,0,0,37,0,44,0,5,0,25,0,38,0,35,0,65,0, +13,0,39,0,7,0,43,0,4,0,95,0,45,0,5,0,27,0,47,0, +13,0,96,0,14,0,97,0,4,0,98,0,0,0,99,0,46,0,24,0, +9,0,100,0,9,0,101,0,25,0,102,0,0,0,35,0,18,0,103,0, +18,0,104,0,14,0,105,0,14,0,106,0,14,0,107,0,8,0,108,0, +8,0,109,0,8,0,110,0,8,0,111,0,8,0,112,0,8,0,113,0, +8,0,114,0,4,0,115,0,4,0,116,0,4,0,117,0,4,0,118,0, +4,0,119,0,4,0,120,0,4,0,121,0,0,0,37,0,47,0,23,0, +9,0,100,0,9,0,101,0,25,0,102,0,0,0,35,0,17,0,103,0, +17,0,104,0,13,0,105,0,13,0,106,0,13,0,107,0,7,0,108,0, +7,0,109,0,7,0,110,0,7,0,111,0,7,0,112,0,7,0,113,0, +7,0,114,0,4,0,115,0,4,0,116,0,4,0,117,0,4,0,118,0, +4,0,119,0,4,0,120,0,4,0,121,0,48,0,21,0,47,0,122,0, +15,0,123,0,13,0,124,0,13,0,125,0,13,0,126,0,13,0,127,0, +13,0,-128,0,13,0,-127,0,13,0,-126,0,13,0,-125,0,13,0,-124,0, +7,0,-123,0,7,0,-122,0,7,0,-121,0,7,0,-120,0,7,0,-119,0, +7,0,-118,0,7,0,-117,0,7,0,-116,0,7,0,-115,0,4,0,-114,0, +49,0,22,0,46,0,122,0,16,0,123,0,14,0,124,0,14,0,125,0, +14,0,126,0,14,0,127,0,14,0,-128,0,14,0,-127,0,14,0,-126,0, +14,0,-125,0,14,0,-124,0,8,0,-123,0,8,0,-122,0,8,0,-121,0, +8,0,-120,0,8,0,-119,0,8,0,-118,0,8,0,-117,0,8,0,-116,0, +8,0,-115,0,4,0,-114,0,0,0,37,0,50,0,2,0,4,0,-113,0, +4,0,-112,0,51,0,11,0,52,0,-111,0,52,0,-110,0,0,0,35,0, +4,0,-109,0,4,0,-108,0,4,0,-107,0,4,0,-106,0,7,0,-105,0, +7,0,-104,0,4,0,-103,0,0,0,-102,0,53,0,3,0,51,0,-101,0, +13,0,-100,0,13,0,-99,0,54,0,3,0,51,0,-101,0,14,0,-100,0, +14,0,-99,0,55,0,13,0,51,0,-101,0,18,0,-98,0,18,0,-97,0, +4,0,-96,0,4,0,-95,0,4,0,-94,0,7,0,-93,0,7,0,-92,0, +7,0,-91,0,7,0,-90,0,7,0,-89,0,7,0,-88,0,7,0,-87,0, +56,0,13,0,51,0,-101,0,17,0,-98,0,17,0,-97,0,4,0,-96,0, +4,0,-95,0,4,0,-94,0,7,0,-93,0,7,0,-92,0,7,0,-91,0, +7,0,-90,0,7,0,-89,0,7,0,-88,0,7,0,-87,0,57,0,11,0, +51,0,-101,0,17,0,-98,0,17,0,-97,0,7,0,-86,0,7,0,-85,0, +7,0,-84,0,7,0,-89,0,7,0,-88,0,7,0,-87,0,7,0,-83,0, +0,0,21,0,58,0,9,0,51,0,-101,0,17,0,-98,0,17,0,-97,0, +13,0,-82,0,13,0,-81,0,13,0,-80,0,13,0,-79,0,4,0,-78,0, +4,0,-77,0,59,0,5,0,58,0,-76,0,4,0,-75,0,7,0,-74,0, +7,0,-73,0,7,0,-72,0,60,0,9,0,51,0,-101,0,17,0,-98,0, +17,0,-97,0,7,0,-82,0,7,0,-81,0,7,0,-80,0,7,0,-79,0, +4,0,-78,0,4,0,-77,0,61,0,4,0,7,0,-71,0,7,0,-70,0, +7,0,-69,0,4,0,78,0,62,0,10,0,61,0,-68,0,13,0,-67,0, +13,0,-66,0,13,0,-65,0,13,0,-64,0,13,0,-63,0,7,0,-123,0, +7,0,-62,0,4,0,-61,0,4,0,53,0,63,0,4,0,61,0,-68,0, +4,0,-60,0,7,0,-59,0,4,0,-58,0,64,0,4,0,13,0,-63,0, +61,0,-68,0,4,0,-57,0,7,0,-56,0,65,0,7,0,13,0,-55,0, +61,0,-68,0,4,0,-54,0,7,0,-53,0,7,0,-52,0,7,0,-51,0, +4,0,53,0,66,0,6,0,15,0,-50,0,13,0,-52,0,13,0,-49,0, +52,0,-48,0,4,0,-47,0,7,0,-51,0,67,0,26,0,4,0,-46,0, +7,0,-45,0,7,0,-83,0,7,0,-44,0,7,0,-43,0,7,0,-42,0, +7,0,-41,0,7,0,-40,0,7,0,-39,0,7,0,-38,0,7,0,-37,0, +7,0,-36,0,7,0,-35,0,7,0,-34,0,7,0,-33,0,7,0,-32,0, +7,0,-31,0,7,0,-30,0,7,0,-29,0,7,0,-28,0,7,0,-27,0, +4,0,-26,0,4,0,-25,0,4,0,-24,0,4,0,-23,0,4,0,116,0, +68,0,12,0,15,0,-22,0,15,0,-21,0,15,0,-20,0,13,0,-19,0, +13,0,-18,0,7,0,-17,0,4,0,-16,0,4,0,-15,0,4,0,-14,0, +4,0,-13,0,7,0,-53,0,4,0,53,0,69,0,27,0,17,0,-12,0, +15,0,-11,0,15,0,-10,0,13,0,-19,0,13,0,-9,0,13,0,-8,0, +13,0,-7,0,13,0,-6,0,13,0,-5,0,4,0,-4,0,7,0,-3,0, +4,0,-2,0,4,0,-1,0,4,0,0,1,7,0,1,1,7,0,2,1, +4,0,3,1,4,0,4,1,7,0,5,1,7,0,6,1,7,0,7,1, +7,0,8,1,7,0,9,1,7,0,10,1,4,0,11,1,4,0,12,1, +4,0,13,1,70,0,12,0,9,0,14,1,9,0,15,1,13,0,16,1, +7,0,17,1,7,0,18,1,7,0,19,1,4,0,20,1,13,0,21,1, +4,0,22,1,4,0,23,1,4,0,24,1,4,0,53,0,71,0,19,0, +47,0,122,0,68,0,25,1,61,0,26,1,62,0,27,1,63,0,28,1, +64,0,29,1,65,0,30,1,66,0,31,1,69,0,32,1,70,0,33,1, +4,0,34,1,4,0,-1,0,4,0,35,1,4,0,36,1,4,0,37,1, +4,0,38,1,4,0,39,1,4,0,40,1,67,0,41,1,}; +int sBulletDNAlen= sizeof(sBulletDNAstr); +unsigned char sBulletDNAstr64[]= { +83,68,78,65,78,65,77,69,42,1,0,0,109,95,115,105,122,101,0,109, +95,99,97,112,97,99,105,116,121,0,42,109,95,100,97,116,97,0,109,95, +99,111,108,108,105,115,105,111,110,83,104,97,112,101,115,0,109,95,99,111, +108,108,105,115,105,111,110,79,98,106,101,99,116,115,0,109,95,99,111,110, +115,116,114,97,105,110,116,115,0,42,102,105,114,115,116,0,42,108,97,115, +116,0,109,95,102,108,111,97,116,115,91,52,93,0,109,95,101,108,91,51, +93,0,109,95,98,97,115,105,115,0,109,95,111,114,105,103,105,110,0,109, +95,114,111,111,116,78,111,100,101,73,110,100,101,120,0,109,95,115,117,98, +116,114,101,101,83,105,122,101,0,109,95,113,117,97,110,116,105,122,101,100, +65,97,98,98,77,105,110,91,51,93,0,109,95,113,117,97,110,116,105,122, +101,100,65,97,98,98,77,97,120,91,51,93,0,109,95,97,97,98,98,77, +105,110,79,114,103,0,109,95,97,97,98,98,77,97,120,79,114,103,0,109, +95,101,115,99,97,112,101,73,110,100,101,120,0,109,95,115,117,98,80,97, +114,116,0,109,95,116,114,105,97,110,103,108,101,73,110,100,101,120,0,109, +95,112,97,100,91,52,93,0,109,95,101,115,99,97,112,101,73,110,100,101, +120,79,114,84,114,105,97,110,103,108,101,73,110,100,101,120,0,109,95,98, +118,104,65,97,98,98,77,105,110,0,109,95,98,118,104,65,97,98,98,77, +97,120,0,109,95,98,118,104,81,117,97,110,116,105,122,97,116,105,111,110, +0,109,95,99,117,114,78,111,100,101,73,110,100,101,120,0,109,95,117,115, +101,81,117,97,110,116,105,122,97,116,105,111,110,0,109,95,110,117,109,67, +111,110,116,105,103,117,111,117,115,76,101,97,102,78,111,100,101,115,0,109, +95,110,117,109,81,117,97,110,116,105,122,101,100,67,111,110,116,105,103,117, +111,117,115,78,111,100,101,115,0,42,109,95,99,111,110,116,105,103,117,111, +117,115,78,111,100,101,115,80,116,114,0,42,109,95,113,117,97,110,116,105, +122,101,100,67,111,110,116,105,103,117,111,117,115,78,111,100,101,115,80,116, +114,0,42,109,95,115,117,98,84,114,101,101,73,110,102,111,80,116,114,0, +109,95,116,114,97,118,101,114,115,97,108,77,111,100,101,0,109,95,110,117, +109,83,117,98,116,114,101,101,72,101,97,100,101,114,115,0,42,109,95,110, +97,109,101,0,109,95,115,104,97,112,101,84,121,112,101,0,109,95,112,97, +100,100,105,110,103,91,52,93,0,109,95,99,111,108,108,105,115,105,111,110, +83,104,97,112,101,68,97,116,97,0,109,95,108,111,99,97,108,83,99,97, +108,105,110,103,0,109,95,112,108,97,110,101,78,111,114,109,97,108,0,109, +95,112,108,97,110,101,67,111,110,115,116,97,110,116,0,109,95,105,109,112, +108,105,99,105,116,83,104,97,112,101,68,105,109,101,110,115,105,111,110,115, +0,109,95,99,111,108,108,105,115,105,111,110,77,97,114,103,105,110,0,109, +95,112,97,100,100,105,110,103,0,109,95,112,111,115,0,109,95,114,97,100, +105,117,115,0,109,95,99,111,110,118,101,120,73,110,116,101,114,110,97,108, +83,104,97,112,101,68,97,116,97,0,42,109,95,108,111,99,97,108,80,111, +115,105,116,105,111,110,65,114,114,97,121,80,116,114,0,109,95,108,111,99, +97,108,80,111,115,105,116,105,111,110,65,114,114,97,121,83,105,122,101,0, +109,95,118,97,108,117,101,0,109,95,112,97,100,91,50,93,0,109,95,118, +97,108,117,101,115,91,51,93,0,109,95,112,97,100,0,42,109,95,118,101, +114,116,105,99,101,115,51,102,0,42,109,95,118,101,114,116,105,99,101,115, +51,100,0,42,109,95,105,110,100,105,99,101,115,51,50,0,42,109,95,51, +105,110,100,105,99,101,115,49,54,0,42,109,95,51,105,110,100,105,99,101, +115,56,0,42,109,95,105,110,100,105,99,101,115,49,54,0,109,95,110,117, +109,84,114,105,97,110,103,108,101,115,0,109,95,110,117,109,86,101,114,116, +105,99,101,115,0,42,109,95,109,101,115,104,80,97,114,116,115,80,116,114, +0,109,95,115,99,97,108,105,110,103,0,109,95,110,117,109,77,101,115,104, +80,97,114,116,115,0,109,95,109,101,115,104,73,110,116,101,114,102,97,99, +101,0,42,109,95,113,117,97,110,116,105,122,101,100,70,108,111,97,116,66, +118,104,0,42,109,95,113,117,97,110,116,105,122,101,100,68,111,117,98,108, +101,66,118,104,0,42,109,95,116,114,105,97,110,103,108,101,73,110,102,111, +77,97,112,0,109,95,112,97,100,51,91,52,93,0,109,95,116,114,105,109, +101,115,104,83,104,97,112,101,68,97,116,97,0,109,95,116,114,97,110,115, +102,111,114,109,0,42,109,95,99,104,105,108,100,83,104,97,112,101,0,109, +95,99,104,105,108,100,83,104,97,112,101,84,121,112,101,0,109,95,99,104, +105,108,100,77,97,114,103,105,110,0,42,109,95,99,104,105,108,100,83,104, +97,112,101,80,116,114,0,109,95,110,117,109,67,104,105,108,100,83,104,97, +112,101,115,0,109,95,117,112,65,120,105,115,0,109,95,102,108,97,103,115, +0,109,95,101,100,103,101,86,48,86,49,65,110,103,108,101,0,109,95,101, +100,103,101,86,49,86,50,65,110,103,108,101,0,109,95,101,100,103,101,86, +50,86,48,65,110,103,108,101,0,42,109,95,104,97,115,104,84,97,98,108, +101,80,116,114,0,42,109,95,110,101,120,116,80,116,114,0,42,109,95,118, +97,108,117,101,65,114,114,97,121,80,116,114,0,42,109,95,107,101,121,65, +114,114,97,121,80,116,114,0,109,95,99,111,110,118,101,120,69,112,115,105, +108,111,110,0,109,95,112,108,97,110,97,114,69,112,115,105,108,111,110,0, +109,95,101,113,117,97,108,86,101,114,116,101,120,84,104,114,101,115,104,111, +108,100,0,109,95,101,100,103,101,68,105,115,116,97,110,99,101,84,104,114, +101,115,104,111,108,100,0,109,95,122,101,114,111,65,114,101,97,84,104,114, +101,115,104,111,108,100,0,109,95,110,101,120,116,83,105,122,101,0,109,95, +104,97,115,104,84,97,98,108,101,83,105,122,101,0,109,95,110,117,109,86, +97,108,117,101,115,0,109,95,110,117,109,75,101,121,115,0,109,95,103,105, +109,112,97,99,116,83,117,98,84,121,112,101,0,42,109,95,117,110,115,99, +97,108,101,100,80,111,105,110,116,115,70,108,111,97,116,80,116,114,0,42, +109,95,117,110,115,99,97,108,101,100,80,111,105,110,116,115,68,111,117,98, +108,101,80,116,114,0,109,95,110,117,109,85,110,115,99,97,108,101,100,80, +111,105,110,116,115,0,109,95,112,97,100,100,105,110,103,51,91,52,93,0, +42,109,95,98,114,111,97,100,112,104,97,115,101,72,97,110,100,108,101,0, +42,109,95,99,111,108,108,105,115,105,111,110,83,104,97,112,101,0,42,109, +95,114,111,111,116,67,111,108,108,105,115,105,111,110,83,104,97,112,101,0, +109,95,119,111,114,108,100,84,114,97,110,115,102,111,114,109,0,109,95,105, +110,116,101,114,112,111,108,97,116,105,111,110,87,111,114,108,100,84,114,97, +110,115,102,111,114,109,0,109,95,105,110,116,101,114,112,111,108,97,116,105, +111,110,76,105,110,101,97,114,86,101,108,111,99,105,116,121,0,109,95,105, +110,116,101,114,112,111,108,97,116,105,111,110,65,110,103,117,108,97,114,86, +101,108,111,99,105,116,121,0,109,95,97,110,105,115,111,116,114,111,112,105, +99,70,114,105,99,116,105,111,110,0,109,95,99,111,110,116,97,99,116,80, +114,111,99,101,115,115,105,110,103,84,104,114,101,115,104,111,108,100,0,109, +95,100,101,97,99,116,105,118,97,116,105,111,110,84,105,109,101,0,109,95, +102,114,105,99,116,105,111,110,0,109,95,114,101,115,116,105,116,117,116,105, +111,110,0,109,95,104,105,116,70,114,97,99,116,105,111,110,0,109,95,99, +99,100,83,119,101,112,116,83,112,104,101,114,101,82,97,100,105,117,115,0, +109,95,99,99,100,77,111,116,105,111,110,84,104,114,101,115,104,111,108,100, +0,109,95,104,97,115,65,110,105,115,111,116,114,111,112,105,99,70,114,105, +99,116,105,111,110,0,109,95,99,111,108,108,105,115,105,111,110,70,108,97, +103,115,0,109,95,105,115,108,97,110,100,84,97,103,49,0,109,95,99,111, +109,112,97,110,105,111,110,73,100,0,109,95,97,99,116,105,118,97,116,105, +111,110,83,116,97,116,101,49,0,109,95,105,110,116,101,114,110,97,108,84, +121,112,101,0,109,95,99,104,101,99,107,67,111,108,108,105,100,101,87,105, +116,104,0,109,95,99,111,108,108,105,115,105,111,110,79,98,106,101,99,116, +68,97,116,97,0,109,95,105,110,118,73,110,101,114,116,105,97,84,101,110, +115,111,114,87,111,114,108,100,0,109,95,108,105,110,101,97,114,86,101,108, +111,99,105,116,121,0,109,95,97,110,103,117,108,97,114,86,101,108,111,99, +105,116,121,0,109,95,97,110,103,117,108,97,114,70,97,99,116,111,114,0, +109,95,108,105,110,101,97,114,70,97,99,116,111,114,0,109,95,103,114,97, +118,105,116,121,0,109,95,103,114,97,118,105,116,121,95,97,99,99,101,108, +101,114,97,116,105,111,110,0,109,95,105,110,118,73,110,101,114,116,105,97, +76,111,99,97,108,0,109,95,116,111,116,97,108,70,111,114,99,101,0,109, +95,116,111,116,97,108,84,111,114,113,117,101,0,109,95,105,110,118,101,114, +115,101,77,97,115,115,0,109,95,108,105,110,101,97,114,68,97,109,112,105, +110,103,0,109,95,97,110,103,117,108,97,114,68,97,109,112,105,110,103,0, +109,95,97,100,100,105,116,105,111,110,97,108,68,97,109,112,105,110,103,70, +97,99,116,111,114,0,109,95,97,100,100,105,116,105,111,110,97,108,76,105, +110,101,97,114,68,97,109,112,105,110,103,84,104,114,101,115,104,111,108,100, +83,113,114,0,109,95,97,100,100,105,116,105,111,110,97,108,65,110,103,117, +108,97,114,68,97,109,112,105,110,103,84,104,114,101,115,104,111,108,100,83, +113,114,0,109,95,97,100,100,105,116,105,111,110,97,108,65,110,103,117,108, +97,114,68,97,109,112,105,110,103,70,97,99,116,111,114,0,109,95,108,105, +110,101,97,114,83,108,101,101,112,105,110,103,84,104,114,101,115,104,111,108, +100,0,109,95,97,110,103,117,108,97,114,83,108,101,101,112,105,110,103,84, +104,114,101,115,104,111,108,100,0,109,95,97,100,100,105,116,105,111,110,97, +108,68,97,109,112,105,110,103,0,109,95,110,117,109,67,111,110,115,116,114, +97,105,110,116,82,111,119,115,0,110,117,98,0,42,109,95,114,98,65,0, +42,109,95,114,98,66,0,109,95,111,98,106,101,99,116,84,121,112,101,0, +109,95,117,115,101,114,67,111,110,115,116,114,97,105,110,116,84,121,112,101, +0,109,95,117,115,101,114,67,111,110,115,116,114,97,105,110,116,73,100,0, +109,95,110,101,101,100,115,70,101,101,100,98,97,99,107,0,109,95,97,112, +112,108,105,101,100,73,109,112,117,108,115,101,0,109,95,100,98,103,68,114, +97,119,83,105,122,101,0,109,95,100,105,115,97,98,108,101,67,111,108,108, +105,115,105,111,110,115,66,101,116,119,101,101,110,76,105,110,107,101,100,66, +111,100,105,101,115,0,109,95,112,97,100,52,91,52,93,0,109,95,116,121, +112,101,67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,109,95,112, +105,118,111,116,73,110,65,0,109,95,112,105,118,111,116,73,110,66,0,109, +95,114,98,65,70,114,97,109,101,0,109,95,114,98,66,70,114,97,109,101, +0,109,95,117,115,101,82,101,102,101,114,101,110,99,101,70,114,97,109,101, +65,0,109,95,97,110,103,117,108,97,114,79,110,108,121,0,109,95,101,110, +97,98,108,101,65,110,103,117,108,97,114,77,111,116,111,114,0,109,95,109, +111,116,111,114,84,97,114,103,101,116,86,101,108,111,99,105,116,121,0,109, +95,109,97,120,77,111,116,111,114,73,109,112,117,108,115,101,0,109,95,108, +111,119,101,114,76,105,109,105,116,0,109,95,117,112,112,101,114,76,105,109, +105,116,0,109,95,108,105,109,105,116,83,111,102,116,110,101,115,115,0,109, +95,98,105,97,115,70,97,99,116,111,114,0,109,95,114,101,108,97,120,97, +116,105,111,110,70,97,99,116,111,114,0,109,95,115,119,105,110,103,83,112, +97,110,49,0,109,95,115,119,105,110,103,83,112,97,110,50,0,109,95,116, +119,105,115,116,83,112,97,110,0,109,95,100,97,109,112,105,110,103,0,109, +95,108,105,110,101,97,114,85,112,112,101,114,76,105,109,105,116,0,109,95, +108,105,110,101,97,114,76,111,119,101,114,76,105,109,105,116,0,109,95,97, +110,103,117,108,97,114,85,112,112,101,114,76,105,109,105,116,0,109,95,97, +110,103,117,108,97,114,76,111,119,101,114,76,105,109,105,116,0,109,95,117, +115,101,76,105,110,101,97,114,82,101,102,101,114,101,110,99,101,70,114,97, +109,101,65,0,109,95,117,115,101,79,102,102,115,101,116,70,111,114,67,111, +110,115,116,114,97,105,110,116,70,114,97,109,101,0,109,95,54,100,111,102, +68,97,116,97,0,109,95,115,112,114,105,110,103,69,110,97,98,108,101,100, +91,54,93,0,109,95,101,113,117,105,108,105,98,114,105,117,109,80,111,105, +110,116,91,54,93,0,109,95,115,112,114,105,110,103,83,116,105,102,102,110, +101,115,115,91,54,93,0,109,95,115,112,114,105,110,103,68,97,109,112,105, +110,103,91,54,93,0,109,95,108,105,110,101,97,114,83,116,105,102,102,110, +101,115,115,0,109,95,97,110,103,117,108,97,114,83,116,105,102,102,110,101, +115,115,0,109,95,118,111,108,117,109,101,83,116,105,102,102,110,101,115,115, +0,42,109,95,109,97,116,101,114,105,97,108,0,109,95,112,111,115,105,116, +105,111,110,0,109,95,112,114,101,118,105,111,117,115,80,111,115,105,116,105, +111,110,0,109,95,118,101,108,111,99,105,116,121,0,109,95,97,99,99,117, +109,117,108,97,116,101,100,70,111,114,99,101,0,109,95,110,111,114,109,97, +108,0,109,95,97,114,101,97,0,109,95,97,116,116,97,99,104,0,109,95, +110,111,100,101,73,110,100,105,99,101,115,91,50,93,0,109,95,114,101,115, +116,76,101,110,103,116,104,0,109,95,98,98,101,110,100,105,110,103,0,109, +95,110,111,100,101,73,110,100,105,99,101,115,91,51,93,0,109,95,114,101, +115,116,65,114,101,97,0,109,95,99,48,91,52,93,0,109,95,110,111,100, +101,73,110,100,105,99,101,115,91,52,93,0,109,95,114,101,115,116,86,111, +108,117,109,101,0,109,95,99,49,0,109,95,99,50,0,109,95,99,48,0, +109,95,108,111,99,97,108,70,114,97,109,101,0,42,109,95,114,105,103,105, +100,66,111,100,121,0,109,95,110,111,100,101,73,110,100,101,120,0,109,95, +97,101,114,111,77,111,100,101,108,0,109,95,98,97,117,109,103,97,114,116, +101,0,109,95,100,114,97,103,0,109,95,108,105,102,116,0,109,95,112,114, +101,115,115,117,114,101,0,109,95,118,111,108,117,109,101,0,109,95,100,121, +110,97,109,105,99,70,114,105,99,116,105,111,110,0,109,95,112,111,115,101, +77,97,116,99,104,0,109,95,114,105,103,105,100,67,111,110,116,97,99,116, +72,97,114,100,110,101,115,115,0,109,95,107,105,110,101,116,105,99,67,111, +110,116,97,99,116,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116, +67,111,110,116,97,99,116,72,97,114,100,110,101,115,115,0,109,95,97,110, +99,104,111,114,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116,82, +105,103,105,100,67,108,117,115,116,101,114,72,97,114,100,110,101,115,115,0, +109,95,115,111,102,116,75,105,110,101,116,105,99,67,108,117,115,116,101,114, +72,97,114,100,110,101,115,115,0,109,95,115,111,102,116,83,111,102,116,67, +108,117,115,116,101,114,72,97,114,100,110,101,115,115,0,109,95,115,111,102, +116,82,105,103,105,100,67,108,117,115,116,101,114,73,109,112,117,108,115,101, +83,112,108,105,116,0,109,95,115,111,102,116,75,105,110,101,116,105,99,67, +108,117,115,116,101,114,73,109,112,117,108,115,101,83,112,108,105,116,0,109, +95,115,111,102,116,83,111,102,116,67,108,117,115,116,101,114,73,109,112,117, +108,115,101,83,112,108,105,116,0,109,95,109,97,120,86,111,108,117,109,101, +0,109,95,116,105,109,101,83,99,97,108,101,0,109,95,118,101,108,111,99, +105,116,121,73,116,101,114,97,116,105,111,110,115,0,109,95,112,111,115,105, +116,105,111,110,73,116,101,114,97,116,105,111,110,115,0,109,95,100,114,105, +102,116,73,116,101,114,97,116,105,111,110,115,0,109,95,99,108,117,115,116, +101,114,73,116,101,114,97,116,105,111,110,115,0,109,95,114,111,116,0,109, +95,115,99,97,108,101,0,109,95,97,113,113,0,109,95,99,111,109,0,42, +109,95,112,111,115,105,116,105,111,110,115,0,42,109,95,119,101,105,103,104, +116,115,0,109,95,110,117,109,80,111,115,105,116,105,111,110,115,0,109,95, +110,117,109,87,101,105,103,116,115,0,109,95,98,118,111,108,117,109,101,0, +109,95,98,102,114,97,109,101,0,109,95,102,114,97,109,101,120,102,111,114, +109,0,109,95,108,111,99,105,105,0,109,95,105,110,118,119,105,0,109,95, +118,105,109,112,117,108,115,101,115,91,50,93,0,109,95,100,105,109,112,117, +108,115,101,115,91,50,93,0,109,95,108,118,0,109,95,97,118,0,42,109, +95,102,114,97,109,101,114,101,102,115,0,42,109,95,110,111,100,101,73,110, +100,105,99,101,115,0,42,109,95,109,97,115,115,101,115,0,109,95,110,117, +109,70,114,97,109,101,82,101,102,115,0,109,95,110,117,109,78,111,100,101, +115,0,109,95,110,117,109,77,97,115,115,101,115,0,109,95,105,100,109,97, +115,115,0,109,95,105,109,97,115,115,0,109,95,110,118,105,109,112,117,108, +115,101,115,0,109,95,110,100,105,109,112,117,108,115,101,115,0,109,95,110, +100,97,109,112,105,110,103,0,109,95,108,100,97,109,112,105,110,103,0,109, +95,97,100,97,109,112,105,110,103,0,109,95,109,97,116,99,104,105,110,103, +0,109,95,109,97,120,83,101,108,102,67,111,108,108,105,115,105,111,110,73, +109,112,117,108,115,101,0,109,95,115,101,108,102,67,111,108,108,105,115,105, +111,110,73,109,112,117,108,115,101,70,97,99,116,111,114,0,109,95,99,111, +110,116,97,105,110,115,65,110,99,104,111,114,0,109,95,99,111,108,108,105, +100,101,0,109,95,99,108,117,115,116,101,114,73,110,100,101,120,0,42,109, +95,98,111,100,121,65,0,42,109,95,98,111,100,121,66,0,109,95,114,101, +102,115,91,50,93,0,109,95,99,102,109,0,109,95,101,114,112,0,109,95, +115,112,108,105,116,0,109,95,100,101,108,101,116,101,0,109,95,114,101,108, +80,111,115,105,116,105,111,110,91,50,93,0,109,95,98,111,100,121,65,116, +121,112,101,0,109,95,98,111,100,121,66,116,121,112,101,0,109,95,106,111, +105,110,116,84,121,112,101,0,42,109,95,112,111,115,101,0,42,42,109,95, +109,97,116,101,114,105,97,108,115,0,42,109,95,110,111,100,101,115,0,42, +109,95,108,105,110,107,115,0,42,109,95,102,97,99,101,115,0,42,109,95, +116,101,116,114,97,104,101,100,114,97,0,42,109,95,97,110,99,104,111,114, +115,0,42,109,95,99,108,117,115,116,101,114,115,0,42,109,95,106,111,105, +110,116,115,0,109,95,110,117,109,77,97,116,101,114,105,97,108,115,0,109, +95,110,117,109,76,105,110,107,115,0,109,95,110,117,109,70,97,99,101,115, +0,109,95,110,117,109,84,101,116,114,97,104,101,100,114,97,0,109,95,110, +117,109,65,110,99,104,111,114,115,0,109,95,110,117,109,67,108,117,115,116, +101,114,115,0,109,95,110,117,109,74,111,105,110,116,115,0,109,95,99,111, +110,102,105,103,0,0,0,0,84,89,80,69,72,0,0,0,99,104,97,114, +0,117,99,104,97,114,0,115,104,111,114,116,0,117,115,104,111,114,116,0, +105,110,116,0,108,111,110,103,0,117,108,111,110,103,0,102,108,111,97,116, +0,100,111,117,98,108,101,0,118,111,105,100,0,80,111,105,110,116,101,114, +65,114,114,97,121,0,98,116,80,104,121,115,105,99,115,83,121,115,116,101, +109,0,76,105,115,116,66,97,115,101,0,98,116,86,101,99,116,111,114,51, +70,108,111,97,116,68,97,116,97,0,98,116,86,101,99,116,111,114,51,68, +111,117,98,108,101,68,97,116,97,0,98,116,77,97,116,114,105,120,51,120, +51,70,108,111,97,116,68,97,116,97,0,98,116,77,97,116,114,105,120,51, +120,51,68,111,117,98,108,101,68,97,116,97,0,98,116,84,114,97,110,115, +102,111,114,109,70,108,111,97,116,68,97,116,97,0,98,116,84,114,97,110, +115,102,111,114,109,68,111,117,98,108,101,68,97,116,97,0,98,116,66,118, +104,83,117,98,116,114,101,101,73,110,102,111,68,97,116,97,0,98,116,79, +112,116,105,109,105,122,101,100,66,118,104,78,111,100,101,70,108,111,97,116, +68,97,116,97,0,98,116,79,112,116,105,109,105,122,101,100,66,118,104,78, +111,100,101,68,111,117,98,108,101,68,97,116,97,0,98,116,81,117,97,110, +116,105,122,101,100,66,118,104,78,111,100,101,68,97,116,97,0,98,116,81, +117,97,110,116,105,122,101,100,66,118,104,70,108,111,97,116,68,97,116,97, +0,98,116,81,117,97,110,116,105,122,101,100,66,118,104,68,111,117,98,108, +101,68,97,116,97,0,98,116,67,111,108,108,105,115,105,111,110,83,104,97, +112,101,68,97,116,97,0,98,116,83,116,97,116,105,99,80,108,97,110,101, +83,104,97,112,101,68,97,116,97,0,98,116,67,111,110,118,101,120,73,110, +116,101,114,110,97,108,83,104,97,112,101,68,97,116,97,0,98,116,80,111, +115,105,116,105,111,110,65,110,100,82,97,100,105,117,115,0,98,116,77,117, +108,116,105,83,112,104,101,114,101,83,104,97,112,101,68,97,116,97,0,98, +116,73,110,116,73,110,100,101,120,68,97,116,97,0,98,116,83,104,111,114, +116,73,110,116,73,110,100,101,120,68,97,116,97,0,98,116,83,104,111,114, +116,73,110,116,73,110,100,101,120,84,114,105,112,108,101,116,68,97,116,97, +0,98,116,67,104,97,114,73,110,100,101,120,84,114,105,112,108,101,116,68, +97,116,97,0,98,116,77,101,115,104,80,97,114,116,68,97,116,97,0,98, +116,83,116,114,105,100,105,110,103,77,101,115,104,73,110,116,101,114,102,97, +99,101,68,97,116,97,0,98,116,84,114,105,97,110,103,108,101,77,101,115, +104,83,104,97,112,101,68,97,116,97,0,98,116,84,114,105,97,110,103,108, +101,73,110,102,111,77,97,112,68,97,116,97,0,98,116,83,99,97,108,101, +100,84,114,105,97,110,103,108,101,77,101,115,104,83,104,97,112,101,68,97, +116,97,0,98,116,67,111,109,112,111,117,110,100,83,104,97,112,101,67,104, +105,108,100,68,97,116,97,0,98,116,67,111,109,112,111,117,110,100,83,104, +97,112,101,68,97,116,97,0,98,116,67,121,108,105,110,100,101,114,83,104, +97,112,101,68,97,116,97,0,98,116,67,97,112,115,117,108,101,83,104,97, +112,101,68,97,116,97,0,98,116,84,114,105,97,110,103,108,101,73,110,102, +111,68,97,116,97,0,98,116,71,73,109,112,97,99,116,77,101,115,104,83, +104,97,112,101,68,97,116,97,0,98,116,67,111,110,118,101,120,72,117,108, +108,83,104,97,112,101,68,97,116,97,0,98,116,67,111,108,108,105,115,105, +111,110,79,98,106,101,99,116,68,111,117,98,108,101,68,97,116,97,0,98, +116,67,111,108,108,105,115,105,111,110,79,98,106,101,99,116,70,108,111,97, +116,68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121,70,108,111, +97,116,68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121,68,111, +117,98,108,101,68,97,116,97,0,98,116,67,111,110,115,116,114,97,105,110, +116,73,110,102,111,49,0,98,116,84,121,112,101,100,67,111,110,115,116,114, +97,105,110,116,68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121, +68,97,116,97,0,98,116,80,111,105,110,116,50,80,111,105,110,116,67,111, +110,115,116,114,97,105,110,116,70,108,111,97,116,68,97,116,97,0,98,116, +80,111,105,110,116,50,80,111,105,110,116,67,111,110,115,116,114,97,105,110, +116,68,111,117,98,108,101,68,97,116,97,0,98,116,72,105,110,103,101,67, +111,110,115,116,114,97,105,110,116,68,111,117,98,108,101,68,97,116,97,0, +98,116,72,105,110,103,101,67,111,110,115,116,114,97,105,110,116,70,108,111, +97,116,68,97,116,97,0,98,116,67,111,110,101,84,119,105,115,116,67,111, +110,115,116,114,97,105,110,116,68,97,116,97,0,98,116,71,101,110,101,114, +105,99,54,68,111,102,67,111,110,115,116,114,97,105,110,116,68,97,116,97, +0,98,116,71,101,110,101,114,105,99,54,68,111,102,83,112,114,105,110,103, +67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,98,116,83,108,105, +100,101,114,67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,83,111, +102,116,66,111,100,121,77,97,116,101,114,105,97,108,68,97,116,97,0,83, +111,102,116,66,111,100,121,78,111,100,101,68,97,116,97,0,83,111,102,116, +66,111,100,121,76,105,110,107,68,97,116,97,0,83,111,102,116,66,111,100, +121,70,97,99,101,68,97,116,97,0,83,111,102,116,66,111,100,121,84,101, +116,114,97,68,97,116,97,0,83,111,102,116,82,105,103,105,100,65,110,99, +104,111,114,68,97,116,97,0,83,111,102,116,66,111,100,121,67,111,110,102, +105,103,68,97,116,97,0,83,111,102,116,66,111,100,121,80,111,115,101,68, +97,116,97,0,83,111,102,116,66,111,100,121,67,108,117,115,116,101,114,68, +97,116,97,0,98,116,83,111,102,116,66,111,100,121,74,111,105,110,116,68, +97,116,97,0,98,116,83,111,102,116,66,111,100,121,70,108,111,97,116,68, +97,116,97,0,84,76,69,78,1,0,1,0,2,0,2,0,4,0,4,0, +4,0,4,0,8,0,0,0,16,0,48,0,16,0,16,0,32,0,48,0, +96,0,64,0,-128,0,20,0,48,0,80,0,16,0,96,0,-112,0,16,0, +56,0,56,0,20,0,72,0,4,0,4,0,8,0,4,0,56,0,32,0, +80,0,72,0,96,0,80,0,32,0,64,0,64,0,16,0,72,0,80,0, +-40,1,8,1,-16,1,-88,3,8,0,56,0,0,0,88,0,120,0,96,1, +-32,0,-40,0,0,1,96,1,-48,0,16,0,104,0,24,0,40,0,104,0, +96,0,104,0,-56,0,104,1,112,0,-40,1,83,84,82,67,61,0,0,0, +10,0,3,0,4,0,0,0,4,0,1,0,9,0,2,0,11,0,3,0, +10,0,3,0,10,0,4,0,10,0,5,0,12,0,2,0,9,0,6,0, +9,0,7,0,13,0,1,0,7,0,8,0,14,0,1,0,8,0,8,0, +15,0,1,0,13,0,9,0,16,0,1,0,14,0,9,0,17,0,2,0, +15,0,10,0,13,0,11,0,18,0,2,0,16,0,10,0,14,0,11,0, +19,0,4,0,4,0,12,0,4,0,13,0,2,0,14,0,2,0,15,0, +20,0,6,0,13,0,16,0,13,0,17,0,4,0,18,0,4,0,19,0, +4,0,20,0,0,0,21,0,21,0,6,0,14,0,16,0,14,0,17,0, +4,0,18,0,4,0,19,0,4,0,20,0,0,0,21,0,22,0,3,0, +2,0,14,0,2,0,15,0,4,0,22,0,23,0,12,0,13,0,23,0, +13,0,24,0,13,0,25,0,4,0,26,0,4,0,27,0,4,0,28,0, +4,0,29,0,20,0,30,0,22,0,31,0,19,0,32,0,4,0,33,0, +4,0,34,0,24,0,12,0,14,0,23,0,14,0,24,0,14,0,25,0, +4,0,26,0,4,0,27,0,4,0,28,0,4,0,29,0,21,0,30,0, +22,0,31,0,4,0,33,0,4,0,34,0,19,0,32,0,25,0,3,0, +0,0,35,0,4,0,36,0,0,0,37,0,26,0,5,0,25,0,38,0, +13,0,39,0,13,0,40,0,7,0,41,0,0,0,21,0,27,0,5,0, +25,0,38,0,13,0,39,0,13,0,42,0,7,0,43,0,4,0,44,0, +28,0,2,0,13,0,45,0,7,0,46,0,29,0,4,0,27,0,47,0, +28,0,48,0,4,0,49,0,0,0,37,0,30,0,1,0,4,0,50,0, +31,0,2,0,2,0,50,0,0,0,51,0,32,0,2,0,2,0,52,0, +0,0,51,0,33,0,2,0,0,0,52,0,0,0,53,0,34,0,8,0, +13,0,54,0,14,0,55,0,30,0,56,0,32,0,57,0,33,0,58,0, +31,0,59,0,4,0,60,0,4,0,61,0,35,0,4,0,34,0,62,0, +13,0,63,0,4,0,64,0,0,0,37,0,36,0,7,0,25,0,38,0, +35,0,65,0,23,0,66,0,24,0,67,0,37,0,68,0,7,0,43,0, +0,0,69,0,38,0,2,0,36,0,70,0,13,0,39,0,39,0,4,0, +17,0,71,0,25,0,72,0,4,0,73,0,7,0,74,0,40,0,4,0, +25,0,38,0,39,0,75,0,4,0,76,0,7,0,43,0,41,0,3,0, +27,0,47,0,4,0,77,0,0,0,37,0,42,0,3,0,27,0,47,0, +4,0,77,0,0,0,37,0,43,0,4,0,4,0,78,0,7,0,79,0, +7,0,80,0,7,0,81,0,37,0,14,0,4,0,82,0,4,0,83,0, +43,0,84,0,4,0,85,0,7,0,86,0,7,0,87,0,7,0,88,0, +7,0,89,0,7,0,90,0,4,0,91,0,4,0,92,0,4,0,93,0, +4,0,94,0,0,0,37,0,44,0,5,0,25,0,38,0,35,0,65,0, +13,0,39,0,7,0,43,0,4,0,95,0,45,0,5,0,27,0,47,0, +13,0,96,0,14,0,97,0,4,0,98,0,0,0,99,0,46,0,24,0, +9,0,100,0,9,0,101,0,25,0,102,0,0,0,35,0,18,0,103,0, +18,0,104,0,14,0,105,0,14,0,106,0,14,0,107,0,8,0,108,0, +8,0,109,0,8,0,110,0,8,0,111,0,8,0,112,0,8,0,113,0, +8,0,114,0,4,0,115,0,4,0,116,0,4,0,117,0,4,0,118,0, +4,0,119,0,4,0,120,0,4,0,121,0,0,0,37,0,47,0,23,0, +9,0,100,0,9,0,101,0,25,0,102,0,0,0,35,0,17,0,103,0, +17,0,104,0,13,0,105,0,13,0,106,0,13,0,107,0,7,0,108,0, +7,0,109,0,7,0,110,0,7,0,111,0,7,0,112,0,7,0,113,0, +7,0,114,0,4,0,115,0,4,0,116,0,4,0,117,0,4,0,118,0, +4,0,119,0,4,0,120,0,4,0,121,0,48,0,21,0,47,0,122,0, +15,0,123,0,13,0,124,0,13,0,125,0,13,0,126,0,13,0,127,0, +13,0,-128,0,13,0,-127,0,13,0,-126,0,13,0,-125,0,13,0,-124,0, +7,0,-123,0,7,0,-122,0,7,0,-121,0,7,0,-120,0,7,0,-119,0, +7,0,-118,0,7,0,-117,0,7,0,-116,0,7,0,-115,0,4,0,-114,0, +49,0,22,0,46,0,122,0,16,0,123,0,14,0,124,0,14,0,125,0, +14,0,126,0,14,0,127,0,14,0,-128,0,14,0,-127,0,14,0,-126,0, +14,0,-125,0,14,0,-124,0,8,0,-123,0,8,0,-122,0,8,0,-121,0, +8,0,-120,0,8,0,-119,0,8,0,-118,0,8,0,-117,0,8,0,-116,0, +8,0,-115,0,4,0,-114,0,0,0,37,0,50,0,2,0,4,0,-113,0, +4,0,-112,0,51,0,11,0,52,0,-111,0,52,0,-110,0,0,0,35,0, +4,0,-109,0,4,0,-108,0,4,0,-107,0,4,0,-106,0,7,0,-105,0, +7,0,-104,0,4,0,-103,0,0,0,-102,0,53,0,3,0,51,0,-101,0, +13,0,-100,0,13,0,-99,0,54,0,3,0,51,0,-101,0,14,0,-100,0, +14,0,-99,0,55,0,13,0,51,0,-101,0,18,0,-98,0,18,0,-97,0, +4,0,-96,0,4,0,-95,0,4,0,-94,0,7,0,-93,0,7,0,-92,0, +7,0,-91,0,7,0,-90,0,7,0,-89,0,7,0,-88,0,7,0,-87,0, +56,0,13,0,51,0,-101,0,17,0,-98,0,17,0,-97,0,4,0,-96,0, +4,0,-95,0,4,0,-94,0,7,0,-93,0,7,0,-92,0,7,0,-91,0, +7,0,-90,0,7,0,-89,0,7,0,-88,0,7,0,-87,0,57,0,11,0, +51,0,-101,0,17,0,-98,0,17,0,-97,0,7,0,-86,0,7,0,-85,0, +7,0,-84,0,7,0,-89,0,7,0,-88,0,7,0,-87,0,7,0,-83,0, +0,0,21,0,58,0,9,0,51,0,-101,0,17,0,-98,0,17,0,-97,0, +13,0,-82,0,13,0,-81,0,13,0,-80,0,13,0,-79,0,4,0,-78,0, +4,0,-77,0,59,0,5,0,58,0,-76,0,4,0,-75,0,7,0,-74,0, +7,0,-73,0,7,0,-72,0,60,0,9,0,51,0,-101,0,17,0,-98,0, +17,0,-97,0,7,0,-82,0,7,0,-81,0,7,0,-80,0,7,0,-79,0, +4,0,-78,0,4,0,-77,0,61,0,4,0,7,0,-71,0,7,0,-70,0, +7,0,-69,0,4,0,78,0,62,0,10,0,61,0,-68,0,13,0,-67,0, +13,0,-66,0,13,0,-65,0,13,0,-64,0,13,0,-63,0,7,0,-123,0, +7,0,-62,0,4,0,-61,0,4,0,53,0,63,0,4,0,61,0,-68,0, +4,0,-60,0,7,0,-59,0,4,0,-58,0,64,0,4,0,13,0,-63,0, +61,0,-68,0,4,0,-57,0,7,0,-56,0,65,0,7,0,13,0,-55,0, +61,0,-68,0,4,0,-54,0,7,0,-53,0,7,0,-52,0,7,0,-51,0, +4,0,53,0,66,0,6,0,15,0,-50,0,13,0,-52,0,13,0,-49,0, +52,0,-48,0,4,0,-47,0,7,0,-51,0,67,0,26,0,4,0,-46,0, +7,0,-45,0,7,0,-83,0,7,0,-44,0,7,0,-43,0,7,0,-42,0, +7,0,-41,0,7,0,-40,0,7,0,-39,0,7,0,-38,0,7,0,-37,0, +7,0,-36,0,7,0,-35,0,7,0,-34,0,7,0,-33,0,7,0,-32,0, +7,0,-31,0,7,0,-30,0,7,0,-29,0,7,0,-28,0,7,0,-27,0, +4,0,-26,0,4,0,-25,0,4,0,-24,0,4,0,-23,0,4,0,116,0, +68,0,12,0,15,0,-22,0,15,0,-21,0,15,0,-20,0,13,0,-19,0, +13,0,-18,0,7,0,-17,0,4,0,-16,0,4,0,-15,0,4,0,-14,0, +4,0,-13,0,7,0,-53,0,4,0,53,0,69,0,27,0,17,0,-12,0, +15,0,-11,0,15,0,-10,0,13,0,-19,0,13,0,-9,0,13,0,-8,0, +13,0,-7,0,13,0,-6,0,13,0,-5,0,4,0,-4,0,7,0,-3,0, +4,0,-2,0,4,0,-1,0,4,0,0,1,7,0,1,1,7,0,2,1, +4,0,3,1,4,0,4,1,7,0,5,1,7,0,6,1,7,0,7,1, +7,0,8,1,7,0,9,1,7,0,10,1,4,0,11,1,4,0,12,1, +4,0,13,1,70,0,12,0,9,0,14,1,9,0,15,1,13,0,16,1, +7,0,17,1,7,0,18,1,7,0,19,1,4,0,20,1,13,0,21,1, +4,0,22,1,4,0,23,1,4,0,24,1,4,0,53,0,71,0,19,0, +47,0,122,0,68,0,25,1,61,0,26,1,62,0,27,1,63,0,28,1, +64,0,29,1,65,0,30,1,66,0,31,1,69,0,32,1,70,0,33,1, +4,0,34,1,4,0,-1,0,4,0,35,1,4,0,36,1,4,0,37,1, +4,0,38,1,4,0,39,1,4,0,40,1,67,0,41,1,}; +int sBulletDNAlen64= sizeof(sBulletDNAstr64); diff --git a/extern/bullet2/src/LinearMath/btSerializer.h b/extern/bullet2/src/LinearMath/btSerializer.h new file mode 100644 index 00000000000..8a89374c612 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btSerializer.h @@ -0,0 +1,655 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SERIALIZER_H +#define BT_SERIALIZER_H + +#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE +#include "btStackAlloc.h" +#include "btHashMap.h" + +#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) +#include <memory.h> +#endif +#include <string.h> + + + +///only the 32bit versions for now +extern unsigned char sBulletDNAstr[]; +extern int sBulletDNAlen; +extern unsigned char sBulletDNAstr64[]; +extern int sBulletDNAlen64; + +SIMD_FORCE_INLINE int btStrLen(const char* str) +{ + if (!str) + return(0); + int len = 0; + + while (*str != 0) + { + str++; + len++; + } + + return len; +} + + +class btChunk +{ +public: + int m_chunkCode; + int m_length; + void *m_oldPtr; + int m_dna_nr; + int m_number; +}; + +enum btSerializationFlags +{ + BT_SERIALIZE_NO_BVH = 1, + BT_SERIALIZE_NO_TRIANGLEINFOMAP = 2, + BT_SERIALIZE_NO_DUPLICATE_ASSERT = 4 +}; + +class btSerializer +{ + +public: + + virtual ~btSerializer() {} + + virtual const unsigned char* getBufferPointer() const = 0; + + virtual int getCurrentBufferSize() const = 0; + + virtual btChunk* allocate(size_t size, int numElements) = 0; + + virtual void finalizeChunk(btChunk* chunk, const char* structType, int chunkCode,void* oldPtr)= 0; + + virtual void* findPointer(void* oldPtr) = 0; + + virtual void* getUniquePointer(void*oldPtr) = 0; + + virtual void startSerialization() = 0; + + virtual void finishSerialization() = 0; + + virtual const char* findNameForPointer(const void* ptr) const = 0; + + virtual void registerNameForPointer(const void* ptr, const char* name) = 0; + + virtual void serializeName(const char* ptr) = 0; + + virtual int getSerializationFlags() const = 0; + + virtual void setSerializationFlags(int flags) = 0; + + +}; + + + +#define BT_HEADER_LENGTH 12 +#if defined(__sgi) || defined (__sparc) || defined (__sparc__) || defined (__PPC__) || defined (__ppc__) || defined (__BIG_ENDIAN__) +# define MAKE_ID(a,b,c,d) ( (int)(a)<<24 | (int)(b)<<16 | (c)<<8 | (d) ) +#else +# define MAKE_ID(a,b,c,d) ( (int)(d)<<24 | (int)(c)<<16 | (b)<<8 | (a) ) +#endif + +#define BT_SOFTBODY_CODE MAKE_ID('S','B','D','Y') +#define BT_COLLISIONOBJECT_CODE MAKE_ID('C','O','B','J') +#define BT_RIGIDBODY_CODE MAKE_ID('R','B','D','Y') +#define BT_CONSTRAINT_CODE MAKE_ID('C','O','N','S') +#define BT_BOXSHAPE_CODE MAKE_ID('B','O','X','S') +#define BT_QUANTIZED_BVH_CODE MAKE_ID('Q','B','V','H') +#define BT_TRIANLGE_INFO_MAP MAKE_ID('T','M','A','P') +#define BT_SHAPE_CODE MAKE_ID('S','H','A','P') +#define BT_ARRAY_CODE MAKE_ID('A','R','A','Y') +#define BT_SBMATERIAL_CODE MAKE_ID('S','B','M','T') +#define BT_SBNODE_CODE MAKE_ID('S','B','N','D') +#define BT_DNA_CODE MAKE_ID('D','N','A','1') + + +struct btPointerUid +{ + union + { + void* m_ptr; + int m_uniqueIds[2]; + }; +}; + +///The btDefaultSerializer is the main Bullet serialization class. +///The constructor takes an optional argument for backwards compatibility, it is recommended to leave this empty/zero. +class btDefaultSerializer : public btSerializer +{ + + + btAlignedObjectArray<char*> mTypes; + btAlignedObjectArray<short*> mStructs; + btAlignedObjectArray<short> mTlens; + btHashMap<btHashInt, int> mStructReverse; + btHashMap<btHashString,int> mTypeLookup; + + + btHashMap<btHashPtr,void*> m_chunkP; + + btHashMap<btHashPtr,const char*> m_nameMap; + + btHashMap<btHashPtr,btPointerUid> m_uniquePointers; + int m_uniqueIdGenerator; + + int m_totalSize; + unsigned char* m_buffer; + int m_currentSize; + void* m_dna; + int m_dnaLength; + + int m_serializationFlags; + + + btAlignedObjectArray<btChunk*> m_chunkPtrs; + +protected: + + virtual void* findPointer(void* oldPtr) + { + void** ptr = m_chunkP.find(oldPtr); + if (ptr && *ptr) + return *ptr; + return 0; + } + + + + + + void writeDNA() + { + btChunk* dnaChunk = allocate(m_dnaLength,1); + memcpy(dnaChunk->m_oldPtr,m_dna,m_dnaLength); + finalizeChunk(dnaChunk,"DNA1",BT_DNA_CODE, m_dna); + } + + int getReverseType(const char *type) const + { + + btHashString key(type); + const int* valuePtr = mTypeLookup.find(key); + if (valuePtr) + return *valuePtr; + + return -1; + } + + void initDNA(const char* bdnaOrg,int dnalen) + { + ///was already initialized + if (m_dna) + return; + + int littleEndian= 1; + littleEndian= ((char*)&littleEndian)[0]; + + + m_dna = btAlignedAlloc(dnalen,16); + memcpy(m_dna,bdnaOrg,dnalen); + m_dnaLength = dnalen; + + int *intPtr=0; + short *shtPtr=0; + char *cp = 0;int dataLen =0;long nr=0; + intPtr = (int*)m_dna; + + /* + SDNA (4 bytes) (magic number) + NAME (4 bytes) + <nr> (4 bytes) amount of names (int) + <string> + <string> + */ + + if (strncmp((const char*)m_dna, "SDNA", 4)==0) + { + // skip ++ NAME + intPtr++; intPtr++; + } + + // Parse names + if (!littleEndian) + *intPtr = btSwapEndian(*intPtr); + + dataLen = *intPtr; + + intPtr++; + + cp = (char*)intPtr; + int i; + for ( i=0; i<dataLen; i++) + { + + while (*cp)cp++; + cp++; + } + { + nr= (long)cp; + // long mask=3; + nr= ((nr+3)&~3)-nr; + while (nr--) + { + cp++; + } + } + + /* + TYPE (4 bytes) + <nr> amount of types (int) + <string> + <string> + */ + + intPtr = (int*)cp; + assert(strncmp(cp, "TYPE", 4)==0); intPtr++; + + if (!littleEndian) + *intPtr = btSwapEndian(*intPtr); + + dataLen = *intPtr; + intPtr++; + + + cp = (char*)intPtr; + for (i=0; i<dataLen; i++) + { + mTypes.push_back(cp); + while (*cp)cp++; + cp++; + } + + { + nr= (long)cp; + // long mask=3; + nr= ((nr+3)&~3)-nr; + while (nr--) + { + cp++; + } + } + + + /* + TLEN (4 bytes) + <len> (short) the lengths of types + <len> + */ + + // Parse type lens + intPtr = (int*)cp; + assert(strncmp(cp, "TLEN", 4)==0); intPtr++; + + dataLen = (int)mTypes.size(); + + shtPtr = (short*)intPtr; + for (i=0; i<dataLen; i++, shtPtr++) + { + if (!littleEndian) + shtPtr[0] = btSwapEndian(shtPtr[0]); + mTlens.push_back(shtPtr[0]); + } + + if (dataLen & 1) shtPtr++; + + /* + STRC (4 bytes) + <nr> amount of structs (int) + <typenr> + <nr_of_elems> + <typenr> + <namenr> + <typenr> + <namenr> + */ + + intPtr = (int*)shtPtr; + cp = (char*)intPtr; + assert(strncmp(cp, "STRC", 4)==0); intPtr++; + + if (!littleEndian) + *intPtr = btSwapEndian(*intPtr); + dataLen = *intPtr ; + intPtr++; + + + shtPtr = (short*)intPtr; + for (i=0; i<dataLen; i++) + { + mStructs.push_back (shtPtr); + + if (!littleEndian) + { + shtPtr[0]= btSwapEndian(shtPtr[0]); + shtPtr[1]= btSwapEndian(shtPtr[1]); + + int len = shtPtr[1]; + shtPtr+= 2; + + for (int a=0; a<len; a++, shtPtr+=2) + { + shtPtr[0]= btSwapEndian(shtPtr[0]); + shtPtr[1]= btSwapEndian(shtPtr[1]); + } + + } else + { + shtPtr+= (2*shtPtr[1])+2; + } + } + + // build reverse lookups + for (i=0; i<(int)mStructs.size(); i++) + { + short *strc = mStructs.at(i); + mStructReverse.insert(strc[0], i); + mTypeLookup.insert(btHashString(mTypes[strc[0]]),i); + } + } + +public: + + + + + btDefaultSerializer(int totalSize=0) + :m_totalSize(totalSize), + m_currentSize(0), + m_dna(0), + m_dnaLength(0), + m_serializationFlags(0) + { + m_buffer = m_totalSize?(unsigned char*)btAlignedAlloc(totalSize,16):0; + + const bool VOID_IS_8 = ((sizeof(void*)==8)); + +#ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + if (VOID_IS_8) + { +#if _WIN64 + initDNA((const char*)sBulletDNAstr64,sBulletDNAlen64); +#else + btAssert(0); +#endif + } else + { +#ifndef _WIN64 + initDNA((const char*)sBulletDNAstr,sBulletDNAlen); +#else + btAssert(0); +#endif + } + +#else //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + if (VOID_IS_8) + { + initDNA((const char*)sBulletDNAstr64,sBulletDNAlen64); + } else + { + initDNA((const char*)sBulletDNAstr,sBulletDNAlen); + } +#endif //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + + } + + virtual ~btDefaultSerializer() + { + if (m_buffer) + btAlignedFree(m_buffer); + if (m_dna) + btAlignedFree(m_dna); + } + + void writeHeader(unsigned char* buffer) const + { + + +#ifdef BT_USE_DOUBLE_PRECISION + memcpy(buffer, "BULLETd", 7); +#else + memcpy(buffer, "BULLETf", 7); +#endif //BT_USE_DOUBLE_PRECISION + + int littleEndian= 1; + littleEndian= ((char*)&littleEndian)[0]; + + if (sizeof(void*)==8) + { + buffer[7] = '-'; + } else + { + buffer[7] = '_'; + } + + if (littleEndian) + { + buffer[8]='v'; + } else + { + buffer[8]='V'; + } + + + buffer[9] = '2'; + buffer[10] = '7'; + buffer[11] = '8'; + + } + + virtual void startSerialization() + { + m_uniqueIdGenerator= 1; + if (m_totalSize) + { + unsigned char* buffer = internalAlloc(BT_HEADER_LENGTH); + writeHeader(buffer); + } + + } + + virtual void finishSerialization() + { + writeDNA(); + + //if we didn't pre-allocate a buffer, we need to create a contiguous buffer now + int mysize = 0; + if (!m_totalSize) + { + if (m_buffer) + btAlignedFree(m_buffer); + + m_currentSize += BT_HEADER_LENGTH; + m_buffer = (unsigned char*)btAlignedAlloc(m_currentSize,16); + + unsigned char* currentPtr = m_buffer; + writeHeader(m_buffer); + currentPtr += BT_HEADER_LENGTH; + mysize+=BT_HEADER_LENGTH; + for (int i=0;i< m_chunkPtrs.size();i++) + { + int curLength = sizeof(btChunk)+m_chunkPtrs[i]->m_length; + memcpy(currentPtr,m_chunkPtrs[i], curLength); + btAlignedFree(m_chunkPtrs[i]); + currentPtr+=curLength; + mysize+=curLength; + } + } + + mTypes.clear(); + mStructs.clear(); + mTlens.clear(); + mStructReverse.clear(); + mTypeLookup.clear(); + m_chunkP.clear(); + m_nameMap.clear(); + m_uniquePointers.clear(); + m_chunkPtrs.clear(); + } + + virtual void* getUniquePointer(void*oldPtr) + { + if (!oldPtr) + return 0; + + btPointerUid* uptr = (btPointerUid*)m_uniquePointers.find(oldPtr); + if (uptr) + { + return uptr->m_ptr; + } + m_uniqueIdGenerator++; + + btPointerUid uid; + uid.m_uniqueIds[0] = m_uniqueIdGenerator; + uid.m_uniqueIds[1] = m_uniqueIdGenerator; + m_uniquePointers.insert(oldPtr,uid); + return uid.m_ptr; + + } + + virtual const unsigned char* getBufferPointer() const + { + return m_buffer; + } + + virtual int getCurrentBufferSize() const + { + return m_currentSize; + } + + virtual void finalizeChunk(btChunk* chunk, const char* structType, int chunkCode,void* oldPtr) + { + if (!(m_serializationFlags&BT_SERIALIZE_NO_DUPLICATE_ASSERT)) + { + btAssert(!findPointer(oldPtr)); + } + + chunk->m_dna_nr = getReverseType(structType); + + chunk->m_chunkCode = chunkCode; + + void* uniquePtr = getUniquePointer(oldPtr); + + m_chunkP.insert(oldPtr,uniquePtr);//chunk->m_oldPtr); + chunk->m_oldPtr = uniquePtr;//oldPtr; + + } + + + virtual unsigned char* internalAlloc(size_t size) + { + unsigned char* ptr = 0; + + if (m_totalSize) + { + ptr = m_buffer+m_currentSize; + m_currentSize += int(size); + btAssert(m_currentSize<m_totalSize); + } else + { + ptr = (unsigned char*)btAlignedAlloc(size,16); + m_currentSize += int(size); + } + return ptr; + } + + + + virtual btChunk* allocate(size_t size, int numElements) + { + + unsigned char* ptr = internalAlloc(int(size)*numElements+sizeof(btChunk)); + + unsigned char* data = ptr + sizeof(btChunk); + + btChunk* chunk = (btChunk*)ptr; + chunk->m_chunkCode = 0; + chunk->m_oldPtr = data; + chunk->m_length = int(size)*numElements; + chunk->m_number = numElements; + + m_chunkPtrs.push_back(chunk); + + + return chunk; + } + + virtual const char* findNameForPointer(const void* ptr) const + { + const char*const * namePtr = m_nameMap.find(ptr); + if (namePtr && *namePtr) + return *namePtr; + return 0; + + } + + virtual void registerNameForPointer(const void* ptr, const char* name) + { + m_nameMap.insert(ptr,name); + } + + virtual void serializeName(const char* name) + { + if (name) + { + //don't serialize name twice + if (findPointer((void*)name)) + return; + + int len = btStrLen(name); + if (len) + { + + int newLen = len+1; + int padding = ((newLen+3)&~3)-newLen; + newLen += padding; + + //serialize name string now + btChunk* chunk = allocate(sizeof(char),newLen); + char* destinationName = (char*)chunk->m_oldPtr; + for (int i=0;i<len;i++) + { + destinationName[i] = name[i]; + } + destinationName[len] = 0; + finalizeChunk(chunk,"char",BT_ARRAY_CODE,(void*)name); + } + } + } + + virtual int getSerializationFlags() const + { + return m_serializationFlags; + } + + virtual void setSerializationFlags(int flags) + { + m_serializationFlags = flags; + } + +}; + + +#endif //BT_SERIALIZER_H + diff --git a/extern/bullet2/src/LinearMath/btTransform.h b/extern/bullet2/src/LinearMath/btTransform.h index c4fe33eecd7..187b09116cc 100644 --- a/extern/bullet2/src/LinearMath/btTransform.h +++ b/extern/bullet2/src/LinearMath/btTransform.h @@ -17,14 +17,26 @@ subject to the following restrictions: #ifndef btTransform_H #define btTransform_H -#include "btVector3.h" + #include "btMatrix3x3.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define btTransformData btTransformDoubleData +#else +#define btTransformData btTransformFloatData +#endif + + + /**@brief The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear. *It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. */ class btTransform { + ///Storage for the rotation + btMatrix3x3 m_basis; + ///Storage for the translation + btVector3 m_origin; public: @@ -195,12 +207,17 @@ public: static const btTransform identityTransform(btMatrix3x3::getIdentity()); return identityTransform; } - -private: - ///Storage for the rotation - btMatrix3x3 m_basis; - ///Storage for the translation - btVector3 m_origin; + + void serialize(struct btTransformData& dataOut) const; + + void serializeFloat(struct btTransformFloatData& dataOut) const; + + void deSerialize(const struct btTransformData& dataIn); + + void deSerializeDouble(const struct btTransformDoubleData& dataIn); + + void deSerializeFloat(const struct btTransformFloatData& dataIn); + }; @@ -234,6 +251,53 @@ SIMD_FORCE_INLINE bool operator==(const btTransform& t1, const btTransform& t2) } +///for serialization +struct btTransformFloatData +{ + btMatrix3x3FloatData m_basis; + btVector3FloatData m_origin; +}; + +struct btTransformDoubleData +{ + btMatrix3x3DoubleData m_basis; + btVector3DoubleData m_origin; +}; + + + +SIMD_FORCE_INLINE void btTransform::serialize(btTransformData& dataOut) const +{ + m_basis.serialize(dataOut.m_basis); + m_origin.serialize(dataOut.m_origin); +} + +SIMD_FORCE_INLINE void btTransform::serializeFloat(btTransformFloatData& dataOut) const +{ + m_basis.serializeFloat(dataOut.m_basis); + m_origin.serializeFloat(dataOut.m_origin); +} + + +SIMD_FORCE_INLINE void btTransform::deSerialize(const btTransformData& dataIn) +{ + m_basis.deSerialize(dataIn.m_basis); + m_origin.deSerialize(dataIn.m_origin); +} + +SIMD_FORCE_INLINE void btTransform::deSerializeFloat(const btTransformFloatData& dataIn) +{ + m_basis.deSerializeFloat(dataIn.m_basis); + m_origin.deSerializeFloat(dataIn.m_origin); +} + +SIMD_FORCE_INLINE void btTransform::deSerializeDouble(const btTransformDoubleData& dataIn) +{ + m_basis.deSerializeDouble(dataIn.m_basis); + m_origin.deSerializeDouble(dataIn.m_origin); +} + + #endif diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h index e8328da4ca6..626110ce4ee 100644 --- a/extern/bullet2/src/LinearMath/btTransformUtil.h +++ b/extern/bullet2/src/LinearMath/btTransformUtil.h @@ -21,9 +21,6 @@ subject to the following restrictions: -#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) - -#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */ SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir) { @@ -33,25 +30,7 @@ SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btV } -SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q) -{ - if (btFabs(n.z()) > SIMDSQRT12) { - // choose p in y-z plane - btScalar a = n[1]*n[1] + n[2]*n[2]; - btScalar k = btRecipSqrt (a); - p.setValue(0,-n[2]*k,n[1]*k); - // set q = n x p - q.setValue(a*k,-n[0]*p[2],n[0]*p[1]); - } - else { - // choose p in x-y plane - btScalar a = n.x()*n.x() + n.y()*n.y(); - btScalar k = btRecipSqrt (a); - p.setValue(-n.y()*k,n.x()*k,0); - // set q = n x p - q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k); - } -} + @@ -117,10 +96,8 @@ public: static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle) { - btQuaternion orn1 = orn0.farthest(orn1a); + btQuaternion orn1 = orn0.nearest(orn1a); btQuaternion dorn = orn1 * orn0.inverse(); - ///floating point inaccuracy can lead to w component > 1..., which breaks - dorn.normalize(); angle = dorn.getAngle(); axis = btVector3(dorn.x(),dorn.y(),dorn.z()); axis[3] = btScalar(0.); @@ -209,7 +186,7 @@ public: btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB); btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB; btVector3 relLinVel = (linVelB-linVelA); - btScalar relLinVelocLength = (linVelB-linVelA).dot(m_separatingNormal); + btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal); if (relLinVelocLength<0.f) { relLinVelocLength = 0.f; @@ -227,17 +204,21 @@ public: void initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB) { - m_separatingNormal = separatingVector; m_separatingDistance = separatingDistance; - - const btVector3& toPosA = transA.getOrigin(); - const btVector3& toPosB = transB.getOrigin(); - btQuaternion toOrnA = transA.getRotation(); - btQuaternion toOrnB = transB.getRotation(); - m_posA = toPosA; - m_posB = toPosB; - m_ornA = toOrnA; - m_ornB = toOrnB; + + if (m_separatingDistance>0.f) + { + m_separatingNormal = separatingVector; + + const btVector3& toPosA = transA.getOrigin(); + const btVector3& toPosB = transB.getOrigin(); + btQuaternion toOrnA = transA.getRotation(); + btQuaternion toOrnB = transB.getRotation(); + m_posA = toPosA; + m_posB = toPosB; + m_ornA = toOrnA; + m_ornB = toOrnB; + } } }; diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h index 5d5c39e8587..068e87c94ab 100644 --- a/extern/bullet2/src/LinearMath/btVector3.h +++ b/extern/bullet2/src/LinearMath/btVector3.h @@ -19,30 +19,37 @@ subject to the following restrictions: #include "btScalar.h" -#include "btScalar.h" #include "btMinMax.h" + +#ifdef BT_USE_DOUBLE_PRECISION +#define btVector3Data btVector3DoubleData +#define btVector3DataName "btVector3DoubleData" +#else +#define btVector3Data btVector3FloatData +#define btVector3DataName "btVector3FloatData" +#endif //BT_USE_DOUBLE_PRECISION + + + + /**@brief btVector3 can be used to represent 3D points and vectors. * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers */ - ATTRIBUTE_ALIGNED16(class) btVector3 { public: #if defined (__SPU__) && defined (__CELLOS_LV2__) - union { - vec_float4 mVec128; btScalar m_floats[4]; - }; public: - vec_float4 get128() const + SIMD_FORCE_INLINE const vec_float4& get128() const { - return mVec128; + return *((const vec_float4*)&m_floats[0]); } public: #else //__CELLOS_LV2__ __SPU__ -#ifdef BT_USE_SSE // WIN32 +#ifdef BT_USE_SSE // _WIN32 union { __m128 mVec128; btScalar m_floats[4]; @@ -141,6 +148,19 @@ public: * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const; + SIMD_FORCE_INLINE btVector3& safeNormalize() + { + btVector3 absVec = this->absolute(); + int maxIndex = absVec.maxAxis(); + if (absVec[maxIndex]>0) + { + *this /= absVec[maxIndex]; + return *this /= length(); + } + setValue(1,0,0); + return *this; + } + /**@brief Normalize this vector * x^2 + y^2 + z^2 = 1 */ SIMD_FORCE_INLINE btVector3& normalize() @@ -151,10 +171,10 @@ public: /**@brief Return a normalized version of this vector */ SIMD_FORCE_INLINE btVector3 normalized() const; - /**@brief Rotate this vector + /**@brief Return a rotated version of this vector * @param wAxis The axis to rotate about * @param angle The angle to rotate by */ - SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ); + SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ) const; /**@brief Return the angle between this and another vector * @param v The other vector */ @@ -306,7 +326,7 @@ public: m_floats[0]=x; m_floats[1]=y; m_floats[2]=z; - m_floats[3] = 0.f; + m_floats[3] = btScalar(0.); } void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const @@ -316,6 +336,33 @@ public: v2->setValue(-y() ,x() ,0.); } + void setZero() + { + setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + } + + SIMD_FORCE_INLINE bool isZero() const + { + return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0); + } + + SIMD_FORCE_INLINE bool fuzzyZero() const + { + return length2() < SIMD_EPSILON; + } + + SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const; + + SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn); + + SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData& dataOut) const; + + SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn); + + SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData& dataOut) const; + + SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData& dataIn); + }; /**@brief Return the sum of two vectors (Point symantics)*/ @@ -376,7 +423,7 @@ operator/(const btVector3& v1, const btVector3& v2) /**@brief Return the dot product between two vectors */ SIMD_FORCE_INLINE btScalar -dot(const btVector3& v1, const btVector3& v2) +btDot(const btVector3& v1, const btVector3& v2) { return v1.dot(v2); } @@ -384,7 +431,7 @@ dot(const btVector3& v1, const btVector3& v2) /**@brief Return the distance squared between two vectors */ SIMD_FORCE_INLINE btScalar -distance2(const btVector3& v1, const btVector3& v2) +btDistance2(const btVector3& v1, const btVector3& v2) { return v1.distance2(v2); } @@ -392,27 +439,27 @@ distance2(const btVector3& v1, const btVector3& v2) /**@brief Return the distance between two vectors */ SIMD_FORCE_INLINE btScalar -distance(const btVector3& v1, const btVector3& v2) +btDistance(const btVector3& v1, const btVector3& v2) { return v1.distance(v2); } /**@brief Return the angle between two vectors */ SIMD_FORCE_INLINE btScalar -angle(const btVector3& v1, const btVector3& v2) +btAngle(const btVector3& v1, const btVector3& v2) { return v1.angle(v2); } /**@brief Return the cross product of two vectors */ SIMD_FORCE_INLINE btVector3 -cross(const btVector3& v1, const btVector3& v2) +btCross(const btVector3& v1, const btVector3& v2) { return v1.cross(v2); } SIMD_FORCE_INLINE btScalar -triple(const btVector3& v1, const btVector3& v2, const btVector3& v3) +btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3) { return v1.triple(v2, v3); } @@ -444,7 +491,7 @@ SIMD_FORCE_INLINE btVector3 btVector3::normalized() const return *this / length(); } -SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle ) +SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle ) const { // wAxis must be a unit lenght vector @@ -488,7 +535,7 @@ public: SIMD_FORCE_INLINE int maxAxis4() const { int maxIndex = -1; - btScalar maxVal = btScalar(-1e30); + btScalar maxVal = btScalar(-BT_LARGE_FLOAT); if (m_floats[0] > maxVal) { maxIndex = 0; @@ -521,7 +568,7 @@ public: SIMD_FORCE_INLINE int minAxis4() const { int minIndex = -1; - btScalar minVal = btScalar(1e30); + btScalar minVal = btScalar(BT_LARGE_FLOAT); if (m_floats[0] < minVal) { minIndex = 0; @@ -585,8 +632,6 @@ public: } - - }; @@ -635,4 +680,87 @@ SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector) vector = swappedVec; } +template <class T> +SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q) +{ + if (btFabs(n[2]) > SIMDSQRT12) { + // choose p in y-z plane + btScalar a = n[1]*n[1] + n[2]*n[2]; + btScalar k = btRecipSqrt (a); + p[0] = 0; + p[1] = -n[2]*k; + p[2] = n[1]*k; + // set q = n x p + q[0] = a*k; + q[1] = -n[0]*p[2]; + q[2] = n[0]*p[1]; + } + else { + // choose p in x-y plane + btScalar a = n[0]*n[0] + n[1]*n[1]; + btScalar k = btRecipSqrt (a); + p[0] = -n[1]*k; + p[1] = n[0]*k; + p[2] = 0; + // set q = n x p + q[0] = -n[2]*p[1]; + q[1] = n[2]*p[0]; + q[2] = a*k; + } +} + + +struct btVector3FloatData +{ + float m_floats[4]; +}; + +struct btVector3DoubleData +{ + double m_floats[4]; + +}; + +SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& dataOut) const +{ + ///could also do a memcpy, check if it is worth it + for (int i=0;i<4;i++) + dataOut.m_floats[i] = float(m_floats[i]); +} + +SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = btScalar(dataIn.m_floats[i]); +} + + +SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData& dataOut) const +{ + ///could also do a memcpy, check if it is worth it + for (int i=0;i<4;i++) + dataOut.m_floats[i] = double(m_floats[i]); +} + +SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = btScalar(dataIn.m_floats[i]); +} + + +SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const +{ + ///could also do a memcpy, check if it is worth it + for (int i=0;i<4;i++) + dataOut.m_floats[i] = m_floats[i]; +} + +SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = dataIn.m_floats[i]; +} + + #endif //SIMD__VECTOR3_H diff --git a/extern/bullet2/src/btBulletCollisionCommon.h b/extern/bullet2/src/btBulletCollisionCommon.h index 4b14f6d00f3..472690c1573 100644 --- a/extern/bullet2/src/btBulletCollisionCommon.h +++ b/extern/bullet2/src/btBulletCollisionCommon.h @@ -33,6 +33,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btTriangleMesh.h" #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" @@ -61,6 +62,8 @@ subject to the following restrictions: #include "LinearMath/btDefaultMotionState.h" #include "LinearMath/btQuickprof.h" #include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btSerializer.h" + #endif //BULLET_COLLISION_COMMON_H diff --git a/extern/bullet2/src/btBulletDynamicsCommon.h b/extern/bullet2/src/btBulletDynamicsCommon.h index b95972cd15d..db8b37989f5 100644 --- a/extern/bullet2/src/btBulletDynamicsCommon.h +++ b/extern/bullet2/src/btBulletDynamicsCommon.h @@ -30,6 +30,9 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h" +#include "BulletDynamics/ConstraintSolver/btUniversalConstraint.h" +#include "BulletDynamics/ConstraintSolver/btHinge2Constraint.h" #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" |