diff options
95 files changed, 4595 insertions, 2483 deletions
diff --git a/extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.cpp b/extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.cpp index 25abf92aea0..235afa1f9fe 100644 --- a/extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.cpp +++ b/extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.cpp @@ -21,11 +21,12 @@ #include <assert.h> -BroadphaseProxy* AxisSweep3::CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr ) +BroadphaseProxy* AxisSweep3::CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask) { - unsigned short handleId = AddHandle(min,max, userPtr); + unsigned short handleId = AddHandle(min,max, userPtr,collisionFilterGroup,collisionFilterMask); Handle* handle = GetHandle(handleId); + return handle; } @@ -47,6 +48,7 @@ void AxisSweep3::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const AxisSweep3::AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAabbMax, int maxHandles, int maxOverlaps) +:OverlappingPairCache(maxOverlaps) { //assert(bounds.HasVolume()); @@ -156,7 +158,7 @@ void AxisSweep3::FreeHandle(unsigned short handle) -unsigned short AxisSweep3::AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner) +unsigned short AxisSweep3::AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask) { // quantize the bounds unsigned short min[3], max[3]; @@ -169,10 +171,11 @@ unsigned short AxisSweep3::AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& Handle* pHandle = GetHandle(handle); - pHandle->m_handleId = handle; //pHandle->m_pOverlaps = 0; pHandle->m_clientObject = pOwner; + pHandle->m_collisionFilterGroup = collisionFilterGroup; + pHandle->m_collisionFilterMask = collisionFilterMask; // compute current limit of edge arrays int limit = m_numHandles * 2; diff --git a/extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.h b/extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.h index 9591bdbb40e..0a4a75d523a 100644 --- a/extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.h +++ b/extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.h @@ -21,10 +21,13 @@ #include "SimdPoint3.h" #include "SimdVector3.h" -#include "SimpleBroadphase.h" +#include "OverlappingPairCache.h" #include "BroadphaseProxy.h" -class AxisSweep3 : public SimpleBroadphase +/// AxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase. +/// It uses arrays rather then lists for storage of the 3 axis. Also it operates using integer coordinates instead of floats. +/// The TestOverlap check is optimized to check the array index, rather then the actual AABB coordinates/pos +class AxisSweep3 : public OverlappingPairCache { public: @@ -96,14 +99,14 @@ public: //this is replace by sweep and prune } - unsigned short AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner); + unsigned short AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask); void RemoveHandle(unsigned short handle); void UpdateHandle(unsigned short handle, const SimdPoint3& aabbMin,const SimdPoint3& aabbMax); inline Handle* GetHandle(unsigned short index) const {return m_pHandles + index;} //Broadphase Interface - virtual BroadphaseProxy* CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr ); + virtual BroadphaseProxy* CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask); virtual void DestroyProxy(BroadphaseProxy* proxy); virtual void SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax); diff --git a/extern/bullet/Bullet/BroadphaseCollision/BroadphaseInterface.h b/extern/bullet/Bullet/BroadphaseCollision/BroadphaseInterface.h index 5b9efc6b92a..3ca273bde54 100644 --- a/extern/bullet/Bullet/BroadphaseCollision/BroadphaseInterface.h +++ b/extern/bullet/Bullet/BroadphaseCollision/BroadphaseInterface.h @@ -29,11 +29,11 @@ class BroadphaseInterface public: virtual ~BroadphaseInterface() {} - virtual BroadphaseProxy* CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr ) =0; + virtual BroadphaseProxy* CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) =0; virtual void DestroyProxy(BroadphaseProxy* proxy)=0; virtual void SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax)=0; virtual void CleanProxyFromPairs(BroadphaseProxy* proxy)=0; - virtual void DispatchAllCollisionPairs(Dispatcher& dispatcher,DispatcherInfo& dispatchInfo)=0; + }; diff --git a/extern/bullet/Bullet/BroadphaseCollision/BroadphaseProxy.h b/extern/bullet/Bullet/BroadphaseCollision/BroadphaseProxy.h index 78bd7ab7697..35b2dc866f1 100644 --- a/extern/bullet/Bullet/BroadphaseCollision/BroadphaseProxy.h +++ b/extern/bullet/Bullet/BroadphaseCollision/BroadphaseProxy.h @@ -27,6 +27,7 @@ enum BroadphaseNativeTypes BOX_SHAPE_PROXYTYPE, TRIANGLE_SHAPE_PROXYTYPE, TETRAHEDRAL_SHAPE_PROXYTYPE, + CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE, CONVEX_HULL_SHAPE_PROXYTYPE, //implicit convex shapes IMPLICIT_CONVEX_SHAPES_START_HERE, @@ -42,6 +43,10 @@ CONCAVE_SHAPES_START_HERE, //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! TRIANGLE_MESH_SHAPE_PROXYTYPE, EMPTY_SHAPE_PROXYTYPE, + STATIC_PLANE_PROXYTYPE, +CONCAVE_SHAPES_END_HERE, + + COMPOUND_SHAPE_PROXYTYPE, MAX_BROADPHASE_COLLISION_TYPES }; @@ -53,12 +58,16 @@ struct BroadphaseProxy //Usually the client CollisionObject or Rigidbody class void* m_clientObject; + short int m_collisionFilterGroup; + short int m_collisionFilterMask; - + //used for memory pools BroadphaseProxy() :m_clientObject(0){} - BroadphaseProxy(int shapeType,void* userPtr) - :m_clientObject(userPtr) - //m_clientObjectType(shapeType) + + BroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) + :m_clientObject(userPtr), + m_collisionFilterGroup(collisionFilterGroup), + m_collisionFilterMask(collisionFilterMask) { } diff --git a/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h b/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h index 7f8351bcd93..5edfbbdd3b5 100644 --- a/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h +++ b/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h @@ -21,6 +21,7 @@ struct BroadphaseProxy; class RigidBody; struct CollisionObject; class ManifoldResult; +class OverlappingPairCache; enum CollisionDispatcherId { @@ -80,10 +81,17 @@ public: virtual bool NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) = 0; + virtual bool NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1)=0; + virtual ManifoldResult* GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold) =0; virtual void ReleaseManifoldResult(ManifoldResult*)=0; + virtual void DispatchAllCollisionPairs(struct BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo)=0; + + virtual int GetNumManifolds() const = 0; + + virtual PersistentManifold* GetManifoldByIndexInternal(int index) = 0; }; diff --git a/extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.cpp b/extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.cpp new file mode 100644 index 00000000000..df535044df1 --- /dev/null +++ b/extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.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 "OverlappingPairCache.h" + +#include "Dispatcher.h" +#include "CollisionAlgorithm.h" + + +OverlappingPairCache::OverlappingPairCache(int maxOverlap): +m_blockedForChanges(false), +m_NumOverlapBroadphasePair(0), +m_maxOverlap(maxOverlap) +{ + m_OverlappingPairs = new BroadphasePair[maxOverlap]; +} + + +OverlappingPairCache::~OverlappingPairCache() +{ + delete [] m_OverlappingPairs; +} + + +void OverlappingPairCache::RemoveOverlappingPair(BroadphasePair& pair) +{ + CleanOverlappingPair(pair); + int index = &pair - &m_OverlappingPairs[0]; + //remove efficiently, swap with the last + m_OverlappingPairs[index] = m_OverlappingPairs[m_NumOverlapBroadphasePair-1]; + m_NumOverlapBroadphasePair--; +} + + +void OverlappingPairCache::CleanOverlappingPair(BroadphasePair& pair) +{ + for (int dispatcherId=0;dispatcherId<SIMPLE_MAX_ALGORITHMS;dispatcherId++) + { + if (pair.m_algorithms[dispatcherId]) + { + { + delete pair.m_algorithms[dispatcherId]; + pair.m_algorithms[dispatcherId]=0; + } + } + } +} + + + + + +void OverlappingPairCache::AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) +{ + //don't add overlap with own + assert(proxy0 != proxy1); + + if (!NeedsCollision(proxy0,proxy1)) + return; + + + BroadphasePair pair(*proxy0,*proxy1); + m_OverlappingPairs[m_NumOverlapBroadphasePair] = pair; + + int i; + for (i=0;i<SIMPLE_MAX_ALGORITHMS;i++) + { + assert(!m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i]); + m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i] = 0; + } + + if (m_NumOverlapBroadphasePair >= m_maxOverlap) + { + //printf("Error: too many overlapping objects: m_NumOverlapBroadphasePair: %d\n",m_NumOverlapBroadphasePair); +#ifdef DEBUG + assert(0); +#endif + } else + { + m_NumOverlapBroadphasePair++; + } + + +} + + +BroadphasePair* OverlappingPairCache::FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) +{ + BroadphasePair* foundPair = 0; + + int i; + for (i=m_NumOverlapBroadphasePair-1;i>=0;i--) + { + BroadphasePair& pair = m_OverlappingPairs[i]; + if (((pair.m_pProxy0 == proxy0) && (pair.m_pProxy1 == proxy1)) || + ((pair.m_pProxy0 == proxy1) && (pair.m_pProxy1 == proxy0))) + { + foundPair = &pair; + return foundPair; + } + } + + return foundPair; +} + + + +void OverlappingPairCache::CleanProxyFromPairs(BroadphaseProxy* proxy) +{ + for (int i=0;i<m_NumOverlapBroadphasePair;i++) + { + BroadphasePair& pair = m_OverlappingPairs[i]; + if (pair.m_pProxy0 == proxy || + pair.m_pProxy1 == proxy) + { + CleanOverlappingPair(pair); + } + } +} + +void OverlappingPairCache::RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy) +{ + int i; + for ( i=m_NumOverlapBroadphasePair-1;i>=0;i--) + { + BroadphasePair& pair = m_OverlappingPairs[i]; + if (pair.m_pProxy0 == proxy || + pair.m_pProxy1 == proxy) + { + RemoveOverlappingPair(pair); + } + } +} + + + diff --git a/extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.h b/extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.h new file mode 100644 index 00000000000..0fba0b2e558 --- /dev/null +++ b/extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.h @@ -0,0 +1,85 @@ + +/* +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 OVERLAPPING_PAIR_CACHE_H +#define OVERLAPPING_PAIR_CACHE_H + + +#include "BroadphaseInterface.h" +#include "BroadphaseProxy.h" +#include "SimdPoint3.h" + + +///OverlappingPairCache maintains the objects with overlapping AABB +///Typically managed by the Broadphase, Axis3Sweep or SimpleBroadphase +class OverlappingPairCache : public BroadphaseInterface +{ + + BroadphasePair* m_OverlappingPairs; + int m_NumOverlapBroadphasePair; + int m_maxOverlap; + + //during the dispatch, check that user doesn't destroy/create proxy + bool m_blockedForChanges; + + + public: + + OverlappingPairCache(int maxOverlap); + virtual ~OverlappingPairCache(); + + int GetNumOverlappingPairs() const + { + return m_NumOverlapBroadphasePair; + } + + BroadphasePair& GetOverlappingPair(int index) + { + return m_OverlappingPairs[index]; + } + + void RemoveOverlappingPair(BroadphasePair& pair); + + void CleanOverlappingPair(BroadphasePair& pair); + + void AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1); + + BroadphasePair* FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1); + + + + void CleanProxyFromPairs(BroadphaseProxy* proxy); + + void RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy); + + + inline bool NeedsCollision(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) const + { + bool collides = proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask; + collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + return collides; + } + + + + virtual void RefreshOverlappingPairs() =0; + + + + +}; +#endif //OVERLAPPING_PAIR_CACHE_H
\ No newline at end of file diff --git a/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp b/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp index c0796f9b9f3..5a648e1d306 100644 --- a/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp +++ b/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp @@ -14,8 +14,8 @@ subject to the following restrictions: */ #include "SimpleBroadphase.h" -#include "BroadphaseCollision/Dispatcher.h" -#include "BroadphaseCollision/CollisionAlgorithm.h" +#include <BroadphaseCollision/Dispatcher.h> +#include <BroadphaseCollision/CollisionAlgorithm.h> #include "SimdVector3.h" #include "SimdTransform.h" @@ -36,19 +36,16 @@ void SimpleBroadphase::validate() } SimpleBroadphase::SimpleBroadphase(int maxProxies,int maxOverlap) - :m_firstFreeProxy(0), + :OverlappingPairCache(maxOverlap), + m_firstFreeProxy(0), m_numProxies(0), - m_blockedForChanges(false), - m_NumOverlapBroadphasePair(0), - m_maxProxies(maxProxies), - m_maxOverlap(maxOverlap) + m_maxProxies(maxProxies) { m_proxies = new SimpleBroadphaseProxy[maxProxies]; m_freeProxies = new int[maxProxies]; m_pProxies = new SimpleBroadphaseProxy*[maxProxies]; - m_OverlappingPairs = new BroadphasePair[maxOverlap]; - + int i; for (i=0;i<m_maxProxies;i++) @@ -62,7 +59,6 @@ SimpleBroadphase::~SimpleBroadphase() delete[] m_proxies; delete []m_freeProxies; delete [] m_pProxies; - delete [] m_OverlappingPairs; /*int i; for (i=m_numProxies-1;i>=0;i--) @@ -74,7 +70,7 @@ SimpleBroadphase::~SimpleBroadphase() } -BroadphaseProxy* SimpleBroadphase::CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr) +BroadphaseProxy* SimpleBroadphase::CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask) { if (m_numProxies >= m_maxProxies) { @@ -84,7 +80,7 @@ BroadphaseProxy* SimpleBroadphase::CreateProxy( const SimdVector3& min, const assert(min[0]<= max[0] && min[1]<= max[1] && min[2]<= max[2]); int freeIndex= m_freeProxies[m_firstFreeProxy]; - SimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])SimpleBroadphaseProxy(min,max,shapeType,userPtr); + SimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])SimpleBroadphaseProxy(min,max,shapeType,userPtr,collisionFilterGroup,collisionFilterMask); m_firstFreeProxy++; SimpleBroadphaseProxy* proxy1 = &m_proxies[0]; @@ -99,19 +95,7 @@ BroadphaseProxy* SimpleBroadphase::CreateProxy( const SimdVector3& min, const return proxy; } -void SimpleBroadphase::RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy) -{ - int i; - for ( i=m_NumOverlapBroadphasePair-1;i>=0;i--) - { - BroadphasePair& pair = m_OverlappingPairs[i]; - if (pair.m_pProxy0 == proxy || - pair.m_pProxy1 == proxy) - { - RemoveOverlappingPair(pair); - } - } -} + void SimpleBroadphase::DestroyProxy(BroadphaseProxy* proxyOrg) { @@ -148,89 +132,13 @@ void SimpleBroadphase::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin sbp->m_max = aabbMax; } -void SimpleBroadphase::CleanOverlappingPair(BroadphasePair& pair) -{ - for (int dispatcherId=0;dispatcherId<SIMPLE_MAX_ALGORITHMS;dispatcherId++) - { - if (pair.m_algorithms[dispatcherId]) - { - { - delete pair.m_algorithms[dispatcherId]; - pair.m_algorithms[dispatcherId]=0; - } - } - } -} - -void SimpleBroadphase::CleanProxyFromPairs(BroadphaseProxy* proxy) -{ - for (int i=0;i<m_NumOverlapBroadphasePair;i++) - { - BroadphasePair& pair = m_OverlappingPairs[i]; - if (pair.m_pProxy0 == proxy || - pair.m_pProxy1 == proxy) - { - CleanOverlappingPair(pair); - } - } -} -void SimpleBroadphase::AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) -{ - //don't add overlap with own - assert(proxy0 != proxy1); - BroadphasePair pair(*proxy0,*proxy1); - m_OverlappingPairs[m_NumOverlapBroadphasePair] = pair; - int i; - for (i=0;i<SIMPLE_MAX_ALGORITHMS;i++) - { - assert(!m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i]); - m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i] = 0; - } - - if (m_NumOverlapBroadphasePair >= m_maxOverlap) - { - //printf("Error: too many overlapping objects: m_NumOverlapBroadphasePair: %d\n",m_NumOverlapBroadphasePair); -#ifdef DEBUG - assert(0); -#endif - } else - { - m_NumOverlapBroadphasePair++; - } - - -} -BroadphasePair* SimpleBroadphase::FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) -{ - BroadphasePair* foundPair = 0; - int i; - for (i=m_NumOverlapBroadphasePair-1;i>=0;i--) - { - BroadphasePair& pair = m_OverlappingPairs[i]; - if (((pair.m_pProxy0 == proxy0) && (pair.m_pProxy1 == proxy1)) || - ((pair.m_pProxy0 == proxy1) && (pair.m_pProxy1 == proxy0))) - { - foundPair = &pair; - return foundPair; - } - } - return foundPair; -} -void SimpleBroadphase::RemoveOverlappingPair(BroadphasePair& pair) -{ - CleanOverlappingPair(pair); - int index = &pair - &m_OverlappingPairs[0]; - //remove efficiently, swap with the last - m_OverlappingPairs[index] = m_OverlappingPairs[m_NumOverlapBroadphasePair-1]; - m_NumOverlapBroadphasePair--; -} bool SimpleBroadphase::AabbOverlap(SimpleBroadphaseProxy* proxy0,SimpleBroadphaseProxy* proxy1) { @@ -265,9 +173,10 @@ void SimpleBroadphase::RefreshOverlappingPairs() } //then remove non-overlapping ones - for (i=0;i<m_NumOverlapBroadphasePair;i++) + for (i=0;i<GetNumOverlappingPairs();i++) { - BroadphasePair& pair = m_OverlappingPairs[i]; + BroadphasePair& pair = GetOverlappingPair(i); + SimpleBroadphaseProxy* proxy0 = GetSimpleProxyFromProxy(pair.m_pProxy0); SimpleBroadphaseProxy* proxy1 = GetSimpleProxyFromProxy(pair.m_pProxy1); if (!AabbOverlap(proxy0,proxy1)) @@ -280,69 +189,4 @@ void SimpleBroadphase::RefreshOverlappingPairs() } -void SimpleBroadphase::DispatchAllCollisionPairs(Dispatcher& dispatcher,DispatcherInfo& dispatchInfo) -{ - m_blockedForChanges = true; - - int i; - - int dispatcherId = dispatcher.GetUniqueId(); - - RefreshOverlappingPairs(); - - for (i=0;i<m_NumOverlapBroadphasePair;i++) - { - - BroadphasePair& pair = m_OverlappingPairs[i]; - - if (dispatcherId>= 0) - { - //dispatcher will keep algorithms persistent in the collision pair - if (!pair.m_algorithms[dispatcherId]) - { - pair.m_algorithms[dispatcherId] = dispatcher.FindAlgorithm( - *pair.m_pProxy0, - *pair.m_pProxy1); - } - - if (pair.m_algorithms[dispatcherId]) - { - if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE) - { - pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); - } else - { - float toi = pair.m_algorithms[dispatcherId]->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); - if (dispatchInfo.m_timeOfImpact > toi) - dispatchInfo.m_timeOfImpact = toi; - - } - } - } else - { - //non-persistent algorithm dispatcher - CollisionAlgorithm* algo = dispatcher.FindAlgorithm( - *pair.m_pProxy0, - *pair.m_pProxy1); - - if (algo) - { - if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE) - { - algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); - } else - { - float toi = algo->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); - if (dispatchInfo.m_timeOfImpact > toi) - dispatchInfo.m_timeOfImpact = toi; - } - } - } - - } - - m_blockedForChanges = false; - -} - diff --git a/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.h b/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.h index b61f82b62a8..e67c7105687 100644 --- a/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.h +++ b/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.h @@ -16,12 +16,9 @@ subject to the following restrictions: #ifndef SIMPLE_BROADPHASE_H #define SIMPLE_BROADPHASE_H -//#define SIMPLE_MAX_PROXIES 8192 -//#define SIMPLE_MAX_OVERLAP 4096 -#include "BroadphaseInterface.h" -#include "BroadphaseProxy.h" -#include "SimdPoint3.h" +#include "OverlappingPairCache.h" + struct SimpleBroadphaseProxy : public BroadphaseProxy { @@ -30,8 +27,8 @@ struct SimpleBroadphaseProxy : public BroadphaseProxy SimpleBroadphaseProxy() {}; - SimpleBroadphaseProxy(const SimdPoint3& minpt,const SimdPoint3& maxpt,int shapeType,void* userPtr) - :BroadphaseProxy(shapeType,userPtr), + SimpleBroadphaseProxy(const SimdPoint3& minpt,const SimdPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask) + :BroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask), m_min(minpt),m_max(maxpt) { } @@ -40,7 +37,7 @@ struct SimpleBroadphaseProxy : public BroadphaseProxy }; ///SimpleBroadphase is a brute force aabb culling broadphase based on O(n^2) aabb checks -class SimpleBroadphase : public BroadphaseInterface +class SimpleBroadphase : public OverlappingPairCache { SimpleBroadphaseProxy* m_proxies; @@ -50,14 +47,10 @@ class SimpleBroadphase : public BroadphaseInterface SimpleBroadphaseProxy** m_pProxies; int m_numProxies; - //during the dispatch, check that user doesn't destroy/create proxy - bool m_blockedForChanges; - - BroadphasePair* m_OverlappingPairs; - int m_NumOverlapBroadphasePair; + int m_maxProxies; - int m_maxOverlap; + inline SimpleBroadphaseProxy* GetSimpleProxyFromProxy(BroadphaseProxy* proxy) { @@ -70,24 +63,24 @@ class SimpleBroadphase : public BroadphaseInterface void validate(); protected: - void RemoveOverlappingPair(BroadphasePair& pair); - void CleanOverlappingPair(BroadphasePair& pair); - void RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy); - void AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1); - BroadphasePair* FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1); virtual void RefreshOverlappingPairs(); public: SimpleBroadphase(int maxProxies=4096,int maxOverlap=8192); virtual ~SimpleBroadphase(); - virtual BroadphaseProxy* CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr); + + virtual BroadphaseProxy* CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask); + virtual void DestroyProxy(BroadphaseProxy* proxy); virtual void SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax); - virtual void CleanProxyFromPairs(BroadphaseProxy* proxy); - virtual void DispatchAllCollisionPairs(Dispatcher& dispatcher,DispatcherInfo& dispatchInfo); + + + + + }; diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionCreateFunc.h b/extern/bullet/Bullet/CollisionDispatch/CollisionCreateFunc.h new file mode 100644 index 00000000000..5df073bec87 --- /dev/null +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionCreateFunc.h @@ -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. +*/ + +#ifndef COLLISION_CREATE_FUNC +#define COLLISION_CREATE_FUNC + +#include <vector> + +typedef std::vector<struct CollisionObject*> CollisionObjectArray; +class CollisionAlgorithm; +struct BroadphaseProxy; + + + +struct CollisionAlgorithmCreateFunc +{ + bool m_swapped; + + CollisionAlgorithmCreateFunc() + :m_swapped(false) + { + } + virtual ~CollisionAlgorithmCreateFunc(){}; + + virtual CollisionAlgorithm* CreateCollisionAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) + { + return 0; + } +}; +#endif //COLLISION_CREATE_FUNC
\ No newline at end of file diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp index 5749db20a05..06e999fbe0d 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp @@ -21,40 +21,15 @@ subject to the following restrictions: #include "CollisionDispatch/ConvexConvexAlgorithm.h" #include "CollisionDispatch/EmptyCollisionAlgorithm.h" #include "CollisionDispatch/ConvexConcaveCollisionAlgorithm.h" +#include "CollisionDispatch/CompoundCollisionAlgorithm.h" #include "CollisionShapes/CollisionShape.h" #include "CollisionDispatch/CollisionObject.h" #include <algorithm> +#include "BroadphaseCollision/OverlappingPairCache.h" int gNumManifold = 0; -void CollisionDispatcher::FindUnions() -{ - if (m_useIslands) - { - for (int i=0;i<GetNumManifolds();i++) - { - const PersistentManifold* manifold = this->GetManifoldByIndexInternal(i); - //static objects (invmass 0.f) don't merge ! - - const CollisionObject* colObj0 = static_cast<const CollisionObject*>(manifold->GetBody0()); - const CollisionObject* colObj1 = static_cast<const CollisionObject*>(manifold->GetBody1()); - - if (colObj0 && colObj1 && NeedsResponse(*colObj0,*colObj1)) - { - if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && - ((colObj1) && ((colObj1)->mergesSimulationIslands()))) - { - m_unionFind.unite((colObj0)->m_islandTag1, - (colObj1)->m_islandTag1); - } - } - - - } - } - -} @@ -79,7 +54,9 @@ CollisionDispatcher::CollisionDispatcher (): PersistentManifold* CollisionDispatcher::GetNewManifold(void* b0,void* b1) { gNumManifold++; - //printf("GetNewManifoldResult: gNumManifold %d\n",gNumManifold); + + //ASSERT(gNumManifold < 65535); + CollisionObject* body0 = (CollisionObject*)b0; CollisionObject* body1 = (CollisionObject*)b1; @@ -119,96 +96,6 @@ void CollisionDispatcher::ReleaseManifold(PersistentManifold* manifold) } -// -// todo: this is random access, it can be walked 'cache friendly'! -// -void CollisionDispatcher::BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback) -{ - int numBodies = collisionObjects.size(); - - for (int islandId=0;islandId<numBodies;islandId++) - { - - std::vector<PersistentManifold*> islandmanifold; - - //int numSleeping = 0; - - bool allSleeping = true; - - int i; - for (i=0;i<numBodies;i++) - { - CollisionObject* colObj0 = collisionObjects[i]; - if (colObj0->m_islandTag1 == islandId) - { - if (colObj0->GetActivationState()== ACTIVE_TAG) - { - allSleeping = false; - } - if (colObj0->GetActivationState()== DISABLE_DEACTIVATION) - { - allSleeping = false; - } - } - } - - - for (i=0;i<GetNumManifolds();i++) - { - PersistentManifold* manifold = this->GetManifoldByIndexInternal(i); - - //filtering for response - - CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0()); - CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1()); - { - if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) || - ((colObj1) && (colObj1)->m_islandTag1 == (islandId))) - { - - if (NeedsResponse(*colObj0,*colObj1)) - islandmanifold.push_back(manifold); - } - } - } - if (allSleeping) - { - int i; - for (i=0;i<numBodies;i++) - { - CollisionObject* colObj0 = collisionObjects[i]; - if (colObj0->m_islandTag1 == islandId) - { - colObj0->SetActivationState( ISLAND_SLEEPING ); - } - } - - - } else - { - - int i; - for (i=0;i<numBodies;i++) - { - CollisionObject* colObj0 = collisionObjects[i]; - if (colObj0->m_islandTag1 == islandId) - { - if ( colObj0->GetActivationState() == ISLAND_SLEEPING) - { - colObj0->SetActivationState( WANTS_DEACTIVATION); - } - } - } - - /// Process the actual simulation, only if not sleeping/deactivated - if (islandmanifold.size()) - { - callback->ProcessIsland(&islandmanifold[0],islandmanifold.size()); - } - - } - } -} @@ -236,6 +123,17 @@ CollisionAlgorithm* CollisionDispatcher::InternalFindAlgorithm(BroadphaseProxy& return new ConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0); } + if (body0->m_collisionShape->IsCompound()) + { + return new CompoundCollisionAlgorithm(ci,&proxy0,&proxy1); + } else + { + if (body1->m_collisionShape->IsCompound()) + { + return new CompoundCollisionAlgorithm(ci,&proxy1,&proxy0); + } + } + //failed to find an algorithm return new EmptyAlgorithm(ci); @@ -291,3 +189,69 @@ void CollisionDispatcher::ReleaseManifoldResult(ManifoldResult*) { } + + +void CollisionDispatcher::DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo) +{ + //m_blockedForChanges = true; + + int i; + + int dispatcherId = GetUniqueId(); + + + + for (i=0;i<numPairs;i++) + { + + BroadphasePair& pair = pairs[i]; + + if (dispatcherId>= 0) + { + //dispatcher will keep algorithms persistent in the collision pair + if (!pair.m_algorithms[dispatcherId]) + { + pair.m_algorithms[dispatcherId] = FindAlgorithm( + *pair.m_pProxy0, + *pair.m_pProxy1); + } + + if (pair.m_algorithms[dispatcherId]) + { + if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE) + { + pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + } else + { + float toi = pair.m_algorithms[dispatcherId]->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; + + } + } + } else + { + //non-persistent algorithm dispatcher + CollisionAlgorithm* algo = FindAlgorithm( + *pair.m_pProxy0, + *pair.m_pProxy1); + + if (algo) + { + if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE) + { + algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + } else + { + float toi = algo->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; + } + } + } + + } + + //m_blockedForChanges = false; + +}
\ No newline at end of file diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h index d559f77bebe..773978b9218 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h @@ -18,32 +18,19 @@ subject to the following restrictions: #include "BroadphaseCollision/Dispatcher.h" #include "NarrowPhaseCollision/PersistentManifold.h" -#include "CollisionDispatch/UnionFind.h" + #include "CollisionDispatch/ManifoldResult.h" #include "BroadphaseCollision/BroadphaseProxy.h" -#include <vector> + class IDebugDraw; +class OverlappingPairCache; -typedef std::vector<struct CollisionObject*> CollisionObjectArray; +#include "CollisionCreateFunc.h" -struct CollisionAlgorithmCreateFunc -{ - bool m_swapped; - - CollisionAlgorithmCreateFunc() - :m_swapped(false) - { - } - virtual ~CollisionAlgorithmCreateFunc(){}; - virtual CollisionAlgorithm* CreateCollisionAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) - { - return 0; - } -}; ///CollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs. @@ -53,7 +40,7 @@ class CollisionDispatcher : public Dispatcher std::vector<PersistentManifold*> m_manifoldsPtr; - UnionFind m_unionFind; + bool m_useIslands; @@ -63,14 +50,9 @@ class CollisionDispatcher : public Dispatcher public: - UnionFind& GetUnionFind() { return m_unionFind;} - - struct IslandCallback - { - virtual ~IslandCallback() {}; + - virtual void ProcessIsland(PersistentManifold** manifolds,int numManifolds) = 0; - }; + int GetNumManifolds() const @@ -88,14 +70,6 @@ public: return m_manifoldsPtr[index]; } - void InitUnionFind(int n) - { - if (m_useIslands) - m_unionFind.reset(n); - } - - void FindUnions(); - int m_count; CollisionDispatcher (); @@ -106,8 +80,6 @@ public: virtual void ReleaseManifold(PersistentManifold* manifold); - virtual void BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback); - ///allows the user to get contact point callbacks virtual ManifoldResult* GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold); @@ -131,6 +103,8 @@ public: virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;} + virtual void DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo); + }; diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp index 056dce59856..a96707f5e86 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp @@ -21,8 +21,11 @@ CollisionObject::CollisionObject() m_deactivationTime(0.f), m_broadphaseHandle(0), m_collisionShape(0), - m_hitFraction(1.f) + m_hitFraction(1.f), + m_ccdSweptShereRadius(0.f), + m_ccdSquareMotionTreshold(0.f) { + m_cachedInvertedWorldTransform.setIdentity(); } diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionObject.h b/extern/bullet/Bullet/CollisionDispatch/CollisionObject.h index 0c2d0be460c..da4e9b934f5 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionObject.h +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionObject.h @@ -28,6 +28,9 @@ subject to the following restrictions: struct BroadphaseProxy; class CollisionShape; +/// CollisionObject can be used to manage collision detection objects. +/// CollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. +/// They can be added to the CollisionWorld. struct CollisionObject { SimdTransform m_worldTransform; @@ -35,12 +38,14 @@ struct CollisionObject //m_interpolationWorldTransform is used for CCD and interpolation //it can be either previous or future (predicted) transform SimdTransform m_interpolationWorldTransform; - + + SimdTransform m_cachedInvertedWorldTransform; + enum CollisionFlags { isStatic = 1, noContactResponse = 2, - + customMaterialCallback = 4,//this allows per-triangle material (friction/restitution) }; int m_collisionFlags; @@ -49,13 +54,22 @@ struct CollisionObject int m_activationState1; float m_deactivationTime; + SimdScalar m_friction; + SimdScalar m_restitution; + BroadphaseProxy* m_broadphaseHandle; CollisionShape* m_collisionShape; void* m_userPointer;//not use by Bullet internally - //time of impact calculation + ///time of impact calculation float m_hitFraction; + + ///Swept sphere radius (0.0 by default), see ConvexConvexAlgorithm:: + float m_ccdSweptShereRadius; + + /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold + float m_ccdSquareMotionTreshold; bool mergesSimulationIslands() const; @@ -91,6 +105,23 @@ struct CollisionObject return ((GetActivationState() != ISLAND_SLEEPING) && (GetActivationState() != DISABLE_SIMULATION)); } + void setRestitution(float rest) + { + m_restitution = rest; + } + float getRestitution() const + { + return m_restitution; + } + void setFriction(float frict) + { + m_friction = frict; + } + float getFriction() const + { + return m_friction; + } + }; diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp index e8f73b2cc07..bc4413c3427 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp @@ -20,6 +20,7 @@ subject to the following restrictions: #include "CollisionShapes/SphereShape.h" //for raycasting #include "CollisionShapes/TriangleMeshShape.h" //for raycasting #include "NarrowPhaseCollision/RaycastCallback.h" +#include "CollisionShapes/CompoundShape.h" #include "NarrowPhaseCollision/SubSimplexConvexCast.h" #include "BroadphaseCollision/BroadphaseInterface.h" @@ -52,67 +53,16 @@ CollisionWorld::~CollisionWorld() } -void CollisionWorld::UpdateActivationState() -{ - m_dispatcher->InitUnionFind(m_collisionObjects.size()); - - // put the index into m_controllers into m_tag - { - std::vector<CollisionObject*>::iterator i; - - int index = 0; - for (i=m_collisionObjects.begin(); - !(i==m_collisionObjects.end()); i++) - { - - CollisionObject* collisionObject= (*i); - collisionObject->m_islandTag1 = index; - collisionObject->m_hitFraction = 1.f; - index++; - - } - } - // do the union find - - m_dispatcher->FindUnions(); - - -} -void CollisionWorld::StoreIslandActivationState() -{ - // put the islandId ('find' value) into m_tag - { - UnionFind& unionFind = m_dispatcher->GetUnionFind(); - - std::vector<CollisionObject*>::iterator i; - - int index = 0; - for (i=m_collisionObjects.begin(); - !(i==m_collisionObjects.end()); i++) - { - CollisionObject* collisionObject= (*i); - - if (collisionObject->mergesSimulationIslands()) - { - collisionObject->m_islandTag1 = unionFind.find(index); - } else - { - collisionObject->m_islandTag1 = -1; - } - index++; - } - } -} -void CollisionWorld::AddCollisionObject(CollisionObject* collisionObject) +void CollisionWorld::AddCollisionObject(CollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) { m_collisionObjects.push_back(collisionObject); @@ -128,7 +78,9 @@ void CollisionWorld::AddCollisionObject(CollisionObject* collisionObject) minAabb, maxAabb, type, - collisionObject + collisionObject, + collisionFilterGroup, + collisionFilterMask ); @@ -148,10 +100,13 @@ void CollisionWorld::PerformDiscreteCollisionDetection() for (size_t i=0;i<m_collisionObjects.size();i++) { m_collisionObjects[i]->m_collisionShape->GetAabb(m_collisionObjects[i]->m_worldTransform,aabbMin,aabbMax); - m_broadphase->SetAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax); + m_pairCache->SetAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax); } - m_broadphase->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo); + Dispatcher* dispatcher = GetDispatcher(); + if (dispatcher) + dispatcher->DispatchAllCollisionPairs(&m_pairCache->GetOverlappingPair(0),m_pairCache->GetNumOverlappingPairs(),dispatchInfo); + } @@ -185,58 +140,27 @@ void CollisionWorld::RemoveCollisionObject(CollisionObject* collisionObject) } } - - -void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback) +void RayTestSingle(const SimdTransform& rayFromTrans,const SimdTransform& rayToTrans, + CollisionObject* collisionObject, + const CollisionShape* collisionShape, + const SimdTransform& colObjWorldTransform, + CollisionWorld::RayResultCallback& resultCallback) { - - - SimdTransform rayFromTrans,rayToTrans; - rayFromTrans.setIdentity(); - rayFromTrans.setOrigin(rayFromWorld); - rayToTrans.setIdentity(); - rayToTrans.setOrigin(rayToWorld); - - //do culling based on aabb (rayFrom/rayTo) - SimdVector3 rayAabbMin = rayFromWorld; - SimdVector3 rayAabbMax = rayFromWorld; - rayAabbMin.setMin(rayToWorld); - rayAabbMax.setMax(rayToWorld); - SphereShape pointShape(0.0f); - /// brute force go over all objects. Once there is a broadphase, use that, or - /// add a raycast against aabb first. - - std::vector<CollisionObject*>::iterator iter; - - for (iter=m_collisionObjects.begin(); - !(iter==m_collisionObjects.end()); iter++) - { - - CollisionObject* collisionObject= (*iter); - - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax); - - //check aabb overlap - - if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax)) - { - if (collisionObject->m_collisionShape->IsConvex()) + if (collisionShape->IsConvex()) { ConvexCast::CastResult castResult; castResult.m_fraction = 1.f;//?? - ConvexShape* convexShape = (ConvexShape*) collisionObject->m_collisionShape; + ConvexShape* convexShape = (ConvexShape*) collisionShape; VoronoiSimplexSolver simplexSolver; SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); - if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,collisionObject->m_worldTransform,collisionObject->m_worldTransform,castResult)) + if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) { //add hit if (castResult.m_normal.length2() > 0.0001f) @@ -263,12 +187,12 @@ void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& else { - if (collisionObject->m_collisionShape->IsConcave()) + if (collisionShape->IsConcave()) { - TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionObject->m_collisionShape; + TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionShape; - SimdTransform worldTocollisionObject = collisionObject->m_worldTransform.inverse(); + SimdTransform worldTocollisionObject = colObjWorldTransform.inverse(); SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); @@ -277,12 +201,12 @@ void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback { - RayResultCallback* m_resultCallback; + CollisionWorld::RayResultCallback* m_resultCallback; CollisionObject* m_collisionObject; TriangleMeshShape* m_triangleMesh; BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to, - RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape* triangleMesh): + CollisionWorld::RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape* triangleMesh): TriangleRaycastCallback(from,to), m_resultCallback(resultCallback), m_collisionObject(collisionObject), @@ -321,10 +245,75 @@ void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); - } - + } else + { + //todo: use AABB tree or other BVH acceleration structure! + if (collisionShape->IsCompound()) + { + const CompoundShape* compoundShape = static_cast<const CompoundShape*>(collisionShape); + int i=0; + for (i=0;i<compoundShape->GetNumChildShapes();i++) + { + SimdTransform childTrans = compoundShape->GetChildTransform(i); + const CollisionShape* childCollisionShape = compoundShape->GetChildShape(i); + SimdTransform childWorldTrans = colObjWorldTransform * childTrans; + RayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + childCollisionShape, + childWorldTrans, + resultCallback); + } + + + } + } } +} + +void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback) +{ + + + SimdTransform rayFromTrans,rayToTrans; + rayFromTrans.setIdentity(); + rayFromTrans.setOrigin(rayFromWorld); + rayToTrans.setIdentity(); + + rayToTrans.setOrigin(rayToWorld); + + //do culling based on aabb (rayFrom/rayTo) + SimdVector3 rayAabbMin = rayFromWorld; + SimdVector3 rayAabbMax = rayFromWorld; + rayAabbMin.setMin(rayToWorld); + rayAabbMax.setMax(rayToWorld); + + + /// brute force go over all objects. Once there is a broadphase, use that, or + /// add a raycast against aabb first. + + std::vector<CollisionObject*>::iterator iter; + + for (iter=m_collisionObjects.begin(); + !(iter==m_collisionObjects.end()); iter++) + { + + CollisionObject* collisionObject= (*iter); + + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax); + + //check aabb overlap + + if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax)) + { + RayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + collisionObject->m_collisionShape, + collisionObject->m_worldTransform, + resultCallback); + } } diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h index 1589da6da69..8414ba4e459 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h @@ -70,6 +70,7 @@ class BroadphaseInterface; #include "SimdTransform.h" #include "CollisionObject.h" #include "CollisionDispatcher.h" //for definition of CollisionObjectArray +#include "BroadphaseCollision/OverlappingPairCache.h" #include <vector> @@ -83,31 +84,36 @@ class CollisionWorld std::vector<CollisionObject*> m_collisionObjects; - CollisionDispatcher* m_dispatcher; + Dispatcher* m_dispatcher1; - BroadphaseInterface* m_broadphase; + OverlappingPairCache* m_pairCache; + public: - CollisionWorld(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase) - :m_dispatcher(dispatcher), - m_broadphase(broadphase) + CollisionWorld(Dispatcher* dispatcher,OverlappingPairCache* pairCache) + :m_dispatcher1(dispatcher), + m_pairCache(pairCache) { } virtual ~CollisionWorld(); - virtual void UpdateActivationState(); - virtual void StoreIslandActivationState(); BroadphaseInterface* GetBroadphase() { - return m_broadphase; + return m_pairCache; } - CollisionDispatcher* GetDispatcher() + OverlappingPairCache* GetPairCache() + { + return m_pairCache; + } + + + Dispatcher* GetDispatcher() { - return m_dispatcher; + return m_dispatcher1; } ///LocalShapeInfo gives extra information for complex shapes @@ -201,7 +207,7 @@ public: void RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback); - void AddCollisionObject(CollisionObject* collisionObject); + void AddCollisionObject(CollisionObject* collisionObject,short int collisionFilterGroup=1,short int collisionFilterMask=1); CollisionObjectArray& GetCollisionObjectArray() { diff --git a/extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp b/extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp new file mode 100644 index 00000000000..0a79d2b7b91 --- /dev/null +++ b/extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp @@ -0,0 +1,144 @@ +/* +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 "CollisionDispatch/CompoundCollisionAlgorithm.h" +#include "CollisionDispatch/CollisionObject.h" +#include "CollisionShapes/CompoundShape.h" + + +CompoundCollisionAlgorithm::CompoundCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) +:m_dispatcher(ci.m_dispatcher), +m_compoundProxy(*proxy0), +m_otherProxy(*proxy1) +{ + CollisionObject* colObj = static_cast<CollisionObject*>(m_compoundProxy.m_clientObject); + assert (colObj->m_collisionShape->IsCompound()); + + CompoundShape* compoundShape = static_cast<CompoundShape*>(colObj->m_collisionShape); + int numChildren = compoundShape->GetNumChildShapes(); + m_childProxies.resize( numChildren ); + int i; + for (i=0;i<numChildren;i++) + { + m_childProxies[i] = BroadphaseProxy(*proxy0); + } + + m_childCollisionAlgorithms.resize(numChildren); + for (i=0;i<numChildren;i++) + { + CollisionShape* childShape = compoundShape->GetChildShape(i); + CollisionObject* colObj = static_cast<CollisionObject*>(m_childProxies[i].m_clientObject); + CollisionShape* orgShape = colObj->m_collisionShape; + colObj->m_collisionShape = childShape; + m_childCollisionAlgorithms[i] = m_dispatcher->FindAlgorithm(m_childProxies[i],m_otherProxy); + colObj->m_collisionShape =orgShape; + } +} + + +CompoundCollisionAlgorithm::~CompoundCollisionAlgorithm() +{ + int numChildren = m_childCollisionAlgorithms.size(); + int i; + for (i=0;i<numChildren;i++) + { + delete m_childCollisionAlgorithms[i]; + } +} + +void CompoundCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo) +{ + CollisionObject* colObj = static_cast<CollisionObject*>(m_compoundProxy.m_clientObject); + assert (colObj->m_collisionShape->IsCompound()); + + CompoundShape* compoundShape = static_cast<CompoundShape*>(colObj->m_collisionShape); + + //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps + //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals + //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: + //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 + //then use each overlapping node AABB against Tree0 + //and vise versa. + + + int numChildren = m_childCollisionAlgorithms.size(); + int i; + for (i=0;i<numChildren;i++) + { + //temporarily exchange parent CollisionShape with childShape, and recurse + CollisionShape* childShape = compoundShape->GetChildShape(i); + CollisionObject* colObj = static_cast<CollisionObject*>(m_childProxies[i].m_clientObject); + + //backup + SimdTransform orgTrans = colObj->m_worldTransform; + CollisionShape* orgShape = colObj->m_collisionShape; + + SimdTransform childTrans = compoundShape->GetChildTransform(i); + SimdTransform newChildWorldTrans = orgTrans*childTrans ; + colObj->m_worldTransform = newChildWorldTrans; + + colObj->m_collisionShape = childShape; + m_childCollisionAlgorithms[i]->ProcessCollision(&m_childProxies[i],&m_otherProxy,dispatchInfo); + //revert back + colObj->m_collisionShape =orgShape; + colObj->m_worldTransform = orgTrans; + } +} + +float CompoundCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo) +{ + CollisionObject* colObj = static_cast<CollisionObject*>(m_compoundProxy.m_clientObject); + assert (colObj->m_collisionShape->IsCompound()); + + CompoundShape* compoundShape = static_cast<CompoundShape*>(colObj->m_collisionShape); + + //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps + //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals + //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: + //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 + //then use each overlapping node AABB against Tree0 + //and vise versa. + + float hitFraction = 1.f; + + int numChildren = m_childCollisionAlgorithms.size(); + int i; + for (i=0;i<numChildren;i++) + { + //temporarily exchange parent CollisionShape with childShape, and recurse + CollisionShape* childShape = compoundShape->GetChildShape(i); + CollisionObject* colObj = static_cast<CollisionObject*>(m_childProxies[i].m_clientObject); + + //backup + SimdTransform orgTrans = colObj->m_worldTransform; + CollisionShape* orgShape = colObj->m_collisionShape; + + SimdTransform childTrans = compoundShape->GetChildTransform(i); + SimdTransform newChildWorldTrans = orgTrans*childTrans ; + colObj->m_worldTransform = newChildWorldTrans; + + colObj->m_collisionShape = childShape; + float frac = m_childCollisionAlgorithms[i]->CalculateTimeOfImpact(&m_childProxies[i],&m_otherProxy,dispatchInfo); + if (frac<hitFraction) + { + hitFraction = frac; + } + //revert back + colObj->m_collisionShape =orgShape; + colObj->m_worldTransform = orgTrans; + } + return hitFraction; + +}
\ No newline at end of file diff --git a/extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.h b/extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.h new file mode 100644 index 00000000000..a41d3a8f455 --- /dev/null +++ b/extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.h @@ -0,0 +1,56 @@ +/* +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 COMPOUND_COLLISION_ALGORITHM_H +#define COMPOUND_COLLISION_ALGORITHM_H + +#include "BroadphaseCollision/CollisionAlgorithm.h" +#include "BroadphaseCollision/Dispatcher.h" +#include "BroadphaseCollision/BroadphaseInterface.h" + +#include "NarrowPhaseCollision/PersistentManifold.h" +class Dispatcher; +#include "BroadphaseCollision/BroadphaseProxy.h" +#include <vector> + + +/// CompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes +/// Place holder, not fully implemented yet +class CompoundCollisionAlgorithm : public CollisionAlgorithm +{ + BroadphaseProxy m_compoundProxy; + BroadphaseProxy m_otherProxy; + std::vector<BroadphaseProxy> m_childProxies; + std::vector<CollisionAlgorithm*> m_childCollisionAlgorithms; + + Dispatcher* m_dispatcher; + BroadphaseProxy m_compound; + + BroadphaseProxy m_other; + + +public: + + CompoundCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1); + + virtual ~CompoundCollisionAlgorithm(); + + virtual void ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo); + + float CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo); + +}; + +#endif //COMPOUND_COLLISION_ALGORITHM_H diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp b/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp index a72432ef952..63ed29888ab 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp @@ -17,18 +17,19 @@ subject to the following restrictions: #include "ConvexConcaveCollisionAlgorithm.h" #include "CollisionDispatch/CollisionObject.h" #include "CollisionShapes/MultiSphereShape.h" -#include "CollisionShapes/BoxShape.h" #include "ConvexConvexAlgorithm.h" #include "BroadphaseCollision/BroadphaseProxy.h" -#include "CollisionShapes/TriangleShape.h" +#include "CollisionShapes/ConcaveShape.h" #include "CollisionDispatch/ManifoldResult.h" #include "NarrowPhaseCollision/RaycastCallback.h" -#include "CollisionShapes/TriangleMeshShape.h" - +#include "CollisionShapes/TriangleShape.h" +#include "CollisionShapes/SphereShape.h" +#include "IDebugDraw.h" +#include "NarrowPhaseCollision/SubSimplexConvexCast.h" ConvexConcaveCollisionAlgorithm::ConvexConcaveCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) : CollisionAlgorithm(ci),m_convex(*proxy0),m_concave(*proxy1), -m_boxTriangleCallback(ci.m_dispatcher,proxy0,proxy1) +m_ConvexTriangleCallback(ci.m_dispatcher,proxy0,proxy1) { } @@ -39,8 +40,8 @@ ConvexConcaveCollisionAlgorithm::~ConvexConcaveCollisionAlgorithm() -BoxTriangleCallback::BoxTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1): - m_boxProxy(proxy0),m_triangleProxy(*proxy1),m_dispatcher(dispatcher), +ConvexTriangleCallback::ConvexTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1): + m_convexProxy(proxy0),m_triangleProxy(*proxy1),m_dispatcher(dispatcher), m_dispatchInfoPtr(0) { @@ -52,7 +53,7 @@ BoxTriangleCallback::BoxTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy ClearCache(); } -BoxTriangleCallback::~BoxTriangleCallback() +ConvexTriangleCallback::~ConvexTriangleCallback() { ClearCache(); m_dispatcher->ReleaseManifold( m_manifoldPtr ); @@ -60,14 +61,14 @@ BoxTriangleCallback::~BoxTriangleCallback() } -void BoxTriangleCallback::ClearCache() +void ConvexTriangleCallback::ClearCache() { m_dispatcher->ClearManifold(m_manifoldPtr); }; -void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex) +void ConvexTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex) { //just for debugging purposes @@ -79,22 +80,42 @@ void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int CollisionAlgorithmConstructionInfo ci; ci.m_dispatcher = m_dispatcher; + CollisionObject* ob = static_cast<CollisionObject*>(m_triangleProxy.m_clientObject); + + + ///debug drawing of the overlapping triangles + if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->GetDebugMode() > 0) + { + SimdVector3 color(255,255,0); + SimdTransform& tr = ob->m_worldTransform; + m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[0]),tr(triangle[1]),color); + m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[1]),tr(triangle[2]),color); + m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[2]),tr(triangle[0]),color); + + //SimdVector3 center = triangle[0] + triangle[1]+triangle[2]; + //center *= 0.333333f; + //m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[0]),tr(center),color); + //m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[1]),tr(center),color); + //m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[2]),tr(center),color); + + } + - CollisionObject* colObj = static_cast<CollisionObject*>(m_boxProxy->m_clientObject); + CollisionObject* colObj = static_cast<CollisionObject*>(m_convexProxy->m_clientObject); if (colObj->m_collisionShape->IsConvex()) { TriangleShape tm(triangle[0],triangle[1],triangle[2]); tm.SetMargin(m_collisionMarginTriangle); - CollisionObject* ob = static_cast<CollisionObject*>(m_triangleProxy.m_clientObject); - + CollisionShape* tmpShape = ob->m_collisionShape; ob->m_collisionShape = &tm; - ConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_boxProxy,&m_triangleProxy); - cvxcvxalgo.ProcessCollision(m_boxProxy,&m_triangleProxy,*m_dispatchInfoPtr); + ConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexProxy,&m_triangleProxy); + cvxcvxalgo.SetShapeIdentifiers(-1,-1,partId,triangleIndex); + cvxcvxalgo.ProcessCollision(m_convexProxy,&m_triangleProxy,*m_dispatchInfoPtr); ob->m_collisionShape = tmpShape; } @@ -105,22 +126,22 @@ void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int -void BoxTriangleCallback::SetTimeStepAndCounters(float collisionMarginTriangle,const DispatcherInfo& dispatchInfo) +void ConvexTriangleCallback::SetTimeStepAndCounters(float collisionMarginTriangle,const DispatcherInfo& dispatchInfo) { m_dispatchInfoPtr = &dispatchInfo; m_collisionMarginTriangle = collisionMarginTriangle; //recalc aabbs - CollisionObject* boxBody = (CollisionObject* )m_boxProxy->m_clientObject; + CollisionObject* convexBody = (CollisionObject* )m_convexProxy->m_clientObject; CollisionObject* triBody = (CollisionObject* )m_triangleProxy.m_clientObject; - SimdTransform boxInTriangleSpace; - boxInTriangleSpace = triBody->m_worldTransform.inverse() * boxBody->m_worldTransform; + SimdTransform convexInTriangleSpace; + convexInTriangleSpace = triBody->m_worldTransform.inverse() * convexBody->m_worldTransform; - CollisionShape* boxshape = static_cast<CollisionShape*>(boxBody->m_collisionShape); + CollisionShape* convexShape = static_cast<CollisionShape*>(convexBody->m_collisionShape); //CollisionShape* triangleShape = static_cast<CollisionShape*>(triBody->m_collisionShape); - boxshape->GetAabb(boxInTriangleSpace,m_aabbMin,m_aabbMax); + convexShape->GetAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); float extraMargin = collisionMarginTriangle;//CONVEX_DISTANCE_MARGIN;//+0.1f; @@ -133,17 +154,17 @@ void BoxTriangleCallback::SetTimeStepAndCounters(float collisionMarginTriangle,c void ConvexConcaveCollisionAlgorithm::ClearCache() { - m_boxTriangleCallback.ClearCache(); + m_ConvexTriangleCallback.ClearCache(); } void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo) { - - CollisionObject* boxBody = static_cast<CollisionObject* >(m_convex.m_clientObject); + + CollisionObject* convexBody = static_cast<CollisionObject* >(m_convex.m_clientObject); CollisionObject* triBody = static_cast<CollisionObject* >(m_concave.m_clientObject); - if (triBody->m_collisionShape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + if (triBody->m_collisionShape->IsConcave()) { if (!m_dispatcher->NeedsCollision(m_convex,m_concave)) @@ -152,19 +173,21 @@ void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,Broadp CollisionObject* triOb = static_cast<CollisionObject*>(m_concave.m_clientObject); - TriangleMeshShape* triangleMesh = static_cast<TriangleMeshShape*>( triOb->m_collisionShape); + ConcaveShape* concaveShape = static_cast<ConcaveShape*>( triOb->m_collisionShape); - if (boxBody->m_collisionShape->IsConvex()) + if (convexBody->m_collisionShape->IsConvex()) { - float collisionMarginTriangle = triangleMesh->GetMargin(); + float collisionMarginTriangle = concaveShape->GetMargin(); - m_boxTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle,dispatchInfo); -#ifdef USE_BOX_TRIANGLE - m_dispatcher->ClearManifold(m_boxTriangleCallback.m_manifoldPtr); -#endif - m_boxTriangleCallback.m_manifoldPtr->SetBodies(m_convex.m_clientObject,m_concave.m_clientObject); + m_ConvexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle,dispatchInfo); + + //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. + //m_dispatcher->ClearManifold(m_ConvexTriangleCallback.m_manifoldPtr); - triangleMesh->ProcessAllTriangles( &m_boxTriangleCallback,m_boxTriangleCallback.GetAabbMin(),m_boxTriangleCallback.GetAabbMax()); + + m_ConvexTriangleCallback.m_manifoldPtr->SetBodies(m_convex.m_clientObject,m_concave.m_clientObject); + + concaveShape->ProcessAllTriangles( &m_ConvexTriangleCallback,m_ConvexTriangleCallback.GetAabbMin(),m_ConvexTriangleCallback.GetAabbMax()); } @@ -181,52 +204,101 @@ float ConvexConcaveCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* ,B CollisionObject* convexbody = (CollisionObject* )m_convex.m_clientObject; CollisionObject* triBody = static_cast<CollisionObject* >(m_concave.m_clientObject); - const SimdVector3& from = convexbody->m_worldTransform.getOrigin(); - - SimdVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); + //only perform CCD above a certain treshold, this prevents blocking on the long run + //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... + float squareMot0 = (convexbody->m_interpolationWorldTransform.getOrigin() - convexbody->m_worldTransform.getOrigin()).length2(); + if (squareMot0 < convexbody->m_ccdSquareMotionTreshold) + { + return 1.f; + } + + //const SimdVector3& from = convexbody->m_worldTransform.getOrigin(); + //SimdVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); //todo: only do if the motion exceeds the 'radius' - struct LocalTriangleRaycastCallback : public TriangleRaycastCallback + SimdTransform convexFromLocal = triBody->m_cachedInvertedWorldTransform * convexbody->m_worldTransform; + SimdTransform convexToLocal = triBody->m_cachedInvertedWorldTransform * convexbody->m_interpolationWorldTransform; + + struct LocalTriangleSphereCastCallback : public TriangleCallback { - LocalTriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to) - :TriangleRaycastCallback(from,to) - { + SimdTransform m_ccdSphereFromTrans; + SimdTransform m_ccdSphereToTrans; + SimdTransform m_meshTransform; + + float m_ccdSphereRadius; + float m_hitFraction; + + + LocalTriangleSphereCastCallback(const SimdTransform& from,const SimdTransform& to,float ccdSphereRadius,float hitFraction) + :m_ccdSphereFromTrans(from), + m_ccdSphereToTrans(to), + m_ccdSphereRadius(ccdSphereRadius), + m_hitFraction(hitFraction) + { } - virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) + + virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex) { - //todo: handle ccd here - return 0.f; + //do a swept sphere for now + SimdTransform ident; + ident.setIdentity(); + ConvexCast::CastResult castResult; + castResult.m_fraction = m_hitFraction; + SphereShape pointShape(m_ccdSphereRadius); + TriangleShape triShape(triangle[0],triangle[1],triangle[2]); + VoronoiSimplexSolver simplexSolver; + SubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); + //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); + //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); + //local space? + + if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, + ident,ident,castResult)) + { + if (m_hitFraction > castResult.m_fraction) + m_hitFraction = castResult.m_fraction; + } } + }; - LocalTriangleRaycastCallback raycastCallback(from,to); + - raycastCallback.m_hitFraction = convexbody->m_hitFraction; + + if (triBody->m_collisionShape->IsConcave()) + { + SimdVector3 rayAabbMin = convexFromLocal.getOrigin(); + rayAabbMin.setMin(convexToLocal.getOrigin()); + SimdVector3 rayAabbMax = convexFromLocal.getOrigin(); + rayAabbMax.setMax(convexToLocal.getOrigin()); + rayAabbMin -= SimdVector3(convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius); + rayAabbMax += SimdVector3(convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius); - SimdVector3 aabbMin (-1e30f,-1e30f,-1e30f); - SimdVector3 aabbMax (SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + float curHitFraction = 1.f; //is this available? + LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, + convexbody->m_ccdSweptShereRadius,curHitFraction); - if (triBody->m_collisionShape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) - { + raycastCallback.m_hitFraction = convexbody->m_hitFraction; CollisionObject* concavebody = (CollisionObject* )m_concave.m_clientObject; - TriangleMeshShape* triangleMesh = (TriangleMeshShape*) concavebody->m_collisionShape; + ConcaveShape* triangleMesh = (ConcaveShape*) concavebody->m_collisionShape; if (triangleMesh) { - triangleMesh->ProcessAllTriangles(&raycastCallback,aabbMin,aabbMax); + triangleMesh->ProcessAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); } - } + - if (raycastCallback.m_hitFraction < convexbody->m_hitFraction) - { - convexbody->m_hitFraction = raycastCallback.m_hitFraction; - return raycastCallback.m_hitFraction; + if (raycastCallback.m_hitFraction < convexbody->m_hitFraction) + { + convexbody->m_hitFraction = raycastCallback.m_hitFraction; + return raycastCallback.m_hitFraction; + } } return 1.f; diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h b/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h index 18652334b4e..47b4de3bbe1 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h +++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h @@ -25,10 +25,10 @@ class Dispatcher; #include "BroadphaseCollision/BroadphaseProxy.h" - -class BoxTriangleCallback : public TriangleCallback +///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), ProcessTriangle is called. +class ConvexTriangleCallback : public TriangleCallback { - BroadphaseProxy* m_boxProxy; + BroadphaseProxy* m_convexProxy; BroadphaseProxy m_triangleProxy; SimdVector3 m_aabbMin; @@ -43,11 +43,11 @@ int m_triangleCount; PersistentManifold* m_manifoldPtr; - BoxTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1); + ConvexTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1); void SetTimeStepAndCounters(float collisionMarginTriangle,const DispatcherInfo& dispatchInfo); - virtual ~BoxTriangleCallback(); + virtual ~ConvexTriangleCallback(); virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex); @@ -75,7 +75,7 @@ class ConvexConcaveCollisionAlgorithm : public CollisionAlgorithm BroadphaseProxy m_concave; - BoxTriangleCallback m_boxTriangleCallback; + ConvexTriangleCallback m_ConvexTriangleCallback; public: diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp index 9c20f28d1ec..765ae8625f6 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp @@ -233,8 +233,9 @@ public: }; #endif //USE_HULL + // -// box-box collision algorithm, for simplicity also applies resolution-impulse +// Convex-Convex collision algorithm // void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo) { @@ -314,7 +315,7 @@ void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy input.m_maximumDistanceSquared = min0->GetMargin() + min1->GetMargin() + m_manifoldPtr->GetContactBreakingTreshold(); input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; - //input.m_maximumDistanceSquared = 1e30f;// +// input.m_maximumDistanceSquared = 1e30f; input.m_transformA = col0->m_worldTransform; input.m_transformB = col1->m_worldTransform; @@ -323,15 +324,37 @@ void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy m_dispatcher->ReleaseManifoldResult(resultOut); } + + + bool disableCcd = false; float ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo) { + ///Rather then checking ALL pairs, only calculate TOI when motion exceeds treshold + + ///Linear motion for one of objects needs to exceed m_ccdSquareMotionTreshold + ///col0->m_worldTransform, + float resultFraction = 1.f; + + CollisionObject* col1 = static_cast<CollisionObject*>(m_box1.m_clientObject); + CollisionObject* col0 = static_cast<CollisionObject*>(m_box0.m_clientObject); + + float squareMot0 = (col0->m_interpolationWorldTransform.getOrigin() - col0->m_worldTransform.getOrigin()).length2(); + float squareMot1 = (col1->m_interpolationWorldTransform.getOrigin() - col1->m_worldTransform.getOrigin()).length2(); + + if (squareMot0 < col0->m_ccdSquareMotionTreshold && + squareMot0 < col0->m_ccdSquareMotionTreshold) + return resultFraction; + + if (disableCcd) + return 1.f; + CheckPenetrationDepthSolver(); //An adhoc way of testing the Continuous Collision Detection algorithms - //One object is approximated as a point, to simplify things + //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) @@ -340,48 +363,70 @@ float ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,Broad if (!needsCollision) return 1.f; - - CollisionObject* col0 = static_cast<CollisionObject*>(m_box0.m_clientObject); - CollisionObject* col1 = static_cast<CollisionObject*>(m_box1.m_clientObject); - - SphereShape sphere(0.f); + + /// Convex0 against sphere for Convex1 + { + ConvexShape* convex0 = static_cast<ConvexShape*>(col0->m_collisionShape); + + SphereShape sphere1(col1->m_ccdSweptShereRadius); //todo: allow non-zero sphere sizes, for better approximation + ConvexCast::CastResult result; + VoronoiSimplexSolver voronoiSimplex; + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + GjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform, + col1->m_worldTransform,col1->m_interpolationWorldTransform,result)) + { + + //store result.m_fraction in both bodies + + if (col0->m_hitFraction > result.m_fraction) + col0->m_hitFraction = result.m_fraction; - ConvexShape* min0 = static_cast<ConvexShape*>(col0->m_collisionShape); - ConvexShape* min1 = static_cast<ConvexShape*>(col1->m_collisionShape); - - ConvexCast::CastResult result; + if (col1->m_hitFraction > result.m_fraction) + col1->m_hitFraction = result.m_fraction; + if (resultFraction > result.m_fraction) + resultFraction = result.m_fraction; - VoronoiSimplexSolver voronoiSimplex; - SubsimplexConvexCast ccd0(&sphere,min1,&voronoiSimplex); + } + + - ///Simplification, one object is simplified as a sphere - GjkConvexCast ccd1(&sphere,min0,&voronoiSimplex); - //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (disableCcd) - return 1.f; + } - if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform, - col1->m_worldTransform,col1->m_interpolationWorldTransform,result)) + /// Sphere (for convex0) against Convex1 { - - //store result.m_fraction in both bodies - - if (col0->m_hitFraction > result.m_fraction) - col0->m_hitFraction = result.m_fraction; - - if (col1->m_hitFraction > result.m_fraction) - col1->m_hitFraction = result.m_fraction; + ConvexShape* convex1 = static_cast<ConvexShape*>(col1->m_collisionShape); + + SphereShape sphere0(col0->m_ccdSweptShereRadius); //todo: allow non-zero sphere sizes, for better approximation + ConvexCast::CastResult result; + VoronoiSimplexSolver voronoiSimplex; + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + GjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform, + col1->m_worldTransform,col1->m_interpolationWorldTransform,result)) + { + + //store result.m_fraction in both bodies + + if (col0->m_hitFraction > result.m_fraction) + col0->m_hitFraction = result.m_fraction; - return result.m_fraction; - } + if (col1->m_hitFraction > result.m_fraction) + col1->m_hitFraction = result.m_fraction; + if (resultFraction > result.m_fraction) + resultFraction = result.m_fraction; + } + } - - - return 1.f; + return resultFraction; } diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.h b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.h index c8ace81dda1..a1cadabe7eb 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.h +++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.h @@ -55,7 +55,13 @@ public: void SetLowLevelOfDetail(bool useLowLevel); - + virtual void SetShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + m_gjkPairDetector.m_partId0=partId0; + m_gjkPairDetector.m_partId1=partId1; + m_gjkPairDetector.m_index0=index0; + m_gjkPairDetector.m_index1=index1; + } const PersistentManifold* GetManifold() { diff --git a/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp b/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp index 15b6512a538..36f7255a737 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp @@ -18,6 +18,31 @@ subject to the following restrictions: #include "NarrowPhaseCollision/PersistentManifold.h" #include "CollisionDispatch/CollisionObject.h" + +///This is to allow MaterialCombiner/Custom Friction/Restitution values +ContactAddedCallback gContactAddedCallback=0; + +///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= CollisionObject::customMaterialCallback; +inline SimdScalar calculateCombinedFriction(const CollisionObject* body0,const CollisionObject* body1) +{ + SimdScalar friction = body0->getFriction() * body1->getFriction(); + + const SimdScalar MAX_FRICTION = 10.f; + if (friction < -MAX_FRICTION) + friction = -MAX_FRICTION; + if (friction > MAX_FRICTION) + friction = MAX_FRICTION; + return friction; + +} + +inline SimdScalar calculateCombinedRestitution(const CollisionObject* body0,const CollisionObject* body1) +{ + return body0->getRestitution() * body1->getRestitution(); +} + + + ManifoldResult::ManifoldResult(CollisionObject* body0,CollisionObject* body1,PersistentManifold* manifoldPtr) :m_manifoldPtr(manifoldPtr), m_body0(body0), @@ -31,8 +56,12 @@ void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const S if (depth > m_manifoldPtr->GetContactBreakingTreshold()) return; - SimdTransform transAInv = m_body0->m_worldTransform.inverse(); - SimdTransform transBInv= m_body1->m_worldTransform.inverse(); + + SimdTransform transAInv = m_body0->m_cachedInvertedWorldTransform; + SimdTransform transBInv= m_body1->m_cachedInvertedWorldTransform; + + //transAInv = m_body0->m_worldTransform.inverse(); + //transBInv= m_body1->m_worldTransform.inverse(); SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth; SimdVector3 localA = transAInv(pointA ); SimdVector3 localB = transBInv(pointInWorld); @@ -52,6 +81,20 @@ void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const S } else { + + newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1); + newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1); + + //User can override friction and/or restitution + if (gContactAddedCallback && + //and if either of the two bodies requires custom material + ((m_body0->m_collisionFlags & CollisionObject::customMaterialCallback) || + (m_body1->m_collisionFlags & CollisionObject::customMaterialCallback))) + { + //experimental feature info, for per-triangle material etc. + (*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1); + } + m_manifoldPtr->AddManifoldPoint(newPt); } } diff --git a/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.h b/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.h index 6126a759744..fe2438980a1 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.h +++ b/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.h @@ -20,22 +20,41 @@ subject to the following restrictions: #include "NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h" struct CollisionObject; class PersistentManifold; +class ManifoldPoint; +typedef bool (*ContactAddedCallback)(ManifoldPoint& cp, const CollisionObject* colObj0,int partId0,int index0,const CollisionObject* colObj1,int partId1,int index1); +extern ContactAddedCallback gContactAddedCallback; + + +///ManifoldResult is a helper class to manage contact results. class ManifoldResult : public DiscreteCollisionDetectorInterface::Result { PersistentManifold* m_manifoldPtr; CollisionObject* m_body0; CollisionObject* m_body1; - + int m_partId0; + int m_partId1; + int m_index0; + int m_index1; public: ManifoldResult(CollisionObject* body0,CollisionObject* body1,PersistentManifold* manifoldPtr); virtual ~ManifoldResult() {}; + virtual void SetShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + m_partId0=partId0; + m_partId1=partId1; + m_index0=index0; + m_index1=index1; + } + virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth); + + }; #endif //MANIFOLD_RESULT_H diff --git a/extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.cpp b/extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.cpp new file mode 100644 index 00000000000..10a7f0b6f14 --- /dev/null +++ b/extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.cpp @@ -0,0 +1,203 @@ + +#include "SimulationIslandManager.h" +#include "BroadphaseCollision/Dispatcher.h" +#include "NarrowPhaseCollision/PersistentManifold.h" +#include "CollisionDispatch/CollisionObject.h" +#include "CollisionDispatch/CollisionWorld.h" + + + +SimulationIslandManager::SimulationIslandManager() +{ +} + +void SimulationIslandManager::InitUnionFind(int n) +{ + m_unionFind.reset(n); +} + + +void SimulationIslandManager::FindUnions(Dispatcher* dispatcher) +{ + + { + for (int i=0;i<dispatcher->GetNumManifolds();i++) + { + const PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i); + //static objects (invmass 0.f) don't merge ! + + const CollisionObject* colObj0 = static_cast<const CollisionObject*>(manifold->GetBody0()); + const CollisionObject* colObj1 = static_cast<const CollisionObject*>(manifold->GetBody1()); + + if (colObj0 && colObj1 && dispatcher->NeedsResponse(*colObj0,*colObj1)) + { + if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && + ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + { + + m_unionFind.unite((colObj0)->m_islandTag1, + (colObj1)->m_islandTag1); + } + } + + + } + } + +} + + +void SimulationIslandManager::UpdateActivationState(CollisionWorld* colWorld,Dispatcher* dispatcher) +{ + + InitUnionFind(colWorld->GetCollisionObjectArray().size()); + + // put the index into m_controllers into m_tag + { + std::vector<CollisionObject*>::iterator i; + + int index = 0; + for (i=colWorld->GetCollisionObjectArray().begin(); + !(i==colWorld->GetCollisionObjectArray().end()); i++) + { + + CollisionObject* collisionObject= (*i); + collisionObject->m_islandTag1 = index; + collisionObject->m_hitFraction = 1.f; + index++; + + } + } + // do the union find + + FindUnions(dispatcher); + + + +} + + + + +void SimulationIslandManager::StoreIslandActivationState(CollisionWorld* colWorld) +{ + // put the islandId ('find' value) into m_tag + { + + + std::vector<CollisionObject*>::iterator i; + + int index = 0; + for (i=colWorld->GetCollisionObjectArray().begin(); + !(i==colWorld->GetCollisionObjectArray().end()); i++) + { + CollisionObject* collisionObject= (*i); + + if (collisionObject->mergesSimulationIslands()) + { + collisionObject->m_islandTag1 = m_unionFind.find(index); + } else + { + collisionObject->m_islandTag1 = -1; + } + index++; + } + } +} + + + + +// +// todo: this is random access, it can be walked 'cache friendly'! +// +void SimulationIslandManager::BuildAndProcessIslands(Dispatcher* dispatcher,CollisionObjectArray& collisionObjects, IslandCallback* callback) +{ + + int numBodies = collisionObjects.size(); + + for (int islandId=0;islandId<numBodies;islandId++) + { + + std::vector<PersistentManifold*> islandmanifold; + + //int numSleeping = 0; + + bool allSleeping = true; + + int i; + for (i=0;i<numBodies;i++) + { + CollisionObject* colObj0 = collisionObjects[i]; + + if (colObj0->m_islandTag1 == islandId) + { + + if (colObj0->GetActivationState()== ACTIVE_TAG) + { + allSleeping = false; + } + if (colObj0->GetActivationState()== DISABLE_DEACTIVATION) + { + allSleeping = false; + } + } + } + + + for (i=0;i<dispatcher->GetNumManifolds();i++) + { + PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i); + + //filtering for response + + CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0()); + CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1()); + { + if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) || + ((colObj1) && (colObj1)->m_islandTag1 == (islandId))) + { + + if (dispatcher->NeedsResponse(*colObj0,*colObj1)) + islandmanifold.push_back(manifold); + } + } + } + if (allSleeping) + { + int i; + for (i=0;i<numBodies;i++) + { + CollisionObject* colObj0 = collisionObjects[i]; + if (colObj0->m_islandTag1 == islandId) + { + colObj0->SetActivationState( ISLAND_SLEEPING ); + } + } + + + } else + { + + int i; + for (i=0;i<numBodies;i++) + { + CollisionObject* colObj0 = collisionObjects[i]; + if (colObj0->m_islandTag1 == islandId) + { + if ( colObj0->GetActivationState() == ISLAND_SLEEPING) + { + colObj0->SetActivationState( WANTS_DEACTIVATION); + } + } + } + + /// Process the actual simulation, only if not sleeping/deactivated + if (islandmanifold.size()) + { + callback->ProcessIsland(&islandmanifold[0],islandmanifold.size()); + } + + } + } +} diff --git a/extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.h b/extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.h new file mode 100644 index 00000000000..84285283628 --- /dev/null +++ b/extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.h @@ -0,0 +1,58 @@ +/* +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 SIMULATION_ISLAND_MANAGER_H +#define SIMULATION_ISLAND_MANAGER_H + +#include "CollisionDispatch/UnionFind.h" +#include "CollisionCreateFunc.h" + +class CollisionWorld; +class Dispatcher; + +///SimulationIslandManager creates and handles simulation islands, using UnionFind +class SimulationIslandManager +{ + UnionFind m_unionFind; + +public: + SimulationIslandManager(); + + + void InitUnionFind(int n); + + + UnionFind& GetUnionFind() { return m_unionFind;} + + virtual void UpdateActivationState(CollisionWorld* colWorld,Dispatcher* dispatcher); + virtual void StoreIslandActivationState(CollisionWorld* world); + + + void FindUnions(Dispatcher* dispatcher); + + + + struct IslandCallback + { + virtual ~IslandCallback() {}; + + virtual void ProcessIsland(class PersistentManifold** manifolds,int numManifolds) = 0; + }; + + void BuildAndProcessIslands(Dispatcher* dispatcher,CollisionObjectArray& collisionObjects, IslandCallback* callback); + +}; + +#endif //SIMULATION_ISLAND_MANAGER_H diff --git a/extern/bullet/Bullet/CollisionShapes/BoxShape.h b/extern/bullet/Bullet/CollisionShapes/BoxShape.h index f47f982112d..67ce82078f3 100644 --- a/extern/bullet/Bullet/CollisionShapes/BoxShape.h +++ b/extern/bullet/Bullet/CollisionShapes/BoxShape.h @@ -48,24 +48,12 @@ public: { SimdVector3 halfExtents = GetHalfExtents(); - SimdVector3 margin(GetMargin(),GetMargin(),GetMargin()); - halfExtents -= margin; SimdVector3 supVertex; supVertex = SimdPoint3(vec.x() < SimdScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), vec.y() < SimdScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), vec.z() < SimdScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); - if ( GetMargin()!=0.f ) - { - SimdVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) - { - vecnorm.setValue(-1.f,-1.f,-1.f); - } - vecnorm.normalize(); - supVertex+= GetMargin() * vecnorm; - } return supVertex; } diff --git a/extern/bullet/Bullet/CollisionShapes/CollisionShape.h b/extern/bullet/Bullet/CollisionShapes/CollisionShape.h index 0c0f6984f9b..57bf97874b2 100644 --- a/extern/bullet/Bullet/CollisionShapes/CollisionShape.h +++ b/extern/bullet/Bullet/CollisionShapes/CollisionShape.h @@ -59,9 +59,13 @@ public: } bool IsConcave() const { - return (GetShapeType() > CONCAVE_SHAPES_START_HERE); + return ((GetShapeType() > CONCAVE_SHAPES_START_HERE) && + (GetShapeType() < CONCAVE_SHAPES_END_HERE)); + } + bool IsCompound() const + { + return (GetShapeType() == COMPOUND_SHAPE_PROXYTYPE); } - virtual void setLocalScaling(const SimdVector3& scaling) =0; virtual const SimdVector3& getLocalScaling() const =0; diff --git a/extern/bullet/Bullet/CollisionShapes/CompoundShape.cpp b/extern/bullet/Bullet/CollisionShapes/CompoundShape.cpp new file mode 100644 index 00000000000..ecb11701373 --- /dev/null +++ b/extern/bullet/Bullet/CollisionShapes/CompoundShape.cpp @@ -0,0 +1,100 @@ +/* +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 "CompoundShape.h" + + +#include "CollisionShape.h" + + +CompoundShape::CompoundShape() +:m_localAabbMin(1e30f,1e30f,1e30f), +m_localAabbMax(-1e30f,-1e30f,-1e30f), +m_aabbTree(0), +m_collisionMargin(0.f), +m_localScaling(1.f,1.f,1.f) +{ +} + + +CompoundShape::~CompoundShape() +{ +} + +void CompoundShape::AddChildShape(const SimdTransform& localTransform,CollisionShape* shape) +{ + m_childTransforms.push_back(localTransform); + m_childShapes.push_back(shape); + + //extend the local aabbMin/aabbMax + SimdVector3 localAabbMin,localAabbMax; + shape->GetAabb(localTransform,localAabbMin,localAabbMax); + for (int i=0;i<3;i++) + { + if (m_localAabbMin[i] > localAabbMin[i]) + { + m_localAabbMin[i] = localAabbMin[i]; + } + if (m_localAabbMax[i] < localAabbMax[i]) + { + m_localAabbMax[i] = localAabbMax[i]; + } + + } +} + + + + ///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void CompoundShape::GetAabb(const SimdTransform& trans,SimdVector3& aabbMin,SimdVector3& aabbMax) const +{ + SimdVector3 localHalfExtents = 0.5f*(m_localAabbMax-m_localAabbMin); + SimdVector3 localCenter = 0.5f*(m_localAabbMax+m_localAabbMin); + + SimdMatrix3x3 abs_b = trans.getBasis().absolute(); + + SimdPoint3 center = trans(localCenter); + + SimdVector3 extent = SimdVector3(abs_b[0].dot(localHalfExtents), + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + extent += SimdVector3(GetMargin(),GetMargin(),GetMargin()); + + aabbMin = center - extent; + aabbMax = center + extent; +} + +void CompoundShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) +{ + //approximation: take the inertia from the aabb for now + SimdTransform ident; + ident.setIdentity(); + SimdVector3 aabbMin,aabbMax; + GetAabb(ident,aabbMin,aabbMax); + + SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f; + + SimdScalar lx=2.f*(halfExtents.x()); + SimdScalar ly=2.f*(halfExtents.y()); + SimdScalar lz=2.f*(halfExtents.z()); + + inertia[0] = mass/(12.0f) * (ly*ly + lz*lz); + inertia[1] = mass/(12.0f) * (lx*lx + lz*lz); + inertia[2] = mass/(12.0f) * (lx*lx + ly*ly); + +} + + + diff --git a/extern/bullet/Bullet/CollisionShapes/CompoundShape.h b/extern/bullet/Bullet/CollisionShapes/CompoundShape.h new file mode 100644 index 00000000000..7a43e44c285 --- /dev/null +++ b/extern/bullet/Bullet/CollisionShapes/CompoundShape.h @@ -0,0 +1,117 @@ +/* +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 COMPOUND_SHAPE_H +#define COMPOUND_SHAPE_H + +#include "CollisionShape.h" + +#include "SimdVector3.h" +#include "SimdTransform.h" +#include "SimdMatrix3x3.h" +#include <vector> +#include "CollisionShapes/CollisionMargin.h" + +class OptimizedBvh; + +/// CompoundShape allows to store multiple other CollisionShapes +/// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape. +class CompoundShape : public CollisionShape +{ + std::vector<SimdTransform> m_childTransforms; + std::vector<CollisionShape*> m_childShapes; + SimdVector3 m_localAabbMin; + SimdVector3 m_localAabbMax; + + OptimizedBvh* m_aabbTree; + +public: + CompoundShape(); + + virtual ~CompoundShape(); + + void AddChildShape(const SimdTransform& localTransform,CollisionShape* shape); + + int GetNumChildShapes() const + { + return m_childShapes.size(); + } + + CollisionShape* GetChildShape(int index) + { + return m_childShapes[index]; + } + const CollisionShape* GetChildShape(int index) const + { + return m_childShapes[index]; + } + + SimdTransform GetChildTransform(int index) + { + return m_childTransforms[index]; + } + const SimdTransform GetChildTransform(int index) const + { + return m_childTransforms[index]; + } + + ///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const; + + + virtual void setLocalScaling(const SimdVector3& scaling) + { + m_localScaling = scaling; + } + virtual const SimdVector3& getLocalScaling() const + { + return m_localScaling; + } + + virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia); + + virtual int GetShapeType() const { return COMPOUND_SHAPE_PROXYTYPE;} + + virtual void SetMargin(float margin) + { + m_collisionMargin = margin; + } + virtual float GetMargin() const + { + return m_collisionMargin; + } + virtual char* GetName()const + { + return "Compound"; + } + + //this is optional, but should make collision queries faster, by culling non-overlapping nodes + void CreateAabbTreeFromChildren(); + + const OptimizedBvh* GetAabbTree() const + { + return m_aabbTree; + } + +private: + SimdScalar m_collisionMargin; +protected: + SimdVector3 m_localScaling; + +}; + + + +#endif //COMPOUND_SHAPE_H diff --git a/extern/bullet/Bullet/CollisionShapes/ConcaveShape.cpp b/extern/bullet/Bullet/CollisionShapes/ConcaveShape.cpp new file mode 100644 index 00000000000..c060a4df592 --- /dev/null +++ b/extern/bullet/Bullet/CollisionShapes/ConcaveShape.cpp @@ -0,0 +1,28 @@ + +/* +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 "ConcaveShape.h" + +ConcaveShape::ConcaveShape() : m_collisionMargin(0.f) +{ + +} + +ConcaveShape::~ConcaveShape() +{ + +} diff --git a/extern/bullet/Bullet/CollisionShapes/ConcaveShape.h b/extern/bullet/Bullet/CollisionShapes/ConcaveShape.h new file mode 100644 index 00000000000..d6c589dd8fc --- /dev/null +++ b/extern/bullet/Bullet/CollisionShapes/ConcaveShape.h @@ -0,0 +1,51 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef CONCAVE_SHAPE_H +#define CONCAVE_SHAPE_H + +#include "CollisionShapes/CollisionShape.h" +#include "BroadphaseCollision/BroadphaseProxy.h" // for the types + +#include "TriangleCallback.h" + + +///Concave shape proves an interface concave shapes that can produce triangles that overlapping a given AABB. +///Static triangle mesh, infinite plane, height field/landscapes are example that implement this interface. +class ConcaveShape : public CollisionShape +{ +protected: + float m_collisionMargin; + +public: + ConcaveShape(); + + virtual ~ConcaveShape(); + + virtual void ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const = 0; + + virtual float GetMargin() const { + return m_collisionMargin; + } + virtual void SetMargin(float collisionMargin) + { + m_collisionMargin = collisionMargin; + } + + + +}; + +#endif //CONCAVE_SHAPE_H diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp deleted file mode 100644 index a34add4c04f..00000000000 --- a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp +++ /dev/null @@ -1,96 +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. -*/ - -#include "ConvexTriangleCallback.h" -#include "NarrowPhaseCollision/PersistentManifold.h" -#include "NarrowPhaseCollision/ManifoldContactAddResult.h" -#include "NarrowPhaseCollision/GjkPairDetector.h" -#include "NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h" - - - -#include "TriangleShape.h" - -//m_manifoldPtr = m_dispatcher->GetNewManifold(proxy0->m_clientObject,proxy1->m_clientObject); - //m_dispatcher->ReleaseManifold( m_manifoldPtr ); - -ConvexTriangleCallback::ConvexTriangleCallback(PersistentManifold* manifold,ConvexShape* convexShape,const SimdTransform&convexTransform,const SimdTransform& triangleMeshTransform) -:m_triangleMeshTransform(triangleMeshTransform), - m_convexTransform(convexTransform), - m_triangleCount(0), - m_convexShape(convexShape), - m_manifoldPtr(manifold) -{ -} - -ConvexTriangleCallback::~ConvexTriangleCallback() -{ - -} - - - - -void ConvexTriangleCallback::ProcessTriangle(SimdVector3* triangle) -{ - - //triangle, convex - TriangleShape tm(triangle[0],triangle[1],triangle[2]); - tm.SetMargin(m_collisionMarginTriangle); - GjkPairDetector::ClosestPointInput input; - input.m_transformA = m_triangleMeshTransform; - input.m_transformB = m_convexTransform; - - ManifoldContactAddResult output(m_triangleMeshTransform,m_convexTransform,m_manifoldPtr); - - - VoronoiSimplexSolver simplexSolver; - MinkowskiPenetrationDepthSolver penetrationDepthSolver; - - GjkPairDetector gjkDetector(&tm,m_convexShape,&simplexSolver,&penetrationDepthSolver); - - gjkDetector.SetMinkowskiA(&tm); - gjkDetector.SetMinkowskiB(m_convexShape); - input.m_maximumDistanceSquared = tm.GetMargin()+ m_convexShape->GetMargin() + m_manifoldPtr->GetContactBreakingTreshold(); - input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; - - input.m_maximumDistanceSquared = 1e30f;//? - - - gjkDetector.GetClosestPoints(input,output,0); - - -} - - - -void ConvexTriangleCallback::Update(float collisionMarginTriangle) -{ - m_triangleCount = 0; - m_collisionMarginTriangle = collisionMarginTriangle; - - SimdTransform boxInTriangleSpace; - boxInTriangleSpace = m_triangleMeshTransform.inverse() * m_convexTransform; - - m_convexShape->GetAabb(boxInTriangleSpace,m_aabbMin,m_aabbMax); - - float extraMargin = CONVEX_DISTANCE_MARGIN; - - SimdVector3 extra(extraMargin,extraMargin,extraMargin); - - m_aabbMax += extra; - m_aabbMin -= extra; - -} diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h deleted file mode 100644 index 9160d1f3226..00000000000 --- a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h +++ /dev/null @@ -1,64 +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. -*/ - -#ifndef CONVEX_TRIANGLE_CALLBACK_H -#define CONVEX_TRIANGLE_CALLBACK_H - -#include "TriangleCallback.h" -class ConvexShape; -class PersistentManifold; -#include "SimdTransform.h" -///ConvexTriangleCallback processes the narrowphase convex-triangle collision detection -class ConvexTriangleCallback: public TriangleCallback -{ - SimdVector3 m_aabbMin; - SimdVector3 m_aabbMax ; - - SimdTransform m_triangleMeshTransform; - SimdTransform m_convexTransform; - -// bool m_useContinuous; - float m_collisionMarginTriangle; - -public: -int m_triangleCount; - - ConvexShape* m_convexShape; - - PersistentManifold* m_manifoldPtr; - - ConvexTriangleCallback(PersistentManifold* manifold,ConvexShape* convexShape,const SimdTransform&convexTransform,const SimdTransform& triangleMeshTransform); - - void Update(float collisionMarginTriangle); - - virtual ~ConvexTriangleCallback(); - - virtual void ProcessTriangle(SimdVector3* triangle); - - - inline const SimdVector3& GetAabbMin() const - { - return m_aabbMin; - } - inline const SimdVector3& GetAabbMax() const - { - return m_aabbMax; - } - -}; - - -#endif //CONVEX_TRIANGLE_CALLBACK_H - diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.cpp new file mode 100644 index 00000000000..44bbaeb85f4 --- /dev/null +++ b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.cpp @@ -0,0 +1,197 @@ +/* +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 "ConvexTriangleMeshShape.h" +#include "CollisionShapes/CollisionMargin.h" + +#include "SimdQuaternion.h" +#include "CollisionShapes/StridingMeshInterface.h" + + +ConvexTriangleMeshShape ::ConvexTriangleMeshShape (StridingMeshInterface* meshInterface) +:m_stridingMesh(meshInterface) +{ +} + + + + +///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once +///but then we are duplicating +class LocalSupportVertexCallback: public InternalTriangleIndexCallback +{ + + SimdVector3 m_supportVertexLocal; +public: + + SimdScalar m_maxDot; + SimdVector3 m_supportVecLocal; + + LocalSupportVertexCallback(const SimdVector3& supportVecLocal) + : m_supportVertexLocal(0.f,0.f,0.f), + m_supportVecLocal(supportVecLocal), + m_maxDot(-1e30f) + { + } + + virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int triangleIndex) + { + for (int i=0;i<3;i++) + { + SimdScalar dot = m_supportVecLocal.dot(triangle[i]); + if (dot > m_maxDot) + { + m_maxDot = dot; + m_supportVertexLocal = triangle[i]; + } + } + } + + SimdVector3 GetSupportVertexLocal() + { + return m_supportVertexLocal; + } + +}; + + + + + +SimdVector3 ConvexTriangleMeshShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const +{ + SimdVector3 supVec(0.f,0.f,0.f); + SimdScalar newDot,maxDot = -1e30f; + + SimdVector3 vec = vec0; + SimdScalar lenSqr = vec.length2(); + if (lenSqr < 0.0001f) + { + vec.setValue(1,0,0); + } else + { + float rlen = 1.f / SimdSqrt(lenSqr ); + vec *= rlen; + } + + LocalSupportVertexCallback supportCallback(vec); + SimdVector3 aabbMax(1e30f,1e30f,1e30f); + m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); + supVec = supportCallback.GetSupportVertexLocal(); + + return supVec; +} + +void ConvexTriangleMeshShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + SimdScalar newDot; + //use 'w' component of supportVerticesOut? + { + for (int i=0;i<numVectors;i++) + { + supportVerticesOut[i][3] = -1e30f; + } + } + + //todo: could do the batch inside the callback! + + + for (int j=0;j<numVectors;j++) + { + const SimdVector3& vec = vectors[j]; + LocalSupportVertexCallback supportCallback(vec); + SimdVector3 aabbMax(1e30f,1e30f,1e30f); + m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); + supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); + } + +} + + + +SimdVector3 ConvexTriangleMeshShape::LocalGetSupportingVertex(const SimdVector3& vec)const +{ + SimdVector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); + + if ( GetMargin()!=0.f ) + { + SimdVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + vecnorm.setValue(-1.f,-1.f,-1.f); + } + vecnorm.normalize(); + supVertex+= GetMargin() * vecnorm; + } + return supVertex; +} + + + + + + + + + +//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection +//Please note that you can debug-draw ConvexTriangleMeshShape with the Raytracer Demo +int ConvexTriangleMeshShape::GetNumVertices() const +{ + //cache this? + assert(0); + return 0; + +} + +int ConvexTriangleMeshShape::GetNumEdges() const +{ + assert(0); + return 0; +} + +void ConvexTriangleMeshShape::GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const +{ + assert(0); +} + +void ConvexTriangleMeshShape::GetVertex(int i,SimdPoint3& vtx) const +{ + assert(0); +} + +int ConvexTriangleMeshShape::GetNumPlanes() const +{ + return 0; +} + +void ConvexTriangleMeshShape::GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const +{ + assert(0); +} + +//not yet +bool ConvexTriangleMeshShape::IsInside(const SimdPoint3& pt,SimdScalar tolerance) const +{ + assert(0); + return false; +} + + + +void ConvexTriangleMeshShape::setLocalScaling(const SimdVector3& scaling) +{ + m_stridingMesh->setScaling(scaling); +} + diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.h b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.h new file mode 100644 index 00000000000..28bcb2e56b8 --- /dev/null +++ b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.h @@ -0,0 +1,49 @@ +#ifndef CONVEX_TRIANGLEMESH_SHAPE_H +#define CONVEX_TRIANGLEMESH_SHAPE_H + + +#include "PolyhedralConvexShape.h" +#include "BroadphaseCollision/BroadphaseProxy.h" // for the types + +#include <vector> + +/// ConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use ConvexHullShape instead. +/// It uses the StridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data. +class ConvexTriangleMeshShape : public PolyhedralConvexShape +{ + + class StridingMeshInterface* m_stridingMesh; + +public: + ConvexTriangleMeshShape(StridingMeshInterface* meshInterface); + + class StridingMeshInterface* GetStridingMesh() + { + return m_stridingMesh; + } + + virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec)const; + virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const; + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const; + + virtual int GetShapeType()const { return CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; } + + //debugging + virtual char* GetName()const {return "ConvexTrimesh";} + + virtual int GetNumVertices() const; + virtual int GetNumEdges() const; + virtual void GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const; + virtual void GetVertex(int i,SimdPoint3& vtx) const; + virtual int GetNumPlanes() const; + virtual void GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const; + virtual bool IsInside(const SimdPoint3& pt,SimdScalar tolerance) const; + + + void setLocalScaling(const SimdVector3& scaling); + +}; + + + +#endif //CONVEX_TRIANGLEMESH_SHAPE_H
\ No newline at end of file diff --git a/extern/bullet/Bullet/CollisionShapes/CylinderShape.h b/extern/bullet/Bullet/CollisionShapes/CylinderShape.h index 49e3c0ab3ee..217ce70369a 100644 --- a/extern/bullet/Bullet/CollisionShapes/CylinderShape.h +++ b/extern/bullet/Bullet/CollisionShapes/CylinderShape.h @@ -66,6 +66,17 @@ public: return CYLINDER_SHAPE_PROXYTYPE; } + virtual int GetUpAxis() const + { + return 1; + } + + //debugging + virtual char* GetName()const + { + return "CylinderY"; + } + }; @@ -77,6 +88,16 @@ public: virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const; virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const; + virtual int GetUpAxis() const + { + return 0; + } + //debugging + virtual char* GetName()const + { + return "CylinderX"; + } + }; class CylinderShapeZ : public CylinderShape @@ -87,6 +108,16 @@ public: virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const; virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const; + virtual int GetUpAxis() const + { + return 2; + } + //debugging + virtual char* GetName()const + { + return "CylinderZ"; + } + }; diff --git a/extern/bullet/Bullet/CollisionShapes/EmptyShape.h b/extern/bullet/Bullet/CollisionShapes/EmptyShape.h index d4b83663a94..4407a22acdf 100644 --- a/extern/bullet/Bullet/CollisionShapes/EmptyShape.h +++ b/extern/bullet/Bullet/CollisionShapes/EmptyShape.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef EMPTY_SHAPE_H #define EMPTY_SHAPE_H -#include "CollisionShape.h" +#include "ConcaveShape.h" #include "SimdVector3.h" #include "SimdTransform.h" @@ -27,8 +27,9 @@ subject to the following restrictions: -/// EmptyShape is a collision shape without actual collision detection. It can be replaced by another shape during runtime -class EmptyShape : public CollisionShape +/// EmptyShape is a collision shape without actual collision detection. +///It can be replaced by another shape during runtime +class EmptyShape : public ConcaveShape { public: EmptyShape(); @@ -53,22 +54,13 @@ public: virtual int GetShapeType() const { return EMPTY_SHAPE_PROXYTYPE;} - virtual void SetMargin(float margin) - { - m_collisionMargin = margin; - } - virtual float GetMargin() const - { - return m_collisionMargin; - } + virtual char* GetName()const { return "Empty"; } -private: - SimdScalar m_collisionMargin; protected: SimdVector3 m_localScaling; diff --git a/extern/bullet/Bullet/CollisionShapes/OptimizedBvh.cpp b/extern/bullet/Bullet/CollisionShapes/OptimizedBvh.cpp index 0cd20ecd048..1b30bfbcb83 100644 --- a/extern/bullet/Bullet/CollisionShapes/OptimizedBvh.cpp +++ b/extern/bullet/Bullet/CollisionShapes/OptimizedBvh.cpp @@ -193,21 +193,14 @@ int OptimizedBvh::CalcSplittingAxis(NodeArray& leafNodes,int startIndex,int endI } - + void OptimizedBvh::ReportAabbOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const { - if (aabbMin.length() > 1000.f) - { - for (size_t i=0;i<m_leafNodes.size();i++) - { - const OptimizedBvhNode& node = m_leafNodes[i]; - nodeCallback->ProcessNode(&node); - } - } else - { - //WalkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); - WalkStacklessTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); - } + //either choose recursive traversal (WalkTree) or stackless (WalkStacklessTree) + + //WalkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); + + WalkStacklessTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); } void OptimizedBvh::WalkTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const diff --git a/extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.cpp b/extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.cpp new file mode 100644 index 00000000000..48aecf5e508 --- /dev/null +++ b/extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.cpp @@ -0,0 +1,100 @@ +/* +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 "StaticPlaneShape.h" + +#include "SimdTransformUtil.h" + + +StaticPlaneShape::StaticPlaneShape(const SimdVector3& planeNormal,SimdScalar planeConstant) +:m_planeNormal(planeNormal), +m_planeConstant(planeConstant), +m_localScaling(0.f,0.f,0.f) +{ +} + + +StaticPlaneShape::~StaticPlaneShape() +{ +} + + + +void StaticPlaneShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const +{ + SimdVector3 infvec (1e30f,1e30f,1e30f); + + SimdVector3 center = m_planeNormal*m_planeConstant; + aabbMin = center + infvec*m_planeNormal; + aabbMax = aabbMin; + aabbMin.setMin(center - infvec*m_planeNormal); + aabbMax.setMax(center - infvec*m_planeNormal); + + aabbMin.setValue(-1e30f,-1e30f,-1e30f); + aabbMax.setValue(1e30f,1e30f,1e30f); + +} + + + + +void StaticPlaneShape::ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const +{ + + SimdVector3 halfExtents = (aabbMax - aabbMin) * 0.5f; + SimdScalar radius = halfExtents.length(); + SimdVector3 center = (aabbMax + aabbMin) * 0.5f; + + //this is where the triangles are generated, given AABB and plane equation (normal/constant) + + SimdVector3 tangentDir0,tangentDir1; + + //tangentDir0/tangentDir1 can be precalculated + SimdPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1); + + SimdVector3 supVertex0,supVertex1; + + SimdVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal; + + SimdVector3 triangle[3]; + triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius; + triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius; + triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius; + + callback->ProcessTriangle(triangle,0,0); + + triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius; + triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius; + triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius; + + callback->ProcessTriangle(triangle,0,1); + +} + +void StaticPlaneShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) +{ + //moving concave objects not supported + + inertia.setValue(0.f,0.f,0.f); +} + +void StaticPlaneShape::setLocalScaling(const SimdVector3& scaling) +{ + m_localScaling = scaling; +} +const SimdVector3& StaticPlaneShape::getLocalScaling() const +{ + return m_localScaling; +} diff --git a/extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.h b/extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.h new file mode 100644 index 00000000000..eaf8761c660 --- /dev/null +++ b/extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.h @@ -0,0 +1,61 @@ +/* +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 STATIC_PLANE_SHAPE_H +#define STATIC_PLANE_SHAPE_H + +#include "CollisionShapes/ConcaveShape.h" + + +///StaticPlaneShape simulates an 'infinite' plane by dynamically reporting triangles approximated by intersection of the plane with the AABB. +///Assumed is that the other objects is not also infinite, so a reasonable sized AABB. +class StaticPlaneShape : public ConcaveShape +{ +protected: + SimdVector3 m_localAabbMin; + SimdVector3 m_localAabbMax; + + SimdVector3 m_planeNormal; + SimdVector3 m_localScaling; + SimdScalar m_planeConstant; + +public: + StaticPlaneShape(const SimdVector3& planeNormal,SimdScalar planeConstant); + + virtual ~StaticPlaneShape(); + + + virtual int GetShapeType() const + { + return STATIC_PLANE_PROXYTYPE; + } + + virtual void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const; + + virtual void ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const; + + virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia); + + virtual void setLocalScaling(const SimdVector3& scaling); + virtual const SimdVector3& getLocalScaling() const; + + + //debugging + virtual char* GetName()const {return "STATICPLANE";} + + +}; + +#endif //STATIC_PLANE_SHAPE_H diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.cpp b/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.cpp index ebbb7c3e571..d01d87d3f5d 100644 --- a/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.cpp +++ b/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.cpp @@ -15,39 +15,51 @@ subject to the following restrictions: #include "TriangleIndexVertexArray.h" -TriangleIndexVertexArray::TriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride) -:m_numTriangleIndices(numTriangleIndices), -m_triangleIndexBase(triangleIndexBase), -m_triangleIndexStride(triangleIndexStride), -m_numVertices(numVertices), -m_vertexBase(vertexBase), -m_vertexStride(vertexStride) +TriangleIndexVertexArray::TriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride) { + IndexedMesh mesh; + + mesh.m_numTriangles = numTriangles; + mesh.m_triangleIndexBase = triangleIndexBase; + mesh.m_triangleIndexStride = triangleIndexStride; + mesh.m_numVertices = numVertices; + mesh.m_vertexBase = vertexBase; + mesh.m_vertexStride = vertexStride; + + AddIndexedMesh(mesh); + } void TriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) { - numverts = m_numVertices; - (*vertexbase) = (unsigned char *)m_vertexBase; + ASSERT(subpart< getNumSubParts() ); + + IndexedMesh& mesh = m_indexedMeshes[subpart]; + + numverts = mesh.m_numVertices; + (*vertexbase) = (unsigned char *) mesh.m_vertexBase; type = PHY_FLOAT; - vertexStride = m_vertexStride; + vertexStride = mesh.m_vertexStride; - numfaces = m_numTriangleIndices; - (*indexbase) = (unsigned char *)m_triangleIndexBase; - indexstride = m_triangleIndexStride; + numfaces = mesh.m_numTriangles; + + (*indexbase) = (unsigned char *)mesh.m_triangleIndexBase; + indexstride = mesh.m_triangleIndexStride; indicestype = PHY_INTEGER; } void TriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const { - numverts = m_numVertices; - (*vertexbase) = (unsigned char *)m_vertexBase; + const IndexedMesh& mesh = m_indexedMeshes[subpart]; + + numverts = mesh.m_numVertices; + (*vertexbase) = (const unsigned char *)mesh.m_vertexBase; type = PHY_FLOAT; - vertexStride = m_vertexStride; + vertexStride = mesh.m_vertexStride; - numfaces = m_numTriangleIndices; - (*indexbase) = (unsigned char *)m_triangleIndexBase; - indexstride = m_triangleIndexStride; + numfaces = mesh.m_numTriangles; + (*indexbase) = (const unsigned char *)mesh.m_triangleIndexBase; + indexstride = mesh.m_triangleIndexStride; indicestype = PHY_INTEGER; } diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.h b/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.h index 1134521ad87..a44ef4514d5 100644 --- a/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.h +++ b/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.h @@ -14,22 +14,47 @@ subject to the following restrictions: */ #include "StridingMeshInterface.h" +#include <vector> +///IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements +///instead of the number of indices, we pass the number of triangles +///todo: explain with pictures +struct IndexedMesh + { + int m_numTriangles; + int* m_triangleIndexBase; + int m_triangleIndexStride; + int m_numVertices; + float* m_vertexBase; + int m_vertexStride; + }; +///TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays. +///Additional meshes can be added using AddIndexedMesh +///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays. +///So keep those arrays around during the lifetime of this TriangleIndexVertexArray. class TriangleIndexVertexArray : public StridingMeshInterface { + std::vector<IndexedMesh> m_indexedMeshes; - int m_numTriangleIndices; - int* m_triangleIndexBase; - int m_triangleIndexStride; - int m_numVertices; - float* m_vertexBase; - int m_vertexStride; - + public: + + + TriangleIndexVertexArray() + { + } + + //just to be backwards compatible TriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride); + void AddIndexedMesh(const IndexedMesh& mesh) + { + m_indexedMeshes.push_back(mesh); + } + + virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0); virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const; @@ -42,7 +67,9 @@ public: /// getNumSubParts returns the number of seperate subparts /// each subpart has a continuous array of vertices and indices - virtual int getNumSubParts() const { return 1;} + virtual int getNumSubParts() const { + return (int)m_indexedMeshes.size(); + } virtual void preallocateVertices(int numverts){} virtual void preallocateIndices(int numindices){} diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp b/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp index 73b53cd0266..f00ee280f11 100644 --- a/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp +++ b/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp @@ -23,8 +23,7 @@ subject to the following restrictions: #include "stdio.h" TriangleMeshShape::TriangleMeshShape(StridingMeshInterface* meshInterface) -: m_meshInterface(meshInterface), -m_collisionMargin(CONVEX_DISTANCE_MARGIN) +: m_meshInterface(meshInterface) { RecalcLocalAabb(); } diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.h b/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.h index ccbb4d75f61..a6ee965da0f 100644 --- a/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.h +++ b/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.h @@ -16,30 +16,24 @@ subject to the following restrictions: #ifndef TRIANGLE_MESH_SHAPE_H #define TRIANGLE_MESH_SHAPE_H -#include "CollisionShapes/CollisionShape.h" -#include "BroadphaseCollision/BroadphaseProxy.h" // for the types - -#include "StridingMeshInterface.h" -#include "TriangleCallback.h" - +#include "CollisionShapes/ConcaveShape.h" +#include "CollisionShapes/StridingMeshInterface.h" ///Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles. -class TriangleMeshShape : public CollisionShape +class TriangleMeshShape : public ConcaveShape { protected: StridingMeshInterface* m_meshInterface; SimdVector3 m_localAabbMin; SimdVector3 m_localAabbMax; - float m_collisionMargin; + public: TriangleMeshShape(StridingMeshInterface* meshInterface); virtual ~TriangleMeshShape(); - - virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec) const; virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const @@ -68,16 +62,6 @@ public: //debugging virtual char* GetName()const {return "TRIANGLEMESH";} - - virtual float GetMargin() const { - return m_collisionMargin; - } - virtual void SetMargin(float collisionMargin) - { - m_collisionMargin = collisionMargin; - } - - }; diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h b/extern/bullet/Bullet/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h index 3d6fdb2be7c..da5ecc2d78b 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h @@ -34,6 +34,9 @@ struct DiscreteCollisionDetectorInterface void operator delete(void* ptr) {}; 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; virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)=0; }; diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.cpp index f87e9ced8a7..2b494e45954 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.cpp @@ -122,12 +122,13 @@ bool GjkConvexCast::calcTimeOfImpact( n = x - c; SimdScalar nDotr = n.dot(r); - if (nDotr >= 0.f) + if (nDotr >= -(SIMD_EPSILON*SIMD_EPSILON)) return false; - + lambda = lambda - n.dot(n) / nDotr; if (lambda <= lastLambda) break; + lastLambda = lambda; x = s + lambda * r; diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.cpp index dcb96c21f0f..0887ba71901 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.cpp @@ -18,14 +18,16 @@ subject to the following restrictions: #include "NarrowPhaseCollision/SimplexSolverInterface.h" #include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h" +#if defined(DEBUG) || defined (_DEBUG) +#include <stdio.h> //for debug printf +#endif + static const SimdScalar rel_error = SimdScalar(1.0e-5); SimdScalar rel_error2 = rel_error * rel_error; float maxdist2 = 1.e30f; -#include <stdio.h> int gGjkMaxIter=1000; -bool gIrregularCatch = true; GjkPairDetector::GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver* penetrationDepthSolver) :m_cachedSeparatingAxis(0.f,0.f,1.f), @@ -33,7 +35,11 @@ m_penetrationDepthSolver(penetrationDepthSolver), m_simplexSolver(simplexSolver), m_minkowskiA(objectA), m_minkowskiB(objectB), -m_ignoreMargin(false) +m_ignoreMargin(false), +m_partId0(-1), +m_index0(-1), +m_partId1(-1), +m_index1(-1) { } @@ -71,7 +77,22 @@ int curIter = 0; while (true) { - + //rare failure case, perhaps deferate shapes? + if (curIter++ > gGjkMaxIter) + { + #if defined(DEBUG) || defined (_DEBUG) + printf("GjkPairDetector maxIter exceeded:%i\n",curIter); + printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", + m_cachedSeparatingAxis.getX(), + m_cachedSeparatingAxis.getY(), + m_cachedSeparatingAxis.getZ(), + squaredDistance, + m_minkowskiA->GetShapeType(), + m_minkowskiB->GetShapeType()); + #endif + break; + + } SimdVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); SimdVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); @@ -134,39 +155,6 @@ int curIter = 0; m_simplexSolver->backup_closest(m_cachedSeparatingAxis); break; } - - //rare failure case, perhaps deferate shapes? - if (curIter++ > gGjkMaxIter) - { - -#define CATCH_ME 1 -#ifdef CATCH_ME -//this should not happen, we need to catch it -//#if defined(DEBUG) || defined (_DEBUG) - if (gIrregularCatch) - { - gIrregularCatch = false; - - printf("Problem with collision geometry\n"); - printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", - m_cachedSeparatingAxis.getX(), - m_cachedSeparatingAxis.getY(), - m_cachedSeparatingAxis.getZ(), - squaredDistance, - m_minkowskiA->GetShapeType(), - m_minkowskiB->GetShapeType()); - - printf("If you can reproduce this, please email bugs@continuousphysics.com\n"); - printf("Please include above information, your Platform, version of OS.\n"); - printf("Thanks.\n"); - } -//#endif -#endif //CATCH_ME - - break; - - } - } if (checkSimplex) @@ -223,6 +211,8 @@ int curIter = 0; if (isValid) { + output.SetShapeIdentifiers(m_partId0,m_index0,m_partId1,m_index1); + output.AddContactPoint( normalInB, pointOnB, diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h b/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h index c550728f845..5eee81e82c6 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h @@ -39,9 +39,17 @@ class GjkPairDetector : public DiscreteCollisionDetectorInterface ConvexShape* m_minkowskiA; ConvexShape* m_minkowskiB; bool m_ignoreMargin; + public: + //experimental feature information, per triangle, per convex etc. + //'material combiner' / contact added callback + int m_partId0; + int m_index0; + int m_partId1; + int m_index1; + GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver* penetrationDepthSolver); virtual ~GjkPairDetector() {}; diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h index 35ebc4a40c6..250d9a9a8b4 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h @@ -40,6 +40,8 @@ class ManifoldPoint m_localPointB( pointB ), m_normalWorldOnB( normal ), m_distance1( distance ), + m_combinedFriction(0.f), + m_combinedRestitution(0.f), m_userPersistentData(0), m_lifeTime(0) { @@ -57,6 +59,8 @@ class ManifoldPoint SimdVector3 m_normalWorldOnB; float m_distance1; + float m_combinedFriction; + float m_combinedRestitution; void* m_userPersistentData; diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp index a5dabd4232d..70b856c32a9 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp @@ -32,6 +32,9 @@ struct MyResult : public DiscreteCollisionDetectorInterface::Result float m_depth; bool m_hasResult; + virtual void SetShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + } void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth) { m_normalOnBInWorld = normalOnBInWorld; diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp index df72b9a0417..457662aa550 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp @@ -19,7 +19,8 @@ subject to the following restrictions: #include <assert.h> float gContactBreakingTreshold = 0.02f; -ContactDestroyedCallback gContactCallback = 0; +ContactDestroyedCallback gContactDestroyedCallback = 0; + PersistentManifold::PersistentManifold() @@ -75,9 +76,9 @@ void PersistentManifold::ClearUserCache(ManifoldPoint& pt) assert(occurance<=0); #endif //DEBUG_PERSISTENCY - if (pt.m_userPersistentData && gContactCallback) + if (pt.m_userPersistentData && gContactDestroyedCallback) { - (*gContactCallback)(pt.m_userPersistentData); + (*gContactDestroyedCallback)(pt.m_userPersistentData); pt.m_userPersistentData = 0; } diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h index 98bef621f60..53895be9ade 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h @@ -27,14 +27,15 @@ struct CollisionResult; extern float gContactBreakingTreshold; typedef bool (*ContactDestroyedCallback)(void* userPersistentData); +extern ContactDestroyedCallback gContactDestroyedCallback; -extern ContactDestroyedCallback gContactCallback; #define MANIFOLD_CACHE_SIZE 4 -///PersistentManifold maintains contact points, and reduces them to 4 +///PersistentManifold maintains contact points, and reduces them to 4. +///It does contact filtering/contact reduction. class PersistentManifold { diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PointCollector.h b/extern/bullet/Bullet/NarrowPhaseCollision/PointCollector.h index a8e6324600e..9d1daf01c46 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/PointCollector.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/PointCollector.h @@ -35,6 +35,11 @@ struct PointCollector : public DiscreteCollisionDetectorInterface::Result { } + virtual void SetShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + //?? + } + virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth) { if (depth< m_distance) diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/SimplexSolverInterface.h b/extern/bullet/Bullet/NarrowPhaseCollision/SimplexSolverInterface.h index 8cefc900d29..6a15d8e4f0c 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/SimplexSolverInterface.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/SimplexSolverInterface.h @@ -26,8 +26,9 @@ subject to the following restrictions: #include "VoronoiSimplexSolver.h" #define SimplexSolverInterface VoronoiSimplexSolver #else -/// for simplices from 1 to 4 vertices -/// for example Johnson-algorithm or alternative approaches based on + +/// SimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices +/// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on /// voronoi regions or barycentric coordinates class SimplexSolverInterface { diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp index 019b4c3bfcb..a913261988a 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp @@ -37,7 +37,7 @@ bool SubsimplexConvexCast::calcTimeOfImpact( CastResult& result) { - MinkowskiSumShape combi(m_convexA,m_convexB); + MinkowskiSumShape combi(m_convexA,m_convexB); MinkowskiSumShape* convex = &combi; SimdTransform rayFromLocalA; @@ -92,7 +92,7 @@ bool SubsimplexConvexCast::calcTimeOfImpact( { VdotR = v.dot(r); - if (VdotR >= 0.f) + if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON)) return false; else { diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.h b/extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.h index 9c05571a927..9fc58e133df 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.h @@ -22,7 +22,9 @@ subject to the following restrictions: class ConvexShape; /// SubsimplexConvexCast implements Gino van den Bergens' paper +///"Ray Casting against General Convex Objects with Application to Continuous Collision Detection" /// GJK based Ray Cast, optimized version +/// Objects should not start in overlap, otherwise results are not defined. class SubsimplexConvexCast : public ConvexCast { SimplexSolverInterface* m_simplexSolver; @@ -34,7 +36,8 @@ public: SubsimplexConvexCast (ConvexShape* shapeA,ConvexShape* shapeB,SimplexSolverInterface* simplexSolver); //virtual ~SubsimplexConvexCast(); - + ///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. + ///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector. virtual bool calcTimeOfImpact( const SimdTransform& fromA, const SimdTransform& toA, diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp index 1b97934aa17..7b72784e721 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp @@ -33,18 +33,7 @@ SimdScalar contactTau = .02f;//0.02f;//*0.02f; -SimdScalar calculateCombinedFriction(RigidBody& body0,RigidBody& body1) -{ - SimdScalar friction = body0.getFriction() * body1.getFriction(); - - const SimdScalar MAX_FRICTION = 10.f; - if (friction < -MAX_FRICTION) - friction = -MAX_FRICTION; - if (friction > MAX_FRICTION) - friction = MAX_FRICTION; - return friction; -} //bilateral constraint between two dynamic objects @@ -128,26 +117,23 @@ float resolveSingleCollision( SimdScalar rel_vel; rel_vel = normal.dot(vel); - float combinedRestitution = body1.getRestitution() * body2.getRestitution(); - - SimdScalar Kfps = 1.f / solverInfo.m_timeStep ; float damping = solverInfo.m_damping ; - float tau = solverInfo.m_tau; + float Kerp = solverInfo.m_erp; if (useGlobalSettingContacts) { damping = contactDamping; - tau = contactTau; + Kerp = contactTau; } - float Kcor = tau *Kfps; + float Kcor = Kerp *Kfps; //printf("dist=%f\n",distance); - ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData; + ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); SimdScalar distance = cpd->m_penetration;//contactPoint.GetDistance(); @@ -156,7 +142,7 @@ float resolveSingleCollision( //distance = 0.f; SimdScalar positionalError = Kcor *-distance; //jacDiagABInv; - SimdScalar velocityError = cpd->m_restitution - rel_vel * damping; + SimdScalar velocityError = cpd->m_restitution - rel_vel;// * damping; SimdScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv; @@ -187,17 +173,19 @@ float resolveSingleFriction( ) { + const SimdVector3& pos1 = contactPoint.GetPositionWorldOnA(); const SimdVector3& pos2 = contactPoint.GetPositionWorldOnB(); const SimdVector3& normal = contactPoint.m_normalWorldOnB; SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); - float combinedFriction = calculateCombinedFriction(body1,body2); - + ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); + float combinedFriction = cpd->m_friction; + SimdScalar limit = cpd->m_appliedImpulse * combinedFriction; //if (contactPoint.m_appliedImpulse>0.f) //friction diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h index 847f2881f41..8304d412d6f 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h @@ -34,6 +34,7 @@ struct ConstraintPersistentData m_jacDiagABInv(0.f), m_persistentLifeTime(0), m_restitution(0.f), + m_friction(0.f), m_penetration(0.f) { } @@ -50,6 +51,7 @@ struct ConstraintPersistentData float m_jacDiagABInvTangent1; int m_persistentLifeTime; float m_restitution; + float m_friction; float m_penetration; SimdVector3 m_frictionWorldTangential0; SimdVector3 m_frictionWorldTangential1; diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h b/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h index 6c6325df8a6..2ec9b7f6ca1 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h @@ -22,7 +22,7 @@ struct ContactSolverInfo inline ContactSolverInfo() { - m_tau = 0.4f; + m_tau = 0.6f; m_damping = 1.0f; m_friction = 0.3f; m_restitution = 0.f; diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp new file mode 100644 index 00000000000..9597f2721c4 --- /dev/null +++ b/extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp @@ -0,0 +1,250 @@ +/* +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 "Generic6DofConstraint.h" +#include "Dynamics/RigidBody.h" +#include "Dynamics/MassProps.h" +#include "SimdTransformUtil.h" + +static const SimdScalar kSign[] = { 1.0f, -1.0f, 1.0f }; +static const int kAxisA[] = { 1, 0, 0 }; +static const int kAxisB[] = { 2, 2, 1 }; + +Generic6DofConstraint::Generic6DofConstraint() +{ +} + +Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB) +: TypedConstraint(rbA, rbB) +, m_frameInA(frameInA) +, m_frameInB(frameInB) +{ + //free means upper < lower, + //locked means upper == lower + //limited means upper > lower + //so start all locked + for (int i=0; i<6;++i) + { + m_lowerLimit[i] = 0.0f; + m_upperLimit[i] = 0.0f; + m_accumulatedImpulse[i] = 0.0f; + } + +} + + +void Generic6DofConstraint::BuildJacobian() +{ + SimdVector3 normal(0,0,0); + + const SimdVector3& pivotInA = m_frameInA.getOrigin(); + const SimdVector3& pivotInB = m_frameInB.getOrigin(); + + SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); + SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); + + SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + int i; + //linear part + for (i=0;i<3;i++) + { + if (isLimited(i)) + { + normal[i] = 1; + + // Create linear atom + new (&m_jacLinear[i]) JacobianEntry( + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(), + m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(), + normal, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + + // Apply accumulated impulse + SimdVector3 impulse_vector = m_accumulatedImpulse[i] * normal; + + m_rbA.applyImpulse( impulse_vector, rel_pos1); + m_rbB.applyImpulse(-impulse_vector, rel_pos2); + + normal[i] = 0; + } + } + + // angular part + for (i=0;i<3;i++) + { + if (isLimited(i+3)) + { + SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); + SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); + + // Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe + SimdVector3 axis = kSign[i] * axisA.cross(axisB); + + // Create angular atom + new (&m_jacAng[i]) JacobianEntry(axis, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + // Apply accumulated impulse + SimdVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis; + + m_rbA.applyTorqueImpulse( impulse_vector); + m_rbB.applyTorqueImpulse(-impulse_vector); + } + } +} + +void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep) +{ + SimdScalar tau = 0.1f; + SimdScalar damping = 1.0f; + + SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); + SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); + + SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + SimdVector3 normal(0,0,0); + int i; + + // linear + for (i=0;i<3;i++) + { + if (isLimited(i)) + { + SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + + normal[i] = 1; + SimdScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal(); + + //velocity error (first order error) + SimdScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + + //positional error (zeroth order error) + SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); + + SimdScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv; + m_accumulatedImpulse[i] += impulse; + + SimdVector3 impulse_vector = normal * impulse; + m_rbA.applyImpulse( impulse_vector, rel_pos1); + m_rbB.applyImpulse(-impulse_vector, rel_pos2); + + normal[i] = 0; + } + } + + // angular + for (i=0;i<3;i++) + { + if (isLimited(i+3)) + { + SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + SimdScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal(); + + //velocity error (first order error) + SimdScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + + //positional error (zeroth order error) + SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); + SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); + + SimdScalar rel_pos = kSign[i] * axisA.dot(axisB); + + //impulse + SimdScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv; + m_accumulatedImpulse[i + 3] += impulse; + + // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above) + SimdVector3 axis = kSign[i] * axisA.cross(axisB); + SimdVector3 impulse_vector = axis * impulse; + + m_rbA.applyTorqueImpulse( impulse_vector); + m_rbB.applyTorqueImpulse(-impulse_vector); + } + } +} + +void Generic6DofConstraint::UpdateRHS(SimdScalar timeStep) +{ + +} + +SimdScalar Generic6DofConstraint::ComputeAngle(int axis) const + { + SimdScalar angle; + + switch (axis) + { + case 0: + { + SimdVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1); + SimdVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1); + SimdVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2); + + SimdScalar s = v1.dot(w2); + SimdScalar c = v1.dot(v2); + + angle = SimdAtan2( s, c ); + } + break; + + case 1: + { + SimdVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2); + SimdVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2); + SimdVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0); + + SimdScalar s = w1.dot(u2); + SimdScalar c = w1.dot(w2); + + angle = SimdAtan2( s, c ); + } + break; + + case 2: + { + SimdVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0); + SimdVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0); + SimdVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1); + + SimdScalar s = u1.dot(v2); + SimdScalar c = u1.dot(u2); + + angle = SimdAtan2( s, c ); + } + break; + } + + return angle; + } + diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h b/extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h new file mode 100644 index 00000000000..7a77253509c --- /dev/null +++ b/extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h @@ -0,0 +1,114 @@ +/* +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 GENERIC_6DOF_CONSTRAINT_H +#define GENERIC_6DOF_CONSTRAINT_H + +#include "SimdVector3.h" + +#include "ConstraintSolver/JacobianEntry.h" +#include "TypedConstraint.h" + +class RigidBody; + + + +/// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space +/// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked' +/// Work in progress (is still a Hinge actually) +class Generic6DofConstraint : public TypedConstraint +{ + JacobianEntry m_jacLinear[3]; // 3 orthogonal linear constraints + JacobianEntry m_jacAng[3]; // 3 orthogonal angular constraints + + SimdTransform m_frameInA; // the constraint space w.r.t body A + SimdTransform m_frameInB; // the constraint space w.r.t body B + + SimdScalar m_lowerLimit[6]; // the constraint lower limits + SimdScalar m_upperLimit[6]; // the constraint upper limits + + SimdScalar m_accumulatedImpulse[6]; + + +public: + Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB ); + + Generic6DofConstraint(); + + virtual void BuildJacobian(); + + virtual void SolveConstraint(SimdScalar timeStep); + + void UpdateRHS(SimdScalar timeStep); + + SimdScalar ComputeAngle(int axis) const; + + void setLinearLowerLimit(const SimdVector3& linearLower) + { + m_lowerLimit[0] = linearLower.getX(); + m_lowerLimit[1] = linearLower.getY(); + m_lowerLimit[2] = linearLower.getZ(); + } + + void setLinearUpperLimit(const SimdVector3& linearUpper) + { + m_upperLimit[0] = linearUpper.getX(); + m_upperLimit[1] = linearUpper.getY(); + m_upperLimit[2] = linearUpper.getZ(); + } + + void setAngularLowerLimit(const SimdVector3& angularLower) + { + m_lowerLimit[3] = angularLower.getX(); + m_lowerLimit[4] = angularLower.getY(); + m_lowerLimit[5] = angularLower.getZ(); + } + + void setAngularUpperLimit(const SimdVector3& angularUpper) + { + m_upperLimit[3] = angularUpper.getX(); + m_upperLimit[4] = angularUpper.getY(); + m_upperLimit[5] = angularUpper.getZ(); + } + + //first 3 are linear, next 3 are angular + void SetLimit(int axis, SimdScalar lo, SimdScalar hi) + { + m_lowerLimit[axis] = lo; + m_upperLimit[axis] = hi; + } + + //free means upper < lower, + //locked means upper == lower + //limited means upper > lower + //limitIndex: first 3 are linear, next 3 are angular + bool isLimited(int limitIndex) + { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + + const RigidBody& GetRigidBodyA() const + { + return m_rbA; + } + const RigidBody& GetRigidBodyB() const + { + return m_rbB; + } + + +}; + +#endif //GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp index 0e5c7cf5715..fb7f1f47cd7 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp @@ -28,7 +28,7 @@ HingeConstraint::HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector SimdVector3& axisInA,SimdVector3& axisInB) :TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), m_axisInA(axisInA), -m_axisInB(axisInB), +m_axisInB(-axisInB), m_angularOnly(false) { @@ -73,7 +73,7 @@ void HingeConstraint::BuildJacobian() //calculate two perpendicular jointAxis, orthogonal to hingeAxis //these two jointAxis require equal angular velocities for both bodies - //this is ununsed for now, it's a todo + //this is unused for now, it's a todo SimdVector3 axisWorldA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; SimdVector3 jointAxis0; SimdVector3 jointAxis1; @@ -96,6 +96,102 @@ void HingeConstraint::BuildJacobian() void HingeConstraint::SolveConstraint(SimdScalar timeStep) { +//#define NEW_IMPLEMENTATION + +#ifdef NEW_IMPLEMENTATION + SimdScalar tau = 0.3f; + SimdScalar damping = 1.f; + + SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; + SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + + // Dirk: Don't we need to update this after each applied impulse + SimdVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + SimdVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + + if (!m_angularOnly) + { + SimdVector3 normal(0,0,0); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + + SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); + SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); + SimdVector3 vel = vel1 - vel2; + + // Dirk: Get new angular velocity since it changed after applying an impulse + angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + //velocity error (first order error) + SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + + //positional error (zeroth order error) + SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); + + SimdScalar impulse = tau*depth/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv; + + SimdVector3 impulse_vector = normal * impulse; + m_rbA.applyImpulse( impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); + m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); + + normal[i] = 0; + } + } + + ///solve angular part + + // get axes in world space + SimdVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + SimdVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB; + + // constraint axes in world space + SimdVector3 jointAxis0; + SimdVector3 jointAxis1; + SimdPlaneSpace1(axisA,jointAxis0,jointAxis1); + + + // Dirk: Get new angular velocity since it changed after applying an impulse + angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + SimdScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal(); + SimdScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + float tau1 = tau;//0.f; + + SimdScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0; + SimdVector3 angular_impulse0 = jointAxis0 * impulse0; + + m_rbA.applyTorqueImpulse( angular_impulse0); + m_rbB.applyTorqueImpulse(-angular_impulse0); + + + + // Dirk: Get new angular velocity since it changed after applying an impulse + angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + SimdScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal(); + SimdScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB);; + + SimdScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1; + SimdVector3 angular_impulse1 = jointAxis1 * impulse1; + + m_rbA.applyTorqueImpulse( angular_impulse1); + m_rbB.applyTorqueImpulse(-angular_impulse1); + +#else + SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; @@ -104,7 +200,7 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep) SimdScalar tau = 0.3f; SimdScalar damping = 1.f; - if (!m_angularOnly) +//linear part { for (int i=0;i<3;i++) { @@ -169,6 +265,8 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep) m_rbA.applyTorqueImpulse(-velrel+angularError); m_rbB.applyTorqueImpulse(velrel-angularError); +#endif + } void HingeConstraint::UpdateRHS(SimdScalar timeStep) diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h b/extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h index 821c681a7bd..d1dd367a012 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h @@ -42,28 +42,48 @@ public: const SimdScalar massInvA, const SimdVector3& inertiaInvB, const SimdScalar massInvB) - :m_jointAxis(jointAxis) + :m_linearJointAxis(jointAxis) { - m_aJ = world2A*(rel_pos1.cross(m_jointAxis)); - m_bJ = world2B*(rel_pos2.cross(-m_jointAxis)); + m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis)); + m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis)); m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); + + ASSERT(m_Adiag > 0.0f); } - //angular constraint between two different rigidbodies + //angular constraint between two different rigidbodies JacobianEntry(const SimdVector3& jointAxis, const SimdMatrix3x3& world2A, const SimdMatrix3x3& world2B, const SimdVector3& inertiaInvA, const SimdVector3& inertiaInvB) - :m_jointAxis(m_jointAxis) + :m_linearJointAxis(SimdVector3(0.f,0.f,0.f)) + { + m_aJ= world2A*jointAxis; + m_bJ = world2B*-jointAxis; + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ASSERT(m_Adiag > 0.0f); + } + + //angular constraint between two different rigidbodies + JacobianEntry(const SimdVector3& axisInA, + const SimdVector3& axisInB, + const SimdVector3& inertiaInvA, + const SimdVector3& inertiaInvB) + : m_linearJointAxis(SimdVector3(0.f,0.f,0.f)) + , m_aJ(axisInA) + , m_bJ(-axisInB) { - m_aJ= world2A*m_jointAxis; - m_bJ = world2B*-m_jointAxis; m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ASSERT(m_Adiag > 0.0f); } //constraint on one rigidbody @@ -73,13 +93,15 @@ public: const SimdVector3& jointAxis, const SimdVector3& inertiaInvA, const SimdScalar massInvA) - :m_jointAxis(jointAxis) + :m_linearJointAxis(jointAxis) { - m_aJ= world2A*(rel_pos1.cross(m_jointAxis)); - m_bJ = world2A*(rel_pos2.cross(-m_jointAxis)); + m_aJ= world2A*(rel_pos1.cross(jointAxis)); + m_bJ = world2A*(rel_pos2.cross(-jointAxis)); m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = SimdVector3(0.f,0.f,0.f); m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); + + ASSERT(m_Adiag > 0.0f); } SimdScalar getDiagonal() const { return m_Adiag; } @@ -88,7 +110,7 @@ public: SimdScalar getNonDiagonal(const JacobianEntry& jacB, const SimdScalar massInvA) const { const JacobianEntry& jacA = *this; - SimdScalar lin = massInvA * jacA.m_jointAxis.dot(jacB.m_jointAxis); + SimdScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); SimdScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ); return lin + ang; } @@ -99,7 +121,7 @@ public: SimdScalar getNonDiagonal(const JacobianEntry& jacB,const SimdScalar massInvA,const SimdScalar massInvB) const { const JacobianEntry& jacA = *this; - SimdVector3 lin = jacA.m_jointAxis* jacB.m_jointAxis; + SimdVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; SimdVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; SimdVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; SimdVector3 lin0 = massInvA * lin ; @@ -113,7 +135,7 @@ public: SimdVector3 linrel = linvelA - linvelB; SimdVector3 angvela = angvelA * m_aJ; SimdVector3 angvelb = angvelB * m_bJ; - linrel *= m_jointAxis; + linrel *= m_linearJointAxis; angvela += angvelb; angvela += linrel; SimdScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2]; @@ -121,7 +143,7 @@ public: } //private: - SimdVector3 m_jointAxis; + SimdVector3 m_linearJointAxis; SimdVector3 m_aJ; SimdVector3 m_bJ; SimdVector3 m_0MinvJt; diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp deleted file mode 100644 index 2d91b4a3fd0..00000000000 --- a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp +++ /dev/null @@ -1,265 +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. -*/ - - - -#include "OdeConstraintSolver.h" - -#include "NarrowPhaseCollision/PersistentManifold.h" -#include "Dynamics/RigidBody.h" -#include "ContactConstraint.h" -#include "Solve2LinearConstraint.h" -#include "ContactSolverInfo.h" -#include "Dynamics/BU_Joint.h" -#include "Dynamics/ContactJoint.h" - -#include "IDebugDraw.h" - -#define USE_SOR_SOLVER - -#include "SorLcp.h" - -#include <math.h> -#include <float.h>//FLT_MAX -#ifdef WIN32 -#include <memory.h> -#endif -#include <string.h> -#include <stdio.h> - -#if defined (WIN32) -#include <malloc.h> -#else -#if defined (__FreeBSD__) || defined (__OpenBSD__) -#include <stdlib.h> -#else -#include <alloca.h> -#endif -#endif - -class BU_Joint; - -//see below - -OdeConstraintSolver::OdeConstraintSolver(): -m_cfm(0.f),//1e-5f), -m_erp(0.4f) -{ -} - - - - - - - -//iterative lcp and penalty method -float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer) -{ - m_CurBody = 0; - m_CurJoint = 0; - - - RigidBody* bodies [MAX_RIGIDBODIES]; - - int numBodies = 0; - BU_Joint* joints [MAX_RIGIDBODIES*4]; - int numJoints = 0; - - for (int j=0;j<numManifolds;j++) - { - - int body0=-1,body1=-1; - - PersistentManifold* manifold = manifoldPtr[j]; - if (manifold->GetNumContacts() > 0) - { - body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies); - body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies); - ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer); - } - } - - SolveInternal1(m_cfm,m_erp,bodies,numBodies,joints,numJoints,infoGlobal); - - return 0.f; - -} - -///////////////////////////////////////////////////////////////////////////////// - - -typedef SimdScalar dQuaternion[4]; -#define _R(i,j) R[(i)*4+(j)] - -void dRfromQ1 (dMatrix3 R, const dQuaternion q) -{ - // q = (s,vx,vy,vz) - SimdScalar qq1 = 2.f*q[1]*q[1]; - SimdScalar qq2 = 2.f*q[2]*q[2]; - SimdScalar qq3 = 2.f*q[3]*q[3]; - _R(0,0) = 1.f - qq2 - qq3; - _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); - _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); - _R(0,3) = 0.f; - - _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); - _R(1,1) = 1.f - qq1 - qq3; - _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); - _R(1,3) = 0.f; - - _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); - _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); - _R(2,2) = 1.f - qq1 - qq2; - _R(2,3) = 0.f; - -} - - - -int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies) -{ - if (!body || (body->getInvMass() == 0.f) ) - { - return -1; - } - //first try to find - int i,j; - for (i=0;i<numBodies;i++) - { - if (bodies[i] == body) - return i; - } - //if not found, create a new body - bodies[numBodies++] = body; - //convert data - - - body->m_facc.setValue(0,0,0,0); - body->m_tacc.setValue(0,0,0,0); - - //are the indices the same ? - for (i=0;i<4;i++) - { - for ( j=0;j<3;j++) - { - body->m_invI[i+4*j] = 0.f; - body->m_I[i+4*j] = 0.f; - } - } - body->m_invI[0+4*0] = body->getInvInertiaDiagLocal()[0]; - body->m_invI[1+4*1] = body->getInvInertiaDiagLocal()[1]; - body->m_invI[2+4*2] = body->getInvInertiaDiagLocal()[2]; - - body->m_I[0+0*4] = 1.f/body->getInvInertiaDiagLocal()[0]; - body->m_I[1+1*4] = 1.f/body->getInvInertiaDiagLocal()[1]; - body->m_I[2+2*4] = 1.f/body->getInvInertiaDiagLocal()[2]; - - - - - dQuaternion q; - - q[1] = body->getOrientation()[0]; - q[2] = body->getOrientation()[1]; - q[3] = body->getOrientation()[2]; - q[0] = body->getOrientation()[3]; - - dRfromQ1(body->m_R,q); - - return numBodies-1; -} - - - - - -#define MAX_JOINTS_1 8192 - -static ContactJoint gJointArray[MAX_JOINTS_1]; - - -void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, - RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer) -{ - - - manifold->RefreshContactPoints(((RigidBody*)manifold->GetBody0())->getCenterOfMassTransform(), - ((RigidBody*)manifold->GetBody1())->getCenterOfMassTransform()); - - int bodyId0 = _bodyId0,bodyId1 = _bodyId1; - - int i,numContacts = manifold->GetNumContacts(); - - bool swapBodies = (bodyId0 < 0); - - - RigidBody* body0,*body1; - - if (swapBodies) - { - bodyId0 = _bodyId1; - bodyId1 = _bodyId0; - - body0 = (RigidBody*)manifold->GetBody1(); - body1 = (RigidBody*)manifold->GetBody0(); - - } else - { - body0 = (RigidBody*)manifold->GetBody0(); - body1 = (RigidBody*)manifold->GetBody1(); - } - - assert(bodyId0 >= 0); - - SimdVector3 color(0,1,0); - for (i=0;i<numContacts;i++) - { - - if (debugDrawer) - { - const ManifoldPoint& cp = manifold->GetContactPoint(i); - - debugDrawer->DrawContactPoint( - cp.m_positionWorldOnB, - cp.m_normalWorldOnB, - cp.GetDistance(), - cp.GetLifeTime(), - color); - - } - assert (m_CurJoint < MAX_JOINTS_1); - -// if (manifold->GetContactPoint(i).GetDistance() < 0.0f) - { - ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1); - - cont->node[0].joint = cont; - cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0; - - cont->node[1].joint = cont; - cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0; - - joints[numJoints++] = cont; - for (int i=0;i<6;i++) - cont->lambda[i] = 0.f; - - cont->flags = 0; - } - } - - //create a new contact constraint -}; - diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h deleted file mode 100644 index 99840557a3b..00000000000 --- a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h +++ /dev/null @@ -1,66 +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. -*/ - -#ifndef ODE_CONSTRAINT_SOLVER_H -#define ODE_CONSTRAINT_SOLVER_H - -#include "ConstraintSolver.h" - -class RigidBody; -class BU_Joint; - -/// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework -/// It uses the the unmodified version of quickstep solver from the open dynamics project -class OdeConstraintSolver : public ConstraintSolver -{ -private: - - int m_CurBody; - int m_CurJoint; - - float m_cfm; - float m_erp; - - - int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies); - void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, - RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer); - -public: - - OdeConstraintSolver(); - - virtual ~OdeConstraintSolver() {} - - virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,IDebugDraw* debugDrawer = 0); - - ///setConstraintForceMixing, the cfm adds some positive value to the main diagonal - ///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy' - void setConstraintForceMixing(float cfm) { - m_cfm = cfm; - } - - ///setErrorReductionParamter sets the maximum amount of error reduction - ///which limits energy addition during penetration depth recovery - void setErrorReductionParamter(float erp) - { - m_erp = erp; - } -}; - - - - -#endif //ODE_CONSTRAINT_SOLVER_H diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cpp index fe59a40683a..26637bb9932 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cpp @@ -14,7 +14,7 @@ subject to the following restrictions: */ -#include "SimpleConstraintSolver.h" +#include "SequentialImpulseConstraintSolver.h" #include "NarrowPhaseCollision/PersistentManifold.h" #include "Dynamics/RigidBody.h" #include "ContactConstraint.h" @@ -45,14 +45,14 @@ bool MyContactDestroyedCallback(void* userPersistentData) } -SimpleConstraintSolver::SimpleConstraintSolver() +SequentialImpulseConstraintSolver::SequentialImpulseConstraintSolver() { - gContactCallback = &MyContactDestroyedCallback; + gContactDestroyedCallback = &MyContactDestroyedCallback; } -/// SimpleConstraintSolver Sequentially applies impulses -float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer) +/// SequentialImpulseConstraintSolver Sequentially applies impulses +float SequentialImpulseConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer) { ContactSolverInfo info = infoGlobal; @@ -113,7 +113,7 @@ SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution) -float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer) +float SequentialImpulseConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer) { RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0(); @@ -188,10 +188,10 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta SimdScalar rel_vel; rel_vel = cp.m_normalWorldOnB.dot(vel); - float combinedRestitution = body0->getRestitution() * body1->getRestitution(); - + float combinedRestitution = cp.m_combinedRestitution; + cpd->m_penetration = cp.GetDistance(); - + cpd->m_friction = cp.m_combinedFriction; cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution); if (cpd->m_restitution <= 0.) //0.f) { @@ -294,7 +294,7 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta return maxImpulse; } -float SimpleConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer) +float SequentialImpulseConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer) { RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0(); RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1(); diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h b/extern/bullet/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.h index 99b77623b4b..598a4cfa903 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.h @@ -13,17 +13,17 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SIMPLE_CONSTRAINT_SOLVER_H -#define SIMPLE_CONSTRAINT_SOLVER_H +#ifndef SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H +#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H #include "ConstraintSolver.h" class IDebugDraw; -/// SimpleConstraintSolver uses a Propagation Method and Sequentially applies impulses +/// SequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses /// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com /// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) /// Applies impulses for combined restitution and penetration recovery and to simulate friction -class SimpleConstraintSolver : public ConstraintSolver +class SequentialImpulseConstraintSolver : public ConstraintSolver { float Solve(PersistentManifold* manifold, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer); float SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer); @@ -31,13 +31,13 @@ class SimpleConstraintSolver : public ConstraintSolver public: - SimpleConstraintSolver(); + SequentialImpulseConstraintSolver(); - virtual ~SimpleConstraintSolver() {} + virtual ~SequentialImpulseConstraintSolver() {} virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info, IDebugDraw* debugDrawer=0); }; -#endif //SIMPLE_CONSTRAINT_SOLVER_H +#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp deleted file mode 100644 index d7c374d937f..00000000000 --- a/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp +++ /dev/null @@ -1,849 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include "SorLcp.h" - -#ifdef USE_SOR_SOLVER - -// SOR LCP taken from ode quickstep, -// todo: write own successive overrelaxation gauss-seidel, or jacobi iterative solver - - -#include "SimdScalar.h" - -#include "Dynamics/RigidBody.h" -#include <math.h> -#include <float.h>//FLT_MAX -#ifdef WIN32 -#include <memory.h> -#endif -#include <string.h> -#include <stdio.h> - -#if defined (WIN32) -#include <malloc.h> -#else -#if defined (__FreeBSD__) || defined (__OpenBSD__) -#include <stdlib.h> -#else -#include <alloca.h> -#endif -#endif - -#include "Dynamics/BU_Joint.h" -#include "ContactSolverInfo.h" - -//////////////////////////////////////////////////////////////////// -//math stuff - -typedef SimdScalar dVector4[4]; -typedef SimdScalar dMatrix3[4*3]; -#define dInfinity FLT_MAX - - - -#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */ - - - -#define dMULTIPLY0_331NEW(A,op,B,C) \ -{\ - float tmp[3];\ - tmp[0] = C.getX();\ - tmp[1] = C.getY();\ - tmp[2] = C.getZ();\ - dMULTIPLYOP0_331(A,op,B,tmp);\ -} - -#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) -#define dMULTIPLYOP0_331(A,op,B,C) \ - (A)[0] op dDOT1((B),(C)); \ - (A)[1] op dDOT1((B+4),(C)); \ - (A)[2] op dDOT1((B+8),(C)); - -#define dAASSERT ASSERT -#define dIASSERT ASSERT - -#define REAL float -#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) -SimdScalar dDOT1 (const SimdScalar *a, const SimdScalar *b) { return dDOTpq(a,b,1,1); } -#define dDOT14(a,b) dDOTpq(a,b,1,4) - -#define dCROSS(a,op,b,c) \ - (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ - (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ - (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); - - -#define dMULTIPLYOP2_333(A,op,B,C) \ - (A)[0] op dDOT1((B),(C)); \ - (A)[1] op dDOT1((B),(C+4)); \ - (A)[2] op dDOT1((B),(C+8)); \ - (A)[4] op dDOT1((B+4),(C)); \ - (A)[5] op dDOT1((B+4),(C+4)); \ - (A)[6] op dDOT1((B+4),(C+8)); \ - (A)[8] op dDOT1((B+8),(C)); \ - (A)[9] op dDOT1((B+8),(C+4)); \ - (A)[10] op dDOT1((B+8),(C+8)); -#define dMULTIPLYOP0_333(A,op,B,C) \ - (A)[0] op dDOT14((B),(C)); \ - (A)[1] op dDOT14((B),(C+1)); \ - (A)[2] op dDOT14((B),(C+2)); \ - (A)[4] op dDOT14((B+4),(C)); \ - (A)[5] op dDOT14((B+4),(C+1)); \ - (A)[6] op dDOT14((B+4),(C+2)); \ - (A)[8] op dDOT14((B+8),(C)); \ - (A)[9] op dDOT14((B+8),(C+1)); \ - (A)[10] op dDOT14((B+8),(C+2)); - -#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C) -#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C) -#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C) - - -//////////////////////////////////////////////////////////////////// -#define EFFICIENT_ALIGNMENT 16 -#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1) -/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste - * up to 15 bytes per allocation, depending on what alloca() returns. - */ - -#define dALLOCA16(n) \ - ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1)))))) - - - -///////////////////////////////////////////////////////////////////// -///////////////////////////////////////////////////////////////////// - -#ifdef DEBUG -#define ANSI_FTOL 1 - -extern "C" { - __declspec(naked) void _ftol2() { - __asm { -#if ANSI_FTOL - fnstcw WORD PTR [esp-2] - mov ax, WORD PTR [esp-2] - - OR AX, 0C00h - - mov WORD PTR [esp-4], ax - fldcw WORD PTR [esp-4] - fistp QWORD PTR [esp-12] - fldcw WORD PTR [esp-2] - mov eax, DWORD PTR [esp-12] - mov edx, DWORD PTR [esp-8] -#else - fistp DWORD PTR [esp-12] - mov eax, DWORD PTR [esp-12] - mov ecx, DWORD PTR [esp-8] -#endif - ret - } - } -} -#endif //DEBUG - - - - - - -#define ALLOCA dALLOCA16 - -typedef const SimdScalar *dRealPtr; -typedef SimdScalar *dRealMutablePtr; -#define dRealArray(name,n) SimdScalar name[n]; -#define dRealAllocaArray(name,n) SimdScalar *name = (SimdScalar*) ALLOCA ((n)*sizeof(SimdScalar)); - -void dSetZero1 (SimdScalar *a, int n) -{ - dAASSERT (a && n >= 0); - while (n > 0) { - *(a++) = 0; - n--; - } -} - -void dSetValue1 (SimdScalar *a, int n, SimdScalar value) -{ - dAASSERT (a && n >= 0); - while (n > 0) { - *(a++) = value; - n--; - } -} - - -//*************************************************************************** -// configuration - -// for the SOR and CG methods: -// uncomment the following line to use warm starting. this definitely -// help for motor-driven joints. unfortunately it appears to hurt -// with high-friction contacts using the SOR method. use with care - -//#define WARM_STARTING 1 - -// for the SOR method: -// uncomment the following line to randomly reorder constraint rows -// during the solution. depending on the situation, this can help a lot -// or hardly at all, but it doesn't seem to hurt. - -#define RANDOMLY_REORDER_CONSTRAINTS 1 - - - -//*************************************************************************** -// various common computations involving the matrix J - -// compute iMJ = inv(M)*J' - -static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb, - RigidBody * const *body, dRealPtr invI) -{ - int i,j; - dRealMutablePtr iMJ_ptr = iMJ; - dRealMutablePtr J_ptr = J; - for (i=0; i<m; i++) { - int b1 = jb[i*2]; - int b2 = jb[i*2+1]; - SimdScalar k = body[b1]->getInvMass(); - for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j]; - dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3); - if (b2 >= 0) { - k = body[b2]->getInvMass(); - for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6]; - dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9); - } - J_ptr += 12; - iMJ_ptr += 12; - } -} - -#if 0 -static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb, - dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2) -{ - int i,j; - - - - dRealMutablePtr out_ptr1 = out + onlyBody1*6; - - for (j=0; j<6; j++) - out_ptr1[j] = 0; - - if (onlyBody2 >= 0) - { - out_ptr1 = out + onlyBody2*6; - - for (j=0; j<6; j++) - out_ptr1[j] = 0; - } - - dRealPtr iMJ_ptr = iMJ; - for (i=0; i<m; i++) { - - int b1 = jb[i*2]; - - dRealMutablePtr out_ptr = out + b1*6; - if ((b1 == onlyBody1) || (b1 == onlyBody2)) - { - for (j=0; j<6; j++) - out_ptr[j] += iMJ_ptr[j] * in[i] ; - } - - iMJ_ptr += 6; - - int b2 = jb[i*2+1]; - if ((b2 == onlyBody1) || (b2 == onlyBody2)) - { - if (b2 >= 0) - { - out_ptr = out + b2*6; - for (j=0; j<6; j++) - out_ptr[j] += iMJ_ptr[j] * in[i]; - } - } - - iMJ_ptr += 6; - - } -} -#endif - - -// compute out = inv(M)*J'*in. - -#if 0 -static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb, - dRealMutablePtr in, dRealMutablePtr out) -{ - int i,j; - dSetZero1 (out,6*nb); - dRealPtr iMJ_ptr = iMJ; - for (i=0; i<m; i++) { - int b1 = jb[i*2]; - int b2 = jb[i*2+1]; - dRealMutablePtr out_ptr = out + b1*6; - for (j=0; j<6; j++) - out_ptr[j] += iMJ_ptr[j] * in[i]; - iMJ_ptr += 6; - if (b2 >= 0) { - out_ptr = out + b2*6; - for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i]; - } - iMJ_ptr += 6; - } -} -#endif - - -// compute out = J*in. - -static void multiply_J (int m, dRealMutablePtr J, int *jb, - dRealMutablePtr in, dRealMutablePtr out) -{ - int i,j; - dRealPtr J_ptr = J; - for (i=0; i<m; i++) { - int b1 = jb[i*2]; - int b2 = jb[i*2+1]; - SimdScalar sum = 0; - dRealMutablePtr in_ptr = in + b1*6; - for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; - J_ptr += 6; - if (b2 >= 0) { - in_ptr = in + b2*6; - for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; - } - J_ptr += 6; - out[i] = sum; - } -} - -//*************************************************************************** -// SOR-LCP method - -// nb is the number of bodies in the body array. -// J is an m*12 matrix of constraint rows -// jb is an array of first and second body numbers for each constraint row -// invI is the global frame inverse inertia for each body (stacked 3x3 matrices) -// -// this returns lambda and fc (the constraint force). -// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda -// -// b, lo and hi are modified on exit - - -struct IndexError { - SimdScalar error; // error to sort on - int findex; - int index; // row index -}; - -static unsigned long seed2 = 0; - -unsigned long dRand2() -{ - seed2 = (1664525L*seed2 + 1013904223L) & 0xffffffff; - return seed2; -} - -int dRandInt2 (int n) -{ - float a = float(n) / 4294967296.0f; - return (int) (float(dRand2()) * a); -} - - -static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * const *body, - dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs, - dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, - int numiter,float overRelax) -{ - const int num_iterations = numiter; - const float sor_w = overRelax; // SOR over-relaxation parameter - - int i,j; - -#ifdef WARM_STARTING - // for warm starting, this seems to be necessary to prevent - // jerkiness in motor-driven joints. i have no idea why this works. - for (i=0; i<m; i++) lambda[i] *= 0.9; -#else - dSetZero1 (lambda,m); -#endif - - // the lambda computed at the previous iteration. - // this is used to measure error for when we are reordering the indexes. - dRealAllocaArray (last_lambda,m); - - // a copy of the 'hi' vector in case findex[] is being used - dRealAllocaArray (hicopy,m); - memcpy (hicopy,hi,m*sizeof(float)); - - // precompute iMJ = inv(M)*J' - dRealAllocaArray (iMJ,m*12); - compute_invM_JT (m,J,iMJ,jb,body,invI); - - // compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc - // as we change lambda. -#ifdef WARM_STARTING - multiply_invM_JT (m,nb,iMJ,jb,lambda,fc); -#else - dSetZero1 (invMforce,nb*6); -#endif - - // precompute 1 / diagonals of A - dRealAllocaArray (Ad,m); - dRealPtr iMJ_ptr = iMJ; - dRealMutablePtr J_ptr = J; - for (i=0; i<m; i++) { - float sum = 0; - for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j]; - if (jb[i*2+1] >= 0) { - for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; - } - iMJ_ptr += 12; - J_ptr += 12; - Ad[i] = sor_w / sum;//(sum + cfm[i]); - } - - // scale J and b by Ad - J_ptr = J; - for (i=0; i<m; i++) { - for (j=0; j<12; j++) { - J_ptr[0] *= Ad[i]; - J_ptr++; - } - rhs[i] *= Ad[i]; - } - - // scale Ad by CFM - for (i=0; i<m; i++) - Ad[i] *= cfm[i]; - - // order to solve constraint rows in - IndexError *order = (IndexError*) alloca (m*sizeof(IndexError)); - -#ifndef REORDER_CONSTRAINTS - // make sure constraints with findex < 0 come first. - j=0; - for (i=0; i<m; i++) - if (findex[i] < 0) - order[j++].index = i; - for (i=0; i<m; i++) - if (findex[i] >= 0) - order[j++].index = i; - dIASSERT (j==m); -#endif - - for (int iteration=0; iteration < num_iterations; iteration++) { - -#ifdef REORDER_CONSTRAINTS - // constraints with findex < 0 always come first. - if (iteration < 2) { - // for the first two iterations, solve the constraints in - // the given order - for (i=0; i<m; i++) { - order[i].error = i; - order[i].findex = findex[i]; - order[i].index = i; - } - } - else { - // sort the constraints so that the ones converging slowest - // get solved last. use the absolute (not relative) error. - for (i=0; i<m; i++) { - float v1 = dFabs (lambda[i]); - float v2 = dFabs (last_lambda[i]); - float max = (v1 > v2) ? v1 : v2; - if (max > 0) { - //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max; - order[i].error = dFabs(lambda[i]-last_lambda[i]); - } - else { - order[i].error = dInfinity; - } - order[i].findex = findex[i]; - order[i].index = i; - } - } - qsort (order,m,sizeof(IndexError),&compare_index_error); -#endif -#ifdef RANDOMLY_REORDER_CONSTRAINTS - if ((iteration & 7) == 0) { - for (i=1; i<m; ++i) { - IndexError tmp = order[i]; - int swapi = dRandInt2(i+1); - order[i] = order[swapi]; - order[swapi] = tmp; - } - } -#endif - - //@@@ potential optimization: swap lambda and last_lambda pointers rather - // than copying the data. we must make sure lambda is properly - // returned to the caller - memcpy (last_lambda,lambda,m*sizeof(float)); - - for (int i=0; i<m; i++) { - // @@@ potential optimization: we could pre-sort J and iMJ, thereby - // linearizing access to those arrays. hmmm, this does not seem - // like a win, but we should think carefully about our memory - // access pattern. - - int index = order[i].index; - J_ptr = J + index*12; - iMJ_ptr = iMJ + index*12; - - // set the limits for this constraint. note that 'hicopy' is used. - // this is the place where the QuickStep method differs from the - // direct LCP solving method, since that method only performs this - // limit adjustment once per time step, whereas this method performs - // once per iteration per constraint row. - // the constraints are ordered so that all lambda[] values needed have - // already been computed. - if (findex[index] >= 0) { - hi[index] = SimdFabs (hicopy[index] * lambda[findex[index]]); - lo[index] = -hi[index]; - } - - int b1 = jb[index*2]; - int b2 = jb[index*2+1]; - float delta = rhs[index] - lambda[index]*Ad[index]; - dRealMutablePtr fc_ptr = invMforce + 6*b1; - - // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case - delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] + - fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] + - fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5]; - // @@@ potential optimization: handle 1-body constraints in a separate - // loop to avoid the cost of test & jump? - if (b2 >= 0) { - fc_ptr = invMforce + 6*b2; - delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] + - fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] + - fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11]; - } - - // compute lambda and clamp it to [lo,hi]. - // @@@ potential optimization: does SSE have clamping instructions - // to save test+jump penalties here? - float new_lambda = lambda[index] + delta; - if (new_lambda < lo[index]) { - delta = lo[index]-lambda[index]; - lambda[index] = lo[index]; - } - else if (new_lambda > hi[index]) { - delta = hi[index]-lambda[index]; - lambda[index] = hi[index]; - } - else { - lambda[index] = new_lambda; - } - - //@@@ a trick that may or may not help - //float ramp = (1-((float)(iteration+1)/(float)num_iterations)); - //delta *= ramp; - - // update invMforce. - // @@@ potential optimization: SIMD for this and the b2 >= 0 case - fc_ptr = invMforce + 6*b1; - fc_ptr[0] += delta * iMJ_ptr[0]; - fc_ptr[1] += delta * iMJ_ptr[1]; - fc_ptr[2] += delta * iMJ_ptr[2]; - fc_ptr[3] += delta * iMJ_ptr[3]; - fc_ptr[4] += delta * iMJ_ptr[4]; - fc_ptr[5] += delta * iMJ_ptr[5]; - // @@@ potential optimization: handle 1-body constraints in a separate - // loop to avoid the cost of test & jump? - if (b2 >= 0) { - fc_ptr = invMforce + 6*b2; - fc_ptr[0] += delta * iMJ_ptr[6]; - fc_ptr[1] += delta * iMJ_ptr[7]; - fc_ptr[2] += delta * iMJ_ptr[8]; - fc_ptr[3] += delta * iMJ_ptr[9]; - fc_ptr[4] += delta * iMJ_ptr[10]; - fc_ptr[5] += delta * iMJ_ptr[11]; - } - } - } -} - - - -void SolveInternal1 (float global_cfm, - float global_erp, - RigidBody * const *body, int nb, - BU_Joint * const *_joint, - int nj, - const ContactSolverInfo& solverInfo) -{ - - int numIter = solverInfo.m_numIterations; - float sOr = solverInfo.m_sor; - - int i,j; - - SimdScalar stepsize1 = dRecip(solverInfo.m_timeStep); - - // number all bodies in the body list - set their tag values - for (i=0; i<nb; i++) - body[i]->m_odeTag = i; - - // make a local copy of the joint array, because we might want to modify it. - // (the "BU_Joint *const*" declaration says we're allowed to modify the joints - // but not the joint array, because the caller might need it unchanged). - //@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints - BU_Joint **joint = (BU_Joint**) alloca (nj * sizeof(BU_Joint*)); - memcpy (joint,_joint,nj * sizeof(BU_Joint*)); - - // for all bodies, compute the inertia tensor and its inverse in the global - // frame, and compute the rotational force and add it to the torque - // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body. - dRealAllocaArray (I,3*4*nb); - dRealAllocaArray (invI,3*4*nb); -/* for (i=0; i<nb; i++) { - dMatrix3 tmp; - // compute inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R); - // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); - dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); - // compute rotational force - dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp); - } -*/ - for (i=0; i<nb; i++) { - dMatrix3 tmp; - // compute inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R); - dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp); - - // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); - dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); - // compute rotational force - dMULTIPLY0_331 (tmp,I+i*12,body[i]->getAngularVelocity()); - dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp); - } - - - - - // get joint information (m = total constraint dimension, nub = number of unbounded variables). - // joints with m=0 are inactive and are removed from the joints array - // entirely, so that the code that follows does not consider them. - //@@@ do we really need to save all the info1's - BU_Joint::Info1 *info = (BU_Joint::Info1*) alloca (nj*sizeof(BU_Joint::Info1)); - for (i=0, j=0; j<nj; j++) { // i=dest, j=src - joint[j]->GetInfo1 (info+i); - dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); - if (info[i].m > 0) { - joint[i] = joint[j]; - i++; - } - } - nj = i; - - // create the row offset array - int m = 0; - int *ofs = (int*) alloca (nj*sizeof(int)); - for (i=0; i<nj; i++) { - ofs[i] = m; - m += info[i].m; - } - - // if there are constraints, compute the constraint force - dRealAllocaArray (J,m*12); - int *jb = (int*) alloca (m*2*sizeof(int)); - if (m > 0) { - // create a constraint equation right hand side vector `c', a constraint - // force mixing vector `cfm', and LCP low and high bound vectors, and an - // 'findex' vector. - dRealAllocaArray (c,m); - dRealAllocaArray (cfm,m); - dRealAllocaArray (lo,m); - dRealAllocaArray (hi,m); - int *findex = (int*) alloca (m*sizeof(int)); - dSetZero1 (c,m); - dSetValue1 (cfm,m,global_cfm); - dSetValue1 (lo,m,-dInfinity); - dSetValue1 (hi,m, dInfinity); - for (i=0; i<m; i++) findex[i] = -1; - - // get jacobian data from constraints. an m*12 matrix will be created - // to store the two jacobian blocks from each constraint. it has this - // format: - // - // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \ . - // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }-- jacobian for joint 0, body 1 and body 2 (3 rows) - // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 / - // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows) - // etc... - // - // (lll) = linear jacobian data - // (aaa) = angular jacobian data - // - dSetZero1 (J,m*12); - BU_Joint::Info2 Jinfo; - Jinfo.rowskip = 12; - Jinfo.fps = stepsize1; - Jinfo.erp = global_erp; - for (i=0; i<nj; i++) { - Jinfo.J1l = J + ofs[i]*12; - Jinfo.J1a = Jinfo.J1l + 3; - Jinfo.J2l = Jinfo.J1l + 6; - Jinfo.J2a = Jinfo.J1l + 9; - Jinfo.c = c + ofs[i]; - Jinfo.cfm = cfm + ofs[i]; - Jinfo.lo = lo + ofs[i]; - Jinfo.hi = hi + ofs[i]; - Jinfo.findex = findex + ofs[i]; - joint[i]->GetInfo2 (&Jinfo); - - if (Jinfo.c[0] > solverInfo.m_maxErrorReduction) - Jinfo.c[0] = solverInfo.m_maxErrorReduction; - - - - - // adjust returned findex values for global index numbering - for (j=0; j<info[i].m; j++) { - if (findex[ofs[i] + j] >= 0) - findex[ofs[i] + j] += ofs[i]; - } - } - - // create an array of body numbers for each joint row - int *jb_ptr = jb; - for (i=0; i<nj; i++) { - int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1; - int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1; - for (j=0; j<info[i].m; j++) { - jb_ptr[0] = b1; - jb_ptr[1] = b2; - jb_ptr += 2; - } - } - dIASSERT (jb_ptr == jb+2*m); - - // compute the right hand side `rhs' - dRealAllocaArray (tmp1,nb*6); - // put v/h + invM*fe into tmp1 - for (i=0; i<nb; i++) { - SimdScalar body_invMass = body[i]->getInvMass(); - for (j=0; j<3; j++) - tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->getLinearVelocity()[j] * stepsize1; - dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc); - for (j=0; j<3; j++) - tmp1[i*6+3+j] += body[i]->getAngularVelocity()[j] * stepsize1; - } - - // put J*tmp1 into rhs - dRealAllocaArray (rhs,m); - multiply_J (m,J,jb,tmp1,rhs); - - // complete rhs - for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i]; - - // scale CFM - for (i=0; i<m; i++) - cfm[i] *= stepsize1; - - // load lambda from the value saved on the previous iteration - dRealAllocaArray (lambda,m); -#ifdef WARM_STARTING - dSetZero1 (lambda,m); //@@@ shouldn't be necessary - for (i=0; i<nj; i++) { - memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(SimdScalar)); - } -#endif - - // solve the LCP problem and get lambda and invM*constraint_force - dRealAllocaArray (cforce,nb*6); - - SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr); - -#ifdef WARM_STARTING - // save lambda for the next iteration - //@@@ note that this doesn't work for contact joints yet, as they are - // recreated every iteration - for (i=0; i<nj; i++) { - memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(SimdScalar)); - } -#endif - - - // note that the SOR method overwrites rhs and J at this point, so - // they should not be used again. - - // add stepsize * cforce to the body velocity - for (i=0; i<nb; i++) { - SimdVector3 linvel = body[i]->getLinearVelocity(); - SimdVector3 angvel = body[i]->getAngularVelocity(); - - for (j=0; j<3; j++) - linvel[j] += solverInfo.m_timeStep* cforce[i*6+j]; - for (j=0; j<3; j++) - angvel[j] += solverInfo.m_timeStep* cforce[i*6+3+j]; - - body[i]->setLinearVelocity(linvel); - body[i]->setAngularVelocity(angvel); - - } - - } - - - - // compute the velocity update: - // add stepsize * invM * fe to the body velocity - - for (i=0; i<nb; i++) { - SimdScalar body_invMass = body[i]->getInvMass(); - SimdVector3 linvel = body[i]->getLinearVelocity(); - SimdVector3 angvel = body[i]->getAngularVelocity(); - - for (j=0; j<3; j++) - { - linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j]; - } - for (j=0; j<3; j++) - { - body[i]->m_tacc[j] *= solverInfo.m_timeStep; - } - dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc); - body[i]->setAngularVelocity(angvel); - - } - - - -} - - -#endif //USE_SOR_SOLVER diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.h b/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.h deleted file mode 100644 index 37fe09c2607..00000000000 --- a/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.h +++ /dev/null @@ -1,45 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#define USE_SOR_SOLVER -#ifdef USE_SOR_SOLVER - -#ifndef SOR_LCP_H -#define SOR_LCP_H -class RigidBody; -class BU_Joint; -#include "SimdScalar.h" - -struct ContactSolverInfo; - -void SolveInternal1 (float global_cfm, - float global_erp, - RigidBody * const *body, int nb, - BU_Joint * const *_joint, int nj, const ContactSolverInfo& info); - -int dRandInt2 (int n); - - -#endif //SOR_LCP_H - -#endif //USE_SOR_SOLVER - diff --git a/extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp b/extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp index 9fd859e0f2d..cab71449f52 100644 --- a/extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp +++ b/extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp @@ -178,7 +178,7 @@ void ContactJoint::GetInfo2(Info2 *info) c2[1] = relativePositionB[1]; c2[2] = relativePositionB[2]; - + //combined friction is available in the contact point float friction = FRICTION_CONSTANT*m_body0->getFriction() * m_body1->getFriction(); // first friction direction diff --git a/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp b/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp index e4ccf83953a..c268515e327 100644 --- a/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp +++ b/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp @@ -32,10 +32,13 @@ RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdSc m_angularVelocity(0.f,0.f,0.f), m_linearDamping(0.f), m_angularDamping(0.5f), - m_friction(friction), - m_restitution(restitution), m_kinematicTimeStep(0.f) { + + //moved to CollisionObject + m_friction = friction; + m_restitution = restitution; + m_debugBodyId = uniqueId++; setMassProps(massProps.m_mass, massProps.m_inertiaLocal); diff --git a/extern/bullet/BulletDynamics/Dynamics/RigidBody.h b/extern/bullet/BulletDynamics/Dynamics/RigidBody.h index fd96dc99d60..ba2399d2da4 100644 --- a/extern/bullet/BulletDynamics/Dynamics/RigidBody.h +++ b/extern/bullet/BulletDynamics/Dynamics/RigidBody.h @@ -31,7 +31,6 @@ typedef SimdScalar dMatrix3[4*3]; extern float gLinearAirDamping; extern bool gUseEpa; -#define MAX_RIGIDBODIES 8192 /// RigidBody class for RigidBody Dynamics @@ -173,22 +172,7 @@ public: void getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const; - void setRestitution(float rest) - { - m_restitution = rest; - } - float getRestitution() const - { - return m_restitution; - } - void setFriction(float frict) - { - m_friction = frict; - } - float getFriction() const - { - return m_friction; - } + inline float ComputeImpulseDenominator(const SimdPoint3& pos, const SimdVector3& normal) const @@ -228,8 +212,6 @@ private: SimdScalar m_angularDamping; SimdScalar m_inverseMass; - SimdScalar m_friction; - SimdScalar m_restitution; SimdScalar m_kinematicTimeStep; @@ -254,6 +236,7 @@ public: m_broadphaseProxy = broadphaseProxy; } + /// for ode solver-binding dMatrix3 m_R;//temp @@ -261,10 +244,11 @@ public: dMatrix3 m_invI; int m_odeTag; - float m_padding[1024]; + SimdVector3 m_tacc;//temp SimdVector3 m_facc; - + + int m_debugBodyId; }; diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp index 60b8dbbf862..83bddc8ee1e 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp @@ -34,7 +34,7 @@ class BP_Proxy; float gDeactivationTime = 2.f; bool gDisableDeactivation = false; -float gLinearSleepingTreshold = 0.4f; +float gLinearSleepingTreshold = 0.8f; float gAngularSleepingTreshold = 1.0f; #include "Dynamics/MassProps.h" @@ -210,28 +210,6 @@ void CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionsta } -void CcdPhysicsController::SetMargin(float margin) -{ - if (m_body && m_body->GetCollisionShape()) - { - m_body->GetCollisionShape()->SetMargin(margin); - } - - -} - -float CcdPhysicsController::GetMargin() const -{ - if (m_body && m_body->GetCollisionShape()) - { - return m_body->GetCollisionShape()->GetMargin(); - } - - return 0.f; - -} - - // kinematic methods void CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local) { @@ -557,6 +535,7 @@ PHY_IPhysicsController* CcdPhysicsController::GetReplica() DefaultMotionState::DefaultMotionState() { m_worldTransform.setIdentity(); + m_localScaling.setValue(1.f,1.f,1.f); } @@ -574,9 +553,9 @@ void DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ) void DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ) { - scaleX = 1.; - scaleY = 1.; - scaleZ = 1.; + scaleX = m_localScaling.getX(); + scaleY = m_localScaling.getY(); + scaleZ = m_localScaling.getZ(); } void DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal) diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h index 7488e74db01..632d5d776d2 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h @@ -39,20 +39,38 @@ extern bool gDisableDeactivation; class CcdPhysicsEnvironment; + + struct CcdConstructionInfo { + + ///CollisionFilterGroups provides some optional usage of basic collision filtering + ///this is done during broadphase, so very early in the pipeline + ///more advanced collision filtering should be done in CollisionDispatcher::NeedsCollision + enum CollisionFilterGroups + { + DefaultFilter = 1, + StaticFilter = 2, + KinematicFilter = 4, + DebrisFilter = 8, + AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter, + }; + + CcdConstructionInfo() : m_gravity(0,0,0), + m_scaling(1.f,1.f,1.f), m_mass(0.f), m_restitution(0.1f), m_friction(0.5f), m_linearDamping(0.1f), m_angularDamping(0.1f), + m_collisionFlags(0), + m_collisionFilterGroup(DefaultFilter), + m_collisionFilterMask(AllFilter), m_MotionState(0), m_physicsEnv(0), - m_inertiaFactor(1.f), - m_scaling(1.f,1.f,1.f), - m_collisionFlags(0) + m_inertiaFactor(1.f) { } @@ -66,6 +84,15 @@ struct CcdConstructionInfo SimdScalar m_angularDamping; int m_collisionFlags; + ///optional use of collision group/mask: + ///only collision with object goups that match the collision mask. + ///this is very basic early out. advanced collision filtering should be + ///done in the CollisionDispatcher::NeedsCollision and NeedsResponse + ///both values default to 1 + short int m_collisionFilterGroup; + short int m_collisionFilterMask; + + CollisionShape* m_collisionShape; class PHY_IMotionState* m_MotionState; @@ -158,10 +185,21 @@ class CcdPhysicsController : public PHY_IPhysicsController virtual void setNewClientInfo(void* clientinfo); virtual PHY_IPhysicsController* GetReplica(); + ///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors + short int GetCollisionFilterGroup() const + { + return m_cci.m_collisionFilterGroup; + } + ///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors + short int GetCollisionFilterMask() const + { + return m_cci.m_collisionFilterMask; + } + virtual void calcXform() {} ; - virtual void SetMargin(float margin); - virtual float GetMargin() const; + virtual void SetMargin(float margin) {}; + virtual float GetMargin() const {return 0.f;}; bool wantsSleeping(); @@ -208,6 +246,7 @@ class DefaultMotionState : public PHY_IMotionState virtual void calculateWorldTransformations(); SimdTransform m_worldTransform; + SimdVector3 m_localScaling; }; diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index 2fee6d06963..9362595170d 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -30,14 +30,12 @@ subject to the following restrictions: #include "CollisionShapes/ConvexShape.h" #include "CollisionShapes/ConeShape.h" - - +#include "CollisionDispatch/SimulationIslandManager.h" #include "BroadphaseCollision/Dispatcher.h" #include "NarrowPhaseCollision/PersistentManifold.h" #include "CollisionShapes/TriangleMeshShape.h" -#include "ConstraintSolver/OdeConstraintSolver.h" -#include "ConstraintSolver/SimpleConstraintSolver.h" +#include "ConstraintSolver/SequentialImpulseConstraintSolver.h" //profiling/timings @@ -56,7 +54,7 @@ subject to the following restrictions: #include "PHY_IMotionState.h" #include "CollisionDispatch/EmptyCollisionAlgorithm.h" -#include "CollisionDispatch/UnionFind.h" + #include "CollisionShapes/SphereShape.h" @@ -77,6 +75,7 @@ RaycastVehicle::VehicleTuning gTuning; #include "ConstraintSolver/ConstraintSolver.h" #include "ConstraintSolver/Point2PointConstraint.h" #include "ConstraintSolver/HingeConstraint.h" +#include "ConstraintSolver/Generic6DofConstraint.h" //#include "BroadphaseCollision/QueryDispatcher.h" @@ -322,7 +321,7 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV -CcdPhysicsEnvironment::CcdPhysicsEnvironment(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase) +CcdPhysicsEnvironment::CcdPhysicsEnvironment(Dispatcher* dispatcher,OverlappingPairCache* pairCache) :m_scalingPropagated(false), m_numIterations(10), m_numTimeSubSteps(1), @@ -340,14 +339,14 @@ m_enableSatCollisionDetection(false) dispatcher = new CollisionDispatcher(); - if(!broadphase) + if(!pairCache) { //todo: calculate/let user specify this world sizes SimdVector3 worldMin(-10000,-10000,-10000); SimdVector3 worldMax(10000,10000,10000); - broadphase = new AxisSweep3(worldMin,worldMax); + pairCache = new AxisSweep3(worldMin,worldMax); //broadphase = new SimpleBroadphase(); } @@ -355,11 +354,13 @@ m_enableSatCollisionDetection(false) setSolverType(1);//issues with quickstep and memory allocations - m_collisionWorld = new CollisionWorld(dispatcher,broadphase); + m_collisionWorld = new CollisionWorld(dispatcher,pairCache); m_debugDrawer = 0; m_gravity = SimdVector3(0.f,-10.f,0.f); + m_islandManager = new SimulationIslandManager(); + } @@ -373,7 +374,7 @@ void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl) body->setGravity( m_gravity ); m_controllers.push_back(ctrl); - m_collisionWorld->AddCollisionObject(body); + m_collisionWorld->AddCollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask()); assert(body->m_broadphaseHandle); @@ -385,7 +386,8 @@ void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl) assert(shapeinterface); const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform(); - + + body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); SimdPoint3 minAabb,maxAabb; @@ -637,6 +639,9 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) CcdPhysicsController* ctrl = m_controllers[k]; // SimdTransform predictedTrans; RigidBody* body = ctrl->GetRigidBody(); + + body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); + if (body->IsActive()) { if (!body->IsStatic()) @@ -654,7 +659,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) Profiler::endBlock("predictIntegratedTransform"); #endif //USE_QUICKPROF - BroadphaseInterface* scene = GetBroadphase(); + OverlappingPairCache* scene = m_collisionWorld->GetPairCache(); // @@ -674,8 +679,10 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection; + dispatchInfo.m_debugDraw = this->m_debugDrawer; - scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g); + scene->RefreshOverlappingPairs(); + GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo); #ifdef USE_QUICKPROF @@ -685,7 +692,8 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) int numRigidBodies = m_controllers.size(); - m_collisionWorld->UpdateActivationState(); + + m_islandManager->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher()); { int i; @@ -702,15 +710,15 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) { if (colObj0->IsActive() || colObj1->IsActive()) { - GetDispatcher()->GetUnionFind().unite((colObj0)->m_islandTag1, + + m_islandManager->GetUnionFind().unite((colObj0)->m_islandTag1, (colObj1)->m_islandTag1); } } } } - m_collisionWorld->StoreIslandActivationState(); - + m_islandManager->StoreIslandActivationState(GetCollisionWorld()); //contacts @@ -762,7 +770,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) #endif //NEW_BULLET_VEHICLE_SUPPORT - struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback + struct InplaceSolverIslandCallback : public SimulationIslandManager::IslandCallback { ContactSolverInfo& m_solverInfo; @@ -803,7 +811,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) #endif //USE_QUICKPROF /// solve all the contact points and contact friction - GetDispatcher()->BuildAndProcessIslands(m_collisionWorld->GetCollisionObjectArray(),&solverCallback); + m_islandManager->BuildAndProcessIslands(GetCollisionWorld()->GetDispatcher(),m_collisionWorld->GetCollisionObjectArray(),&solverCallback); #ifdef USE_QUICKPROF Profiler::endBlock("BuildAndProcessIslands"); @@ -842,7 +850,9 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) dispatchInfo.m_stepCount = 0; dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS; - scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g); + //pairCache->RefreshOverlappingPairs();//?? + GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo); + toi = dispatchInfo.m_timeOfImpact; } @@ -871,6 +881,11 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) if (!body->IsStatic()) { + if (body->m_hitFraction < 1.f) + { + //set velocity to zero... until we have proper CCD integrated + body->setLinearVelocity(body->getLinearVelocity()*0.5f); + } body->predictIntegratedTransform(timeStep* toi, predictedTrans); body->proceedToTransform( predictedTrans); } @@ -1033,7 +1048,7 @@ void CcdPhysicsEnvironment::setSolverType(int solverType) if (m_solverType != solverType) { - m_solver = new SimpleConstraintSolver(); + m_solver = new SequentialImpulseConstraintSolver(); break; } @@ -1043,7 +1058,7 @@ void CcdPhysicsEnvironment::setSolverType(int solverType) default: if (m_solverType != solverType) { - m_solver = new OdeConstraintSolver(); +// m_solver = new OdeConstraintSolver(); break; } @@ -1182,8 +1197,8 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA; SimdVector3 axisInA(axisX,axisY,axisZ); SimdVector3 axisInB = rb1 ? - (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : - rb0->getCenterOfMassTransform().getBasis() * -axisInA; + (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : + rb0->getCenterOfMassTransform().getBasis() * axisInA; bool angularOnly = false; @@ -1213,9 +1228,55 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl break; } + case PHY_GENERIC_6DOF_CONSTRAINT: + { + Generic6DofConstraint* genericConstraint = 0; + + if (rb1) + { + SimdTransform frameInA; + SimdTransform frameInB; + + SimdVector3 axis1, axis2; + SimdPlaneSpace1( axisInA, axis1, axis2 ); + + frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(), + axisInA.y(), axis1.y(), axis2.y(), + axisInA.z(), axis1.z(), axis2.z() ); + + + SimdPlaneSpace1( axisInB, axis1, axis2 ); + frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(), + axisInB.y(), axis1.y(), axis2.y(), + axisInB.z(), axis1.z(), axis2.z() ); + + frameInA.setOrigin( pivotInA ); + frameInB.setOrigin( pivotInB ); + + genericConstraint = new Generic6DofConstraint( + *rb0,*rb1, + frameInA,frameInB); + + + } else + { + // TODO: Implement single body case... + + } + + + m_constraints.push_back(genericConstraint); + genericConstraint->SetUserConstraintId(gConstraintUid++); + genericConstraint->SetUserConstraintType(type); + //64 bit systems can't cast pointer to int. could use size_t instead. + return genericConstraint->GetUserConstraintId(); + + break; + } case PHY_ANGULAR_CONSTRAINT: angularOnly = true; + case PHY_LINEHINGE_CONSTRAINT: { HingeConstraint* hinge = 0; @@ -1271,21 +1332,62 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl } -float CcdPhysicsEnvironment::getAppliedImpulse(int constraintid) + + + +//Following the COLLADA physics specification for constraints +int CcdPhysicsEnvironment::createUniversalD6Constraint( + class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther, + SimdTransform& frameInA, + SimdTransform& frameInB, + const SimdVector3& linearMinLimits, + const SimdVector3& linearMaxLimits, + const SimdVector3& angularMinLimits, + const SimdVector3& angularMaxLimits +) { - std::vector<TypedConstraint*>::iterator i; - for (i=m_constraints.begin(); - !(i==m_constraints.end()); i++) + //we could either add some logic to recognize ball-socket and hinge, or let that up to the user + //perhaps some warning or hint that hinge/ball-socket is more efficient? + + Generic6DofConstraint* genericConstraint = 0; + CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef; + CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther; + + RigidBody* rb0 = ctrl0->GetRigidBody(); + RigidBody* rb1 = ctrl1->GetRigidBody(); + + if (rb1) { - TypedConstraint* constraint = (*i); - if (constraint->GetUserConstraintId() == constraintid) - { - return constraint->GetAppliedImpulse(); - } + + + genericConstraint = new Generic6DofConstraint( + *rb0,*rb1, + frameInA,frameInB); + genericConstraint->setLinearLowerLimit(linearMinLimits); + genericConstraint->setLinearUpperLimit(linearMaxLimits); + genericConstraint->setAngularLowerLimit(angularMinLimits); + genericConstraint->setAngularUpperLimit(angularMaxLimits); + } else + { + // TODO: Implement single body case... + //No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code + } - return 0.f; + + if (genericConstraint) + { + m_constraints.push_back(genericConstraint); + genericConstraint->SetUserConstraintId(gConstraintUid++); + genericConstraint->SetUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT); + //64 bit systems can't cast pointer to int. could use size_t instead. + return genericConstraint->GetUserConstraintId(); + } + return 0; } + + + void CcdPhysicsEnvironment::removeConstraint(int constraintId) { std::vector<TypedConstraint*>::iterator i; @@ -1296,15 +1398,7 @@ void CcdPhysicsEnvironment::removeConstraint(int constraintId) TypedConstraint* constraint = (*i); if (constraint->GetUserConstraintId() == constraintId) { - //activate objects - if (constraint->GetRigidBodyA().mergesSimulationIslands()) - constraint->GetRigidBodyA().activate(); - if (constraint->GetRigidBodyB().mergesSimulationIslands()) - constraint->GetRigidBodyB().activate(); - std::swap(*i, m_constraints.back()); - - m_constraints.pop_back(); break; } @@ -1352,13 +1446,6 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i SimdVector3 rayFrom(fromX,fromY,fromZ); SimdVector3 rayTo(toX,toY,toZ); - - if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb) - { - SimdVector3 color (1,0,0); - m_debugDrawer->DrawLine(rayFrom,rayTo,color); - } - SimdVector3 hitPointWorld,normalWorld; //Either Ray Cast with or without filtering @@ -1376,26 +1463,10 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i hitX = rayCallback.m_hitPointWorld.getX(); hitY = rayCallback.m_hitPointWorld.getY(); hitZ = rayCallback.m_hitPointWorld.getZ(); - if (rayCallback.m_hitNormalWorld.length2() > SIMD_EPSILON) - { - rayCallback.m_hitNormalWorld.normalize(); - } normalX = rayCallback.m_hitNormalWorld.getX(); normalY = rayCallback.m_hitNormalWorld.getY(); normalZ = rayCallback.m_hitNormalWorld.getZ(); - - if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb) - { - SimdVector3 colorNormal(0,0,1); - m_debugDrawer->DrawLine(rayCallback.m_hitPointWorld,rayCallback.m_hitPointWorld+rayCallback.m_hitNormalWorld,colorNormal); - - SimdVector3 color (0,1,0); - m_debugDrawer->DrawLine(rayFrom,rayCallback.m_hitPointWorld,color); - - - } - } @@ -1425,15 +1496,6 @@ BroadphaseInterface* CcdPhysicsEnvironment::GetBroadphase() -const CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher() const -{ - return m_collisionWorld->GetDispatcher(); -} - -CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher() -{ - return m_collisionWorld->GetDispatcher(); -} CcdPhysicsEnvironment::~CcdPhysicsEnvironment() { @@ -1448,6 +1510,8 @@ CcdPhysicsEnvironment::~CcdPhysicsEnvironment() //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher //delete m_dispatcher; delete m_collisionWorld; + + delete m_islandManager; } @@ -1464,15 +1528,9 @@ CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index) } -int CcdPhysicsEnvironment::GetNumManifolds() const -{ - return GetDispatcher()->GetNumManifolds(); -} -const PersistentManifold* CcdPhysicsEnvironment::GetManifold(int index) const -{ - return GetDispatcher()->GetManifoldByIndexInternal(index); -} + + TypedConstraint* CcdPhysicsEnvironment::getConstraintById(int constraintId) { @@ -1565,9 +1623,10 @@ void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctr void CcdPhysicsEnvironment::CallbackTriggers() { + CcdPhysicsController* ctrl0=0,*ctrl1=0; - if (m_triggerCallbacks[PHY_OBJECT_RESPONSE]) + if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawContactPoints))) { //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback int numManifolds = m_collisionWorld->GetDispatcher()->GetNumManifolds(); @@ -1577,6 +1636,16 @@ void CcdPhysicsEnvironment::CallbackTriggers() int numContacts = manifold->GetNumContacts(); if (numContacts) { + if (m_debugDrawer && (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawContactPoints)) + { + for (int j=0;j<numContacts;j++) + { + SimdVector3 color(1,0,0); + const ManifoldPoint& cp = manifold->GetContactPoint(j); + if (m_debugDrawer) + m_debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color); + } + } RigidBody* obj0 = static_cast<RigidBody* >(manifold->GetBody0()); RigidBody* obj1 = static_cast<RigidBody* >(manifold->GetBody1()); @@ -1603,6 +1672,7 @@ void CcdPhysicsEnvironment::CallbackTriggers() } + } @@ -1764,3 +1834,18 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float conera return sphereController; } +float CcdPhysicsEnvironment::getAppliedImpulse(int constraintid) +{ + std::vector<TypedConstraint*>::iterator i; + + for (i=m_constraints.begin(); + !(i==m_constraints.end()); i++) + { + TypedConstraint* constraint = (*i); + if (constraint->GetUserConstraintId() == constraintid) + { + return constraint->GetAppliedImpulse(); + } + } + return 0.f; +} diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h index 73bd9f8c95e..5f111c636bf 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h @@ -20,11 +20,13 @@ subject to the following restrictions: #include <vector> class CcdPhysicsController; #include "SimdVector3.h" +#include "SimdTransform.h" -class TypedConstraint; +class TypedConstraint; +class SimulationIslandManager; class CollisionDispatcher; class Dispatcher; //#include "BroadphaseInterface.h" @@ -37,6 +39,7 @@ class Dispatcher; class WrapperVehicle; class PersistentManifold; class BroadphaseInterface; +class OverlappingPairCache; class IDebugDraw; /// CcdPhysicsEnvironment is experimental mainloop for physics simulation using optional continuous collision detection. @@ -47,12 +50,17 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment { SimdVector3 m_gravity; + + +protected: IDebugDraw* m_debugDrawer; //solver iterations int m_numIterations; //timestep subdivisions int m_numTimeSubSteps; + + int m_ccdMode; int m_solverType; int m_profileTimings; @@ -60,9 +68,10 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment ContactSolverInfo m_solverInfo; + SimulationIslandManager* m_islandManager; public: - CcdPhysicsEnvironment(CollisionDispatcher* dispatcher=0, BroadphaseInterface* broadphase=0); + CcdPhysicsEnvironment(Dispatcher* dispatcher=0, OverlappingPairCache* pairCache=0); virtual ~CcdPhysicsEnvironment(); @@ -98,7 +107,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment virtual void endFrame() {}; /// Perform an integration step of duration 'timeStep'. virtual bool proceedDeltaTime(double curTime,float timeStep); - bool proceedDeltaTimeOneStep(float timeStep); + virtual bool proceedDeltaTimeOneStep(float timeStep); virtual void setFixedTimeStep(bool useFixedTimeStep,float fixedTimeStep){}; //returns 0.f if no fixed timestep is used @@ -112,7 +121,22 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment virtual int createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type, float pivotX,float pivotY,float pivotZ, float axisX,float axisY,float axisZ); - virtual void removeConstraint(int constraintid); + + + //Following the COLLADA physics specification for constraints + virtual int createUniversalD6Constraint( + class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther, + SimdTransform& localAttachmentFrameRef, + SimdTransform& localAttachmentOther, + const SimdVector3& linearMinLimits, + const SimdVector3& linearMaxLimits, + const SimdVector3& angularMinLimits, + const SimdVector3& angularMaxLimits + ); + + + virtual void `straint(int constraintid); + virtual float getAppliedImpulse(int constraintid); @@ -160,9 +184,9 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment BroadphaseInterface* GetBroadphase(); - CollisionDispatcher* GetDispatcher(); - const CollisionDispatcher* GetDispatcher() const; + + bool IsSatCollisionDetectionEnabled() const { @@ -180,16 +204,39 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment CcdPhysicsController* GetPhysicsController( int index); - int GetNumManifolds() const; + const PersistentManifold* GetManifold(int index) const; std::vector<TypedConstraint*> m_constraints; - private: + void SyncMotionStates(float timeStep); + + class CollisionWorld* GetCollisionWorld() + { + return m_collisionWorld; + } + + const class CollisionWorld* GetCollisionWorld() const + { + return m_collisionWorld; + } + + SimulationIslandManager* GetSimulationIslandManager() + { + return m_islandManager; + } + + const SimulationIslandManager* GetSimulationIslandManager() const + { + return m_islandManager; + } + + protected: - void SyncMotionStates(float timeStep); + + std::vector<CcdPhysicsController*> m_controllers; @@ -206,6 +253,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment bool m_scalingPropagated; + }; diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp new file mode 100644 index 00000000000..1e3e44a8785 --- /dev/null +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp @@ -0,0 +1,349 @@ +/* +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 "ParallelIslandDispatcher.h" + + +#include "BroadphaseCollision/CollisionAlgorithm.h" +#include "CollisionDispatch/ConvexConvexAlgorithm.h" +#include "CollisionDispatch/EmptyCollisionAlgorithm.h" +#include "CollisionDispatch/ConvexConcaveCollisionAlgorithm.h" +#include "CollisionDispatch/CompoundCollisionAlgorithm.h" + +#include "CollisionShapes/CollisionShape.h" +#include "CollisionDispatch/CollisionObject.h" +#include <algorithm> + +static int gNumManifold2 = 0; + + + + + +ParallelIslandDispatcher::ParallelIslandDispatcher (): + m_useIslands(true), + m_defaultManifoldResult(0,0,0), + m_count(0) +{ + int i; + + for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++) + { + for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++) + { + m_doubleDispatch[i][j] = 0; + } + } + + +}; + +PersistentManifold* ParallelIslandDispatcher::GetNewManifold(void* b0,void* b1) +{ + gNumManifold2++; + + //ASSERT(gNumManifold < 65535); + + + CollisionObject* body0 = (CollisionObject*)b0; + CollisionObject* body1 = (CollisionObject*)b1; + + PersistentManifold* manifold = new PersistentManifold (body0,body1); + m_manifoldsPtr.push_back(manifold); + + return manifold; +} + +void ParallelIslandDispatcher::ClearManifold(PersistentManifold* manifold) +{ + manifold->ClearManifold(); +} + + +void ParallelIslandDispatcher::ReleaseManifold(PersistentManifold* manifold) +{ + + gNumManifold2--; + + //printf("ReleaseManifold: gNumManifold2 %d\n",gNumManifold2); + + ClearManifold(manifold); + + std::vector<PersistentManifold*>::iterator i = + std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold); + if (!(i == m_manifoldsPtr.end())) + { + std::swap(*i, m_manifoldsPtr.back()); + m_manifoldsPtr.pop_back(); + delete manifold; + + } + + +} + + +// +// todo: this is random access, it can be walked 'cache friendly'! +// +void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback) +{ + int numBodies = collisionObjects.size(); + + for (int islandId=0;islandId<numBodies;islandId++) + { + + std::vector<PersistentManifold*> islandmanifold; + + //int numSleeping = 0; + + bool allSleeping = true; + + int i; + for (i=0;i<numBodies;i++) + { + CollisionObject* colObj0 = collisionObjects[i]; + if (colObj0->m_islandTag1 == islandId) + { + if (colObj0->GetActivationState()== ACTIVE_TAG) + { + allSleeping = false; + } + if (colObj0->GetActivationState()== DISABLE_DEACTIVATION) + { + allSleeping = false; + } + } + } + + + for (i=0;i<GetNumManifolds();i++) + { + PersistentManifold* manifold = this->GetManifoldByIndexInternal(i); + + //filtering for response + + CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0()); + CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1()); + { + if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) || + ((colObj1) && (colObj1)->m_islandTag1 == (islandId))) + { + + if (NeedsResponse(*colObj0,*colObj1)) + islandmanifold.push_back(manifold); + } + } + } + if (allSleeping) + { + int i; + for (i=0;i<numBodies;i++) + { + CollisionObject* colObj0 = collisionObjects[i]; + if (colObj0->m_islandTag1 == islandId) + { + colObj0->SetActivationState( ISLAND_SLEEPING ); + } + } + + + } else + { + + int i; + for (i=0;i<numBodies;i++) + { + CollisionObject* colObj0 = collisionObjects[i]; + if (colObj0->m_islandTag1 == islandId) + { + if ( colObj0->GetActivationState() == ISLAND_SLEEPING) + { + colObj0->SetActivationState( WANTS_DEACTIVATION); + } + } + } + + /// Process the actual simulation, only if not sleeping/deactivated + if (islandmanifold.size()) + { + callback->ProcessIsland(&islandmanifold[0],islandmanifold.size()); + } + + } + } +} + + + +CollisionAlgorithm* ParallelIslandDispatcher::InternalFindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) +{ + m_count++; + CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject; + CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject; + + CollisionAlgorithmConstructionInfo ci; + ci.m_dispatcher = this; + + if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConvex() ) + { + return new ConvexConvexAlgorithm(0,ci,&proxy0,&proxy1); + } + + if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConcave()) + { + return new ConvexConcaveCollisionAlgorithm(ci,&proxy0,&proxy1); + } + + if (body1->m_collisionShape->IsConvex() && body0->m_collisionShape->IsConcave()) + { + return new ConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0); + } + + if (body0->m_collisionShape->IsCompound()) + { + return new CompoundCollisionAlgorithm(ci,&proxy0,&proxy1); + } else + { + if (body1->m_collisionShape->IsCompound()) + { + return new CompoundCollisionAlgorithm(ci,&proxy1,&proxy0); + } + } + + + //failed to find an algorithm + return new EmptyAlgorithm(ci); + +} + +bool ParallelIslandDispatcher::NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1) +{ + + + //here you can do filtering + bool hasResponse = + (!(colObj0.m_collisionFlags & CollisionObject::noContactResponse)) && + (!(colObj1.m_collisionFlags & CollisionObject::noContactResponse)); + hasResponse = hasResponse && + (colObj0.IsActive() || colObj1.IsActive()); + return hasResponse; +} + +bool ParallelIslandDispatcher::NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) +{ + + CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject; + CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject; + + assert(body0); + assert(body1); + + bool needsCollision = true; + + if ((body0->m_collisionFlags & CollisionObject::isStatic) && + (body1->m_collisionFlags & CollisionObject::isStatic)) + needsCollision = false; + + if ((!body0->IsActive()) && (!body1->IsActive())) + needsCollision = false; + + return needsCollision ; + +} + +///allows the user to get contact point callbacks +ManifoldResult* ParallelIslandDispatcher::GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold) +{ + + + //in-place, this prevents parallel dispatching, but just adding a list would fix that. + ManifoldResult* manifoldResult = new (&m_defaultManifoldResult) ManifoldResult(obj0,obj1,manifold); + return manifoldResult; +} + +///allows the user to get contact point callbacks +void ParallelIslandDispatcher::ReleaseManifoldResult(ManifoldResult*) +{ + +} + + +void ParallelIslandDispatcher::DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo) +{ + //m_blockedForChanges = true; + + int i; + + int dispatcherId = GetUniqueId(); + + + + for (i=0;i<numPairs;i++) + { + + BroadphasePair& pair = pairs[i]; + + if (dispatcherId>= 0) + { + //dispatcher will keep algorithms persistent in the collision pair + if (!pair.m_algorithms[dispatcherId]) + { + pair.m_algorithms[dispatcherId] = FindAlgorithm( + *pair.m_pProxy0, + *pair.m_pProxy1); + } + + if (pair.m_algorithms[dispatcherId]) + { + if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE) + { + pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + } else + { + float toi = pair.m_algorithms[dispatcherId]->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; + + } + } + } else + { + //non-persistent algorithm dispatcher + CollisionAlgorithm* algo = FindAlgorithm( + *pair.m_pProxy0, + *pair.m_pProxy1); + + if (algo) + { + if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE) + { + algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + } else + { + float toi = algo->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; + } + } + } + + } + + //m_blockedForChanges = false; + +} + diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h new file mode 100644 index 00000000000..e1e055f65b3 --- /dev/null +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h @@ -0,0 +1,128 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef PARALLEL_ISLAND_DISPATCHER_H +#define PARALLEL_ISLAND_DISPATCHER_H + +#include "BroadphaseCollision/Dispatcher.h" +#include "NarrowPhaseCollision/PersistentManifold.h" +#include "CollisionDispatch/UnionFind.h" +#include "CollisionDispatch/ManifoldResult.h" + +#include "BroadphaseCollision/BroadphaseProxy.h" +#include <vector> + +class IDebugDraw; + + +#include "CollisionDispatch/CollisionCreateFunc.h" + + + + +///ParallelIslandDispatcher dispatches entire simulation islands in parallel. +///For both collision detection and constraint solving. +///This development heads toward multi-core, CELL SPU and GPU approach +class ParallelIslandDispatcher : public Dispatcher +{ + + std::vector<PersistentManifold*> m_manifoldsPtr; + + UnionFind m_unionFind; + + bool m_useIslands; + + ManifoldResult m_defaultManifoldResult; + + CollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; + +public: + + UnionFind& GetUnionFind() { return m_unionFind;} + + struct IslandCallback + { + virtual ~IslandCallback() {}; + + virtual void ProcessIsland(PersistentManifold** manifolds,int numManifolds) = 0; + }; + + + int GetNumManifolds() const + { + return m_manifoldsPtr.size(); + } + + PersistentManifold* GetManifoldByIndexInternal(int index) + { + return m_manifoldsPtr[index]; + } + + const PersistentManifold* GetManifoldByIndexInternal(int index) const + { + return m_manifoldsPtr[index]; + } + + void InitUnionFind(int n) + { + if (m_useIslands) + m_unionFind.reset(n); + } + + void FindUnions(); + + int m_count; + + ParallelIslandDispatcher (); + virtual ~ParallelIslandDispatcher() {}; + + virtual PersistentManifold* GetNewManifold(void* b0,void* b1); + + virtual void ReleaseManifold(PersistentManifold* manifold); + + + virtual void BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback); + + ///allows the user to get contact point callbacks + virtual ManifoldResult* GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold); + + ///allows the user to get contact point callbacks + virtual void ReleaseManifoldResult(ManifoldResult*); + + virtual void ClearManifold(PersistentManifold* manifold); + + + CollisionAlgorithm* FindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) + { + CollisionAlgorithm* algo = InternalFindAlgorithm(proxy0,proxy1); + return algo; + } + + CollisionAlgorithm* InternalFindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1); + + virtual bool NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1); + + virtual bool NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1); + + virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;} + + virtual void DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo); + + + +}; + +#endif //PARALLEL_ISLAND_DISPATCHER_H + diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp new file mode 100644 index 00000000000..452fa10a3f0 --- /dev/null +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp @@ -0,0 +1,194 @@ +/* +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 "ParallelPhysicsEnvironment.h" +#include "CcdPhysicsController.h" +#include "ParallelIslandDispatcher.h" +#include "CollisionDispatch/CollisionWorld.h" +#include "ConstraintSolver/TypedConstraint.h" +#include "CollisionDispatch/SimulationIslandManager.h" +#include "SimulationIsland.h" + + +ParallelPhysicsEnvironment::ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher, OverlappingPairCache* pairCache): +CcdPhysicsEnvironment(dispatcher,pairCache) +{ + +} + +ParallelPhysicsEnvironment::~ParallelPhysicsEnvironment() +{ + +} + + + +/// Perform an integration step of duration 'timeStep'. +bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) +{ + // Make sure the broadphase / overlapping AABB paircache is up-to-date + OverlappingPairCache* scene = m_collisionWorld->GetPairCache(); + scene->RefreshOverlappingPairs(); + + // Find the connected sets that can be simulated in parallel + // Using union find + +#ifdef USE_QUICKPROF + Profiler::beginBlock("IslandUnionFind"); +#endif //USE_QUICKPROF + + GetSimulationIslandManager()->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher()); + + { + int i; + int numConstraints = m_constraints.size(); + for (i=0;i< numConstraints ; i++ ) + { + TypedConstraint* constraint = m_constraints[i]; + + const RigidBody* colObj0 = &constraint->GetRigidBodyA(); + const RigidBody* colObj1 = &constraint->GetRigidBodyB(); + + if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && + ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + { + if (colObj0->IsActive() || colObj1->IsActive()) + { + + GetSimulationIslandManager()->GetUnionFind().unite((colObj0)->m_islandTag1, + (colObj1)->m_islandTag1); + } + } + } + } + + //Store the island id in each body + GetSimulationIslandManager()->StoreIslandActivationState(GetCollisionWorld()); + +#ifdef USE_QUICKPROF + Profiler::endBlock("IslandUnionFind"); +#endif //USE_QUICKPROF + + + + ///build simulation islands + +#ifdef USE_QUICKPROF + Profiler::beginBlock("BuildIslands"); +#endif //USE_QUICKPROF + + std::vector<SimulationIsland> simulationIslands; + simulationIslands.resize(GetNumControllers()); + + int k; + for (k=0;k<GetNumControllers();k++) + { + CcdPhysicsController* ctrl = m_controllers[k]; + int tag = ctrl->GetRigidBody()->m_islandTag1; + if (tag>=0) + { + simulationIslands[tag].m_controllers.push_back(ctrl); + } + } + + Dispatcher* dispatcher = GetCollisionWorld()->GetDispatcher(); + + + //this is a brute force approach, will rethink later about more subtle ways + int i; + for (i=0;i< scene->GetNumOverlappingPairs();i++) + { + BroadphasePair* pair = &scene->GetOverlappingPair(i); + + CollisionObject* col0 = static_cast<CollisionObject*>(pair->m_pProxy0->m_clientObject); + CollisionObject* col1 = static_cast<CollisionObject*>(pair->m_pProxy1->m_clientObject); + + if (col0->m_islandTag1 > col1->m_islandTag1) + { + simulationIslands[col0->m_islandTag1].m_overlappingPairIndices.push_back(i); + } else + { + simulationIslands[col1->m_islandTag1].m_overlappingPairIndices.push_back(i); + } + } + + //store constraint indices for each island + for (i=0;i<m_constraints.size();i++) + { + TypedConstraint& constraint = *m_constraints[i]; + if (constraint.GetRigidBodyA().m_islandTag1 > constraint.GetRigidBodyB().m_islandTag1) + { + simulationIslands[constraint.GetRigidBodyA().m_islandTag1].m_constraintIndices.push_back(i); + } else + { + simulationIslands[constraint.GetRigidBodyB().m_islandTag1].m_constraintIndices.push_back(i); + } + + } + + //add all overlapping pairs for each island + + for (i=0;i<dispatcher->GetNumManifolds();i++) + { + PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i); + + //filtering for response + + CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0()); + CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1()); + { + int islandTag = colObj0->m_islandTag1; + if (colObj1->m_islandTag1 > islandTag) + islandTag = colObj1->m_islandTag1; + + if (dispatcher->NeedsResponse(*colObj0,*colObj1)) + simulationIslands[islandTag].m_manifolds.push_back(manifold); + + } + } + + #ifdef USE_QUICKPROF + Profiler::endBlock("BuildIslands"); + #endif //USE_QUICKPROF + + +#ifdef USE_QUICKPROF + Profiler::beginBlock("SimulateIsland"); +#endif //USE_QUICKPROF + + TypedConstraint** constraintBase = 0; + if (m_constraints.size()) + constraintBase = &m_constraints[0]; + + + + //Each simulation island can be processed in parallel (will be put on a job queue) + for (k=0;k<simulationIslands.size();k++) + { + if (simulationIslands[k].m_controllers.size()) + { + simulationIslands[k].Simulate(m_debugDrawer,m_numIterations, constraintBase ,&scene->GetOverlappingPair(0),dispatcher,GetBroadphase(),m_solver,timeStep); + } + } + +#ifdef USE_QUICKPROF + Profiler::endBlock("SimulateIsland"); +#endif //USE_QUICKPROF + + return true; + +}
\ No newline at end of file diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.h new file mode 100644 index 00000000000..530ba5845e5 --- /dev/null +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.h @@ -0,0 +1,44 @@ +/* +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 PARALLELPHYSICSENVIRONMENT +#define PARALLELPHYSICSENVIRONMENT + +#include "CcdPhysicsEnvironment.h" +class ParallelIslandDispatcher; + + +/// ParallelPhysicsEnvironment is experimental parallel mainloop for physics simulation +/// Physics Environment takes care of stepping the simulation and is a container for physics entities. +/// It stores rigidbodies,constraints, materials etc. +/// A derived class may be able to 'construct' entities by loading and/or converting +class ParallelPhysicsEnvironment : public CcdPhysicsEnvironment +{ + + + public: + ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher=0, OverlappingPairCache* pairCache=0); + + virtual ~ParallelPhysicsEnvironment(); + + + /// Perform an integration step of duration 'timeStep'. + virtual bool proceedDeltaTimeOneStep(float timeStep); + + //void BuildSimulationIslands(); + +}; + +#endif //PARALLELPHYSICSENVIRONMENT diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp new file mode 100644 index 00000000000..47c3094ca05 --- /dev/null +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp @@ -0,0 +1,468 @@ +/* +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 "SimulationIsland.h" +#include "SimdTransform.h" +#include "CcdPhysicsController.h" +#include "BroadphaseCollision/OverlappingPairCache.h" +#include "CollisionShapes/CollisionShape.h" +#include "BroadphaseCollision/Dispatcher.h" +#include "ConstraintSolver/ContactSolverInfo.h" +#include "ConstraintSolver/ConstraintSolver.h" +#include "ConstraintSolver/TypedConstraint.h" +#include "IDebugDraw.h" + +extern float gContactBreakingTreshold; + +bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,TypedConstraint** constraintsBaseAddress,BroadphasePair* overlappingPairBaseAddress, Dispatcher* dispatcher,BroadphaseInterface* broadphase,class ConstraintSolver* solver,float timeStep) +{ + + +#ifdef USE_QUICKPROF + + Profiler::beginBlock("predictIntegratedTransform"); +#endif //USE_QUICKPROF + + { + // std::vector<CcdPhysicsController*>::iterator i; + + + + int k; + for (k=0;k<GetNumControllers();k++) + { + CcdPhysicsController* ctrl = m_controllers[k]; + // SimdTransform predictedTrans; + RigidBody* body = ctrl->GetRigidBody(); + //todo: only do this when necessary, it's used for contact points + body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); + + if (body->IsActive()) + { + if (!body->IsStatic()) + { + body->applyForces( timeStep); + body->integrateVelocities( timeStep); + body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform); + } + } + + } + } + +#ifdef USE_QUICKPROF + Profiler::endBlock("predictIntegratedTransform"); +#endif //USE_QUICKPROF + + //BroadphaseInterface* scene = GetBroadphase(); + + + // + // collision detection (?) + // + + + #ifdef USE_QUICKPROF + Profiler::beginBlock("DispatchAllCollisionPairs"); + #endif //USE_QUICKPROF + + +// int numsubstep = m_numIterations; + + + DispatcherInfo dispatchInfo; + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_enableSatConvex = false;//m_enableSatCollisionDetection; + dispatchInfo.m_debugDraw = debugDrawer; + + std::vector<BroadphasePair> overlappingPairs; + overlappingPairs.resize(this->m_overlappingPairIndices.size()); + + //gather overlapping pair info + int i; + for (i=0;i<m_overlappingPairIndices.size();i++) + { + overlappingPairs[i] = overlappingPairBaseAddress[m_overlappingPairIndices[i]]; + } + + + //pairCache->RefreshOverlappingPairs(); + if (overlappingPairs.size()) + { + dispatcher->DispatchAllCollisionPairs(&overlappingPairs[0],overlappingPairs.size(),dispatchInfo);///numsubstep,g); + } + + //scatter overlapping pair info, mainly the created algorithms/contact caches + + for (i=0;i<m_overlappingPairIndices.size();i++) + { + overlappingPairBaseAddress[m_overlappingPairIndices[i]] = overlappingPairs[i]; + } + + + #ifdef USE_QUICKPROF + Profiler::endBlock("DispatchAllCollisionPairs"); + #endif //USE_QUICKPROF + + + + + int numRigidBodies = m_controllers.size(); + + + + + //contacts + #ifdef USE_QUICKPROF + Profiler::beginBlock("SolveConstraint"); + #endif //USE_QUICKPROF + + + //solve the regular constraints (point 2 point, hinge, etc) + + for (int g=0;g<numSolverIterations;g++) + { + // + // constraint solving + // + + int i; + int numConstraints = m_constraintIndices.size(); + + //point to point constraints + for (i=0;i< numConstraints ; i++ ) + { + TypedConstraint* constraint = constraintsBaseAddress[m_constraintIndices[i]]; + constraint->BuildJacobian(); + constraint->SolveConstraint( timeStep ); + + } + + + } + + #ifdef USE_QUICKPROF + Profiler::endBlock("SolveConstraint"); + #endif //USE_QUICKPROF + + /* + + //solve the vehicles + + #ifdef NEW_BULLET_VEHICLE_SUPPORT + //vehicles + int numVehicles = m_wrapperVehicles.size(); + for (int i=0;i<numVehicles;i++) + { + WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i]; + RaycastVehicle* vehicle = wrapperVehicle->GetVehicle(); + vehicle->UpdateVehicle( timeStep); + } + #endif //NEW_BULLET_VEHICLE_SUPPORT +*/ + + /* + + Profiler::beginBlock("CallbackTriggers"); + #endif //USE_QUICKPROF + + CallbackTriggers(); + + #ifdef USE_QUICKPROF + Profiler::endBlock("CallbackTriggers"); + + } + */ + + //OverlappingPairCache* scene = GetCollisionWorld()->GetPairCache(); + + ContactSolverInfo solverInfo; + + solverInfo.m_friction = 0.9f; + solverInfo.m_numIterations = numSolverIterations; + solverInfo.m_timeStep = timeStep; + solverInfo.m_restitution = 0.f;//m_restitution; + + + if (m_manifolds.size()) + { + solver->SolveGroup( &m_manifolds[0],m_manifolds.size(),solverInfo,0); + } + + +#ifdef USE_QUICKPROF + Profiler::beginBlock("proceedToTransform"); +#endif //USE_QUICKPROF + { + + + + { + + + UpdateAabbs(debugDrawer,broadphase,timeStep); + + + float toi = 1.f; + + //experimental continuous collision detection + + /* if (m_ccdMode == 3) + { + DispatcherInfo dispatchInfo; + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS; + + // GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(scene,dispatchInfo); + toi = dispatchInfo.m_timeOfImpact; + + } + */ + + + + // + // integrating solution + // + + { + + std::vector<CcdPhysicsController*>::iterator i; + + for (i=m_controllers.begin(); + !(i==m_controllers.end()); i++) + { + + CcdPhysicsController* ctrl = *i; + + SimdTransform predictedTrans; + RigidBody* body = ctrl->GetRigidBody(); + + if (body->IsActive()) + { + + if (!body->IsStatic()) + { + body->predictIntegratedTransform(timeStep* toi, predictedTrans); + body->proceedToTransform( predictedTrans); + } + + } + } + + } + + + + + + // + // disable sleeping physics objects + // + + std::vector<CcdPhysicsController*> m_sleepingControllers; + + std::vector<CcdPhysicsController*>::iterator i; + + for (i=m_controllers.begin(); + !(i==m_controllers.end()); i++) + { + CcdPhysicsController* ctrl = (*i); + RigidBody* body = ctrl->GetRigidBody(); + + ctrl->UpdateDeactivation(timeStep); + + + if (ctrl->wantsSleeping()) + { + if (body->GetActivationState() == ACTIVE_TAG) + body->SetActivationState( WANTS_DEACTIVATION ); + } else + { + if (body->GetActivationState() != DISABLE_DEACTIVATION) + body->SetActivationState( ACTIVE_TAG ); + } + + if (true) + { + if (body->GetActivationState() == ISLAND_SLEEPING) + { + m_sleepingControllers.push_back(ctrl); + } + } else + { + if (ctrl->wantsSleeping()) + { + m_sleepingControllers.push_back(ctrl); + } + } + } + + + + + } + + +#ifdef USE_QUICKPROF + Profiler::endBlock("proceedToTransform"); + + Profiler::beginBlock("SyncMotionStates"); +#endif //USE_QUICKPROF + + SyncMotionStates(timeStep); + +#ifdef USE_QUICKPROF + Profiler::endBlock("SyncMotionStates"); + +#endif //USE_QUICKPROF + + +#ifdef NEW_BULLET_VEHICLE_SUPPORT + //sync wheels for vehicles + int numVehicles = m_wrapperVehicles.size(); + for (int i=0;i<numVehicles;i++) + { + WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i]; + + wrapperVehicle->SyncWheels(); + } +#endif //NEW_BULLET_VEHICLE_SUPPORT + + return true; + } +} + + + +void SimulationIsland::SyncMotionStates(float timeStep) +{ + std::vector<CcdPhysicsController*>::iterator i; + + // + // synchronize the physics and graphics transformations + // + + for (i=m_controllers.begin(); + !(i==m_controllers.end()); i++) + { + CcdPhysicsController* ctrl = (*i); + ctrl->SynchronizeMotionStates(timeStep); + + } + +} + + + +void SimulationIsland::UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface* scene,float timeStep) +{ + std::vector<CcdPhysicsController*>::iterator i; + + + // + // update aabbs, only for moving objects (!) + // + for (i=m_controllers.begin(); + !(i==m_controllers.end()); i++) + { + CcdPhysicsController* ctrl = (*i); + RigidBody* body = ctrl->GetRigidBody(); + + + SimdPoint3 minAabb,maxAabb; + CollisionShape* shapeinterface = ctrl->GetCollisionShape(); + + + + shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(), + body->getLinearVelocity(), + //body->getAngularVelocity(), + SimdVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(), + timeStep,minAabb,maxAabb); + + + SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold); + minAabb -= manifoldExtraExtents; + maxAabb += manifoldExtraExtents; + + BroadphaseProxy* bp = body->m_broadphaseHandle; + if (bp) + { + + SimdVector3 color (1,1,0); + + class IDebugDraw* m_debugDrawer = 0; +/* + if (m_debugDrawer) + { + //draw aabb + switch (body->GetActivationState()) + { + case ISLAND_SLEEPING: + { + color.setValue(1,1,1); + break; + } + case WANTS_DEACTIVATION: + { + color.setValue(0,0,1); + break; + } + case ACTIVE_TAG: + { + break; + } + case DISABLE_DEACTIVATION: + { + color.setValue(1,0,1); + }; + + }; + + if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb) + { + DrawAabb(m_debugDrawer,minAabb,maxAabb,color); + } + } +*/ + + + if ( (maxAabb-minAabb).length2() < 1e12f) + { + scene->SetAabb(bp,minAabb,maxAabb); + } else + { + //something went wrong, investigate + //removeCcdPhysicsController(ctrl); + body->SetActivationState(DISABLE_SIMULATION); + + static bool reportMe = true; + if (reportMe) + { + reportMe = false; + printf("Overflow in AABB, object removed from simulation \n"); + printf("If you can reproduce this, please email bugs@continuousphysics.com\n"); + printf("Please include above information, your Platform, version of OS.\n"); + printf("Thanks.\n"); + } + + } + + } + } +}
\ No newline at end of file diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h new file mode 100644 index 00000000000..c4c41bf03c4 --- /dev/null +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h @@ -0,0 +1,53 @@ +/* +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 SIMULATION_ISLAND_H +#define SIMULATION_ISLAND_H + +#include <vector> +class BroadphaseInterface; +class Dispatcher; +class IDebugDraw; + +///SimulationIsland groups all computations and data (for collision detection and dynamics) that can execute in parallel with other SimulationIsland's +///The ParallelPhysicsEnvironment and ParallelIslandDispatcher will dispatch SimulationIsland's +///At the start of the simulation timestep the simulation islands are re-calculated +///During one timestep there is no merging or splitting of Simulation Islands +class SimulationIsland +{ + + public: + std::vector<class CcdPhysicsController*> m_controllers; + std::vector<class PersistentManifold*> m_manifolds; + + std::vector<int> m_overlappingPairIndices; + std::vector<int> m_constraintIndices; + + bool Simulate(IDebugDraw* debugDrawer,int numSolverIterations,class TypedConstraint** constraintsBaseAddress,struct BroadphasePair* overlappingPairBaseAddress, Dispatcher* dispatcher,BroadphaseInterface* broadphase, class ConstraintSolver* solver, float timeStep); + + + int GetNumControllers() + { + return m_controllers.size(); + } + + + + + void SyncMotionStates(float timeStep); + void UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface* broadphase,float timeStep); +}; + +#endif //SIMULATION_ISLAND_H
\ No newline at end of file diff --git a/extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h b/extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h index 79cd57756b0..3952377181a 100644 --- a/extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h +++ b/extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h @@ -82,7 +82,7 @@ typedef enum PHY_ConstraintType { PHY_LINEHINGE_CONSTRAINT=2, PHY_ANGULAR_CONSTRAINT = 3,//hinge without ball socket PHY_VEHICLE_CONSTRAINT=11,//complex 'constraint' that turns a rigidbody into a vehicle - + PHY_GENERIC_6DOF_CONSTRAINT=12,//can leave any of the 6 degree of freedom 'free' or 'locked' } PHY_ConstraintType; diff --git a/extern/bullet/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h b/extern/bullet/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h index 3b00dad39b6..cbf9094037f 100644 --- a/extern/bullet/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h +++ b/extern/bullet/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h @@ -39,6 +39,8 @@ class PHY_IPhysicsEnvironment float axisX,float axisY,float axisZ)=0; virtual void removeConstraint(int constraintid)=0; + virtual float getAppliedImpulse(int constraintid){ return 0.f;} + virtual PHY_IPhysicsController* rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)=0; diff --git a/extern/bullet/LinearMath/SimdScalar.h b/extern/bullet/LinearMath/SimdScalar.h index c97b02671cd..e866382c022 100644 --- a/extern/bullet/LinearMath/SimdScalar.h +++ b/extern/bullet/LinearMath/SimdScalar.h @@ -27,29 +27,29 @@ subject to the following restrictions: #include <float.h> #ifdef WIN32 -#pragma warning(disable:4530) -#pragma warning(disable:4996) -#ifdef __MINGW32__ -#define SIMD_FORCE_INLINE inline -#else -#define SIMD_FORCE_INLINE __forceinline -#endif //__MINGW32__ - -//#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a -#define ATTRIBUTE_ALIGNED16(a) a -#include <assert.h> -#define ASSERT assert + #if defined(__MINGW32__) || defined(__CYGWIN__) + #define SIMD_FORCE_INLINE inline + #else + #pragma warning(disable:4530) + #pragma warning(disable:4996) + #define SIMD_FORCE_INLINE __forceinline + #endif //__MINGW32__ + + //#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a + #define ATTRIBUTE_ALIGNED16(a) a + #include <assert.h> + #define ASSERT assert #else -#define SIMD_FORCE_INLINE inline -#define ATTRIBUTE_ALIGNED16(a) a -#ifndef assert -#include <assert.h> -#endif - - -#define ASSERT assert - + + //non-windows systems + + #define SIMD_FORCE_INLINE inline + #define ATTRIBUTE_ALIGNED16(a) a + #ifndef assert + #include <assert.h> + #endif + #define ASSERT assert #endif diff --git a/extern/bullet/LinearMath/SimdTransformUtil.h b/extern/bullet/LinearMath/SimdTransformUtil.h index 9f6619ab43e..6bc0d297310 100644 --- a/extern/bullet/LinearMath/SimdTransformUtil.h +++ b/extern/bullet/LinearMath/SimdTransformUtil.h @@ -25,6 +25,13 @@ subject to the following restrictions: #define SimdRecipSqrt(x) ((float)(1.0f/SimdSqrt(float(x)))) /* reciprocal square root */ +inline SimdVector3 SimdAabbSupport(const SimdVector3& halfExtents,const SimdVector3& supportDir) +{ + return SimdVector3(supportDir.x() < SimdScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), + supportDir.y() < SimdScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), + supportDir.z() < SimdScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); +} + inline void SimdPlaneSpace1 (const SimdVector3& n, SimdVector3& p, SimdVector3& q) { diff --git a/extern/bullet/LinearMath/SimdVector3.h b/extern/bullet/LinearMath/SimdVector3.h index 93f5182133c..b44ef6c099f 100644 --- a/extern/bullet/LinearMath/SimdVector3.h +++ b/extern/bullet/LinearMath/SimdVector3.h @@ -183,7 +183,7 @@ operator+(const SimdVector3& v1, const SimdVector3& v2) SIMD_FORCE_INLINE SimdVector3 operator*(const SimdVector3& v1, const SimdVector3& v2) { - return SimdVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() *+ v2.z()); + return SimdVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z()); } SIMD_FORCE_INLINE SimdVector3 diff --git a/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp b/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp index 7a01849d1a1..e08e5e7cbf1 100644 --- a/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp +++ b/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp @@ -1091,7 +1091,7 @@ void KX_ConvertBulletObject( class KX_GameObject* gameobj, if (objprop->m_disableSleeping) physicscontroller->GetRigidBody()->SetActivationState(DISABLE_DEACTIVATION); - if (!objprop->m_angular_rigidbody) + if (objprop->m_dyna && !objprop->m_angular_rigidbody) { /* //setting the inertia could achieve similar results to constraint the up diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp index 60b8dbbf862..83bddc8ee1e 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp +++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp @@ -34,7 +34,7 @@ class BP_Proxy; float gDeactivationTime = 2.f; bool gDisableDeactivation = false; -float gLinearSleepingTreshold = 0.4f; +float gLinearSleepingTreshold = 0.8f; float gAngularSleepingTreshold = 1.0f; #include "Dynamics/MassProps.h" @@ -210,28 +210,6 @@ void CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionsta } -void CcdPhysicsController::SetMargin(float margin) -{ - if (m_body && m_body->GetCollisionShape()) - { - m_body->GetCollisionShape()->SetMargin(margin); - } - - -} - -float CcdPhysicsController::GetMargin() const -{ - if (m_body && m_body->GetCollisionShape()) - { - return m_body->GetCollisionShape()->GetMargin(); - } - - return 0.f; - -} - - // kinematic methods void CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local) { @@ -557,6 +535,7 @@ PHY_IPhysicsController* CcdPhysicsController::GetReplica() DefaultMotionState::DefaultMotionState() { m_worldTransform.setIdentity(); + m_localScaling.setValue(1.f,1.f,1.f); } @@ -574,9 +553,9 @@ void DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ) void DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ) { - scaleX = 1.; - scaleY = 1.; - scaleZ = 1.; + scaleX = m_localScaling.getX(); + scaleY = m_localScaling.getY(); + scaleZ = m_localScaling.getZ(); } void DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal) diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.h b/source/gameengine/Physics/Bullet/CcdPhysicsController.h index 7488e74db01..632d5d776d2 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsController.h +++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.h @@ -39,20 +39,38 @@ extern bool gDisableDeactivation; class CcdPhysicsEnvironment; + + struct CcdConstructionInfo { + + ///CollisionFilterGroups provides some optional usage of basic collision filtering + ///this is done during broadphase, so very early in the pipeline + ///more advanced collision filtering should be done in CollisionDispatcher::NeedsCollision + enum CollisionFilterGroups + { + DefaultFilter = 1, + StaticFilter = 2, + KinematicFilter = 4, + DebrisFilter = 8, + AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter, + }; + + CcdConstructionInfo() : m_gravity(0,0,0), + m_scaling(1.f,1.f,1.f), m_mass(0.f), m_restitution(0.1f), m_friction(0.5f), m_linearDamping(0.1f), m_angularDamping(0.1f), + m_collisionFlags(0), + m_collisionFilterGroup(DefaultFilter), + m_collisionFilterMask(AllFilter), m_MotionState(0), m_physicsEnv(0), - m_inertiaFactor(1.f), - m_scaling(1.f,1.f,1.f), - m_collisionFlags(0) + m_inertiaFactor(1.f) { } @@ -66,6 +84,15 @@ struct CcdConstructionInfo SimdScalar m_angularDamping; int m_collisionFlags; + ///optional use of collision group/mask: + ///only collision with object goups that match the collision mask. + ///this is very basic early out. advanced collision filtering should be + ///done in the CollisionDispatcher::NeedsCollision and NeedsResponse + ///both values default to 1 + short int m_collisionFilterGroup; + short int m_collisionFilterMask; + + CollisionShape* m_collisionShape; class PHY_IMotionState* m_MotionState; @@ -158,10 +185,21 @@ class CcdPhysicsController : public PHY_IPhysicsController virtual void setNewClientInfo(void* clientinfo); virtual PHY_IPhysicsController* GetReplica(); + ///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors + short int GetCollisionFilterGroup() const + { + return m_cci.m_collisionFilterGroup; + } + ///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors + short int GetCollisionFilterMask() const + { + return m_cci.m_collisionFilterMask; + } + virtual void calcXform() {} ; - virtual void SetMargin(float margin); - virtual float GetMargin() const; + virtual void SetMargin(float margin) {}; + virtual float GetMargin() const {return 0.f;}; bool wantsSleeping(); @@ -208,6 +246,7 @@ class DefaultMotionState : public PHY_IMotionState virtual void calculateWorldTransformations(); SimdTransform m_worldTransform; + SimdVector3 m_localScaling; }; diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp index 2fee6d06963..813462fdf5e 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp @@ -30,14 +30,12 @@ subject to the following restrictions: #include "CollisionShapes/ConvexShape.h" #include "CollisionShapes/ConeShape.h" - - +#include "CollisionDispatch/SimulationIslandManager.h" #include "BroadphaseCollision/Dispatcher.h" #include "NarrowPhaseCollision/PersistentManifold.h" #include "CollisionShapes/TriangleMeshShape.h" -#include "ConstraintSolver/OdeConstraintSolver.h" -#include "ConstraintSolver/SimpleConstraintSolver.h" +#include "ConstraintSolver/SequentialImpulseConstraintSolver.h" //profiling/timings @@ -56,7 +54,7 @@ subject to the following restrictions: #include "PHY_IMotionState.h" #include "CollisionDispatch/EmptyCollisionAlgorithm.h" -#include "CollisionDispatch/UnionFind.h" + #include "CollisionShapes/SphereShape.h" @@ -77,6 +75,7 @@ RaycastVehicle::VehicleTuning gTuning; #include "ConstraintSolver/ConstraintSolver.h" #include "ConstraintSolver/Point2PointConstraint.h" #include "ConstraintSolver/HingeConstraint.h" +#include "ConstraintSolver/Generic6DofConstraint.h" //#include "BroadphaseCollision/QueryDispatcher.h" @@ -322,7 +321,7 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV -CcdPhysicsEnvironment::CcdPhysicsEnvironment(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase) +CcdPhysicsEnvironment::CcdPhysicsEnvironment(Dispatcher* dispatcher,OverlappingPairCache* pairCache) :m_scalingPropagated(false), m_numIterations(10), m_numTimeSubSteps(1), @@ -336,18 +335,19 @@ m_enableSatCollisionDetection(false) { m_triggerCallbacks[i] = 0; } + if (!dispatcher) dispatcher = new CollisionDispatcher(); - if(!broadphase) + if(!pairCache) { //todo: calculate/let user specify this world sizes SimdVector3 worldMin(-10000,-10000,-10000); SimdVector3 worldMax(10000,10000,10000); - broadphase = new AxisSweep3(worldMin,worldMax); + pairCache = new AxisSweep3(worldMin,worldMax); //broadphase = new SimpleBroadphase(); } @@ -355,11 +355,13 @@ m_enableSatCollisionDetection(false) setSolverType(1);//issues with quickstep and memory allocations - m_collisionWorld = new CollisionWorld(dispatcher,broadphase); + m_collisionWorld = new CollisionWorld(dispatcher,pairCache); m_debugDrawer = 0; m_gravity = SimdVector3(0.f,-10.f,0.f); + m_islandManager = new SimulationIslandManager(); + } @@ -373,7 +375,7 @@ void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl) body->setGravity( m_gravity ); m_controllers.push_back(ctrl); - m_collisionWorld->AddCollisionObject(body); + m_collisionWorld->AddCollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask()); assert(body->m_broadphaseHandle); @@ -385,7 +387,8 @@ void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl) assert(shapeinterface); const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform(); - + + body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); SimdPoint3 minAabb,maxAabb; @@ -637,6 +640,9 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) CcdPhysicsController* ctrl = m_controllers[k]; // SimdTransform predictedTrans; RigidBody* body = ctrl->GetRigidBody(); + + body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); + if (body->IsActive()) { if (!body->IsStatic()) @@ -654,7 +660,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) Profiler::endBlock("predictIntegratedTransform"); #endif //USE_QUICKPROF - BroadphaseInterface* scene = GetBroadphase(); + OverlappingPairCache* scene = m_collisionWorld->GetPairCache(); // @@ -674,8 +680,10 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection; + dispatchInfo.m_debugDraw = this->m_debugDrawer; - scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g); + scene->RefreshOverlappingPairs(); + GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo); #ifdef USE_QUICKPROF @@ -685,7 +693,8 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) int numRigidBodies = m_controllers.size(); - m_collisionWorld->UpdateActivationState(); + + m_islandManager->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher()); { int i; @@ -702,15 +711,15 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) { if (colObj0->IsActive() || colObj1->IsActive()) { - GetDispatcher()->GetUnionFind().unite((colObj0)->m_islandTag1, + + m_islandManager->GetUnionFind().unite((colObj0)->m_islandTag1, (colObj1)->m_islandTag1); } } } } - m_collisionWorld->StoreIslandActivationState(); - + m_islandManager->StoreIslandActivationState(GetCollisionWorld()); //contacts @@ -762,7 +771,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) #endif //NEW_BULLET_VEHICLE_SUPPORT - struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback + struct InplaceSolverIslandCallback : public SimulationIslandManager::IslandCallback { ContactSolverInfo& m_solverInfo; @@ -803,7 +812,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) #endif //USE_QUICKPROF /// solve all the contact points and contact friction - GetDispatcher()->BuildAndProcessIslands(m_collisionWorld->GetCollisionObjectArray(),&solverCallback); + m_islandManager->BuildAndProcessIslands(GetCollisionWorld()->GetDispatcher(),m_collisionWorld->GetCollisionObjectArray(),&solverCallback); #ifdef USE_QUICKPROF Profiler::endBlock("BuildAndProcessIslands"); @@ -842,7 +851,9 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) dispatchInfo.m_stepCount = 0; dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS; - scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g); + //pairCache->RefreshOverlappingPairs();//?? + GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo); + toi = dispatchInfo.m_timeOfImpact; } @@ -871,6 +882,11 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) if (!body->IsStatic()) { + if (body->m_hitFraction < 1.f) + { + //set velocity to zero... until we have proper CCD integrated + body->setLinearVelocity(body->getLinearVelocity()*0.5f); + } body->predictIntegratedTransform(timeStep* toi, predictedTrans); body->proceedToTransform( predictedTrans); } @@ -1033,7 +1049,7 @@ void CcdPhysicsEnvironment::setSolverType(int solverType) if (m_solverType != solverType) { - m_solver = new SimpleConstraintSolver(); + m_solver = new SequentialImpulseConstraintSolver(); break; } @@ -1043,7 +1059,7 @@ void CcdPhysicsEnvironment::setSolverType(int solverType) default: if (m_solverType != solverType) { - m_solver = new OdeConstraintSolver(); +// m_solver = new OdeConstraintSolver(); break; } @@ -1182,8 +1198,8 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA; SimdVector3 axisInA(axisX,axisY,axisZ); SimdVector3 axisInB = rb1 ? - (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : - rb0->getCenterOfMassTransform().getBasis() * -axisInA; + (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : + rb0->getCenterOfMassTransform().getBasis() * axisInA; bool angularOnly = false; @@ -1213,9 +1229,55 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl break; } + case PHY_GENERIC_6DOF_CONSTRAINT: + { + Generic6DofConstraint* genericConstraint = 0; + + if (rb1) + { + SimdTransform frameInA; + SimdTransform frameInB; + + SimdVector3 axis1, axis2; + SimdPlaneSpace1( axisInA, axis1, axis2 ); + + frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(), + axisInA.y(), axis1.y(), axis2.y(), + axisInA.z(), axis1.z(), axis2.z() ); + + + SimdPlaneSpace1( axisInB, axis1, axis2 ); + frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(), + axisInB.y(), axis1.y(), axis2.y(), + axisInB.z(), axis1.z(), axis2.z() ); + + frameInA.setOrigin( pivotInA ); + frameInB.setOrigin( pivotInB ); + + genericConstraint = new Generic6DofConstraint( + *rb0,*rb1, + frameInA,frameInB); + + + } else + { + // TODO: Implement single body case... + + } + + + m_constraints.push_back(genericConstraint); + genericConstraint->SetUserConstraintId(gConstraintUid++); + genericConstraint->SetUserConstraintType(type); + //64 bit systems can't cast pointer to int. could use size_t instead. + return genericConstraint->GetUserConstraintId(); + + break; + } case PHY_ANGULAR_CONSTRAINT: angularOnly = true; + case PHY_LINEHINGE_CONSTRAINT: { HingeConstraint* hinge = 0; @@ -1271,21 +1333,62 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl } -float CcdPhysicsEnvironment::getAppliedImpulse(int constraintid) + + + +//Following the COLLADA physics specification for constraints +int CcdPhysicsEnvironment::createUniversalD6Constraint( + class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther, + SimdTransform& frameInA, + SimdTransform& frameInB, + const SimdVector3& linearMinLimits, + const SimdVector3& linearMaxLimits, + const SimdVector3& angularMinLimits, + const SimdVector3& angularMaxLimits +) { - std::vector<TypedConstraint*>::iterator i; - for (i=m_constraints.begin(); - !(i==m_constraints.end()); i++) + //we could either add some logic to recognize ball-socket and hinge, or let that up to the user + //perhaps some warning or hint that hinge/ball-socket is more efficient? + + Generic6DofConstraint* genericConstraint = 0; + CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef; + CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther; + + RigidBody* rb0 = ctrl0->GetRigidBody(); + RigidBody* rb1 = ctrl1->GetRigidBody(); + + if (rb1) { - TypedConstraint* constraint = (*i); - if (constraint->GetUserConstraintId() == constraintid) - { - return constraint->GetAppliedImpulse(); - } + + + genericConstraint = new Generic6DofConstraint( + *rb0,*rb1, + frameInA,frameInB); + genericConstraint->setLinearLowerLimit(linearMinLimits); + genericConstraint->setLinearUpperLimit(linearMaxLimits); + genericConstraint->setAngularLowerLimit(angularMinLimits); + genericConstraint->setAngularUpperLimit(angularMaxLimits); + } else + { + // TODO: Implement single body case... + //No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code + } - return 0.f; + + if (genericConstraint) + { + m_constraints.push_back(genericConstraint); + genericConstraint->SetUserConstraintId(gConstraintUid++); + genericConstraint->SetUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT); + //64 bit systems can't cast pointer to int. could use size_t instead. + return genericConstraint->GetUserConstraintId(); + } + return 0; } + + + void CcdPhysicsEnvironment::removeConstraint(int constraintId) { std::vector<TypedConstraint*>::iterator i; @@ -1296,15 +1399,7 @@ void CcdPhysicsEnvironment::removeConstraint(int constraintId) TypedConstraint* constraint = (*i); if (constraint->GetUserConstraintId() == constraintId) { - //activate objects - if (constraint->GetRigidBodyA().mergesSimulationIslands()) - constraint->GetRigidBodyA().activate(); - if (constraint->GetRigidBodyB().mergesSimulationIslands()) - constraint->GetRigidBodyB().activate(); - std::swap(*i, m_constraints.back()); - - m_constraints.pop_back(); break; } @@ -1352,13 +1447,6 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i SimdVector3 rayFrom(fromX,fromY,fromZ); SimdVector3 rayTo(toX,toY,toZ); - - if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb) - { - SimdVector3 color (1,0,0); - m_debugDrawer->DrawLine(rayFrom,rayTo,color); - } - SimdVector3 hitPointWorld,normalWorld; //Either Ray Cast with or without filtering @@ -1376,26 +1464,10 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i hitX = rayCallback.m_hitPointWorld.getX(); hitY = rayCallback.m_hitPointWorld.getY(); hitZ = rayCallback.m_hitPointWorld.getZ(); - if (rayCallback.m_hitNormalWorld.length2() > SIMD_EPSILON) - { - rayCallback.m_hitNormalWorld.normalize(); - } normalX = rayCallback.m_hitNormalWorld.getX(); normalY = rayCallback.m_hitNormalWorld.getY(); normalZ = rayCallback.m_hitNormalWorld.getZ(); - - if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb) - { - SimdVector3 colorNormal(0,0,1); - m_debugDrawer->DrawLine(rayCallback.m_hitPointWorld,rayCallback.m_hitPointWorld+rayCallback.m_hitNormalWorld,colorNormal); - - SimdVector3 color (0,1,0); - m_debugDrawer->DrawLine(rayFrom,rayCallback.m_hitPointWorld,color); - - - } - } @@ -1425,15 +1497,6 @@ BroadphaseInterface* CcdPhysicsEnvironment::GetBroadphase() -const CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher() const -{ - return m_collisionWorld->GetDispatcher(); -} - -CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher() -{ - return m_collisionWorld->GetDispatcher(); -} CcdPhysicsEnvironment::~CcdPhysicsEnvironment() { @@ -1448,6 +1511,8 @@ CcdPhysicsEnvironment::~CcdPhysicsEnvironment() //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher //delete m_dispatcher; delete m_collisionWorld; + + delete m_islandManager; } @@ -1464,15 +1529,9 @@ CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index) } -int CcdPhysicsEnvironment::GetNumManifolds() const -{ - return GetDispatcher()->GetNumManifolds(); -} -const PersistentManifold* CcdPhysicsEnvironment::GetManifold(int index) const -{ - return GetDispatcher()->GetManifoldByIndexInternal(index); -} + + TypedConstraint* CcdPhysicsEnvironment::getConstraintById(int constraintId) { @@ -1565,9 +1624,10 @@ void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctr void CcdPhysicsEnvironment::CallbackTriggers() { + CcdPhysicsController* ctrl0=0,*ctrl1=0; - if (m_triggerCallbacks[PHY_OBJECT_RESPONSE]) + if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawContactPoints))) { //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback int numManifolds = m_collisionWorld->GetDispatcher()->GetNumManifolds(); @@ -1577,6 +1637,16 @@ void CcdPhysicsEnvironment::CallbackTriggers() int numContacts = manifold->GetNumContacts(); if (numContacts) { + if (m_debugDrawer && (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawContactPoints)) + { + for (int j=0;j<numContacts;j++) + { + SimdVector3 color(1,0,0); + const ManifoldPoint& cp = manifold->GetContactPoint(j); + if (m_debugDrawer) + m_debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color); + } + } RigidBody* obj0 = static_cast<RigidBody* >(manifold->GetBody0()); RigidBody* obj1 = static_cast<RigidBody* >(manifold->GetBody1()); @@ -1603,6 +1673,7 @@ void CcdPhysicsEnvironment::CallbackTriggers() } + } @@ -1764,3 +1835,18 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float conera return sphereController; } +float CcdPhysicsEnvironment::getAppliedImpulse(int constraintid) +{ + std::vector<TypedConstraint*>::iterator i; + + for (i=m_constraints.begin(); + !(i==m_constraints.end()); i++) + { + TypedConstraint* constraint = (*i); + if (constraint->GetUserConstraintId() == constraintid) + { + return constraint->GetAppliedImpulse(); + } + } + return 0.f; +} diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h index 73bd9f8c95e..a1f075e3882 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h @@ -20,11 +20,13 @@ subject to the following restrictions: #include <vector> class CcdPhysicsController; #include "SimdVector3.h" +#include "SimdTransform.h" -class TypedConstraint; +class TypedConstraint; +class SimulationIslandManager; class CollisionDispatcher; class Dispatcher; //#include "BroadphaseInterface.h" @@ -37,6 +39,7 @@ class Dispatcher; class WrapperVehicle; class PersistentManifold; class BroadphaseInterface; +class OverlappingPairCache; class IDebugDraw; /// CcdPhysicsEnvironment is experimental mainloop for physics simulation using optional continuous collision detection. @@ -47,12 +50,17 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment { SimdVector3 m_gravity; + + +protected: IDebugDraw* m_debugDrawer; //solver iterations int m_numIterations; //timestep subdivisions int m_numTimeSubSteps; + + int m_ccdMode; int m_solverType; int m_profileTimings; @@ -60,9 +68,10 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment ContactSolverInfo m_solverInfo; + SimulationIslandManager* m_islandManager; public: - CcdPhysicsEnvironment(CollisionDispatcher* dispatcher=0, BroadphaseInterface* broadphase=0); + CcdPhysicsEnvironment(Dispatcher* dispatcher=0, OverlappingPairCache* pairCache=0); virtual ~CcdPhysicsEnvironment(); @@ -98,7 +107,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment virtual void endFrame() {}; /// Perform an integration step of duration 'timeStep'. virtual bool proceedDeltaTime(double curTime,float timeStep); - bool proceedDeltaTimeOneStep(float timeStep); + virtual bool proceedDeltaTimeOneStep(float timeStep); virtual void setFixedTimeStep(bool useFixedTimeStep,float fixedTimeStep){}; //returns 0.f if no fixed timestep is used @@ -112,9 +121,24 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment virtual int createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type, float pivotX,float pivotY,float pivotZ, float axisX,float axisY,float axisZ); + + + //Following the COLLADA physics specification for constraints + virtual int createUniversalD6Constraint( + class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther, + SimdTransform& localAttachmentFrameRef, + SimdTransform& localAttachmentOther, + const SimdVector3& linearMinLimits, + const SimdVector3& linearMaxLimits, + const SimdVector3& angularMinLimits, + const SimdVector3& angularMaxLimits + ); + + virtual void removeConstraint(int constraintid); - virtual float getAppliedImpulse(int constraintid); + + virtual float getAppliedImpulse(int constraintid); virtual void CallbackTriggers(); @@ -160,9 +184,9 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment BroadphaseInterface* GetBroadphase(); - CollisionDispatcher* GetDispatcher(); - const CollisionDispatcher* GetDispatcher() const; + + bool IsSatCollisionDetectionEnabled() const { @@ -180,16 +204,39 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment CcdPhysicsController* GetPhysicsController( int index); - int GetNumManifolds() const; + const PersistentManifold* GetManifold(int index) const; std::vector<TypedConstraint*> m_constraints; - private: + void SyncMotionStates(float timeStep); + + class CollisionWorld* GetCollisionWorld() + { + return m_collisionWorld; + } + + const class CollisionWorld* GetCollisionWorld() const + { + return m_collisionWorld; + } + + SimulationIslandManager* GetSimulationIslandManager() + { + return m_islandManager; + } + + const SimulationIslandManager* GetSimulationIslandManager() const + { + return m_islandManager; + } + + protected: - void SyncMotionStates(float timeStep); + + std::vector<CcdPhysicsController*> m_controllers; @@ -206,6 +253,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment bool m_scalingPropagated; + }; diff --git a/source/gameengine/Physics/common/PHY_DynamicTypes.h b/source/gameengine/Physics/common/PHY_DynamicTypes.h index c9ca3221355..3952377181a 100644 --- a/source/gameengine/Physics/common/PHY_DynamicTypes.h +++ b/source/gameengine/Physics/common/PHY_DynamicTypes.h @@ -1,38 +1,24 @@ -/** - * $Id$ - * - * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** - * - * This program is free software; you can redistribute it and/or - * modify it under the terms of the GNU General Public License - * as published by the Free Software Foundation; either version 2 - * of the License, or (at your option) any later version. The Blender - * Foundation also sells licenses for use in proprietary software under - * the Blender License. See http://www.blender.org/BL/ for information - * about this. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software Foundation, - * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. - * All rights reserved. - * - * The Original Code is: all of this file. - * - * Contributor(s): none yet. - * - * ***** END GPL/BL DUAL LICENSE BLOCK ***** - */ +/* +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 __PHY_DYNAMIC_TYPES #define __PHY_DYNAMIC_TYPES + class PHY_ResponseTable; class PHY_Shape; @@ -68,11 +54,6 @@ typedef enum PHY__Vector3 m_normal; /* point2 - point1 */ } PHY_CollData; -/* A response callback is called by SOLID for each pair of collding objects. 'client-data' - is a pointer to an arbitrary structure in the client application. The client objects are - pointers to structures in the client application associated with the coliding objects. - 'coll_data' is the collision data computed by SOLID. -*/ typedef bool (*PHY_ResponseCallback)(void *client_data, void *client_object1, @@ -101,9 +82,11 @@ typedef enum PHY_ConstraintType { PHY_LINEHINGE_CONSTRAINT=2, PHY_ANGULAR_CONSTRAINT = 3,//hinge without ball socket PHY_VEHICLE_CONSTRAINT=11,//complex 'constraint' that turns a rigidbody into a vehicle + PHY_GENERIC_6DOF_CONSTRAINT=12,//can leave any of the 6 degree of freedom 'free' or 'locked' } PHY_ConstraintType; +typedef float PHY_Vector3[3]; #endif //__PHY_DYNAMIC_TYPES diff --git a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h index fe4370fa725..2f3d8283d10 100644 --- a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h +++ b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h @@ -90,6 +90,7 @@ class PHY_IPhysicsEnvironment virtual void removeConstraint(int constraintid)=0; virtual float getAppliedImpulse(int constraintid){ return 0.f;} + //complex constraint for vehicles virtual PHY_IVehicle* getVehicleConstraint(int constraintId) =0; |