Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
View File

@@ -2,7 +2,6 @@
//Bullet Continuous Collision Detection and Physics Library
//Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
//
// btAxisSweep3
//
@@ -19,18 +18,15 @@
// 3. This notice may not be removed or altered from any source distribution.
#include "btAxisSweep3.h"
btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
:btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache,disableRaycastAccelerator)
btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
: btAxisSweep3Internal<unsigned short int>(worldAabbMin, worldAabbMax, 0xfffe, 0xffff, maxHandles, pairCache, disableRaycastAccelerator)
{
// 1 handle is reserved as sentinel
btAssert(maxHandles > 1 && maxHandles < 32767);
}
bt32BitAxisSweep3::bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache , bool disableRaycastAccelerator)
:btAxisSweep3Internal<unsigned int>(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache,disableRaycastAccelerator)
bt32BitAxisSweep3::bt32BitAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
: btAxisSweep3Internal<unsigned int>(worldAabbMin, worldAabbMax, 0xfffffffe, 0x7fffffff, maxHandles, pairCache, disableRaycastAccelerator)
{
// 1 handle is reserved as sentinel
btAssert(maxHandles > 1 && maxHandles < 2147483647);

8
src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
View File

@@ -33,9 +33,7 @@
class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int>
{
public:
btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
btAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
};
/// The bt32BitAxisSweep3 allows higher precision quantization and more objects compared to the btAxisSweep3 sweep and prune.
@@ -44,9 +42,7 @@ public:
class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int>
{
public:
bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
bt32BitAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
};
#endif

528
src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h
View File

File diff suppressed because it is too large Load Diff

53
src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
View File

@@ -13,10 +13,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_BROADPHASE_INTERFACE_H
#define BT_BROADPHASE_INTERFACE_H
#ifndef BT_BROADPHASE_INTERFACE_H
#define BT_BROADPHASE_INTERFACE_H
struct btDispatcherInfo;
class btDispatcher;
@@ -24,27 +22,23 @@ class btDispatcher;
class btOverlappingPairCache;
struct btBroadphaseAabbCallback
struct btBroadphaseAabbCallback
{
virtual ~btBroadphaseAabbCallback() {}
virtual bool process(const btBroadphaseProxy* proxy) = 0;
virtual bool process(const btBroadphaseProxy* proxy) = 0;
};
struct btBroadphaseRayCallback : public btBroadphaseAabbCallback
struct btBroadphaseRayCallback : public btBroadphaseAabbCallback
{
///added some cached data to accelerate ray-AABB tests
btVector3 m_rayDirectionInverse;
unsigned int m_signs[3];
btScalar m_lambda_max;
btVector3 m_rayDirectionInverse;
unsigned int m_signs[3];
btScalar m_lambda_max;
virtual ~btBroadphaseRayCallback() {}
protected:
btBroadphaseRayCallback() {}
btBroadphaseRayCallback() {}
};
#include "LinearMath/btVector3.h"
@@ -57,30 +51,29 @@ class btBroadphaseInterface
public:
virtual ~btBroadphaseInterface() {}
virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher) =0;
virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0;
virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0;
virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const =0;
virtual btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher) = 0;
virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher) = 0;
virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher) = 0;
virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const = 0;
virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)) = 0;
virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0)) = 0;
virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) = 0;
virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) = 0;
///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
virtual void calculateOverlappingPairs(btDispatcher* dispatcher)=0;
virtual void calculateOverlappingPairs(btDispatcher* dispatcher) = 0;
virtual btOverlappingPairCache* getOverlappingPairCache()=0;
virtual const btOverlappingPairCache* getOverlappingPairCache() const =0;
virtual btOverlappingPairCache* getOverlappingPairCache() = 0;
virtual const btOverlappingPairCache* getOverlappingPairCache() const = 0;
///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
///will add some transform later
virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0;
virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const = 0;
///reset broadphase internal structures, to ensure determinism/reproducability
virtual void resetPool(btDispatcher* dispatcher) { (void) dispatcher; };
virtual void printStats() = 0;
virtual void resetPool(btDispatcher* dispatcher) { (void)dispatcher; };
virtual void printStats() = 0;
};
#endif //BT_BROADPHASE_INTERFACE_H
#endif //BT_BROADPHASE_INTERFACE_H

2
src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp
View File

@@ -15,4 +15,4 @@ subject to the following restrictions:
#include "btBroadphaseProxy.h"
BT_NOT_EMPTY_FILE // fix warning LNK4221: This object file does not define any previously undefined public symbols, so it will not be used by any link operation that consumes this library
BT_NOT_EMPTY_FILE // fix warning LNK4221: This object file does not define any previously undefined public symbols, so it will not be used by any link operation that consumes this library

191
src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
View File

@@ -16,11 +16,10 @@ subject to the following restrictions:
#ifndef BT_BROADPHASE_PROXY_H
#define BT_BROADPHASE_PROXY_H
#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE
#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE
#include "LinearMath/btVector3.h"
#include "LinearMath/btAlignedAllocator.h"
/// btDispatcher uses these types
/// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave
/// to facilitate type checking
@@ -35,8 +34,8 @@ enum BroadphaseNativeTypes
CONVEX_HULL_SHAPE_PROXYTYPE,
CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE,
CUSTOM_POLYHEDRAL_SHAPE_TYPE,
//implicit convex shapes
IMPLICIT_CONVEX_SHAPES_START_HERE,
//implicit convex shapes
IMPLICIT_CONVEX_SHAPES_START_HERE,
SPHERE_SHAPE_PROXYTYPE,
MULTI_SPHERE_SHAPE_PROXYTYPE,
CAPSULE_SHAPE_PROXYTYPE,
@@ -49,8 +48,8 @@ IMPLICIT_CONVEX_SHAPES_START_HERE,
BOX_2D_SHAPE_PROXYTYPE,
CONVEX_2D_SHAPE_PROXYTYPE,
CUSTOM_CONVEX_SHAPE_TYPE,
//concave shapes
CONCAVE_SHAPES_START_HERE,
//concave shapes
CONCAVE_SHAPES_START_HERE,
//keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
TRIANGLE_MESH_SHAPE_PROXYTYPE,
SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE,
@@ -58,16 +57,16 @@ CONCAVE_SHAPES_START_HERE,
FAST_CONCAVE_MESH_PROXYTYPE,
//terrain
TERRAIN_SHAPE_PROXYTYPE,
///Used for GIMPACT Trimesh integration
///Used for GIMPACT Trimesh integration
GIMPACT_SHAPE_PROXYTYPE,
///Multimaterial mesh
MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE,
///Multimaterial mesh
MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE,
EMPTY_SHAPE_PROXYTYPE,
STATIC_PLANE_PROXYTYPE,
CUSTOM_CONCAVE_SHAPE_TYPE,
SDF_SHAPE_PROXYTYPE=CUSTOM_CONCAVE_SHAPE_TYPE,
CONCAVE_SHAPES_END_HERE,
SDF_SHAPE_PROXYTYPE = CUSTOM_CONCAVE_SHAPE_TYPE,
CONCAVE_SHAPES_END_HERE,
COMPOUND_SHAPE_PROXYTYPE,
@@ -77,38 +76,37 @@ CONCAVE_SHAPES_END_HERE,
INVALID_SHAPE_PROXYTYPE,
MAX_BROADPHASE_COLLISION_TYPES
};
///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
///It stores collision shape type information, collision filter information and a client object, typically a btCollisionObject or btRigidBody.
ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy
ATTRIBUTE_ALIGNED16(struct)
btBroadphaseProxy
{
BT_DECLARE_ALIGNED_ALLOCATOR();
BT_DECLARE_ALIGNED_ALLOCATOR();
///optional filtering to cull potential collisions
enum CollisionFilterGroups
{
DefaultFilter = 1,
StaticFilter = 2,
KinematicFilter = 4,
DebrisFilter = 8,
SensorTrigger = 16,
CharacterFilter = 32,
AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
DefaultFilter = 1,
StaticFilter = 2,
KinematicFilter = 4,
DebrisFilter = 8,
SensorTrigger = 16,
CharacterFilter = 32,
AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
};
//Usually the client btCollisionObject or Rigidbody class
void* m_clientObject;
int m_collisionFilterGroup;
int m_collisionFilterMask;
void* m_clientObject;
int m_collisionFilterGroup;
int m_collisionFilterMask;
int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
int m_uniqueId; //m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
btVector3 m_aabbMin;
btVector3 m_aabbMax;
btVector3 m_aabbMin;
btVector3 m_aabbMax;
SIMD_FORCE_INLINE int getUid() const
{
@@ -116,47 +114,45 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
}
//used for memory pools
btBroadphaseProxy() :m_clientObject(0)
btBroadphaseProxy() : m_clientObject(0)
{
}
btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask)
:m_clientObject(userPtr),
m_collisionFilterGroup(collisionFilterGroup),
m_collisionFilterMask(collisionFilterMask),
m_aabbMin(aabbMin),
m_aabbMax(aabbMax)
btBroadphaseProxy(const btVector3& aabbMin, const btVector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask)
: m_clientObject(userPtr),
m_collisionFilterGroup(collisionFilterGroup),
m_collisionFilterMask(collisionFilterMask),
m_aabbMin(aabbMin),
m_aabbMax(aabbMax)
{
}
static SIMD_FORCE_INLINE bool isPolyhedral(int proxyType)
{
return (proxyType < IMPLICIT_CONVEX_SHAPES_START_HERE);
return (proxyType < IMPLICIT_CONVEX_SHAPES_START_HERE);
}
static SIMD_FORCE_INLINE bool isConvex(int proxyType)
static SIMD_FORCE_INLINE bool isConvex(int proxyType)
{
return (proxyType < CONCAVE_SHAPES_START_HERE);
}
static SIMD_FORCE_INLINE bool isNonMoving(int proxyType)
static SIMD_FORCE_INLINE bool isNonMoving(int proxyType)
{
return (isConcave(proxyType) && !(proxyType==GIMPACT_SHAPE_PROXYTYPE));
return (isConcave(proxyType) && !(proxyType == GIMPACT_SHAPE_PROXYTYPE));
}
static SIMD_FORCE_INLINE bool isConcave(int proxyType)
static SIMD_FORCE_INLINE bool isConcave(int proxyType)
{
return ((proxyType > CONCAVE_SHAPES_START_HERE) &&
(proxyType < CONCAVE_SHAPES_END_HERE));
(proxyType < CONCAVE_SHAPES_END_HERE));
}
static SIMD_FORCE_INLINE bool isCompound(int proxyType)
static SIMD_FORCE_INLINE bool isCompound(int proxyType)
{
return (proxyType == COMPOUND_SHAPE_PROXYTYPE);
}
static SIMD_FORCE_INLINE bool isSoftBody(int proxyType)
static SIMD_FORCE_INLINE bool isSoftBody(int proxyType)
{
return (proxyType == SOFTBODY_SHAPE_PROXYTYPE);
}
@@ -168,67 +164,62 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
static SIMD_FORCE_INLINE bool isConvex2d(int proxyType)
{
return (proxyType == BOX_2D_SHAPE_PROXYTYPE) || (proxyType == CONVEX_2D_SHAPE_PROXYTYPE);
return (proxyType == BOX_2D_SHAPE_PROXYTYPE) || (proxyType == CONVEX_2D_SHAPE_PROXYTYPE);
}
}
;
};
class btCollisionAlgorithm;
struct btBroadphaseProxy;
///The btBroadphasePair class contains a pair of aabb-overlapping objects.
///A btDispatcher can search a btCollisionAlgorithm that performs exact/narrowphase collision detection on the actual collision shapes.
ATTRIBUTE_ALIGNED16(struct) btBroadphasePair
ATTRIBUTE_ALIGNED16(struct)
btBroadphasePair
{
btBroadphasePair ()
:
m_pProxy0(0),
m_pProxy1(0),
m_algorithm(0),
m_internalInfo1(0)
btBroadphasePair()
: m_pProxy0(0),
m_pProxy1(0),
m_algorithm(0),
m_internalInfo1(0)
{
}
BT_DECLARE_ALIGNED_ALLOCATOR();
BT_DECLARE_ALIGNED_ALLOCATOR();
btBroadphasePair(const btBroadphasePair& other)
: m_pProxy0(other.m_pProxy0),
m_pProxy1(other.m_pProxy1),
m_algorithm(other.m_algorithm),
m_internalInfo1(other.m_internalInfo1)
: m_pProxy0(other.m_pProxy0),
m_pProxy1(other.m_pProxy1),
m_algorithm(other.m_algorithm),
m_internalInfo1(other.m_internalInfo1)
{
}
btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
btBroadphasePair(btBroadphaseProxy & proxy0, btBroadphaseProxy & proxy1)
{
//keep them sorted, so the std::set operations work
if (proxy0.m_uniqueId < proxy1.m_uniqueId)
{
m_pProxy0 = &proxy0;
m_pProxy1 = &proxy1;
}
else
{
m_pProxy0 = &proxy1;
m_pProxy1 = &proxy0;
}
{
m_pProxy0 = &proxy0;
m_pProxy1 = &proxy1;
}
else
{
m_pProxy0 = &proxy1;
m_pProxy1 = &proxy0;
}
m_algorithm = 0;
m_internalInfo1 = 0;
}
btBroadphaseProxy* m_pProxy0;
btBroadphaseProxy* m_pProxy1;
mutable btCollisionAlgorithm* m_algorithm;
union { void* m_internalInfo1; int m_internalTmpValue;};//don't use this data, it will be removed in future version.
mutable btCollisionAlgorithm* m_algorithm;
union {
void* m_internalInfo1;
int m_internalTmpValue;
}; //don't use this data, it will be removed in future version.
};
/*
@@ -240,31 +231,25 @@ SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePa
}
*/
class btBroadphasePairSortPredicate
{
public:
public:
bool operator()(const btBroadphasePair& a, const btBroadphasePair& b) const
{
const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1;
const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1;
const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1;
const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1;
bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b ) const
{
const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1;
const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1;
const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1;
const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1;
return uidA0 > uidB0 ||
(a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) ||
(a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm);
}
return uidA0 > uidB0 ||
(a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) ||
(a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm);
}
};
SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b)
SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b)
{
return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1);
return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1);
}
#endif //BT_BROADPHASE_PROXY_H
#endif //BT_BROADPHASE_PROXY_H

1
src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp
View File

@@ -20,4 +20,3 @@ btCollisionAlgorithm::btCollisionAlgorithm(const btCollisionAlgorithmConstructio
{
m_dispatcher = ci.m_dispatcher1;
}

42
src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
View File

@@ -25,57 +25,51 @@ class btManifoldResult;
class btCollisionObject;
struct btCollisionObjectWrapper;
struct btDispatcherInfo;
class btPersistentManifold;
class btPersistentManifold;
typedef btAlignedObjectArray<btPersistentManifold*> btManifoldArray;
typedef btAlignedObjectArray<btPersistentManifold*> btManifoldArray;
struct btCollisionAlgorithmConstructionInfo
{
btCollisionAlgorithmConstructionInfo()
:m_dispatcher1(0),
m_manifold(0)
: m_dispatcher1(0),
m_manifold(0)
{
}
btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp)
:m_dispatcher1(dispatcher)
btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher, int temp)
: m_dispatcher1(dispatcher)
{
(void)temp;
}
btDispatcher* m_dispatcher1;
btPersistentManifold* m_manifold;
// int getDispatcherId();
btDispatcher* m_dispatcher1;
btPersistentManifold* m_manifold;
// int getDispatcherId();
};
///btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatcher.
///It is persistent over frames
class btCollisionAlgorithm
{
protected:
btDispatcher* m_dispatcher;
protected:
// int getDispatcherId();
btDispatcher* m_dispatcher;
protected:
// int getDispatcherId();
public:
btCollisionAlgorithm() {};
btCollisionAlgorithm(){};
btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
virtual ~btCollisionAlgorithm() {};
virtual ~btCollisionAlgorithm(){};
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) = 0;
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) = 0;
virtual void getAllContactManifolds(btManifoldArray& manifoldArray) = 0;
virtual void getAllContactManifolds(btManifoldArray& manifoldArray) = 0;
};
#endif //BT_COLLISION_ALGORITHM_H
#endif //BT_COLLISION_ALGORITHM_H

1421
src/BulletCollision/BroadphaseCollision/btDbvt.cpp
View File

File diff suppressed because it is too large Load Diff

1637
src/BulletCollision/BroadphaseCollision/btDbvt.h
View File

File diff suppressed because it is too large Load Diff

849
src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
View File

File diff suppressed because it is too large Load Diff

146
src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
View File

@@ -24,16 +24,16 @@ subject to the following restrictions:
// Compile time config
//
#define DBVT_BP_PROFILE 0
#define DBVT_BP_PROFILE 0
//#define DBVT_BP_SORTPAIRS 1
#define DBVT_BP_PREVENTFALSEUPDATE 0
#define DBVT_BP_ACCURATESLEEPING 0
#define DBVT_BP_ENABLE_BENCHMARK 0
#define DBVT_BP_PREVENTFALSEUPDATE 0
#define DBVT_BP_ACCURATESLEEPING 0
#define DBVT_BP_ENABLE_BENCHMARK 0
//#define DBVT_BP_MARGIN (btScalar)0.05
extern btScalar gDbvtMargin;
#if DBVT_BP_PROFILE
#define DBVT_BP_PROFILING_RATE 256
#define DBVT_BP_PROFILING_RATE 256
#include "LinearMath/btQuickprof.h"
#endif
@@ -42,90 +42,90 @@ extern btScalar gDbvtMargin;
//
struct btDbvtProxy : btBroadphaseProxy
{
/* Fields */
/* Fields */
//btDbvtAabbMm aabb;
btDbvtNode* leaf;
btDbvtProxy* links[2];
int stage;
/* ctor */
btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask) :
btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
btDbvtNode* leaf;
btDbvtProxy* links[2];
int stage;
/* ctor */
btDbvtProxy(const btVector3& aabbMin, const btVector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask) : btBroadphaseProxy(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask)
{
links[0]=links[1]=0;
links[0] = links[1] = 0;
}
};
typedef btAlignedObjectArray<btDbvtProxy*> btDbvtProxyArray;
typedef btAlignedObjectArray<btDbvtProxy*> btDbvtProxyArray;
///The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see btDbvt).
///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3.
struct btDbvtBroadphase : btBroadphaseInterface
struct btDbvtBroadphase : btBroadphaseInterface
{
/* Config */
enum {
DYNAMIC_SET = 0, /* Dynamic set index */
FIXED_SET = 1, /* Fixed set index */
STAGECOUNT = 2 /* Number of stages */
/* Config */
enum
{
DYNAMIC_SET = 0, /* Dynamic set index */
FIXED_SET = 1, /* Fixed set index */
STAGECOUNT = 2 /* Number of stages */
};
/* Fields */
btDbvt m_sets[2]; // Dbvt sets
btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list
btOverlappingPairCache* m_paircache; // Pair cache
btScalar m_prediction; // Velocity prediction
int m_stageCurrent; // Current stage
int m_fupdates; // % of fixed updates per frame
int m_dupdates; // % of dynamic updates per frame
int m_cupdates; // % of cleanup updates per frame
int m_newpairs; // Number of pairs created
int m_fixedleft; // Fixed optimization left
unsigned m_updates_call; // Number of updates call
unsigned m_updates_done; // Number of updates done
btScalar m_updates_ratio; // m_updates_done/m_updates_call
int m_pid; // Parse id
int m_cid; // Cleanup index
int m_gid; // Gen id
bool m_releasepaircache; // Release pair cache on delete
bool m_deferedcollide; // Defere dynamic/static collision to collide call
bool m_needcleanup; // Need to run cleanup?
btAlignedObjectArray< btAlignedObjectArray<const btDbvtNode*> > m_rayTestStacks;
/* Fields */
btDbvt m_sets[2]; // Dbvt sets
btDbvtProxy* m_stageRoots[STAGECOUNT + 1]; // Stages list
btOverlappingPairCache* m_paircache; // Pair cache
btScalar m_prediction; // Velocity prediction
int m_stageCurrent; // Current stage
int m_fupdates; // % of fixed updates per frame
int m_dupdates; // % of dynamic updates per frame
int m_cupdates; // % of cleanup updates per frame
int m_newpairs; // Number of pairs created
int m_fixedleft; // Fixed optimization left
unsigned m_updates_call; // Number of updates call
unsigned m_updates_done; // Number of updates done
btScalar m_updates_ratio; // m_updates_done/m_updates_call
int m_pid; // Parse id
int m_cid; // Cleanup index
int m_gid; // Gen id
bool m_releasepaircache; // Release pair cache on delete
bool m_deferedcollide; // Defere dynamic/static collision to collide call
bool m_needcleanup; // Need to run cleanup?
btAlignedObjectArray<btAlignedObjectArray<const btDbvtNode*> > m_rayTestStacks;
#if DBVT_BP_PROFILE
btClock m_clock;
struct {
unsigned long m_total;
unsigned long m_ddcollide;
unsigned long m_fdcollide;
unsigned long m_cleanup;
unsigned long m_jobcount;
} m_profiling;
btClock m_clock;
struct
{
unsigned long m_total;
unsigned long m_ddcollide;
unsigned long m_fdcollide;
unsigned long m_cleanup;
unsigned long m_jobcount;
} m_profiling;
#endif
/* Methods */
btDbvtBroadphase(btOverlappingPairCache* paircache=0);
/* Methods */
btDbvtBroadphase(btOverlappingPairCache* paircache = 0);
~btDbvtBroadphase();
void collide(btDispatcher* dispatcher);
void optimize();
void collide(btDispatcher* dispatcher);
void optimize();
/* btBroadphaseInterface Implementation */
btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher);
virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));
virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
virtual btOverlappingPairCache* getOverlappingPairCache();
virtual const btOverlappingPairCache* getOverlappingPairCache() const;
virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
virtual void printStats();
btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher);
virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher);
virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher);
virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0));
virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const;
virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
virtual btOverlappingPairCache* getOverlappingPairCache();
virtual const btOverlappingPairCache* getOverlappingPairCache() const;
virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const;
virtual void printStats();
///reset broadphase internal structures, to ensure determinism/reproducability
virtual void resetPool(btDispatcher* dispatcher);
void performDeferredRemoval(btDispatcher* dispatcher);
void setVelocityPrediction(btScalar prediction)
void performDeferredRemoval(btDispatcher* dispatcher);
void setVelocityPrediction(btScalar prediction)
{
m_prediction = prediction;
}
@@ -134,15 +134,13 @@ struct btDbvtBroadphase : btBroadphaseInterface
return m_prediction;
}
///this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
///this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
///it is not part of the btBroadphaseInterface but specific to btDbvtBroadphase.
///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see
///http://code.google.com/p/bullet/issues/detail?id=223
void setAabbForceUpdate( btBroadphaseProxy* absproxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* /*dispatcher*/);
static void benchmark(btBroadphaseInterface*);
void setAabbForceUpdate(btBroadphaseProxy* absproxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* /*dispatcher*/);
static void benchmark(btBroadphaseInterface*);
};
#endif

2
src/BulletCollision/BroadphaseCollision/btDispatcher.cpp
View File

@@ -17,6 +17,4 @@ subject to the following restrictions:
btDispatcher::~btDispatcher()
{
}

87
src/BulletCollision/BroadphaseCollision/btDispatcher.h
View File

@@ -20,7 +20,7 @@ subject to the following restrictions:
class btCollisionAlgorithm;
struct btBroadphaseProxy;
class btRigidBody;
class btCollisionObject;
class btCollisionObject;
class btOverlappingPairCache;
struct btCollisionObjectWrapper;
@@ -35,35 +35,34 @@ struct btDispatcherInfo
DISPATCH_CONTINUOUS
};
btDispatcherInfo()
:m_timeStep(btScalar(0.)),
m_stepCount(0),
m_dispatchFunc(DISPATCH_DISCRETE),
m_timeOfImpact(btScalar(1.)),
m_useContinuous(true),
m_debugDraw(0),
m_enableSatConvex(false),
m_enableSPU(true),
m_useEpa(true),
m_allowedCcdPenetration(btScalar(0.04)),
m_useConvexConservativeDistanceUtil(false),
m_convexConservativeDistanceThreshold(0.0f),
m_deterministicOverlappingPairs(false)
: m_timeStep(btScalar(0.)),
m_stepCount(0),
m_dispatchFunc(DISPATCH_DISCRETE),
m_timeOfImpact(btScalar(1.)),
m_useContinuous(true),
m_debugDraw(0),
m_enableSatConvex(false),
m_enableSPU(true),
m_useEpa(true),
m_allowedCcdPenetration(btScalar(0.04)),
m_useConvexConservativeDistanceUtil(false),
m_convexConservativeDistanceThreshold(0.0f),
m_deterministicOverlappingPairs(false)
{
}
btScalar m_timeStep;
int m_stepCount;
int m_dispatchFunc;
mutable btScalar m_timeOfImpact;
bool m_useContinuous;
class btIDebugDraw* m_debugDraw;
bool m_enableSatConvex;
bool m_enableSPU;
bool m_useEpa;
btScalar m_allowedCcdPenetration;
bool m_useConvexConservativeDistanceUtil;
btScalar m_convexConservativeDistanceThreshold;
bool m_deterministicOverlappingPairs;
btScalar m_timeStep;
int m_stepCount;
int m_dispatchFunc;
mutable btScalar m_timeOfImpact;
bool m_useContinuous;
class btIDebugDraw* m_debugDraw;
bool m_enableSatConvex;
bool m_enableSPU;
bool m_useEpa;
btScalar m_allowedCcdPenetration;
bool m_useConvexConservativeDistanceUtil;
btScalar m_convexConservativeDistanceThreshold;
bool m_deterministicOverlappingPairs;
};
enum ebtDispatcherQueryType
@@ -76,40 +75,36 @@ enum ebtDispatcherQueryType
///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic).
class btDispatcher
{
public:
virtual ~btDispatcher() ;
virtual ~btDispatcher();
virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType) = 0;
virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType) = 0;
virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1)=0;
virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0, const btCollisionObject* b1) = 0;
virtual void releaseManifold(btPersistentManifold* manifold)=0;
virtual void releaseManifold(btPersistentManifold* manifold) = 0;
virtual void clearManifold(btPersistentManifold* manifold)=0;
virtual void clearManifold(btPersistentManifold* manifold) = 0;
virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1) = 0;
virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1) = 0;
virtual bool needsResponse(const btCollisionObject* body0,const btCollisionObject* body1)=0;
virtual bool needsResponse(const btCollisionObject* body0, const btCollisionObject* body1) = 0;
virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) =0;
virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher) = 0;
virtual int getNumManifolds() const = 0;
virtual btPersistentManifold* getManifoldByIndexInternal(int index) = 0;
virtual btPersistentManifold** getInternalManifoldPointer() = 0;
virtual btPersistentManifold** getInternalManifoldPointer() = 0;
virtual btPoolAllocator* getInternalManifoldPool() = 0;
virtual btPoolAllocator* getInternalManifoldPool() = 0;
virtual const btPoolAllocator* getInternalManifoldPool() const = 0;
virtual const btPoolAllocator* getInternalManifoldPool() const = 0;
virtual void* allocateCollisionAlgorithm(int size) = 0;
virtual void freeCollisionAlgorithm(void* ptr) = 0;
virtual void* allocateCollisionAlgorithm(int size) = 0;
virtual void freeCollisionAlgorithm(void* ptr) = 0;
};
#endif //BT_DISPATCHER_H
#endif //BT_DISPATCHER_H

386
src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
View File

@@ -13,8 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btOverlappingPairCache.h"
#include "btDispatcher.h"
@@ -23,121 +21,95 @@ subject to the following restrictions:
#include <stdio.h>
btHashedOverlappingPairCache::btHashedOverlappingPairCache():
m_overlapFilterCallback(0),
m_ghostPairCallback(0)
btHashedOverlappingPairCache::btHashedOverlappingPairCache() : m_overlapFilterCallback(0),
m_ghostPairCallback(0)
{
int initialAllocatedSize= 2;
int initialAllocatedSize = 2;
m_overlappingPairArray.reserve(initialAllocatedSize);
growTables();
}
btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
{
}
void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher)
{
if (pair.m_algorithm && dispatcher)
{
{
pair.m_algorithm->~btCollisionAlgorithm();
dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
pair.m_algorithm=0;
pair.m_algorithm = 0;
}
}
}
void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
{
class CleanPairCallback : public btOverlapCallback
class CleanPairCallback : public btOverlapCallback
{
btBroadphaseProxy* m_cleanProxy;
btOverlappingPairCache* m_pairCache;
btOverlappingPairCache* m_pairCache;
btDispatcher* m_dispatcher;
public:
CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
:m_cleanProxy(cleanProxy),
m_pairCache(pairCache),
m_dispatcher(dispatcher)
CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher)
: m_cleanProxy(cleanProxy),
m_pairCache(pairCache),
m_dispatcher(dispatcher)
{
}
virtual bool processOverlap(btBroadphasePair& pair)
virtual bool processOverlap(btBroadphasePair& pair)
{
if ((pair.m_pProxy0 == m_cleanProxy) ||
(pair.m_pProxy1 == m_cleanProxy))
{
m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
}
return false;
}
};
CleanPairCallback cleanPairs(proxy,this,dispatcher);
processAllOverlappingPairs(&cleanPairs,dispatcher);
CleanPairCallback cleanPairs(proxy, this, dispatcher);
processAllOverlappingPairs(&cleanPairs, dispatcher);
}
void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
{
class RemovePairCallback : public btOverlapCallback
class RemovePairCallback : public btOverlapCallback
{
btBroadphaseProxy* m_obsoleteProxy;
public:
RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
:m_obsoleteProxy(obsoleteProxy)
: m_obsoleteProxy(obsoleteProxy)
{
}
virtual bool processOverlap(btBroadphasePair& pair)
virtual bool processOverlap(btBroadphasePair& pair)
{
return ((pair.m_pProxy0 == m_obsoleteProxy) ||
(pair.m_pProxy1 == m_obsoleteProxy));
(pair.m_pProxy1 == m_obsoleteProxy));
}
};
RemovePairCallback removeCallback(proxy);
processAllOverlappingPairs(&removeCallback,dispatcher);
processAllOverlappingPairs(&removeCallback, dispatcher);
}
btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
{
if(proxy0->m_uniqueId>proxy1->m_uniqueId)
btSwap(proxy0,proxy1);
if (proxy0->m_uniqueId > proxy1->m_uniqueId)
btSwap(proxy0, proxy1);
int proxyId1 = proxy0->getUid();
int proxyId2 = proxy1->getUid();
/*if (proxyId1 > proxyId2)
btSwap(proxyId1, proxyId2);*/
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
if (hash >= m_hashTable.size())
{
@@ -162,9 +134,8 @@ btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* prox
//#include <stdio.h>
void btHashedOverlappingPairCache::growTables()
void btHashedOverlappingPairCache::growTables()
{
int newCapacity = m_overlappingPairArray.capacity();
if (m_hashTable.size() < newCapacity)
@@ -175,10 +146,9 @@ void btHashedOverlappingPairCache::growTables()
m_hashTable.resize(newCapacity);
m_next.resize(newCapacity);
int i;
for (i= 0; i < newCapacity; ++i)
for (i = 0; i < newCapacity; ++i)
{
m_hashTable[i] = BT_NULL_PAIR;
}
@@ -187,35 +157,31 @@ void btHashedOverlappingPairCache::growTables()
m_next[i] = BT_NULL_PAIR;
}
for(i=0;i<curHashtableSize;i++)
for (i = 0; i < curHashtableSize; i++)
{
const btBroadphasePair& pair = m_overlappingPairArray[i];
int proxyId1 = pair.m_pProxy0->getUid();
int proxyId2 = pair.m_pProxy1->getUid();
/*if (proxyId1 > proxyId2)
btSwap(proxyId1, proxyId2);*/
int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask
int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask
m_next[i] = m_hashTable[hashValue];
m_hashTable[hashValue] = i;
}
}
}
btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
{
if(proxy0->m_uniqueId>proxy1->m_uniqueId)
btSwap(proxy0,proxy1);
if (proxy0->m_uniqueId > proxy1->m_uniqueId)
btSwap(proxy0, proxy1);
int proxyId1 = proxy0->getUid();
int proxyId2 = proxy1->getUid();
/*if (proxyId1 > proxyId2)
btSwap(proxyId1, proxyId2);*/
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask
btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
if (pair != NULL)
@@ -237,7 +203,7 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx
//this is where we add an actual pair, so also call the 'ghost'
if (m_ghostPairCallback)
m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
int newCapacity = m_overlappingPairArray.capacity();
@@ -245,15 +211,14 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx
{
growTables();
//hash with new capacity
hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
}
pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
// pair->m_pProxy0 = proxy0;
// pair->m_pProxy1 = proxy1;
pair = new (mem) btBroadphasePair(*proxy0, *proxy1);
// pair->m_pProxy0 = proxy0;
// pair->m_pProxy1 = proxy1;
pair->m_algorithm = 0;
pair->m_internalTmpValue = 0;
m_next[count] = m_hashTable[hash];
m_hashTable[hash] = count;
@@ -261,19 +226,17 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx
return pair;
}
void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher)
{
if(proxy0->m_uniqueId>proxy1->m_uniqueId)
btSwap(proxy0,proxy1);
if (proxy0->m_uniqueId > proxy1->m_uniqueId)
btSwap(proxy0, proxy1);
int proxyId1 = proxy0->getUid();
int proxyId2 = proxy1->getUid();
/*if (proxyId1 > proxyId2)
btSwap(proxyId1, proxyId2);*/
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
if (pair == NULL)
@@ -281,7 +244,7 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
return 0;
}
cleanOverlappingPair(*pair,dispatcher);
cleanOverlappingPair(*pair, dispatcher);
void* userData = pair->m_internalInfo1;
@@ -319,7 +282,7 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
int lastPairIndex = m_overlappingPairArray.size() - 1;
if (m_ghostPairCallback)
m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher);
// If the removed pair is the last pair, we are done.
if (lastPairIndex == pairIndex)
@@ -330,8 +293,8 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
// Remove the last pair from the hash table.
const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
/* missing swap here too, Nat. */
int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_pProxy0->getUid()), static_cast<unsigned int>(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity()-1));
/* missing swap here too, Nat. */
int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_pProxy0->getUid()), static_cast<unsigned int>(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity() - 1));
index = m_hashTable[lastHash];
btAssert(index != BT_NULL_PAIR);
@@ -366,20 +329,20 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
}
//#include <stdio.h>
#include "LinearMath/btQuickprof.h"
void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher)
{
BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
int i;
// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
for (i=0;i<m_overlappingPairArray.size();)
// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
for (i = 0; i < m_overlappingPairArray.size();)
{
btBroadphasePair* pair = &m_overlappingPairArray[i];
if (callback->processOverlap(*pair))
{
removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
} else
removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher);
}
else
{
i++;
}
@@ -388,83 +351,83 @@ void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback*
struct MyPairIndex
{
int m_orgIndex;
int m_uidA0;
int m_uidA1;
int m_orgIndex;
int m_uidA0;
int m_uidA1;
};
class MyPairIndeSortPredicate
{
public:
bool operator() ( const MyPairIndex& a, const MyPairIndex& b ) const
{
const int uidA0 = a.m_uidA0;
const int uidB0 = b.m_uidA0;
const int uidA1 = a.m_uidA1;
const int uidB1 = b.m_uidA1;
return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
}
bool operator()(const MyPairIndex& a, const MyPairIndex& b) const
{
const int uidA0 = a.m_uidA0;
const int uidB0 = b.m_uidA0;
const int uidA1 = a.m_uidA1;
const int uidB1 = b.m_uidA1;
return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
}
};
void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
{
if (dispatchInfo.m_deterministicOverlappingPairs)
{
btBroadphasePairArray& pa = getOverlappingPairArray();
btAlignedObjectArray<MyPairIndex> indices;
{
BT_PROFILE("sortOverlappingPairs");
indices.resize(pa.size());
for (int i=0;i<indices.size();i++)
{
const btBroadphasePair& p = pa[i];
const int uidA0 = p.m_pProxy0 ? p.m_pProxy0->m_uniqueId : -1;
const int uidA1 = p.m_pProxy1 ? p.m_pProxy1->m_uniqueId : -1;
indices[i].m_uidA0 = uidA0;
indices[i].m_uidA1 = uidA1;
indices[i].m_orgIndex = i;
}
indices.quickSort(MyPairIndeSortPredicate());
}
{
BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
int i;
for (i=0;i<indices.size();)
{
btBroadphasePair* pair = &pa[indices[i].m_orgIndex];
if (callback->processOverlap(*pair))
{
removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
} else
{
i++;
}
}
}
} else
{
processAllOverlappingPairs(callback, dispatcher);
}
if (dispatchInfo.m_deterministicOverlappingPairs)
{
btBroadphasePairArray& pa = getOverlappingPairArray();
btAlignedObjectArray<MyPairIndex> indices;
{
BT_PROFILE("sortOverlappingPairs");
indices.resize(pa.size());
for (int i = 0; i < indices.size(); i++)
{
const btBroadphasePair& p = pa[i];
const int uidA0 = p.m_pProxy0 ? p.m_pProxy0->m_uniqueId : -1;
const int uidA1 = p.m_pProxy1 ? p.m_pProxy1->m_uniqueId : -1;
indices[i].m_uidA0 = uidA0;
indices[i].m_uidA1 = uidA1;
indices[i].m_orgIndex = i;
}
indices.quickSort(MyPairIndeSortPredicate());
}
{
BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
int i;
for (i = 0; i < indices.size();)
{
btBroadphasePair* pair = &pa[indices[i].m_orgIndex];
if (callback->processOverlap(*pair))
{
removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher);
}
else
{
i++;
}
}
}
}
else
{
processAllOverlappingPairs(callback, dispatcher);
}
}
void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
{
///need to keep hashmap in sync with pair address, so rebuild all
btBroadphasePairArray tmpPairs;
int i;
for (i=0;i<m_overlappingPairArray.size();i++)
for (i = 0; i < m_overlappingPairArray.size(); i++)
{
tmpPairs.push_back(m_overlappingPairArray[i]);
}
for (i=0;i<tmpPairs.size();i++)
for (i = 0; i < tmpPairs.size(); i++)
{
removeOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1,dispatcher);
removeOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1, dispatcher);
}
for (i = 0; i < m_next.size(); i++)
{
m_next[i] = BT_NULL_PAIR;
@@ -472,31 +435,28 @@ void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher
tmpPairs.quickSort(btBroadphasePairSortPredicate());
for (i=0;i<tmpPairs.size();i++)
for (i = 0; i < tmpPairs.size(); i++)
{
addOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1);
addOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1);
}
}
void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher )
void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher)
{
if (!hasDeferredRemoval())
{
btBroadphasePair findPair(*proxy0,*proxy1);
btBroadphasePair findPair(*proxy0, *proxy1);
int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
if (findIndex < m_overlappingPairArray.size())
{
btBroadphasePair& pair = m_overlappingPairArray[findIndex];
void* userData = pair.m_internalInfo1;
cleanOverlappingPair(pair,dispatcher);
cleanOverlappingPair(pair, dispatcher);
if (m_ghostPairCallback)
m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1);
m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher);
m_overlappingPairArray.swap(findIndex, m_overlappingPairArray.capacity() - 1);
m_overlappingPairArray.pop_back();
return userData;
}
@@ -505,95 +465,73 @@ void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
return 0;
}
btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
{
//don't add overlap with own
btAssert(proxy0 != proxy1);
if (!needsBroadphaseCollision(proxy0,proxy1))
if (!needsBroadphaseCollision(proxy0, proxy1))
return 0;
void* mem = &m_overlappingPairArray.expandNonInitializing();
btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
if (m_ghostPairCallback)
void* mem = &m_overlappingPairArray.expandNonInitializing();
btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0, *proxy1);
if (m_ghostPairCallback)
m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
return pair;
}
///this findPair becomes really slow. Either sort the list to speedup the query, or
///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
{
if (!needsBroadphaseCollision(proxy0,proxy1))
if (!needsBroadphaseCollision(proxy0, proxy1))
return 0;
btBroadphasePair tmpPair(*proxy0,*proxy1);
btBroadphasePair tmpPair(*proxy0, *proxy1);
int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
if (findIndex < m_overlappingPairArray.size())
{
//btAssert(it != m_overlappingPairSet.end());
btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
return pair;
}
return 0;
}
//#include <stdio.h>
void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher)
{
int i;
for (i=0;i<m_overlappingPairArray.size();)
for (i = 0; i < m_overlappingPairArray.size();)
{
btBroadphasePair* pair = &m_overlappingPairArray[i];
if (callback->processOverlap(*pair))
{
cleanOverlappingPair(*pair,dispatcher);
cleanOverlappingPair(*pair, dispatcher);
pair->m_pProxy0 = 0;
pair->m_pProxy1 = 0;
m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1);
m_overlappingPairArray.pop_back();
} else
}
else
{
i++;
}
}
}
btSortedOverlappingPairCache::btSortedOverlappingPairCache():
m_blockedForChanges(false),
m_hasDeferredRemoval(true),
m_overlapFilterCallback(0),
m_ghostPairCallback(0)
btSortedOverlappingPairCache::btSortedOverlappingPairCache() : m_blockedForChanges(false),
m_hasDeferredRemoval(true),
m_overlapFilterCallback(0),
m_ghostPairCallback(0)
{
int initialAllocatedSize= 2;
int initialAllocatedSize = 2;
m_overlappingPairArray.reserve(initialAllocatedSize);
}
@@ -601,81 +539,73 @@ btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
{
}
void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher)
{
if (pair.m_algorithm)
{
{
pair.m_algorithm->~btCollisionAlgorithm();
dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
pair.m_algorithm=0;
pair.m_algorithm = 0;
}
}
}
void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
{
class CleanPairCallback : public btOverlapCallback
class CleanPairCallback : public btOverlapCallback
{
btBroadphaseProxy* m_cleanProxy;
btOverlappingPairCache* m_pairCache;
btOverlappingPairCache* m_pairCache;
btDispatcher* m_dispatcher;
public:
CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
:m_cleanProxy(cleanProxy),
m_pairCache(pairCache),
m_dispatcher(dispatcher)
CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher)
: m_cleanProxy(cleanProxy),
m_pairCache(pairCache),
m_dispatcher(dispatcher)
{
}
virtual bool processOverlap(btBroadphasePair& pair)
virtual bool processOverlap(btBroadphasePair& pair)
{
if ((pair.m_pProxy0 == m_cleanProxy) ||
(pair.m_pProxy1 == m_cleanProxy))
{
m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
}
return false;
}
};
CleanPairCallback cleanPairs(proxy,this,dispatcher);
processAllOverlappingPairs(&cleanPairs,dispatcher);
CleanPairCallback cleanPairs(proxy, this, dispatcher);
processAllOverlappingPairs(&cleanPairs, dispatcher);
}
void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
{
class RemovePairCallback : public btOverlapCallback
class RemovePairCallback : public btOverlapCallback
{
btBroadphaseProxy* m_obsoleteProxy;
public:
RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
:m_obsoleteProxy(obsoleteProxy)
: m_obsoleteProxy(obsoleteProxy)
{
}
virtual bool processOverlap(btBroadphasePair& pair)
virtual bool processOverlap(btBroadphasePair& pair)
{
return ((pair.m_pProxy0 == m_obsoleteProxy) ||
(pair.m_pProxy1 == m_obsoleteProxy));
(pair.m_pProxy1 == m_obsoleteProxy));
}
};
RemovePairCallback removeCallback(proxy);
processAllOverlappingPairs(&removeCallback,dispatcher);
processAllOverlappingPairs(&removeCallback, dispatcher);
}
void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
{
//should already be sorted
}

358
src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
View File

@@ -16,7 +16,6 @@ subject to the following restrictions:
#ifndef BT_OVERLAPPING_PAIR_CACHE_H
#define BT_OVERLAPPING_PAIR_CACHE_H
#include "btBroadphaseInterface.h"
#include "btBroadphaseProxy.h"
#include "btOverlappingPairCallback.h"
@@ -24,177 +23,163 @@ subject to the following restrictions:
#include "LinearMath/btAlignedObjectArray.h"
class btDispatcher;
typedef btAlignedObjectArray<btBroadphasePair> btBroadphasePairArray;
typedef btAlignedObjectArray<btBroadphasePair> btBroadphasePairArray;
struct btOverlapCallback
struct btOverlapCallback
{
virtual ~btOverlapCallback()
{}
{
}
//return true for deletion of the pair
virtual bool processOverlap(btBroadphasePair& pair) = 0;
virtual bool processOverlap(btBroadphasePair& pair) = 0;
};
struct btOverlapFilterCallback
{
virtual ~btOverlapFilterCallback()
{}
{
}
// return true when pairs need collision
virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0;
virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const = 0;
};
const int BT_NULL_PAIR=0xffffffff;
const int BT_NULL_PAIR = 0xffffffff;
///The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases.
///The btHashedOverlappingPairCache and btSortedOverlappingPairCache classes are two implementations.
class btOverlappingPairCache : public btOverlappingPairCallback
{
public:
virtual ~btOverlappingPairCache() {} // this is needed so we can get to the derived class destructor
virtual ~btOverlappingPairCache() {} // this is needed so we can get to the derived class destructor
virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0;
virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0;
virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0;
virtual btBroadphasePairArray& getOverlappingPairArray() = 0;
virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0;
virtual void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) = 0;
virtual btBroadphasePairArray& getOverlappingPairArray() = 0;
virtual void cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher) = 0;
virtual int getNumOverlappingPairs() const = 0;
virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) = 0;
virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher) = 0;
virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0;
virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0;
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0;
virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher* dispatcher) = 0;
virtual void processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
virtual void processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
{
processAllOverlappingPairs(callback, dispatcher);
}
virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0;
virtual bool hasDeferredRemoval() = 0;
virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0;
virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 0;
virtual bool hasDeferredRemoval() = 0;
virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) = 0;
virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 0;
};
/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
ATTRIBUTE_ALIGNED16(class) btHashedOverlappingPairCache : public btOverlappingPairCache
ATTRIBUTE_ALIGNED16(class)
btHashedOverlappingPairCache : public btOverlappingPairCache
{
btBroadphasePairArray m_overlappingPairArray;
btBroadphasePairArray m_overlappingPairArray;
btOverlapFilterCallback* m_overlapFilterCallback;
protected:
btAlignedObjectArray<int> m_hashTable;
btAlignedObjectArray<int> m_next;
btOverlappingPairCallback* m_ghostPairCallback;
btAlignedObjectArray<int> m_hashTable;
btAlignedObjectArray<int> m_next;
btOverlappingPairCallback* m_ghostPairCallback;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btHashedOverlappingPairCache();
virtual ~btHashedOverlappingPairCache();
void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
void removeOverlappingPairsContainingProxy(btBroadphaseProxy * proxy, btDispatcher * dispatcher);
virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
virtual void* removeOverlappingPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1, btDispatcher * dispatcher);
SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1) const
{
if (m_overlapFilterCallback)
return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
return collides;
}
// Add a pair and return the new pair. If the pair already exists,
// no new pair is created and the old one is returned.
virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1)
{
if (!needsBroadphaseCollision(proxy0,proxy1))
if (!needsBroadphaseCollision(proxy0, proxy1))
return 0;
return internalAddPair(proxy0,proxy1);
return internalAddPair(proxy0, proxy1);
}
void cleanProxyFromPairs(btBroadphaseProxy * proxy, btDispatcher * dispatcher);
void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher * dispatcher);
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
virtual void processAllOverlappingPairs(btOverlapCallback * callback, btDispatcher * dispatcher, const struct btDispatcherInfo& dispatchInfo);
virtual void processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo);
virtual btBroadphasePair* getOverlappingPairArrayPtr()
virtual btBroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
}
const btBroadphasePair* getOverlappingPairArrayPtr() const
const btBroadphasePair* getOverlappingPairArrayPtr() const
{
return &m_overlappingPairArray[0];
}
btBroadphasePairArray& getOverlappingPairArray()
btBroadphasePairArray& getOverlappingPairArray()
{
return m_overlappingPairArray;
}
const btBroadphasePairArray& getOverlappingPairArray() const
const btBroadphasePairArray& getOverlappingPairArray() const
{
return m_overlappingPairArray;
}
void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
void cleanOverlappingPair(btBroadphasePair & pair, btDispatcher * dispatcher);
btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
btBroadphasePair* findPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1);
int GetCount() const { return m_overlappingPairArray.size(); }
// btBroadphasePair* GetPairs() { return m_pairs; }
// btBroadphasePair* GetPairs() { return m_pairs; }
btOverlapFilterCallback* getOverlapFilterCallback()
{
return m_overlapFilterCallback;
}
void setOverlapFilterCallback(btOverlapFilterCallback* callback)
void setOverlapFilterCallback(btOverlapFilterCallback * callback)
{
m_overlapFilterCallback = callback;
}
int getNumOverlappingPairs() const
int getNumOverlappingPairs() const
{
return m_overlappingPairArray.size();
}
private:
btBroadphasePair* internalAddPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
void growTables();
private:
btBroadphasePair* internalAddPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1);
void growTables();
SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2)
{
{
return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2;
}
@@ -214,40 +199,37 @@ private:
}
*/
SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
{
unsigned int key = proxyId1 | (proxyId2 << 16);
// Thomas Wang's hash
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
key ^= (key >> 16);
return key;
}
SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash)
SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1, int hash)
{
int proxyId1 = proxy0->getUid();
int proxyId2 = proxy1->getUid();
#if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
#if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
if (proxyId1 > proxyId2)
btSwap(proxyId1, proxyId2);
#endif
#endif
int index = m_hashTable[hash];
while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
{
index = m_next[index];
}
if ( index == BT_NULL_PAIR )
if (index == BT_NULL_PAIR)
{
return NULL;
}
@@ -257,155 +239,136 @@ private:
return &m_overlappingPairArray[index];
}
virtual bool hasDeferredRemoval()
virtual bool hasDeferredRemoval()
{
return false;
}
virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
virtual void setInternalGhostPairCallback(btOverlappingPairCallback * ghostPairCallback)
{
m_ghostPairCallback = ghostPairCallback;
}
virtual void sortOverlappingPairs(btDispatcher* dispatcher);
virtual void sortOverlappingPairs(btDispatcher * dispatcher);
};
///btSortedOverlappingPairCache maintains the objects with overlapping AABB
///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase
class btSortedOverlappingPairCache : public btOverlappingPairCache
class btSortedOverlappingPairCache : public btOverlappingPairCache
{
protected:
//avoid brute-force finding all the time
btBroadphasePairArray m_overlappingPairArray;
protected:
//avoid brute-force finding all the time
btBroadphasePairArray m_overlappingPairArray;
//during the dispatch, check that user doesn't destroy/create proxy
bool m_blockedForChanges;
//during the dispatch, check that user doesn't destroy/create proxy
bool m_blockedForChanges;
///by default, do the removal during the pair traversal
bool m_hasDeferredRemoval;
//if set, use the callback instead of the built in filter in needBroadphaseCollision
btOverlapFilterCallback* m_overlapFilterCallback;
///by default, do the removal during the pair traversal
bool m_hasDeferredRemoval;
btOverlappingPairCallback* m_ghostPairCallback;
//if set, use the callback instead of the built in filter in needBroadphaseCollision
btOverlapFilterCallback* m_overlapFilterCallback;
public:
btSortedOverlappingPairCache();
virtual ~btSortedOverlappingPairCache();
btOverlappingPairCallback* m_ghostPairCallback;
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
public:
btSortedOverlappingPairCache();
virtual ~btSortedOverlappingPairCache();
void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher* dispatcher);
void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
void* removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher);
btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
void cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher);
void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
{
if (m_overlapFilterCallback)
return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
void cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher);
bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
return collides;
}
btBroadphasePairArray& getOverlappingPairArray()
{
return m_overlappingPairArray;
}
void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher);
const btBroadphasePairArray& getOverlappingPairArray() const
{
return m_overlappingPairArray;
}
inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const
{
if (m_overlapFilterCallback)
return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
return collides;
}
btBroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
}
btBroadphasePairArray& getOverlappingPairArray()
{
return m_overlappingPairArray;
}
const btBroadphasePair* getOverlappingPairArrayPtr() const
{
return &m_overlappingPairArray[0];
}
const btBroadphasePairArray& getOverlappingPairArray() const
{
return m_overlappingPairArray;
}
int getNumOverlappingPairs() const
{
return m_overlappingPairArray.size();
}
btOverlapFilterCallback* getOverlapFilterCallback()
{
return m_overlapFilterCallback;
}
btBroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
}
void setOverlapFilterCallback(btOverlapFilterCallback* callback)
{
m_overlapFilterCallback = callback;
}
const btBroadphasePair* getOverlappingPairArrayPtr() const
{
return &m_overlappingPairArray[0];
}
virtual bool hasDeferredRemoval()
{
return m_hasDeferredRemoval;
}
int getNumOverlappingPairs() const
{
return m_overlappingPairArray.size();
}
virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
{
m_ghostPairCallback = ghostPairCallback;
}
btOverlapFilterCallback* getOverlapFilterCallback()
{
return m_overlapFilterCallback;
}
virtual void sortOverlappingPairs(btDispatcher* dispatcher);
void setOverlapFilterCallback(btOverlapFilterCallback* callback)
{
m_overlapFilterCallback = callback;
}
virtual bool hasDeferredRemoval()
{
return m_hasDeferredRemoval;
}
virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
{
m_ghostPairCallback = ghostPairCallback;
}
virtual void sortOverlappingPairs(btDispatcher* dispatcher);
};
///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
class btNullPairCache : public btOverlappingPairCache
{
btBroadphasePairArray m_overlappingPairArray;
btBroadphasePairArray m_overlappingPairArray;
public:
virtual btBroadphasePair* getOverlappingPairArrayPtr()
virtual btBroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
}
const btBroadphasePair* getOverlappingPairArrayPtr() const
const btBroadphasePair* getOverlappingPairArrayPtr() const
{
return &m_overlappingPairArray[0];
}
btBroadphasePairArray& getOverlappingPairArray()
btBroadphasePairArray& getOverlappingPairArray()
{
return m_overlappingPairArray;
}
virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/)
{
virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/, btDispatcher* /*dispatcher*/)
{
}
virtual int getNumOverlappingPairs() const
@@ -413,16 +376,15 @@ public:
return 0;
}
virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
{
}
virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/)
virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/, btDispatcher* /*dispatcher*/)
{
}
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/)
virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/)
{
}
virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher* /*dispatcher*/)
{
}
@@ -431,39 +393,33 @@ public:
return 0;
}
virtual bool hasDeferredRemoval()
virtual bool hasDeferredRemoval()
{
return true;
}
virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */)
virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */)
{
}
virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/)
virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/)
{
return 0;
}
virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/,btDispatcher* /*dispatcher*/)
virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/, btDispatcher* /*dispatcher*/)
{
return 0;
}
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/)
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/, btDispatcher* /*dispatcher*/)
{
}
virtual void sortOverlappingPairs(btDispatcher* dispatcher)
virtual void sortOverlappingPairs(btDispatcher* dispatcher)
{
(void) dispatcher;
(void)dispatcher;
}
};
#endif //BT_OVERLAPPING_PAIR_CACHE_H
#endif //BT_OVERLAPPING_PAIR_CACHE_H

16
src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h
View File

@@ -18,26 +18,24 @@ subject to the following restrictions:
#define OVERLAPPING_PAIR_CALLBACK_H
class btDispatcher;
struct btBroadphasePair;
struct btBroadphasePair;
///The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache.
class btOverlappingPairCallback
{
protected:
btOverlappingPairCallback() {}
btOverlappingPairCallback() {}
public:
virtual ~btOverlappingPairCallback()
{
}
virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) = 0;
virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) = 0;
virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0;
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) = 0;
virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher) = 0;
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0, btDispatcher* dispatcher) = 0;
};
#endif //OVERLAPPING_PAIR_CALLBACK_H
#endif //OVERLAPPING_PAIR_CALLBACK_H

600
src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
View File

File diff suppressed because it is too large Load Diff

338
src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h
View File

@@ -22,11 +22,11 @@ class btSerializer;
#ifdef DEBUG_CHECK_DEQUANTIZATION
#ifdef __SPU__
#define printf spu_printf
#endif //__SPU__
#endif //__SPU__
#include <stdio.h>
#include <stdlib.h>
#endif //DEBUG_CHECK_DEQUANTIZATION
#endif //DEBUG_CHECK_DEQUANTIZATION
#include "LinearMath/btVector3.h"
#include "LinearMath/btAlignedAllocator.h"
@@ -41,13 +41,10 @@ class btSerializer;
#define btQuantizedBvhDataName "btQuantizedBvhFloatData"
#endif
//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
//Note: currently we have 16 bytes per quantized node
#define MAX_SUBTREE_SIZE_IN_BYTES 2048
#define MAX_SUBTREE_SIZE_IN_BYTES 2048
// 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one
// actually) triangles each (since the sign bit is reserved
@@ -55,15 +52,16 @@ class btSerializer;
///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode
ATTRIBUTE_ALIGNED16(struct)
btQuantizedBvhNode
{
BT_DECLARE_ALIGNED_ALLOCATOR();
//12 bytes
unsigned short int m_quantizedAabbMin[3];
unsigned short int m_quantizedAabbMax[3];
unsigned short int m_quantizedAabbMin[3];
unsigned short int m_quantizedAabbMax[3];
//4 bytes
int m_escapeIndexOrTriangleIndex;
int m_escapeIndexOrTriangleIndex;
bool isLeafNode() const
{
@@ -75,68 +73,67 @@ ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode
btAssert(!isLeafNode());
return -m_escapeIndexOrTriangleIndex;
}
int getTriangleIndex() const
int getTriangleIndex() const
{
btAssert(isLeafNode());
unsigned int x=0;
unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);
unsigned int x = 0;
unsigned int y = (~(x & 0)) << (31 - MAX_NUM_PARTS_IN_BITS);
// Get only the lower bits where the triangle index is stored
return (m_escapeIndexOrTriangleIndex&~(y));
return (m_escapeIndexOrTriangleIndex & ~(y));
}
int getPartId() const
int getPartId() const
{
btAssert(isLeafNode());
// Get only the highest bits where the part index is stored
return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
return (m_escapeIndexOrTriangleIndex >> (31 - MAX_NUM_PARTS_IN_BITS));
}
}
;
};
/// btOptimizedBvhNode contains both internal and leaf node information.
/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes.
ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode
ATTRIBUTE_ALIGNED16(struct)
btOptimizedBvhNode
{
BT_DECLARE_ALIGNED_ALLOCATOR();
//32 bytes
btVector3 m_aabbMinOrg;
btVector3 m_aabbMaxOrg;
btVector3 m_aabbMinOrg;
btVector3 m_aabbMaxOrg;
//4
int m_escapeIndex;
int m_escapeIndex;
//8
//for child nodes
int m_subPart;
int m_triangleIndex;
int m_subPart;
int m_triangleIndex;
//pad the size to 64 bytes
char m_padding[20];
//pad the size to 64 bytes
char m_padding[20];
};
///btBvhSubtreeInfo provides info to gather a subtree of limited size
ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo
ATTRIBUTE_ALIGNED16(class)
btBvhSubtreeInfo
{
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
//12 bytes
unsigned short int m_quantizedAabbMin[3];
unsigned short int m_quantizedAabbMax[3];
unsigned short int m_quantizedAabbMin[3];
unsigned short int m_quantizedAabbMax[3];
//4 bytes, points to the root of the subtree
int m_rootNodeIndex;
int m_rootNodeIndex;
//4 bytes
int m_subtreeSize;
int m_padding[3];
int m_subtreeSize;
int m_padding[3];
btBvhSubtreeInfo()
{
//memset(&m_padding[0], 0, sizeof(m_padding));
}
void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode)
void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode)
{
m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0];
m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1];
@@ -145,14 +142,12 @@ public:
m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1];
m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2];
}
}
;
};
class btNodeOverlapCallback
{
public:
virtual ~btNodeOverlapCallback() {};
virtual ~btNodeOverlapCallback(){};
virtual void processNode(int subPart, int triangleIndex) = 0;
};
@@ -160,18 +155,16 @@ public:
#include "LinearMath/btAlignedAllocator.h"
#include "LinearMath/btAlignedObjectArray.h"
///for code readability:
typedef btAlignedObjectArray<btOptimizedBvhNode> NodeArray;
typedef btAlignedObjectArray<btQuantizedBvhNode> QuantizedNodeArray;
typedef btAlignedObjectArray<btBvhSubtreeInfo> BvhSubtreeInfoArray;
typedef btAlignedObjectArray<btOptimizedBvhNode> NodeArray;
typedef btAlignedObjectArray<btQuantizedBvhNode> QuantizedNodeArray;
typedef btAlignedObjectArray<btBvhSubtreeInfo> BvhSubtreeInfoArray;
///The btQuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU.
///It is used by the btBvhTriangleMeshShape as midphase.
///It is recommended to use quantization for better performance and lower memory requirements.
ATTRIBUTE_ALIGNED16(class) btQuantizedBvh
ATTRIBUTE_ALIGNED16(class)
btQuantizedBvh
{
public:
enum btTraversalMode
@@ -182,54 +175,47 @@ public:
};
protected:
btVector3 m_bvhAabbMin;
btVector3 m_bvhAabbMax;
btVector3 m_bvhQuantization;
int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess.
btVector3 m_bvhAabbMin;
btVector3 m_bvhAabbMax;
btVector3 m_bvhQuantization;
int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess.
int m_curNodeIndex;
int m_curNodeIndex;
//quantization data
bool m_useQuantization;
bool m_useQuantization;
NodeArray m_leafNodes;
NodeArray m_contiguousNodes;
QuantizedNodeArray m_quantizedLeafNodes;
QuantizedNodeArray m_quantizedContiguousNodes;
NodeArray m_leafNodes;
NodeArray m_contiguousNodes;
QuantizedNodeArray m_quantizedLeafNodes;
QuantizedNodeArray m_quantizedContiguousNodes;
btTraversalMode m_traversalMode;
BvhSubtreeInfoArray m_SubtreeHeaders;
btTraversalMode m_traversalMode;
BvhSubtreeInfoArray m_SubtreeHeaders;
//This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray
mutable int m_subtreeHeaderCount;
///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!)
///this might be refactored into a virtual, it is usually not calculated at run-time
void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin)
void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin)
{
if (m_useQuantization)
{
quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0);
} else
quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0], aabbMin, 0);
}
else
{
m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin;
}
}
void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax)
void setInternalNodeAabbMax(int nodeIndex, const btVector3& aabbMax)
{
if (m_useQuantization)
{
quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1);
} else
quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0], aabbMax, 1);
}
else
{
m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax;
}
@@ -243,115 +229,102 @@ protected:
}
//non-quantized
return m_leafNodes[nodeIndex].m_aabbMinOrg;
}
btVector3 getAabbMax(int nodeIndex) const
{
if (m_useQuantization)
{
return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]);
}
}
//non-quantized
return m_leafNodes[nodeIndex].m_aabbMaxOrg;
}
void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
{
if (m_useQuantization)
{
m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex;
}
}
else
{
m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex;
}
}
void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax)
void mergeInternalNodeAabb(int nodeIndex, const btVector3& newAabbMin, const btVector3& newAabbMax)
{
if (m_useQuantization)
{
unsigned short int quantizedAabbMin[3];
unsigned short int quantizedAabbMax[3];
quantize(quantizedAabbMin,newAabbMin,0);
quantize(quantizedAabbMax,newAabbMax,1);
for (int i=0;i<3;i++)
quantize(quantizedAabbMin, newAabbMin, 0);
quantize(quantizedAabbMax, newAabbMax, 1);
for (int i = 0; i < 3; i++)
{
if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i])
m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i];
if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i])
m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i];
}
} else
}
else
{
//non-quantized
m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin);
m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);
m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);
}
}
void swapLeafNodes(int firstIndex,int secondIndex);
void swapLeafNodes(int firstIndex, int secondIndex);
void assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex);
void assignInternalNodeFromLeafNode(int internalNode, int leafNodeIndex);
protected:
void buildTree(int startIndex, int endIndex);
int calcSplittingAxis(int startIndex, int endIndex);
void buildTree (int startIndex,int endIndex);
int sortAndCalcSplittingIndex(int startIndex, int endIndex, int splitAxis);
int calcSplittingAxis(int startIndex,int endIndex);
void walkStacklessTree(btNodeOverlapCallback * nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const;
int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis);
void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
void walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const;
void walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback * nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const;
void walkStacklessQuantizedTree(btNodeOverlapCallback * nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax, int startNodeIndex, int endNodeIndex) const;
void walkStacklessTreeAgainstRay(btNodeOverlapCallback * nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const;
///tree traversal designed for small-memory processors like PS3 SPU
void walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
void walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback * nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax) const;
///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
void walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
void walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode, btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax) const;
///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const;
void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA, const btQuantizedBvhNode* treeNodeB, btNodeOverlapCallback* nodeCallback) const;
void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex);
void updateSubtreeHeaders(int leftChildNodexIndex, int rightChildNodexIndex);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btQuantizedBvh();
virtual ~btQuantizedBvh();
///***************************************** expert/internal use only *************************
void setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0));
QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; }
void setQuantizationValues(const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, btScalar quantizationMargin = btScalar(1.0));
QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; }
///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized
void buildInternal();
void buildInternal();
///***************************************** expert/internal use only *************************
void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
void reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const;
void reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const;
void reportAabbOverlappingNodex(btNodeOverlapCallback * nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const;
void reportRayOverlappingNodex(btNodeOverlapCallback * nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const;
void reportBoxCastOverlappingNodex(btNodeOverlapCallback * nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) const;
SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const
SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point, int isMax) const
{
btAssert(m_useQuantization);
btAssert(point.getX() <= m_bvhAabbMax.getX());
@@ -368,16 +341,16 @@ public:
///@todo: double-check this
if (isMax)
{
out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1));
out[1] = (unsigned short) (((unsigned short)(v.getY()+btScalar(1.)) | 1));
out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(1.)) | 1));
} else
{
out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe));
out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe));
out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe));
out[0] = (unsigned short)(((unsigned short)(v.getX() + btScalar(1.)) | 1));
out[1] = (unsigned short)(((unsigned short)(v.getY() + btScalar(1.)) | 1));
out[2] = (unsigned short)(((unsigned short)(v.getZ() + btScalar(1.)) | 1));
}
else
{
out[0] = (unsigned short)(((unsigned short)(v.getX()) & 0xfffe));
out[1] = (unsigned short)(((unsigned short)(v.getY()) & 0xfffe));
out[2] = (unsigned short)(((unsigned short)(v.getZ()) & 0xfffe));
}
#ifdef DEBUG_CHECK_DEQUANTIZATION
btVector3 newPoint = unQuantize(out);
@@ -385,105 +358,97 @@ public:
{
if (newPoint.getX() < point.getX())
{
printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n", newPoint.getX() - point.getX(), newPoint.getX(), point.getX());
}
if (newPoint.getY() < point.getY())
{
printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n", newPoint.getY() - point.getY(), newPoint.getY(), point.getY());
}
if (newPoint.getZ() < point.getZ())
{
printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n", newPoint.getZ() - point.getZ(), newPoint.getZ(), point.getZ());
}
} else
}
else
{
if (newPoint.getX() > point.getX())
{
printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n", newPoint.getX() - point.getX(), newPoint.getX(), point.getX());
}
if (newPoint.getY() > point.getY())
{
printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n", newPoint.getY() - point.getY(), newPoint.getY(), point.getY());
}
if (newPoint.getZ() > point.getZ())
{
printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n", newPoint.getZ() - point.getZ(), newPoint.getZ(), point.getZ());
}
}
#endif //DEBUG_CHECK_DEQUANTIZATION
#endif //DEBUG_CHECK_DEQUANTIZATION
}
SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const
SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2, int isMax) const
{
btAssert(m_useQuantization);
btVector3 clampedPoint(point2);
clampedPoint.setMax(m_bvhAabbMin);
clampedPoint.setMin(m_bvhAabbMax);
quantize(out,clampedPoint,isMax);
quantize(out, clampedPoint, isMax);
}
SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const
SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const
{
btVector3 vecOut;
vecOut.setValue(
btVector3 vecOut;
vecOut.setValue(
(btScalar)(vecIn[0]) / (m_bvhQuantization.getX()),
(btScalar)(vecIn[1]) / (m_bvhQuantization.getY()),
(btScalar)(vecIn[2]) / (m_bvhQuantization.getZ()));
vecOut += m_bvhAabbMin;
return vecOut;
vecOut += m_bvhAabbMin;
return vecOut;
}
///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees.
void setTraversalMode(btTraversalMode traversalMode)
void setTraversalMode(btTraversalMode traversalMode)
{
m_traversalMode = traversalMode;
}
SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray()
{
return m_quantizedContiguousNodes;
SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray()
{
return m_quantizedContiguousNodes;
}
SIMD_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray()
SIMD_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray()
{
return m_SubtreeHeaders;
}
////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////
/////Calculate space needed to store BVH for serialization
unsigned calculateSerializeBufferSize() const;
/// Data buffer MUST be 16 byte aligned
virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const;
virtual bool serialize(void* o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const;
///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
static btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
static btQuantizedBvh* deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
static unsigned int getAlignmentSerializationPadding();
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
virtual int calculateSerializeBufferSizeNew() const;
virtual int calculateSerializeBufferSizeNew() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual void deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData);
virtual void deSerializeFloat(struct btQuantizedBvhFloatData & quantizedBvhFloatData);
virtual void deSerializeDouble(struct btQuantizedBvhDoubleData& quantizedBvhDoubleData);
virtual void deSerializeDouble(struct btQuantizedBvhDoubleData & quantizedBvhDoubleData);
////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////
SIMD_FORCE_INLINE bool isQuantized()
{
@@ -494,38 +459,37 @@ private:
// Special "copy" constructor that allows for in-place deserialization
// Prevents btVector3's default constructor from being called, but doesn't inialize much else
// ownsMemory should most likely be false if deserializing, and if you are not, don't call this (it also changes the function signature, which we need)
btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory);
btQuantizedBvh(btQuantizedBvh & other, bool ownsMemory);
};
}
;
struct btBvhSubtreeInfoData
// clang-format off
// parser needs * with the name
struct btBvhSubtreeInfoData
{
int m_rootNodeIndex;
int m_subtreeSize;
int m_rootNodeIndex;
int m_subtreeSize;
unsigned short m_quantizedAabbMin[3];
unsigned short m_quantizedAabbMax[3];
};
struct btOptimizedBvhNodeFloatData
{
btVector3FloatData m_aabbMinOrg;
btVector3FloatData m_aabbMaxOrg;
int m_escapeIndex;
int m_subPart;
int m_triangleIndex;
btVector3FloatData m_aabbMinOrg;
btVector3FloatData m_aabbMaxOrg;
int m_escapeIndex;
int m_subPart;
int m_triangleIndex;
char m_pad[4];
};
struct btOptimizedBvhNodeDoubleData
{
btVector3DoubleData m_aabbMinOrg;
btVector3DoubleData m_aabbMaxOrg;
int m_escapeIndex;
int m_subPart;
int m_triangleIndex;
char m_pad[4];
btVector3DoubleData m_aabbMinOrg;
btVector3DoubleData m_aabbMaxOrg;
int m_escapeIndex;
int m_subPart;
int m_triangleIndex;
char m_pad[4];
};
@@ -569,13 +533,11 @@ struct btQuantizedBvhDoubleData
int m_numSubtreeHeaders;
btBvhSubtreeInfoData *m_subTreeInfoPtr;
};
// clang-format on
SIMD_FORCE_INLINE int btQuantizedBvh::calculateSerializeBufferSizeNew() const
SIMD_FORCE_INLINE int btQuantizedBvh::calculateSerializeBufferSizeNew() const
{
return sizeof(btQuantizedBvhData);
}
#endif //BT_QUANTIZED_BVH_H
#endif //BT_QUANTIZED_BVH_H

147
src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
View File

@@ -24,50 +24,45 @@ subject to the following restrictions:
#include <new>
void btSimpleBroadphase::validate()
void btSimpleBroadphase::validate()
{
for (int i=0;i<m_numHandles;i++)
for (int i = 0; i < m_numHandles; i++)
{
for (int j=i+1;j<m_numHandles;j++)
for (int j = i + 1; j < m_numHandles; j++)
{
btAssert(&m_pHandles[i] != &m_pHandles[j]);
}
}
}
btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
:m_pairCache(overlappingPairCache),
m_ownsPairCache(false),
m_invalidPair(0)
: m_pairCache(overlappingPairCache),
m_ownsPairCache(false),
m_invalidPair(0)
{
if (!overlappingPairCache)
{
void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
m_pairCache = new (mem)btHashedOverlappingPairCache();
void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16);
m_pairCache = new (mem) btHashedOverlappingPairCache();
m_ownsPairCache = true;
}
// allocate handles buffer and put all handles on free list
m_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy)*maxProxies,16);
m_pHandles = new(m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies];
m_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy) * maxProxies, 16);
m_pHandles = new (m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies];
m_maxHandles = maxProxies;
m_numHandles = 0;
m_firstFreeHandle = 0;
m_LastHandleIndex = -1;
{
for (int i = m_firstFreeHandle; i < maxProxies; i++)
{
m_pHandles[i].SetNextFree(i + 1);
m_pHandles[i].m_uniqueId = i+2;//any UID will do, we just avoid too trivial values (0,1) for debugging purposes
m_pHandles[i].m_uniqueId = i + 2; //any UID will do, we just avoid too trivial values (0,1) for debugging purposes
}
m_pHandles[maxProxies - 1].SetNextFree(0);
}
}
btSimpleBroadphase::~btSimpleBroadphase()
@@ -81,26 +76,25 @@ btSimpleBroadphase::~btSimpleBroadphase()
}
}
btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr , int collisionFilterGroup, int collisionFilterMask, btDispatcher* /*dispatcher*/)
btBroadphaseProxy* btSimpleBroadphase::createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* /*dispatcher*/)
{
if (m_numHandles >= m_maxHandles)
{
btAssert(0);
return 0; //should never happen, but don't let the game crash ;-)
return 0; //should never happen, but don't let the game crash ;-)
}
btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
btAssert(aabbMin[0] <= aabbMax[0] && aabbMin[1] <= aabbMax[1] && aabbMin[2] <= aabbMax[2]);
int newHandleIndex = allocHandle();
btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex]) btSimpleBroadphaseProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask);
return proxy;
}
class RemovingOverlapCallback : public btOverlapCallback
class RemovingOverlapCallback : public btOverlapCallback
{
protected:
virtual bool processOverlap(btBroadphasePair& pair)
virtual bool processOverlap(btBroadphasePair& pair)
{
(void)pair;
btAssert(0);
@@ -110,12 +104,13 @@ protected:
class RemovePairContainingProxy
{
btBroadphaseProxy* m_targetProxy;
btBroadphaseProxy* m_targetProxy;
public:
public:
virtual ~RemovePairContainingProxy()
{
}
protected:
virtual bool processOverlap(btBroadphasePair& pair)
{
@@ -126,38 +121,36 @@ protected:
};
};
void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher)
void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg, btDispatcher* dispatcher)
{
btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
freeHandle(proxy0);
btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
freeHandle(proxy0);
m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);
m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg, dispatcher);
//validate();
//validate();
}
void btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
void btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const
{
const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
aabbMin = sbp->m_aabbMin;
aabbMax = sbp->m_aabbMax;
}
void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
{
btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
sbp->m_aabbMin = aabbMin;
sbp->m_aabbMax = aabbMax;
}
void btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
void btSimpleBroadphase::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax)
{
for (int i=0; i <= m_LastHandleIndex; i++)
for (int i = 0; i <= m_LastHandleIndex; i++)
{
btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
if(!proxy->m_clientObject)
if (!proxy->m_clientObject)
{
continue;
}
@@ -165,69 +158,59 @@ void btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo
}
}
void btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
void btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
{
for (int i=0; i <= m_LastHandleIndex; i++)
for (int i = 0; i <= m_LastHandleIndex; i++)
{
btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
if(!proxy->m_clientObject)
if (!proxy->m_clientObject)
{
continue;
}
if (TestAabbAgainstAabb2(aabbMin,aabbMax,proxy->m_aabbMin,proxy->m_aabbMax))
if (TestAabbAgainstAabb2(aabbMin, aabbMax, proxy->m_aabbMin, proxy->m_aabbMax))
{
callback.process(proxy);
}
}
}
bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1)
bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0, btSimpleBroadphaseProxy* proxy1)
{
return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] &&
return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] &&
proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
}
//then remove non-overlapping ones
class CheckOverlapCallback : public btOverlapCallback
{
public:
virtual bool processOverlap(btBroadphasePair& pair)
{
return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0),static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1)));
return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0), static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1)));
}
};
void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
{
//first check for new overlapping pairs
int i,j;
int i, j;
if (m_numHandles >= 0)
{
int new_largest_index = -1;
for (i=0; i <= m_LastHandleIndex; i++)
for (i = 0; i <= m_LastHandleIndex; i++)
{
btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
if(!proxy0->m_clientObject)
if (!proxy0->m_clientObject)
{
continue;
}
new_largest_index = i;
for (j=i+1; j <= m_LastHandleIndex; j++)
for (j = i + 1; j <= m_LastHandleIndex; j++)
{
btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
btAssert(proxy0 != proxy1);
if(!proxy1->m_clientObject)
if (!proxy1->m_clientObject)
{
continue;
}
@@ -235,19 +218,20 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
if (aabbOverlap(p0,p1))
if (aabbOverlap(p0, p1))
{
if ( !m_pairCache->findPair(proxy0,proxy1))
if (!m_pairCache->findPair(proxy0, proxy1))
{
m_pairCache->addOverlappingPair(proxy0,proxy1);
m_pairCache->addOverlappingPair(proxy0, proxy1);
}
} else
}
else
{
if (!m_pairCache->hasDeferredRemoval())
{
if ( m_pairCache->findPair(proxy0,proxy1))
if (m_pairCache->findPair(proxy0, proxy1))
{
m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
m_pairCache->removeOverlappingPair(proxy0, proxy1, dispatcher);
}
}
}
@@ -258,8 +242,7 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
{
btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
@@ -267,16 +250,13 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
m_invalidPair = 0;
btBroadphasePair previousPair;
previousPair.m_pProxy0 = 0;
previousPair.m_pProxy1 = 0;
previousPair.m_algorithm = 0;
for (i=0;i<overlappingPairArray.size();i++)
for (i = 0; i < overlappingPairArray.size(); i++)
{
btBroadphasePair& pair = overlappingPairArray[i];
bool isDuplicate = (pair == previousPair);
@@ -287,16 +267,18 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
if (!isDuplicate)
{
bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
bool hasOverlap = testAabbOverlap(pair.m_pProxy0, pair.m_pProxy1);
if (hasOverlap)
{
needsRemoval = false;//callback->processOverlap(pair);
} else
needsRemoval = false; //callback->processOverlap(pair);
}
else
{
needsRemoval = true;
}
} else
}
else
{
//remove duplicate
needsRemoval = true;
@@ -306,7 +288,7 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
if (needsRemoval)
{
m_pairCache->cleanOverlappingPair(pair,dispatcher);
m_pairCache->cleanOverlappingPair(pair, dispatcher);
// m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
// m_overlappingPairArray.pop_back();
@@ -314,7 +296,6 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
pair.m_pProxy1 = 0;
m_invalidPair++;
}
}
///if you don't like to skip the invalid pairs in the array, execute following code:
@@ -326,21 +307,19 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
m_invalidPair = 0;
#endif//CLEAN_INVALID_PAIRS
#endif //CLEAN_INVALID_PAIRS
}
}
}
bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
{
btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
return aabbOverlap(p0,p1);
return aabbOverlap(p0, p1);
}
void btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
void btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
{
//not yet
}

107
src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
View File

@@ -16,57 +16,47 @@ subject to the following restrictions:
#ifndef BT_SIMPLE_BROADPHASE_H
#define BT_SIMPLE_BROADPHASE_H
#include "btOverlappingPairCache.h"
struct btSimpleBroadphaseProxy : public btBroadphaseProxy
{
int m_nextFree;
// int m_handleId;
int m_nextFree;
btSimpleBroadphaseProxy() {};
// int m_handleId;
btSimpleBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask)
:btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask)
btSimpleBroadphaseProxy(){};
btSimpleBroadphaseProxy(const btVector3& minpt, const btVector3& maxpt, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask)
: btBroadphaseProxy(minpt, maxpt, userPtr, collisionFilterGroup, collisionFilterMask)
{
(void)shapeType;
}
SIMD_FORCE_INLINE void SetNextFree(int next) {m_nextFree = next;}
SIMD_FORCE_INLINE int GetNextFree() const {return m_nextFree;}
SIMD_FORCE_INLINE void SetNextFree(int next) { m_nextFree = next; }
SIMD_FORCE_INLINE int GetNextFree() const { return m_nextFree; }
};
///The SimpleBroadphase is just a unit-test for btAxisSweep3, bt32BitAxisSweep3, or btDbvtBroadphase, so use those classes instead.
///It is a brute force aabb culling broadphase based on O(n^2) aabb checks
class btSimpleBroadphase : public btBroadphaseInterface
{
protected:
int m_numHandles; // number of active handles
int m_maxHandles; // max number of handles
int m_LastHandleIndex;
int m_numHandles; // number of active handles
int m_maxHandles; // max number of handles
int m_LastHandleIndex;
btSimpleBroadphaseProxy* m_pHandles; // handles pool
btSimpleBroadphaseProxy* m_pHandles; // handles pool
void* m_pHandlesRawPtr;
int m_firstFreeHandle; // free handles list
int m_firstFreeHandle; // free handles list
int allocHandle()
{
btAssert(m_numHandles < m_maxHandles);
int freeHandle = m_firstFreeHandle;
m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree();
m_numHandles++;
if(freeHandle > m_LastHandleIndex)
if (freeHandle > m_LastHandleIndex)
{
m_LastHandleIndex = freeHandle;
}
@@ -75,9 +65,9 @@ protected:
void freeHandle(btSimpleBroadphaseProxy* proxy)
{
int handle = int(proxy-m_pHandles);
int handle = int(proxy - m_pHandles);
btAssert(handle >= 0 && handle < m_maxHandles);
if(handle == m_LastHandleIndex)
if (handle == m_LastHandleIndex)
{
m_LastHandleIndex--;
}
@@ -89,20 +79,18 @@ protected:
m_numHandles--;
}
btOverlappingPairCache* m_pairCache;
bool m_ownsPairCache;
btOverlappingPairCache* m_pairCache;
bool m_ownsPairCache;
int m_invalidPair;
int m_invalidPair;
inline btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy)
inline btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy)
{
btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxy);
return proxy0;
}
inline const btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const
inline const btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const
{
const btSimpleBroadphaseProxy* proxy0 = static_cast<const btSimpleBroadphaseProxy*>(proxy);
return proxy0;
@@ -111,61 +99,50 @@ protected:
///reset broadphase internal structures, to ensure determinism/reproducability
virtual void resetPool(btDispatcher* dispatcher);
void validate();
void validate();
protected:
public:
btSimpleBroadphase(int maxProxies=16384,btOverlappingPairCache* overlappingPairCache=0);
btSimpleBroadphase(int maxProxies = 16384, btOverlappingPairCache* overlappingPairCache = 0);
virtual ~btSimpleBroadphase();
static bool aabbOverlap(btSimpleBroadphaseProxy* proxy0, btSimpleBroadphaseProxy* proxy1);
static bool aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1);
virtual btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher);
virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr , int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher);
virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher);
virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher);
virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const;
virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0));
virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0));
virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
btOverlappingPairCache* getOverlappingPairCache()
btOverlappingPairCache* getOverlappingPairCache()
{
return m_pairCache;
}
const btOverlappingPairCache* getOverlappingPairCache() const
const btOverlappingPairCache* getOverlappingPairCache() const
{
return m_pairCache;
}
bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
bool testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
///will add some transform later
virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const
{
aabbMin.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
aabbMin.setValue(-BT_LARGE_FLOAT, -BT_LARGE_FLOAT, -BT_LARGE_FLOAT);
aabbMax.setValue(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
}
virtual void printStats()
virtual void printStats()
{
// printf("btSimpleBroadphase.h\n");
// printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
// printf("btSimpleBroadphase.h\n");
// printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
}
};
#endif //BT_SIMPLE_BROADPHASE_H
#endif //BT_SIMPLE_BROADPHASE_H

148
src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
View File

@@ -18,94 +18,95 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold)
:m_sphere(sphere),
m_triangle(triangle),
m_contactBreakingThreshold(contactBreakingThreshold)
SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold)
: m_sphere(sphere),
m_triangle(triangle),
m_contactBreakingThreshold(contactBreakingThreshold)
{
}
void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults)
{
(void)debugDraw;
const btTransform& transformA = input.m_transformA;
const btTransform& transformB = input.m_transformB;
btVector3 point,normal;
btVector3 point, normal;
btScalar timeOfImpact = btScalar(1.);
btScalar depth = btScalar(0.);
// output.m_distance = btScalar(BT_LARGE_FLOAT);
// output.m_distance = btScalar(BT_LARGE_FLOAT);
//move sphere into triangle space
btTransform sphereInTr = transformB.inverseTimes(transformA);
btTransform sphereInTr = transformB.inverseTimes(transformA);
if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold))
if (collide(sphereInTr.getOrigin(), point, normal, depth, timeOfImpact, m_contactBreakingThreshold))
{
if (swapResults)
{
btVector3 normalOnB = transformB.getBasis()*normal;
btVector3 normalOnB = transformB.getBasis() * normal;
btVector3 normalOnA = -normalOnB;
btVector3 pointOnA = transformB*point+normalOnB*depth;
output.addContactPoint(normalOnA,pointOnA,depth);
} else
btVector3 pointOnA = transformB * point + normalOnB * depth;
output.addContactPoint(normalOnA, pointOnA, depth);
}
else
{
output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
output.addContactPoint(transformB.getBasis() * normal, transformB * point, depth);
}
}
}
// See also geometrictools.com
// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest);
btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest);
btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest)
{
btVector3 diff = p - from;
btVector3 v = to - from;
btScalar t = v.dot(diff);
if (t > 0) {
if (t > 0)
{
btScalar dotVV = v.dot(v);
if (t < dotVV) {
if (t < dotVV)
{
t /= dotVV;
diff -= t*v;
} else {
diff -= t * v;
}
else
{
t = 1;
diff -= v;
}
} else
}
else
t = 0;
nearest = from + t*v;
return diff.dot(diff);
nearest = from + t * v;
return diff.dot(diff);
}
bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) {
bool SphereTriangleDetector::facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal)
{
btVector3 lp(p);
btVector3 lnormal(normal);
return pointInTriangle(vertices, lnormal, &lp);
}
bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
bool SphereTriangleDetector::collide(const btVector3& sphereCenter, btVector3& point, btVector3& resultNormal, btScalar& depth, btScalar& timeOfImpact, btScalar contactBreakingThreshold)
{
const btVector3* vertices = &m_triangle->getVertexPtr(0);
btScalar radius = m_sphere->getRadius();
btScalar radiusWithThreshold = radius + contactBreakingThreshold;
btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
btVector3 normal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]);
btScalar l2 = normal.length2();
bool hasContact = false;
btVector3 contactPoint;
if (l2 >= SIMD_EPSILON*SIMD_EPSILON)
if (l2 >= SIMD_EPSILON * SIMD_EPSILON)
{
normal /= btSqrt(l2);
@@ -120,54 +121,59 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
}
bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
// Check for contact / intersection
if (isInsideContactPlane) {
if (facecontains(sphereCenter, vertices, normal)) {
if (isInsideContactPlane)
{
if (facecontains(sphereCenter, vertices, normal))
{
// Inside the contact wedge - touches a point on the shell plane
hasContact = true;
contactPoint = sphereCenter - normal*distanceFromPlane;
contactPoint = sphereCenter - normal * distanceFromPlane;
}
else {
else
{
// Could be inside one of the contact capsules
btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
btScalar contactCapsuleRadiusSqr = radiusWithThreshold * radiusWithThreshold;
btScalar minDistSqr = contactCapsuleRadiusSqr;
btVector3 nearestOnEdge;
for (int i = 0; i < m_triangle->getNumEdges(); i++) {
for (int i = 0; i < m_triangle->getNumEdges(); i++)
{
btVector3 pa;
btVector3 pb;
m_triangle->getEdge(i, pa, pb);
btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge);
if (distanceSqr < minDistSqr) {
if (distanceSqr < minDistSqr)
{
// Yep, we're inside a capsule, and record the capsule with smallest distance
minDistSqr = distanceSqr;
hasContact = true;
contactPoint = nearestOnEdge;
}
}
}
}
}
if (hasContact) {
if (hasContact)
{
btVector3 contactToCentre = sphereCenter - contactPoint;
btScalar distanceSqr = contactToCentre.length2();
if (distanceSqr < radiusWithThreshold*radiusWithThreshold)
if (distanceSqr < radiusWithThreshold * radiusWithThreshold)
{
if (distanceSqr>SIMD_EPSILON)
if (distanceSqr > SIMD_EPSILON)
{
btScalar distance = btSqrt(distanceSqr);
resultNormal = contactToCentre;
resultNormal.normalize();
point = contactPoint;
depth = -(radius-distance);
} else
depth = -(radius - distance);
}
else
{
resultNormal = normal;
point = contactPoint;
@@ -176,36 +182,34 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
return true;
}
}
return false;
}
bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p)
{
const btVector3* p1 = &vertices[0];
const btVector3* p2 = &vertices[1];
const btVector3* p3 = &vertices[2];
btVector3 edge1( *p2 - *p1 );
btVector3 edge2( *p3 - *p2 );
btVector3 edge3( *p1 - *p3 );
btVector3 edge1(*p2 - *p1);
btVector3 edge2(*p3 - *p2);
btVector3 edge3(*p1 - *p3);
btVector3 p1_to_p( *p - *p1 );
btVector3 p2_to_p( *p - *p2 );
btVector3 p3_to_p( *p - *p3 );
btVector3 p1_to_p(*p - *p1);
btVector3 p2_to_p(*p - *p2);
btVector3 p3_to_p(*p - *p3);
btVector3 edge1_normal(edge1.cross(normal));
btVector3 edge2_normal(edge2.cross(normal));
btVector3 edge3_normal(edge3.cross(normal));
btVector3 edge1_normal( edge1.cross(normal));
btVector3 edge2_normal( edge2.cross(normal));
btVector3 edge3_normal( edge3.cross(normal));
btScalar r1, r2, r3;
r1 = edge1_normal.dot( p1_to_p );
r2 = edge2_normal.dot( p2_to_p );
r3 = edge3_normal.dot( p3_to_p );
if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
r1 = edge1_normal.dot(p1_to_p);
r2 = edge2_normal.dot(p2_to_p);
r3 = edge3_normal.dot(p3_to_p);
if ((r1 > 0 && r2 > 0 && r3 > 0) ||
(r1 <= 0 && r2 <= 0 && r3 <= 0))
return true;
return false;
}

24
src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
View File

@@ -18,34 +18,26 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
class btSphereShape;
class btTriangleShape;
/// sphere-triangle to match the btDiscreteCollisionDetectorInterface
struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface
{
virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false);
SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle, btScalar contactBreakingThreshold);
SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold);
virtual ~SphereTriangleDetector() {};
virtual ~SphereTriangleDetector(){};
bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold);
bool collide(const btVector3& sphereCenter, btVector3& point, btVector3& resultNormal, btScalar& depth, btScalar& timeOfImpact, btScalar contactBreakingThreshold);
private:
bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p );
bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal);
bool pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p);
bool facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal);
btSphereShape* m_sphere;
btTriangleShape* m_triangle;
btScalar m_contactBreakingThreshold;
btScalar m_contactBreakingThreshold;
};
#endif //BT_SPHERE_TRIANGLE_DETECTOR_H
#endif //BT_SPHERE_TRIANGLE_DETECTOR_H

28
src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp
View File

@@ -17,31 +17,31 @@ subject to the following restrictions:
#include "btCollisionDispatcher.h"
#include "btCollisionObject.h"
btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci)
:btCollisionAlgorithm(ci)
btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btCollisionAlgorithm(ci)
//,
//m_colObj0(0),
//m_colObj1(0)
{
}
btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* )
:btCollisionAlgorithm(ci)
btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper*, const btCollisionObjectWrapper*)
: btCollisionAlgorithm(ci)
//,
//m_colObj0(0),
//m_colObj1(0)
{
// if (ci.m_dispatcher1->needsCollision(colObj0,colObj1))
// {
// m_colObj0 = colObj0;
// m_colObj1 = colObj1;
//
// m_colObj0->activate();
// m_colObj1->activate();
// }
// if (ci.m_dispatcher1->needsCollision(colObj0,colObj1))
// {
// m_colObj0 = colObj0;
// m_colObj1 = colObj1;
//
// m_colObj0->activate();
// m_colObj1->activate();
// }
}
btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm()
{
// m_colObj0->activate();
// m_colObj1->activate();
// m_colObj0->activate();
// m_colObj1->activate();
}

12
src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h
View File

@@ -21,17 +21,15 @@ subject to the following restrictions:
///This class is not enabled yet (work-in-progress) to more aggressively activate objects.
class btActivatingCollisionAlgorithm : public btCollisionAlgorithm
{
// btCollisionObject* m_colObj0;
// btCollisionObject* m_colObj1;
// btCollisionObject* m_colObj0;
// btCollisionObject* m_colObj1;
protected:
btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci);
btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap);
public:
virtual ~btActivatingCollisionAlgorithm();
};
#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H
#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H

106
src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp
View File

@@ -26,61 +26,55 @@ subject to the following restrictions:
#define USE_PERSISTENT_CONTACTS 1
btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap)
: btActivatingCollisionAlgorithm(ci,obj0Wrap,obj1Wrap),
m_ownManifold(false),
m_manifoldPtr(mf)
btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap)
: btActivatingCollisionAlgorithm(ci, obj0Wrap, obj1Wrap),
m_ownManifold(false),
m_manifoldPtr(mf)
{
if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject()))
if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(), obj1Wrap->getCollisionObject()))
{
m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject());
m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(), obj1Wrap->getCollisionObject());
m_ownManifold = true;
}
}
btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm()
{
if (m_ownManifold)
{
if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr);
}
}
void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
//#include <stdio.h>
void btBox2dBox2dCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
void btBox2dBox2dCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
if (!m_manifoldPtr)
return;
const btBox2dShape* box0 = (const btBox2dShape*)body0Wrap->getCollisionShape();
const btBox2dShape* box1 = (const btBox2dShape*)body1Wrap->getCollisionShape();
resultOut->setPersistentManifold(m_manifoldPtr);
b2CollidePolygons(resultOut,box0,body0Wrap->getWorldTransform(),box1,body1Wrap->getWorldTransform());
b2CollidePolygons(resultOut, box0, body0Wrap->getWorldTransform(), box1, body1Wrap->getWorldTransform());
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut->refreshContactPoints();
}
}
btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/)
{
//not yet
return 1.f;
}
struct ClipVertex
{
btVector3 v;
@@ -89,16 +83,16 @@ struct ClipVertex
//b2ContactID id;
};
#define b2Dot(a,b) (a).dot(b)
#define b2Mul(a,b) (a)*(b)
#define b2MulT(a,b) (a).transpose()*(b)
#define b2Cross(a,b) (a).cross(b)
#define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f)
#define b2Dot(a, b) (a).dot(b)
#define b2Mul(a, b) (a) * (b)
#define b2MulT(a, b) (a).transpose() * (b)
#define b2Cross(a, b) (a).cross(b)
#define btCrossS(a, s) btVector3(s* a.getY(), -s* a.getX(), 0.f)
int b2_maxManifoldPoints =2;
int b2_maxManifoldPoints = 2;
static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
const btVector3& normal, btScalar offset)
const btVector3& normal, btScalar offset)
{
// Start with no output points
int numOut = 0;
@@ -133,7 +127,7 @@ static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
// Find the separation between poly1 and poly2 for a give edge normal on poly1.
static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1,
const btBox2dShape* poly2, const btTransform& xf2)
const btBox2dShape* poly2, const btTransform& xf2)
{
const btVector3* vertices1 = poly1->getVertices();
const btVector3* normals1 = poly1->getNormals();
@@ -151,8 +145,8 @@ static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1
int index = 0;
btScalar minDot = BT_LARGE_FLOAT;
if( count2 > 0 )
index = (int) normal1.minDot( vertices2, count2, minDot);
if (count2 > 0)
index = (int)normal1.minDot(vertices2, count2, minDot);
btVector3 v1 = b2Mul(xf1, vertices1[edge1]);
btVector3 v2 = b2Mul(xf2, vertices2[index]);
@@ -162,8 +156,8 @@ static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1
// Find the max separation between poly1 and poly2 using edge normals from poly1.
static btScalar FindMaxSeparation(int* edgeIndex,
const btBox2dShape* poly1, const btTransform& xf1,
const btBox2dShape* poly2, const btTransform& xf2)
const btBox2dShape* poly1, const btTransform& xf1,
const btBox2dShape* poly2, const btTransform& xf2)
{
int count1 = poly1->getVertexCount();
const btVector3* normals1 = poly1->getNormals();
@@ -174,9 +168,9 @@ static btScalar FindMaxSeparation(int* edgeIndex,
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
btScalar maxDot;
if( count1 > 0 )
edge = (int) dLocal1.maxDot( normals1, count1, maxDot);
btScalar maxDot;
if (count1 > 0)
edge = (int)dLocal1.maxDot(normals1, count1, maxDot);
// Get the separation for the edge normal.
btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
@@ -224,7 +218,7 @@ static btScalar FindMaxSeparation(int* edgeIndex,
}
// Perform a local search for the best edge normal.
for ( ; ; )
for (;;)
{
if (increment == -1)
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
@@ -285,14 +279,14 @@ static void FindIncidentEdge(ClipVertex c[2],
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c[0].v = b2Mul(xf2, vertices2[i1]);
// c[0].id.features.referenceEdge = (unsigned char)edge1;
// c[0].id.features.incidentEdge = (unsigned char)i1;
// c[0].id.features.incidentVertex = 0;
// c[0].id.features.referenceEdge = (unsigned char)edge1;
// c[0].id.features.incidentEdge = (unsigned char)i1;
// c[0].id.features.incidentVertex = 0;
c[1].v = b2Mul(xf2, vertices2[i2]);
// c[1].id.features.referenceEdge = (unsigned char)edge1;
// c[1].id.features.incidentEdge = (unsigned char)i2;
// c[1].id.features.incidentVertex = 1;
// c[1].id.features.referenceEdge = (unsigned char)edge1;
// c[1].id.features.incidentEdge = (unsigned char)i2;
// c[1].id.features.incidentVertex = 1;
}
// Find edge normal of max separation on A - return if separating axis is found
@@ -303,10 +297,9 @@ static void FindIncidentEdge(ClipVertex c[2],
// The normal points from 1 to 2
void b2CollidePolygons(btManifoldResult* manifold,
const btBox2dShape* polyA, const btTransform& xfA,
const btBox2dShape* polyB, const btTransform& xfB)
const btBox2dShape* polyA, const btTransform& xfA,
const btBox2dShape* polyB, const btTransform& xfB)
{
int edgeA = 0;
btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
if (separationA > 0.0f)
@@ -317,10 +310,10 @@ void b2CollidePolygons(btManifoldResult* manifold,
if (separationB > 0.0f)
return;
const btBox2dShape* poly1; // reference poly
const btBox2dShape* poly2; // incident poly
const btBox2dShape* poly1; // reference poly
const btBox2dShape* poly2; // incident poly
btTransform xf1, xf2;
int edge1; // reference edge
int edge1; // reference edge
unsigned char flip;
const btScalar k_relativeTol = 0.98f;
const btScalar k_absoluteTol = 0.001f;
@@ -352,14 +345,13 @@ void b2CollidePolygons(btManifoldResult* manifold,
const btVector3* vertices1 = poly1->getVertices();
btVector3 v11 = vertices1[edge1];
btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
//btVector3 dv = v12 - v11;
btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11);
sideNormal.normalize();
btVector3 frontNormal = btCrossS(sideNormal, 1.0f);
v11 = b2Mul(xf1, v11);
v12 = b2Mul(xf1, v12);
@@ -369,13 +361,12 @@ void b2CollidePolygons(btManifoldResult* manifold,
// Clip incident edge against extruded edge1 side edges.
ClipVertex clipPoints1[2];
clipPoints1[0].v.setValue(0,0,0);
clipPoints1[1].v.setValue(0,0,0);
clipPoints1[0].v.setValue(0, 0, 0);
clipPoints1[1].v.setValue(0, 0, 0);
ClipVertex clipPoints2[2];
clipPoints2[0].v.setValue(0,0,0);
clipPoints2[1].v.setValue(0,0,0);
clipPoints2[0].v.setValue(0, 0, 0);
clipPoints2[1].v.setValue(0, 0, 0);
int np;
@@ -386,7 +377,7 @@ void b2CollidePolygons(btManifoldResult* manifold,
return;
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
@@ -403,19 +394,18 @@ void b2CollidePolygons(btManifoldResult* manifold,
if (separation <= 0.0f)
{
//b2ManifoldPoint* cp = manifold->points + pointCount;
//btScalar separation = separation;
//cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
//cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation);
manifold->addContactPoint(-manifoldNormal, clipPoints2[i].v, separation);
// cp->id = clipPoints2[i].id;
// cp->id.features.flip = flip;
// cp->id = clipPoints2[i].id;
// cp->id.features.flip = flip;
++pointCount;
}
}
// manifold->pointCount = pointCount;}
// manifold->pointCount = pointCount;}
}

25
src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h
View File

@@ -26,22 +26,22 @@ class btPersistentManifold;
///box-box collision detection
class btBox2dBox2dCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
public:
btBox2dBox2dCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btActivatingCollisionAlgorithm(ci) {}
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap);
virtual ~btBox2dBox2dCollisionAlgorithm();
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
@@ -49,18 +49,15 @@ public:
}
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
struct CreateFunc : public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
int bbsize = sizeof(btBox2dBox2dCollisionAlgorithm);
void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
return new(ptr) btBox2dBox2dCollisionAlgorithm(0,ci,body0Wrap,body1Wrap);
return new (ptr) btBox2dBox2dCollisionAlgorithm(0, ci, body0Wrap, body1Wrap);
}
};
};
#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H

30
src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
View File

@@ -21,14 +21,14 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
#define USE_PERSISTENT_CONTACTS 1
btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
m_ownManifold(false),
m_manifoldPtr(mf)
btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
m_ownManifold(false),
m_manifoldPtr(mf)
{
if (!m_manifoldPtr && m_dispatcher->needsCollision(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()))
if (!m_manifoldPtr && m_dispatcher->needsCollision(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()))
{
m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
m_ownManifold = true;
}
}
@@ -42,30 +42,27 @@ btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm()
}
}
void btBoxBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
void btBoxBoxCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
if (!m_manifoldPtr)
return;
const btBoxShape* box0 = (btBoxShape*)body0Wrap->getCollisionShape();
const btBoxShape* box1 = (btBoxShape*)body1Wrap->getCollisionShape();
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr);
#ifndef USE_PERSISTENT_CONTACTS
#ifndef USE_PERSISTENT_CONTACTS
m_manifoldPtr->clearManifold();
#endif //USE_PERSISTENT_CONTACTS
#endif //USE_PERSISTENT_CONTACTS
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
input.m_transformA = body0Wrap->getWorldTransform();
input.m_transformB = body1Wrap->getWorldTransform();
btBoxBoxDetector detector(box0,box1);
detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
btBoxBoxDetector detector(box0, box1);
detector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw);
#ifdef USE_PERSISTENT_CONTACTS
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
@@ -73,11 +70,10 @@ void btBoxBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrappe
{
resultOut->refreshContactPoints();
}
#endif //USE_PERSISTENT_CONTACTS
#endif //USE_PERSISTENT_CONTACTS
}
btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/)
{
//not yet
return 1.f;

25
src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h
View File

@@ -26,22 +26,22 @@ class btPersistentManifold;
///box-box collision detection
class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
public:
btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btActivatingCollisionAlgorithm(ci) {}
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
btBoxBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap);
virtual ~btBoxBoxCollisionAlgorithm();
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
@@ -49,18 +49,15 @@ public:
}
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
struct CreateFunc : public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
int bbsize = sizeof(btBoxBoxCollisionAlgorithm);
void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap);
return new (ptr) btBoxBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap);
}
};
};
#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H
#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H

1187
src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
View File

File diff suppressed because it is too large Load Diff

12
src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h
View File

@@ -19,11 +19,9 @@ subject to the following restrictions:
#ifndef BT_BOX_BOX_DETECTOR_H
#define BT_BOX_BOX_DETECTOR_H
class btBoxShape;
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
/// btBoxBoxDetector wraps the ODE box-box collision detector
/// re-distributed under the Zlib license with permission from Russell L. Smith
struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface
@@ -32,13 +30,11 @@ struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface
const btBoxShape* m_box2;
public:
btBoxBoxDetector(const btBoxShape* box1, const btBoxShape* box2);
btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2);
virtual ~btBoxBoxDetector() {};
virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
virtual ~btBoxBoxDetector(){};
virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false);
};
#endif //BT_BOX_BOX_DETECTOR_H
#endif //BT_BOX_BOX_DETECTOR_H

12
src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h
View File

@@ -23,11 +23,9 @@ class btPoolAllocator;
///btCollisionConfiguration allows to configure Bullet collision detection
///stack allocator size, default collision algorithms and persistent manifold pool size
///@todo: describe the meaning
class btCollisionConfiguration
class btCollisionConfiguration
{
public:
virtual ~btCollisionConfiguration()
{
}
@@ -37,13 +35,9 @@ public:
virtual btPoolAllocator* getCollisionAlgorithmPool() = 0;
virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0;
virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0;
virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0;
};
#endif //BT_COLLISION_CONFIGURATION
#endif //BT_COLLISION_CONFIGURATION

10
src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
View File

@@ -26,20 +26,18 @@ struct btCollisionAlgorithmConstructionInfo;
struct btCollisionAlgorithmCreateFunc
{
bool m_swapped;
btCollisionAlgorithmCreateFunc()
:m_swapped(false)
: m_swapped(false)
{
}
virtual ~btCollisionAlgorithmCreateFunc(){};
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& , const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo&, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
(void)body0Wrap;
(void)body1Wrap;
return 0;
}
};
#endif //BT_COLLISION_CREATE_FUNC
#endif //BT_COLLISION_CREATE_FUNC

196
src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
View File

@@ -13,8 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btCollisionDispatcher.h"
#include "LinearMath/btQuickprof.h"
@@ -31,40 +29,34 @@ subject to the following restrictions:
#include <stdio.h>
#endif
btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration):
m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD),
m_collisionConfiguration(collisionConfiguration)
btCollisionDispatcher::btCollisionDispatcher(btCollisionConfiguration* collisionConfiguration) : m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD),
m_collisionConfiguration(collisionConfiguration)
{
int i;
setNearCallback(defaultNearCallback);
m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool();
m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool();
for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++)
{
for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
for (int j = 0; j < MAX_BROADPHASE_COLLISION_TYPES; j++)
{
m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i, j);
btAssert(m_doubleDispatchContactPoints[i][j]);
m_doubleDispatchClosestPoints[i][j] = m_collisionConfiguration->getClosestPointsAlgorithmCreateFunc(i, j);
}
}
}
void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc)
{
m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc;
}
void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc)
{
m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc;
}
@@ -73,35 +65,33 @@ btCollisionDispatcher::~btCollisionDispatcher()
{
}
btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1)
{
btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1)
{
//btAssert(gNumManifold < 65535);
//optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ?
btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold) , body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
: gContactBreakingThreshold ;
btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
void* mem = m_persistentManifoldPoolAllocator->allocate( sizeof( btPersistentManifold ) );
if (NULL == mem)
btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold), body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
: gContactBreakingThreshold;
btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold());
void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
if (NULL == mem)
{
//we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
if ((m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION)==0)
//we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
if ((m_dispatcherFlags & CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION) == 0)
{
mem = btAlignedAlloc(sizeof(btPersistentManifold),16);
} else
mem = btAlignedAlloc(sizeof(btPersistentManifold), 16);
}
else
{
btAssert(0);
//make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration
return 0;
}
}
btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold);
btPersistentManifold* manifold = new (mem) btPersistentManifold(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold);
manifold->m_index1a = m_manifoldsPtr.size();
m_manifoldsPtr.push_back(manifold);
@@ -113,17 +103,14 @@ void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold)
manifold->clearManifold();
}
void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
{
//printf("releaseManifold: gNumManifold %d\n",gNumManifold);
clearManifold(manifold);
int findIndex = manifold->m_index1a;
btAssert(findIndex < m_manifoldsPtr.size());
m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
m_manifoldsPtr.swap(findIndex, m_manifoldsPtr.size() - 1);
m_manifoldsPtr[findIndex]->m_index1a = findIndex;
m_manifoldsPtr.pop_back();
@@ -131,19 +118,15 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
if (m_persistentManifoldPoolAllocator->validPtr(manifold))
{
m_persistentManifoldPoolAllocator->freeMemory(manifold);
} else
}
else
{
btAlignedFree(manifold);
}
}
btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType)
btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType)
{
btCollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher1 = this;
@@ -161,21 +144,18 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObje
return algo;
}
bool btCollisionDispatcher::needsResponse(const btCollisionObject* body0,const btCollisionObject* body1)
bool btCollisionDispatcher::needsResponse(const btCollisionObject* body0, const btCollisionObject* body1)
{
//here you can do filtering
bool hasResponse =
bool hasResponse =
(body0->hasContactResponse() && body1->hasContactResponse());
//no response between two static/kinematic bodies:
hasResponse = hasResponse &&
((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject()));
((!body0->isStaticOrKinematicObject()) || (!body1->isStaticOrKinematicObject()));
return hasResponse;
}
bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const btCollisionObject* body1)
bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
{
btAssert(body0);
btAssert(body1);
@@ -192,31 +172,27 @@ bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const
printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
}
}
#endif //BT_DEBUG
#endif //BT_DEBUG
if ((!body0->isActive()) && (!body1->isActive()))
needsCollision = false;
else if ((!body0->checkCollideWith(body1)) || (!body1->checkCollideWith(body0)))
needsCollision = false;
return needsCollision ;
return needsCollision;
}
///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc)
///this is useful for the collision dispatcher.
class btCollisionPairCallback : public btOverlapCallback
{
const btDispatcherInfo& m_dispatchInfo;
btCollisionDispatcher* m_dispatcher;
btCollisionDispatcher* m_dispatcher;
public:
btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher* dispatcher)
:m_dispatchInfo(dispatchInfo),
m_dispatcher(dispatcher)
btCollisionPairCallback(const btDispatcherInfo& dispatchInfo, btCollisionDispatcher* dispatcher)
: m_dispatchInfo(dispatchInfo),
m_dispatcher(dispatcher)
{
}
@@ -228,87 +204,76 @@ public:
}
*/
virtual ~btCollisionPairCallback() {}
virtual bool processOverlap(btBroadphasePair& pair)
virtual bool processOverlap(btBroadphasePair& pair)
{
(*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
(*m_dispatcher->getNearCallback())(pair, *m_dispatcher, m_dispatchInfo);
return false;
}
};
void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher)
void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher)
{
//m_blockedForChanges = true;
btCollisionPairCallback collisionCallback(dispatchInfo,this);
btCollisionPairCallback collisionCallback(dispatchInfo, this);
{
{
BT_PROFILE("processAllOverlappingPairs");
pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher, dispatchInfo);
pairCache->processAllOverlappingPairs(&collisionCallback, dispatcher, dispatchInfo);
}
//m_blockedForChanges = false;
}
//by default, Bullet will use this near callback
void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)
{
btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
if (dispatcher.needsCollision(colObj0,colObj1))
if (dispatcher.needsCollision(colObj0, colObj1))
{
btCollisionObjectWrapper obj0Wrap(0, colObj0->getCollisionShape(), colObj0, colObj0->getWorldTransform(), -1, -1);
btCollisionObjectWrapper obj1Wrap(0, colObj1->getCollisionShape(), colObj1, colObj1->getWorldTransform(), -1, -1);
//dispatcher will keep algorithms persistent in the collision pair
if (!collisionPair.m_algorithm)
{
btCollisionObjectWrapper obj0Wrap(0,colObj0->getCollisionShape(),colObj0,colObj0->getWorldTransform(),-1,-1);
btCollisionObjectWrapper obj1Wrap(0,colObj1->getCollisionShape(),colObj1,colObj1->getWorldTransform(),-1,-1);
//dispatcher will keep algorithms persistent in the collision pair
if (!collisionPair.m_algorithm)
{
collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap,0, BT_CONTACT_POINT_ALGORITHMS);
}
if (collisionPair.m_algorithm)
{
btManifoldResult contactPointResult(&obj0Wrap,&obj1Wrap);
if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE)
{
//discrete collision detection query
collisionPair.m_algorithm->processCollision(&obj0Wrap,&obj1Wrap,dispatchInfo,&contactPointResult);
} else
{
//continuous collision detection query, time of impact (toi)
btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult);
if (dispatchInfo.m_timeOfImpact > toi)
dispatchInfo.m_timeOfImpact = toi;
}
}
collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap, &obj1Wrap, 0, BT_CONTACT_POINT_ALGORITHMS);
}
}
if (collisionPair.m_algorithm)
{
btManifoldResult contactPointResult(&obj0Wrap, &obj1Wrap);
if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE)
{
//discrete collision detection query
collisionPair.m_algorithm->processCollision(&obj0Wrap, &obj1Wrap, dispatchInfo, &contactPointResult);
}
else
{
//continuous collision detection query, time of impact (toi)
btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0, colObj1, dispatchInfo, &contactPointResult);
if (dispatchInfo.m_timeOfImpact > toi)
dispatchInfo.m_timeOfImpact = toi;
}
}
}
}
void* btCollisionDispatcher::allocateCollisionAlgorithm(int size)
{
void* mem = m_collisionAlgorithmPoolAllocator->allocate( size );
if (NULL == mem)
{
//warn user for overflow?
return btAlignedAlloc(static_cast<size_t>(size), 16);
}
return mem;
void* mem = m_collisionAlgorithmPoolAllocator->allocate(size);
if (NULL == mem)
{
//warn user for overflow?
return btAlignedAlloc(static_cast<size_t>(size), 16);
}
return mem;
}
void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)
@@ -316,7 +281,8 @@ void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)
if (m_collisionAlgorithmPoolAllocator->validPtr(ptr))
{
m_collisionAlgorithmPoolAllocator->freeMemory(ptr);
} else
}
else
{
btAlignedFree(ptr);
}

92
src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
View File

@@ -37,35 +37,30 @@ class btCollisionDispatcher;
///user can override this nearcallback for collision filtering and more finegrained control over collision detection
typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo);
///btCollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs.
///Time of Impact, Closest Points and Penetration Depth.
class btCollisionDispatcher : public btDispatcher
{
protected:
int m_dispatcherFlags;
int m_dispatcherFlags;
btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr;
btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr;
btManifoldResult m_defaultManifoldResult;
btManifoldResult m_defaultManifoldResult;
btNearCallback m_nearCallback;
btNearCallback m_nearCallback;
btPoolAllocator* m_collisionAlgorithmPoolAllocator;
btPoolAllocator* m_collisionAlgorithmPoolAllocator;
btPoolAllocator* m_persistentManifoldPoolAllocator;
btPoolAllocator* m_persistentManifoldPoolAllocator;
btCollisionAlgorithmCreateFunc* m_doubleDispatchContactPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
btCollisionAlgorithmCreateFunc* m_doubleDispatchClosestPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
btCollisionConfiguration* m_collisionConfiguration;
btCollisionConfiguration* m_collisionConfiguration;
public:
enum DispatcherFlags
{
CD_STATIC_STATIC_REPORTED = 1,
@@ -73,103 +68,100 @@ public:
CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION = 4
};
int getDispatcherFlags() const
int getDispatcherFlags() const
{
return m_dispatcherFlags;
}
void setDispatcherFlags(int flags)
void setDispatcherFlags(int flags)
{
m_dispatcherFlags = flags;
}
///registerCollisionCreateFunc allows registration of custom/alternative collision create functions
void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
void registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc);
void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
int getNumManifolds() const
{
return int( m_manifoldsPtr.size());
}
btPersistentManifold** getInternalManifoldPointer()
int getNumManifolds() const
{
return m_manifoldsPtr.size()? &m_manifoldsPtr[0] : 0;
return int(m_manifoldsPtr.size());
}
btPersistentManifold* getManifoldByIndexInternal(int index)
btPersistentManifold** getInternalManifoldPointer()
{
return m_manifoldsPtr.size() ? &m_manifoldsPtr[0] : 0;
}
btPersistentManifold* getManifoldByIndexInternal(int index)
{
return m_manifoldsPtr[index];
}
const btPersistentManifold* getManifoldByIndexInternal(int index) const
const btPersistentManifold* getManifoldByIndexInternal(int index) const
{
return m_manifoldsPtr[index];
}
btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration);
btCollisionDispatcher(btCollisionConfiguration* collisionConfiguration);
virtual ~btCollisionDispatcher();
virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1);
virtual void releaseManifold(btPersistentManifold* manifold);
virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0, const btCollisionObject* b1);
virtual void releaseManifold(btPersistentManifold* manifold);
virtual void clearManifold(btPersistentManifold* manifold);
btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType);
virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1);
virtual bool needsResponse(const btCollisionObject* body0,const btCollisionObject* body1);
virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ;
btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType);
void setNearCallback(btNearCallback nearCallback)
virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1);
virtual bool needsResponse(const btCollisionObject* body0, const btCollisionObject* body1);
virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher);
void setNearCallback(btNearCallback nearCallback)
{
m_nearCallback = nearCallback;
m_nearCallback = nearCallback;
}
btNearCallback getNearCallback() const
btNearCallback getNearCallback() const
{
return m_nearCallback;
}
//by default, Bullet will use this near callback
static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo);
static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo);
virtual void* allocateCollisionAlgorithm(int size);
virtual void* allocateCollisionAlgorithm(int size);
virtual void freeCollisionAlgorithm(void* ptr);
virtual void freeCollisionAlgorithm(void* ptr);
btCollisionConfiguration* getCollisionConfiguration()
btCollisionConfiguration* getCollisionConfiguration()
{
return m_collisionConfiguration;
}
const btCollisionConfiguration* getCollisionConfiguration() const
const btCollisionConfiguration* getCollisionConfiguration() const
{
return m_collisionConfiguration;
}
void setCollisionConfiguration(btCollisionConfiguration* config)
void setCollisionConfiguration(btCollisionConfiguration* config)
{
m_collisionConfiguration = config;
}
virtual btPoolAllocator* getInternalManifoldPool()
virtual btPoolAllocator* getInternalManifoldPool()
{
return m_persistentManifoldPoolAllocator;
}
virtual const btPoolAllocator* getInternalManifoldPool() const
virtual const btPoolAllocator* getInternalManifoldPool() const
{
return m_persistentManifoldPoolAllocator;
}
};
#endif //BT_COLLISION__DISPATCHER_H
#endif //BT_COLLISION__DISPATCHER_H

216
src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp
View File

@@ -13,8 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btCollisionDispatcherMt.h"
#include "LinearMath/btQuickprof.h"
@@ -27,138 +25,132 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
btCollisionDispatcherMt::btCollisionDispatcherMt( btCollisionConfiguration* config, int grainSize )
: btCollisionDispatcher( config )
btCollisionDispatcherMt::btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize)
: btCollisionDispatcher(config)
{
m_batchUpdating = false;
m_grainSize = grainSize; // iterations per task
m_batchUpdating = false;
m_grainSize = grainSize; // iterations per task
}
btPersistentManifold* btCollisionDispatcherMt::getNewManifold( const btCollisionObject* body0, const btCollisionObject* body1 )
btPersistentManifold* btCollisionDispatcherMt::getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1)
{
//optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
//optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
btScalar contactBreakingThreshold = ( m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD ) ?
btMin( body0->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ), body1->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ) )
: gContactBreakingThreshold;
btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold), body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
: gContactBreakingThreshold;
btScalar contactProcessingThreshold = btMin( body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold() );
btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold());
void* mem = m_persistentManifoldPoolAllocator->allocate( sizeof( btPersistentManifold ) );
if ( NULL == mem )
{
//we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
if ( ( m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION ) == 0 )
{
mem = btAlignedAlloc( sizeof( btPersistentManifold ), 16 );
}
else
{
btAssert( 0 );
//make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration
return 0;
}
}
btPersistentManifold* manifold = new( mem ) btPersistentManifold( body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold );
if ( !m_batchUpdating )
{
// batch updater will update manifold pointers array after finishing, so
// only need to update array when not batch-updating
//btAssert( !btThreadsAreRunning() );
manifold->m_index1a = m_manifoldsPtr.size();
m_manifoldsPtr.push_back( manifold );
}
void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
if (NULL == mem)
{
//we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
if ((m_dispatcherFlags & CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION) == 0)
{
mem = btAlignedAlloc(sizeof(btPersistentManifold), 16);
}
else
{
btAssert(0);
//make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration
return 0;
}
}
btPersistentManifold* manifold = new (mem) btPersistentManifold(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold);
if (!m_batchUpdating)
{
// batch updater will update manifold pointers array after finishing, so
// only need to update array when not batch-updating
//btAssert( !btThreadsAreRunning() );
manifold->m_index1a = m_manifoldsPtr.size();
m_manifoldsPtr.push_back(manifold);
}
return manifold;
return manifold;
}
void btCollisionDispatcherMt::releaseManifold( btPersistentManifold* manifold )
void btCollisionDispatcherMt::releaseManifold(btPersistentManifold* manifold)
{
clearManifold( manifold );
//btAssert( !btThreadsAreRunning() );
if ( !m_batchUpdating )
{
// batch updater will update manifold pointers array after finishing, so
// only need to update array when not batch-updating
int findIndex = manifold->m_index1a;
btAssert( findIndex < m_manifoldsPtr.size() );
m_manifoldsPtr.swap( findIndex, m_manifoldsPtr.size() - 1 );
m_manifoldsPtr[ findIndex ]->m_index1a = findIndex;
m_manifoldsPtr.pop_back();
}
clearManifold(manifold);
//btAssert( !btThreadsAreRunning() );
if (!m_batchUpdating)
{
// batch updater will update manifold pointers array after finishing, so
// only need to update array when not batch-updating
int findIndex = manifold->m_index1a;
btAssert(findIndex < m_manifoldsPtr.size());
m_manifoldsPtr.swap(findIndex, m_manifoldsPtr.size() - 1);
m_manifoldsPtr[findIndex]->m_index1a = findIndex;
m_manifoldsPtr.pop_back();
}
manifold->~btPersistentManifold();
if ( m_persistentManifoldPoolAllocator->validPtr( manifold ) )
{
m_persistentManifoldPoolAllocator->freeMemory( manifold );
}
else
{
btAlignedFree( manifold );
}
manifold->~btPersistentManifold();
if (m_persistentManifoldPoolAllocator->validPtr(manifold))
{
m_persistentManifoldPoolAllocator->freeMemory(manifold);
}
else
{
btAlignedFree(manifold);
}
}
struct CollisionDispatcherUpdater : public btIParallelForBody
{
btBroadphasePair* mPairArray;
btNearCallback mCallback;
btCollisionDispatcher* mDispatcher;
const btDispatcherInfo* mInfo;
btBroadphasePair* mPairArray;
btNearCallback mCallback;
btCollisionDispatcher* mDispatcher;
const btDispatcherInfo* mInfo;
CollisionDispatcherUpdater()
{
mPairArray = NULL;
mCallback = NULL;
mDispatcher = NULL;
mInfo = NULL;
}
void forLoop( int iBegin, int iEnd ) const
{
for ( int i = iBegin; i < iEnd; ++i )
{
btBroadphasePair* pair = &mPairArray[ i ];
mCallback( *pair, *mDispatcher, *mInfo );
}
}
CollisionDispatcherUpdater()
{
mPairArray = NULL;
mCallback = NULL;
mDispatcher = NULL;
mInfo = NULL;
}
void forLoop(int iBegin, int iEnd) const
{
for (int i = iBegin; i < iEnd; ++i)
{
btBroadphasePair* pair = &mPairArray[i];
mCallback(*pair, *mDispatcher, *mInfo);
}
}
};
void btCollisionDispatcherMt::dispatchAllCollisionPairs( btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher )
void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher)
{
int pairCount = pairCache->getNumOverlappingPairs();
if ( pairCount == 0 )
{
return;
}
CollisionDispatcherUpdater updater;
updater.mCallback = getNearCallback();
updater.mPairArray = pairCache->getOverlappingPairArrayPtr();
updater.mDispatcher = this;
updater.mInfo = &info;
int pairCount = pairCache->getNumOverlappingPairs();
if (pairCount == 0)
{
return;
}
CollisionDispatcherUpdater updater;
updater.mCallback = getNearCallback();
updater.mPairArray = pairCache->getOverlappingPairArrayPtr();
updater.mDispatcher = this;
updater.mInfo = &info;
m_batchUpdating = true;
btParallelFor( 0, pairCount, m_grainSize, updater );
m_batchUpdating = false;
m_batchUpdating = true;
btParallelFor(0, pairCount, m_grainSize, updater);
m_batchUpdating = false;
// reconstruct the manifolds array to ensure determinism
m_manifoldsPtr.resizeNoInitialize( 0 );
// reconstruct the manifolds array to ensure determinism
m_manifoldsPtr.resizeNoInitialize(0);
btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr();
for ( int i = 0; i < pairCount; ++i )
{
if (btCollisionAlgorithm* algo = pairs[ i ].m_algorithm)
{
algo->getAllContactManifolds( m_manifoldsPtr );
}
}
btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr();
for (int i = 0; i < pairCount; ++i)
{
if (btCollisionAlgorithm* algo = pairs[i].m_algorithm)
{
algo->getAllContactManifolds(m_manifoldsPtr);
}
}
// update the indices (used when releasing manifolds)
for ( int i = 0; i < m_manifoldsPtr.size(); ++i )
{
m_manifoldsPtr[ i ]->m_index1a = i;
}
// update the indices (used when releasing manifolds)
for (int i = 0; i < m_manifoldsPtr.size(); ++i)
{
m_manifoldsPtr[i]->m_index1a = i;
}
}

16
src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h
View File

@@ -19,21 +19,19 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "LinearMath/btThreads.h"
class btCollisionDispatcherMt : public btCollisionDispatcher
{
public:
btCollisionDispatcherMt( btCollisionConfiguration* config, int grainSize = 40 );
btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize = 40);
virtual btPersistentManifold* getNewManifold( const btCollisionObject* body0, const btCollisionObject* body1 ) BT_OVERRIDE;
virtual void releaseManifold( btPersistentManifold* manifold ) BT_OVERRIDE;
virtual btPersistentManifold* getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1) BT_OVERRIDE;
virtual void releaseManifold(btPersistentManifold* manifold) BT_OVERRIDE;
virtual void dispatchAllCollisionPairs( btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher ) BT_OVERRIDE;
virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) BT_OVERRIDE;
protected:
bool m_batchUpdating;
int m_grainSize;
bool m_batchUpdating;
int m_grainSize;
};
#endif //BT_COLLISION_DISPATCHER_MT_H
#endif //BT_COLLISION_DISPATCHER_MT_H

79
src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
View File

@@ -13,42 +13,41 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btCollisionObject.h"
#include "LinearMath/btSerializer.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
btCollisionObject::btCollisionObject()
: m_interpolationLinearVelocity(0.f, 0.f, 0.f),
m_interpolationAngularVelocity(0.f, 0.f, 0.f),
m_anisotropicFriction(1.f,1.f,1.f),
m_hasAnisotropicFriction(false),
m_contactProcessingThreshold(BT_LARGE_FLOAT),
m_broadphaseHandle(0),
m_collisionShape(0),
m_extensionPointer(0),
m_rootCollisionShape(0),
m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
m_islandTag1(-1),
m_companionId(-1),
m_worldArrayIndex(-1),
m_activationState1(1),
m_deactivationTime(btScalar(0.)),
m_friction(btScalar(0.5)),
m_restitution(btScalar(0.)),
m_rollingFriction(0.0f),
m_spinningFriction(0.f),
m_contactDamping(.1),
m_contactStiffness(BT_LARGE_FLOAT),
m_internalType(CO_COLLISION_OBJECT),
m_userObjectPointer(0),
m_userIndex2(-1),
m_userIndex(-1),
m_hitFraction(btScalar(1.)),
m_ccdSweptSphereRadius(btScalar(0.)),
m_ccdMotionThreshold(btScalar(0.)),
m_checkCollideWith(false),
m_updateRevision(0)
: m_interpolationLinearVelocity(0.f, 0.f, 0.f),
m_interpolationAngularVelocity(0.f, 0.f, 0.f),
m_anisotropicFriction(1.f, 1.f, 1.f),
m_hasAnisotropicFriction(false),
m_contactProcessingThreshold(BT_LARGE_FLOAT),
m_broadphaseHandle(0),
m_collisionShape(0),
m_extensionPointer(0),
m_rootCollisionShape(0),
m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
m_islandTag1(-1),
m_companionId(-1),
m_worldArrayIndex(-1),
m_activationState1(1),
m_deactivationTime(btScalar(0.)),
m_friction(btScalar(0.5)),
m_restitution(btScalar(0.)),
m_rollingFriction(0.0f),
m_spinningFriction(0.f),
m_contactDamping(.1),
m_contactStiffness(BT_LARGE_FLOAT),
m_internalType(CO_COLLISION_OBJECT),
m_userObjectPointer(0),
m_userIndex2(-1),
m_userIndex(-1),
m_hitFraction(btScalar(1.)),
m_ccdSweptSphereRadius(btScalar(0.)),
m_ccdMotionThreshold(btScalar(0.)),
m_checkCollideWith(false),
m_updateRevision(0)
{
m_worldTransform.setIdentity();
m_interpolationWorldTransform.setIdentity();
@@ -59,8 +58,8 @@ btCollisionObject::~btCollisionObject()
}
void btCollisionObject::setActivationState(int newState) const
{
if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION))
{
if ((m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION))
m_activationState1 = newState;
}
@@ -71,7 +70,7 @@ void btCollisionObject::forceActivationState(int newState) const
void btCollisionObject::activate(bool forceActivation) const
{
if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT)))
if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT)))
{
setActivationState(ACTIVE_TAG);
m_deactivationTime = btScalar(0.);
@@ -80,7 +79,6 @@ void btCollisionObject::activate(bool forceActivation) const
const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* serializer) const
{
btCollisionObjectData* dataOut = (btCollisionObjectData*)dataBuffer;
m_worldTransform.serialize(dataOut->m_worldTransform);
@@ -92,7 +90,7 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
dataOut->m_contactProcessingThreshold = m_contactProcessingThreshold;
dataOut->m_broadphaseHandle = 0;
dataOut->m_collisionShape = serializer->getUniquePointer(m_collisionShape);
dataOut->m_rootCollisionShape = 0;//@todo
dataOut->m_rootCollisionShape = 0; //@todo
dataOut->m_collisionFlags = m_collisionFlags;
dataOut->m_islandTag1 = m_islandTag1;
dataOut->m_companionId = m_companionId;
@@ -104,8 +102,8 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
dataOut->m_contactStiffness = m_contactStiffness;
dataOut->m_restitution = m_restitution;
dataOut->m_internalType = m_internalType;
char* name = (char*) serializer->findNameForPointer(this);
char* name = (char*)serializer->findNameForPointer(this);
dataOut->m_name = (char*)serializer->getUniquePointer(name);
if (dataOut->m_name)
{
@@ -130,11 +128,10 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
return btCollisionObjectDataName;
}
void btCollisionObject::serializeSingleObject(class btSerializer* serializer) const
{
int len = calculateSerializeBufferSize();
btChunk* chunk = serializer->allocate(len,1);
btChunk* chunk = serializer->allocate(len, 1);
const char* structType = serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk,structType,BT_COLLISIONOBJECT_CODE,(void*)this);
serializer->finalizeChunk(chunk, structType, BT_COLLISIONOBJECT_CODE, (void*)this);
}

372
src/BulletCollision/CollisionDispatch/btCollisionObject.h
View File

@@ -25,8 +25,8 @@ subject to the following restrictions:
#define DISABLE_DEACTIVATION 4
#define DISABLE_SIMULATION 5
struct btBroadphaseProxy;
class btCollisionShape;
struct btBroadphaseProxy;
class btCollisionShape;
struct btCollisionShapeData;
#include "LinearMath/btMotionState.h"
#include "LinearMath/btAlignedAllocator.h"
@@ -42,123 +42,118 @@ typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
#define btCollisionObjectDataName "btCollisionObjectFloatData"
#endif
/// btCollisionObject can be used to manage collision detection objects.
/// btCollisionObject can be used to manage collision detection objects.
/// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
/// They can be added to the btCollisionWorld.
ATTRIBUTE_ALIGNED16(class) btCollisionObject
ATTRIBUTE_ALIGNED16(class)
btCollisionObject
{
protected:
btTransform m_worldTransform;
btTransform m_worldTransform;
///m_interpolationWorldTransform is used for CCD and interpolation
///it can be either previous or future (predicted) transform
btTransform m_interpolationWorldTransform;
//those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities)
btTransform m_interpolationWorldTransform;
//those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities)
//without destroying the continuous interpolated motion (which uses this interpolation velocities)
btVector3 m_interpolationLinearVelocity;
btVector3 m_interpolationAngularVelocity;
btVector3 m_anisotropicFriction;
int m_hasAnisotropicFriction;
btScalar m_contactProcessingThreshold;
btVector3 m_interpolationLinearVelocity;
btVector3 m_interpolationAngularVelocity;
btBroadphaseProxy* m_broadphaseHandle;
btCollisionShape* m_collisionShape;
btVector3 m_anisotropicFriction;
int m_hasAnisotropicFriction;
btScalar m_contactProcessingThreshold;
btBroadphaseProxy* m_broadphaseHandle;
btCollisionShape* m_collisionShape;
///m_extensionPointer is used by some internal low-level Bullet extensions.
void* m_extensionPointer;
void* m_extensionPointer;
///m_rootCollisionShape is temporarily used to store the original collision shape
///The m_collisionShape might be temporarily replaced by a child collision shape during collision detection purposes
///If it is NULL, the m_collisionShape is not temporarily replaced.
btCollisionShape* m_rootCollisionShape;
btCollisionShape* m_rootCollisionShape;
int m_collisionFlags;
int m_collisionFlags;
int m_islandTag1;
int m_companionId;
int m_worldArrayIndex; // index of object in world's collisionObjects array
int m_islandTag1;
int m_companionId;
int m_worldArrayIndex; // index of object in world's collisionObjects array
mutable int m_activationState1;
mutable btScalar m_deactivationTime;
mutable int m_activationState1;
mutable btScalar m_deactivationTime;
btScalar m_friction;
btScalar m_restitution;
btScalar m_rollingFriction;//torsional friction orthogonal to contact normal (useful to stop spheres rolling forever)
btScalar m_spinningFriction; // torsional friction around the contact normal (useful for grasping)
btScalar m_contactDamping;
btScalar m_contactStiffness;
btScalar m_friction;
btScalar m_restitution;
btScalar m_rollingFriction; //torsional friction orthogonal to contact normal (useful to stop spheres rolling forever)
btScalar m_spinningFriction; // torsional friction around the contact normal (useful for grasping)
btScalar m_contactDamping;
btScalar m_contactStiffness;
///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
///do not assign your own m_internalType unless you write a new dynamics object class.
int m_internalType;
int m_internalType;
///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
void* m_userObjectPointer;
void* m_userObjectPointer;
int m_userIndex2;
int m_userIndex;
int m_userIndex2;
int m_userIndex;
///time of impact calculation
btScalar m_hitFraction;
btScalar m_hitFraction;
///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
btScalar m_ccdSweptSphereRadius;
btScalar m_ccdSweptSphereRadius;
/// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
btScalar m_ccdMotionThreshold;
btScalar m_ccdMotionThreshold;
/// If some object should have elaborate collision filtering by sub-classes
int m_checkCollideWith;
int m_checkCollideWith;
btAlignedObjectArray<const btCollisionObject*> m_objectsWithoutCollisionCheck;
///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation.
int m_updateRevision;
int m_updateRevision;
btVector3 m_customDebugColorRGB;
btVector3 m_customDebugColorRGB;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
enum CollisionFlags
{
CF_STATIC_OBJECT= 1,
CF_KINEMATIC_OBJECT= 2,
CF_STATIC_OBJECT = 1,
CF_KINEMATIC_OBJECT = 2,
CF_NO_CONTACT_RESPONSE = 4,
CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
CF_CUSTOM_MATERIAL_CALLBACK = 8, //this allows per-triangle material (friction/restitution)
CF_CHARACTER_OBJECT = 16,
CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing
CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
CF_DISABLE_SPU_COLLISION_PROCESSING = 64, //disable parallel/SPU processing
CF_HAS_CONTACT_STIFFNESS_DAMPING = 128,
CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256,
CF_HAS_FRICTION_ANCHOR = 512,
CF_HAS_COLLISION_SOUND_TRIGGER = 1024
};
enum CollisionObjectTypes
enum CollisionObjectTypes
{
CO_COLLISION_OBJECT =1,
CO_RIGID_BODY=2,
CO_COLLISION_OBJECT = 1,
CO_RIGID_BODY = 2,
///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter
///It is useful for collision sensors, explosion objects, character controller etc.
CO_GHOST_OBJECT=4,
CO_SOFT_BODY=8,
CO_HF_FLUID=16,
CO_USER_TYPE=32,
CO_FEATHERSTONE_LINK=64
CO_GHOST_OBJECT = 4,
CO_SOFT_BODY = 8,
CO_HF_FLUID = 16,
CO_USER_TYPE = 32,
CO_FEATHERSTONE_LINK = 64
};
enum AnisotropicFrictionFlags
{
CF_ANISOTROPIC_FRICTION_DISABLED=0,
CF_ANISOTROPIC_FRICTION_DISABLED = 0,
CF_ANISOTROPIC_FRICTION = 1,
CF_ANISOTROPIC_ROLLING_FRICTION = 2
};
@@ -166,76 +161,77 @@ public:
SIMD_FORCE_INLINE bool mergesSimulationIslands() const
{
///static objects, kinematic and object without contact response don't merge islands
return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0);
return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE)) == 0);
}
const btVector3& getAnisotropicFriction() const
{
return m_anisotropicFriction;
}
void setAnisotropicFriction(const btVector3& anisotropicFriction, int frictionMode = CF_ANISOTROPIC_FRICTION)
void setAnisotropicFriction(const btVector3& anisotropicFriction, int frictionMode = CF_ANISOTROPIC_FRICTION)
{
m_anisotropicFriction = anisotropicFriction;
bool isUnity = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f);
m_hasAnisotropicFriction = isUnity?frictionMode : 0;
bool isUnity = (anisotropicFriction[0] != 1.f) || (anisotropicFriction[1] != 1.f) || (anisotropicFriction[2] != 1.f);
m_hasAnisotropicFriction = isUnity ? frictionMode : 0;
}
bool hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const
bool hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const
{
return (m_hasAnisotropicFriction&frictionMode)!=0;
return (m_hasAnisotropicFriction & frictionMode) != 0;
}
///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default.
///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges
void setContactProcessingThreshold( btScalar contactProcessingThreshold)
void setContactProcessingThreshold(btScalar contactProcessingThreshold)
{
m_contactProcessingThreshold = contactProcessingThreshold;
}
btScalar getContactProcessingThreshold() const
btScalar getContactProcessingThreshold() const
{
return m_contactProcessingThreshold;
}
SIMD_FORCE_INLINE bool isStaticObject() const {
SIMD_FORCE_INLINE bool isStaticObject() const
{
return (m_collisionFlags & CF_STATIC_OBJECT) != 0;
}
SIMD_FORCE_INLINE bool isKinematicObject() const
SIMD_FORCE_INLINE bool isKinematicObject() const
{
return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0;
}
SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const
SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const
{
return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ;
return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0;
}
SIMD_FORCE_INLINE bool hasContactResponse() const {
return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0;
SIMD_FORCE_INLINE bool hasContactResponse() const
{
return (m_collisionFlags & CF_NO_CONTACT_RESPONSE) == 0;
}
btCollisionObject();
virtual ~btCollisionObject();
virtual void setCollisionShape(btCollisionShape* collisionShape)
virtual void setCollisionShape(btCollisionShape * collisionShape)
{
m_updateRevision++;
m_collisionShape = collisionShape;
m_rootCollisionShape = collisionShape;
}
SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const
SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const
{
return m_collisionShape;
}
SIMD_FORCE_INLINE btCollisionShape* getCollisionShape()
SIMD_FORCE_INLINE btCollisionShape* getCollisionShape()
{
return m_collisionShape;
}
void setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck)
void setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck)
{
if (ignoreCollisionCheck)
{
@@ -253,7 +249,7 @@ public:
m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0;
}
virtual bool checkCollideWithOverride(const btCollisionObject* co) const
virtual bool checkCollideWithOverride(const btCollisionObject* co) const
{
int index = m_objectsWithoutCollisionCheck.findLinearSearch(co);
if (index < m_objectsWithoutCollisionCheck.size())
@@ -263,317 +259,309 @@ public:
return true;
}
///Avoid using this internal API call, the extension pointer is used by some Bullet extensions.
///Avoid using this internal API call, the extension pointer is used by some Bullet extensions.
///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
void* internalGetExtensionPointer() const
void* internalGetExtensionPointer() const
{
return m_extensionPointer;
}
///Avoid using this internal API call, the extension pointer is used by some Bullet extensions
///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
void internalSetExtensionPointer(void* pointer)
void internalSetExtensionPointer(void* pointer)
{
m_extensionPointer = pointer;
}
SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1;}
SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1; }
void setActivationState(int newState) const;
void setDeactivationTime(btScalar time)
void setDeactivationTime(btScalar time)
{
m_deactivationTime = time;
}
btScalar getDeactivationTime() const
btScalar getDeactivationTime() const
{
return m_deactivationTime;
}
void forceActivationState(int newState) const;
void activate(bool forceActivation = false) const;
void activate(bool forceActivation = false) const;
SIMD_FORCE_INLINE bool isActive() const
{
return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
}
void setRestitution(btScalar rest)
void setRestitution(btScalar rest)
{
m_updateRevision++;
m_restitution = rest;
}
btScalar getRestitution() const
btScalar getRestitution() const
{
return m_restitution;
}
void setFriction(btScalar frict)
void setFriction(btScalar frict)
{
m_updateRevision++;
m_friction = frict;
}
btScalar getFriction() const
btScalar getFriction() const
{
return m_friction;
}
void setRollingFriction(btScalar frict)
void setRollingFriction(btScalar frict)
{
m_updateRevision++;
m_rollingFriction = frict;
}
btScalar getRollingFriction() const
btScalar getRollingFriction() const
{
return m_rollingFriction;
}
void setSpinningFriction(btScalar frict)
{
m_updateRevision++;
m_spinningFriction = frict;
}
btScalar getSpinningFriction() const
{
return m_spinningFriction;
}
void setContactStiffnessAndDamping(btScalar stiffness, btScalar damping)
void setSpinningFriction(btScalar frict)
{
m_updateRevision++;
m_spinningFriction = frict;
}
btScalar getSpinningFriction() const
{
return m_spinningFriction;
}
void setContactStiffnessAndDamping(btScalar stiffness, btScalar damping)
{
m_updateRevision++;
m_contactStiffness = stiffness;
m_contactDamping = damping;
m_collisionFlags |=CF_HAS_CONTACT_STIFFNESS_DAMPING;
//avoid divisions by zero...
if (m_contactStiffness< SIMD_EPSILON)
{
m_contactStiffness = SIMD_EPSILON;
}
m_collisionFlags |= CF_HAS_CONTACT_STIFFNESS_DAMPING;
//avoid divisions by zero...
if (m_contactStiffness < SIMD_EPSILON)
{
m_contactStiffness = SIMD_EPSILON;
}
}
btScalar getContactStiffness() const
btScalar getContactStiffness() const
{
return m_contactStiffness;
}
btScalar getContactDamping() const
btScalar getContactDamping() const
{
return m_contactDamping;
}
///reserved for Bullet internal usage
int getInternalType() const
int getInternalType() const
{
return m_internalType;
}
btTransform& getWorldTransform()
btTransform& getWorldTransform()
{
return m_worldTransform;
}
const btTransform& getWorldTransform() const
const btTransform& getWorldTransform() const
{
return m_worldTransform;
}
void setWorldTransform(const btTransform& worldTrans)
void setWorldTransform(const btTransform& worldTrans)
{
m_updateRevision++;
m_worldTransform = worldTrans;
}
SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle()
SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle()
{
return m_broadphaseHandle;
}
SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const
SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const
{
return m_broadphaseHandle;
}
void setBroadphaseHandle(btBroadphaseProxy* handle)
void setBroadphaseHandle(btBroadphaseProxy * handle)
{
m_broadphaseHandle = handle;
}
const btTransform& getInterpolationWorldTransform() const
const btTransform& getInterpolationWorldTransform() const
{
return m_interpolationWorldTransform;
}
btTransform& getInterpolationWorldTransform()
btTransform& getInterpolationWorldTransform()
{
return m_interpolationWorldTransform;
}
void setInterpolationWorldTransform(const btTransform& trans)
void setInterpolationWorldTransform(const btTransform& trans)
{
m_updateRevision++;
m_interpolationWorldTransform = trans;
}
void setInterpolationLinearVelocity(const btVector3& linvel)
void setInterpolationLinearVelocity(const btVector3& linvel)
{
m_updateRevision++;
m_interpolationLinearVelocity = linvel;
}
void setInterpolationAngularVelocity(const btVector3& angvel)
void setInterpolationAngularVelocity(const btVector3& angvel)
{
m_updateRevision++;
m_interpolationAngularVelocity = angvel;
}
const btVector3& getInterpolationLinearVelocity() const
const btVector3& getInterpolationLinearVelocity() const
{
return m_interpolationLinearVelocity;
}
const btVector3& getInterpolationAngularVelocity() const
const btVector3& getInterpolationAngularVelocity() const
{
return m_interpolationAngularVelocity;
}
SIMD_FORCE_INLINE int getIslandTag() const
{
return m_islandTag1;
return m_islandTag1;
}
void setIslandTag(int tag)
void setIslandTag(int tag)
{
m_islandTag1 = tag;
}
SIMD_FORCE_INLINE int getCompanionId() const
{
return m_companionId;
return m_companionId;
}
void setCompanionId(int id)
void setCompanionId(int id)
{
m_companionId = id;
}
SIMD_FORCE_INLINE int getWorldArrayIndex() const
{
return m_worldArrayIndex;
}
// only should be called by CollisionWorld
void setWorldArrayIndex(int ix)
{
m_worldArrayIndex = ix;
}
SIMD_FORCE_INLINE btScalar getHitFraction() const
SIMD_FORCE_INLINE int getWorldArrayIndex() const
{
return m_hitFraction;
return m_worldArrayIndex;
}
void setHitFraction(btScalar hitFraction)
// only should be called by CollisionWorld
void setWorldArrayIndex(int ix)
{
m_worldArrayIndex = ix;
}
SIMD_FORCE_INLINE btScalar getHitFraction() const
{
return m_hitFraction;
}
void setHitFraction(btScalar hitFraction)
{
m_hitFraction = hitFraction;
}
SIMD_FORCE_INLINE int getCollisionFlags() const
SIMD_FORCE_INLINE int getCollisionFlags() const
{
return m_collisionFlags;
}
void setCollisionFlags(int flags)
void setCollisionFlags(int flags)
{
m_collisionFlags = flags;
}
///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
btScalar getCcdSweptSphereRadius() const
btScalar getCcdSweptSphereRadius() const
{
return m_ccdSweptSphereRadius;
}
///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
void setCcdSweptSphereRadius(btScalar radius)
void setCcdSweptSphereRadius(btScalar radius)
{
m_ccdSweptSphereRadius = radius;
}
btScalar getCcdMotionThreshold() const
btScalar getCcdMotionThreshold() const
{
return m_ccdMotionThreshold;
}
btScalar getCcdSquareMotionThreshold() const
btScalar getCcdSquareMotionThreshold() const
{
return m_ccdMotionThreshold*m_ccdMotionThreshold;
return m_ccdMotionThreshold * m_ccdMotionThreshold;
}
/// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
void setCcdMotionThreshold(btScalar ccdMotionThreshold)
void setCcdMotionThreshold(btScalar ccdMotionThreshold)
{
m_ccdMotionThreshold = ccdMotionThreshold;
}
///users can point to their objects, userPointer is not used by Bullet
void* getUserPointer() const
void* getUserPointer() const
{
return m_userObjectPointer;
}
int getUserIndex() const
int getUserIndex() const
{
return m_userIndex;
}
int getUserIndex2() const
int getUserIndex2() const
{
return m_userIndex2;
}
///users can point to their objects, userPointer is not used by Bullet
void setUserPointer(void* userPointer)
void setUserPointer(void* userPointer)
{
m_userObjectPointer = userPointer;
}
///users can point to their objects, userPointer is not used by Bullet
void setUserIndex(int index)
void setUserIndex(int index)
{
m_userIndex = index;
}
void setUserIndex2(int index)
void setUserIndex2(int index)
{
m_userIndex2 = index;
}
int getUpdateRevisionInternal() const
int getUpdateRevisionInternal() const
{
return m_updateRevision;
}
void setCustomDebugColor(const btVector3& colorRGB)
void setCustomDebugColor(const btVector3& colorRGB)
{
m_customDebugColorRGB = colorRGB;
m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR;
}
void removeCustomDebugColor()
void removeCustomDebugColor()
{
m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR;
}
bool getCustomDebugColor(btVector3& colorRGB) const
bool getCustomDebugColor(btVector3 & colorRGB) const
{
bool hasCustomColor = (0!=(m_collisionFlags&CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR));
bool hasCustomColor = (0 != (m_collisionFlags & CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR));
if (hasCustomColor)
{
colorRGB = m_customDebugColorRGB;
@@ -589,15 +577,16 @@ public:
return true;
}
virtual int calculateSerializeBufferSize() const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
virtual void serializeSingleObject(class btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
virtual void serializeSingleObject(class btSerializer * serializer) const;
};
// clang-format off
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btCollisionObjectDoubleData
{
@@ -667,14 +656,11 @@ struct btCollisionObjectFloatData
int m_collisionFilterMask;
int m_uniqueId;
};
// clang-format on
SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const
{
return sizeof(btCollisionObjectData);
}
#endif //BT_COLLISION_OBJECT_H
#endif //BT_COLLISION_OBJECT_H

26
src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h
View File

@@ -1,25 +1,25 @@
#ifndef BT_COLLISION_OBJECT_WRAPPER_H
#define BT_COLLISION_OBJECT_WRAPPER_H
///btCollisionObjectWrapperis an internal data structure.
///btCollisionObjectWrapperis an internal data structure.
///Most users can ignore this and use btCollisionObject and btCollisionShape instead
class btCollisionShape;
class btCollisionObject;
class btTransform;
#include "LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition
#include "LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition
#define BT_DECLARE_STACK_ONLY_OBJECT \
private: \
void* operator new(size_t size); \
void operator delete(void*);
private: \
void* operator new(size_t size); \
void operator delete(void*);
struct btCollisionObjectWrapper;
struct btCollisionObjectWrapper
{
BT_DECLARE_STACK_ONLY_OBJECT
BT_DECLARE_STACK_ONLY_OBJECT
private:
btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed.
btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed.
btCollisionObjectWrapper* operator=(const btCollisionObjectWrapper&);
public:
@@ -27,17 +27,17 @@ public:
const btCollisionShape* m_shape;
const btCollisionObject* m_collisionObject;
const btTransform& m_worldTransform;
int m_partId;
int m_index;
int m_partId;
int m_index;
btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, int partId, int index)
: m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform),
m_partId(partId), m_index(index)
{}
: m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_partId(partId), m_index(index)
{
}
SIMD_FORCE_INLINE const btTransform& getWorldTransform() const { return m_worldTransform; }
SIMD_FORCE_INLINE const btCollisionObject* getCollisionObject() const { return m_collisionObject; }
SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_shape; }
};
#endif //BT_COLLISION_OBJECT_WRAPPER_H
#endif //BT_COLLISION_OBJECT_WRAPPER_H

1343
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
View File

File diff suppressed because it is too large Load Diff

345
src/BulletCollision/CollisionDispatch/btCollisionWorld.h
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
/**
* @mainpage Bullet Documentation
*
@@ -66,8 +65,6 @@ subject to the following restrictions:
* For up-to-data information and copyright and contributors list check out the Bullet_User_Manual.pdf
*
*/
#ifndef BT_COLLISION_WORLD_H
#define BT_COLLISION_WORLD_H
@@ -87,147 +84,138 @@ class btSerializer;
///CollisionWorld is interface and container for the collision detection
class btCollisionWorld
{
protected:
btAlignedObjectArray<btCollisionObject*> m_collisionObjects;
btAlignedObjectArray<btCollisionObject*> m_collisionObjects;
btDispatcher* m_dispatcher1;
btDispatcher* m_dispatcher1;
btDispatcherInfo m_dispatchInfo;
btDispatcherInfo m_dispatchInfo;
btBroadphaseInterface* m_broadphasePairCache;
btBroadphaseInterface* m_broadphasePairCache;
btIDebugDraw* m_debugDrawer;
btIDebugDraw* m_debugDrawer;
///m_forceUpdateAllAabbs can be set to false as an optimization to only update active object AABBs
///it is true by default, because it is error-prone (setting the position of static objects wouldn't update their AABB)
bool m_forceUpdateAllAabbs;
void serializeCollisionObjects(btSerializer* serializer);
void serializeCollisionObjects(btSerializer* serializer);
void serializeContactManifolds(btSerializer* serializer);
public:
//this constructor doesn't own the dispatcher and paircache/broadphase
btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration);
btCollisionWorld(btDispatcher* dispatcher, btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration);
virtual ~btCollisionWorld();
void setBroadphase(btBroadphaseInterface* pairCache)
void setBroadphase(btBroadphaseInterface* pairCache)
{
m_broadphasePairCache = pairCache;
}
const btBroadphaseInterface* getBroadphase() const
const btBroadphaseInterface* getBroadphase() const
{
return m_broadphasePairCache;
}
btBroadphaseInterface* getBroadphase()
btBroadphaseInterface* getBroadphase()
{
return m_broadphasePairCache;
}
btOverlappingPairCache* getPairCache()
btOverlappingPairCache* getPairCache()
{
return m_broadphasePairCache->getOverlappingPairCache();
}
btDispatcher* getDispatcher()
btDispatcher* getDispatcher()
{
return m_dispatcher1;
}
const btDispatcher* getDispatcher() const
const btDispatcher* getDispatcher() const
{
return m_dispatcher1;
}
void updateSingleAabb(btCollisionObject* colObj);
void updateSingleAabb(btCollisionObject* colObj);
virtual void updateAabbs();
virtual void updateAabbs();
///the computeOverlappingPairs is usually already called by performDiscreteCollisionDetection (or stepSimulation)
///it can be useful to use if you perform ray tests without collision detection/simulation
virtual void computeOverlappingPairs();
virtual void computeOverlappingPairs();
virtual void setDebugDrawer(btIDebugDraw* debugDrawer)
virtual void setDebugDrawer(btIDebugDraw* debugDrawer)
{
m_debugDrawer = debugDrawer;
m_debugDrawer = debugDrawer;
}
virtual btIDebugDraw* getDebugDrawer()
virtual btIDebugDraw* getDebugDrawer()
{
return m_debugDrawer;
}
virtual void debugDrawWorld();
virtual void debugDrawWorld();
virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
///LocalShapeInfo gives extra information for complex shapes
///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart
struct LocalShapeInfo
struct LocalShapeInfo
{
int m_shapePart;
int m_triangleIndex;
int m_shapePart;
int m_triangleIndex;
//const btCollisionShape* m_shapeTemp;
//const btTransform* m_shapeLocalTransform;
};
struct LocalRayResult
struct LocalRayResult
{
LocalRayResult(const btCollisionObject* collisionObject,
LocalShapeInfo* localShapeInfo,
const btVector3& hitNormalLocal,
btScalar hitFraction)
:m_collisionObject(collisionObject),
m_localShapeInfo(localShapeInfo),
m_hitNormalLocal(hitNormalLocal),
m_hitFraction(hitFraction)
LocalRayResult(const btCollisionObject* collisionObject,
LocalShapeInfo* localShapeInfo,
const btVector3& hitNormalLocal,
btScalar hitFraction)
: m_collisionObject(collisionObject),
m_localShapeInfo(localShapeInfo),
m_hitNormalLocal(hitNormalLocal),
m_hitFraction(hitFraction)
{
}
const btCollisionObject* m_collisionObject;
LocalShapeInfo* m_localShapeInfo;
btVector3 m_hitNormalLocal;
btScalar m_hitFraction;
const btCollisionObject* m_collisionObject;
LocalShapeInfo* m_localShapeInfo;
btVector3 m_hitNormalLocal;
btScalar m_hitFraction;
};
///RayResultCallback is used to report new raycast results
struct RayResultCallback
struct RayResultCallback
{
btScalar m_closestHitFraction;
const btCollisionObject* m_collisionObject;
int m_collisionFilterGroup;
int m_collisionFilterMask;
btScalar m_closestHitFraction;
const btCollisionObject* m_collisionObject;
int m_collisionFilterGroup;
int m_collisionFilterMask;
//@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback.h. Apply any of the EFlags defined there on m_flags here to invoke.
unsigned int m_flags;
virtual ~RayResultCallback()
{
}
bool hasHit() const
bool hasHit() const
{
return (m_collisionObject != 0);
}
RayResultCallback()
:m_closestHitFraction(btScalar(1.)),
m_collisionObject(0),
m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
m_collisionFilterMask(btBroadphaseProxy::AllFilter),
//@BP Mod
m_flags(0)
: m_closestHitFraction(btScalar(1.)),
m_collisionObject(0),
m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
m_collisionFilterMask(btBroadphaseProxy::AllFilter),
//@BP Mod
m_flags(0)
{
}
@@ -238,62 +226,62 @@ public:
return collides;
}
virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) = 0;
virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) = 0;
};
struct ClosestRayResultCallback : public RayResultCallback
struct ClosestRayResultCallback : public RayResultCallback
{
ClosestRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld)
:m_rayFromWorld(rayFromWorld),
m_rayToWorld(rayToWorld)
ClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld)
: m_rayFromWorld(rayFromWorld),
m_rayToWorld(rayToWorld)
{
}
btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
btVector3 m_rayToWorld;
btVector3 m_rayFromWorld; //used to calculate hitPointWorld from hitFraction
btVector3 m_rayToWorld;
btVector3 m_hitNormalWorld;
btVector3 m_hitPointWorld;
virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
btVector3 m_hitNormalWorld;
btVector3 m_hitPointWorld;
virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace)
{
//caller already does the filter on the m_closestHitFraction
btAssert(rayResult.m_hitFraction <= m_closestHitFraction);
m_closestHitFraction = rayResult.m_hitFraction;
m_collisionObject = rayResult.m_collisionObject;
if (normalInWorldSpace)
{
m_hitNormalWorld = rayResult.m_hitNormalLocal;
} else
}
else
{
///need to transform normal into worldspace
m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal;
}
m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction);
return rayResult.m_hitFraction;
}
};
struct AllHitsRayResultCallback : public RayResultCallback
struct AllHitsRayResultCallback : public RayResultCallback
{
AllHitsRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld)
:m_rayFromWorld(rayFromWorld),
m_rayToWorld(rayToWorld)
AllHitsRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld)
: m_rayFromWorld(rayFromWorld),
m_rayToWorld(rayToWorld)
{
}
btAlignedObjectArray<const btCollisionObject*> m_collisionObjects;
btAlignedObjectArray<const btCollisionObject*> m_collisionObjects;
btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
btVector3 m_rayToWorld;
btVector3 m_rayFromWorld; //used to calculate hitPointWorld from hitFraction
btVector3 m_rayToWorld;
btAlignedObjectArray<btVector3> m_hitNormalWorld;
btAlignedObjectArray<btVector3> m_hitPointWorld;
btAlignedObjectArray<btVector3> m_hitNormalWorld;
btAlignedObjectArray<btVector3> m_hitPointWorld;
btAlignedObjectArray<btScalar> m_hitFractions;
virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace)
{
m_collisionObject = rayResult.m_collisionObject;
m_collisionObjects.push_back(rayResult.m_collisionObject);
@@ -301,69 +289,66 @@ public:
if (normalInWorldSpace)
{
hitNormalWorld = rayResult.m_hitNormalLocal;
} else
}
else
{
///need to transform normal into worldspace
hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal;
}
m_hitNormalWorld.push_back(hitNormalWorld);
btVector3 hitPointWorld;
hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction);
m_hitPointWorld.push_back(hitPointWorld);
m_hitFractions.push_back(rayResult.m_hitFraction);
return m_closestHitFraction;
}
};
struct LocalConvexResult
{
LocalConvexResult(const btCollisionObject* hitCollisionObject,
LocalShapeInfo* localShapeInfo,
const btVector3& hitNormalLocal,
const btVector3& hitPointLocal,
btScalar hitFraction
)
:m_hitCollisionObject(hitCollisionObject),
m_localShapeInfo(localShapeInfo),
m_hitNormalLocal(hitNormalLocal),
m_hitPointLocal(hitPointLocal),
m_hitFraction(hitFraction)
LocalConvexResult(const btCollisionObject* hitCollisionObject,
LocalShapeInfo* localShapeInfo,
const btVector3& hitNormalLocal,
const btVector3& hitPointLocal,
btScalar hitFraction)
: m_hitCollisionObject(hitCollisionObject),
m_localShapeInfo(localShapeInfo),
m_hitNormalLocal(hitNormalLocal),
m_hitPointLocal(hitPointLocal),
m_hitFraction(hitFraction)
{
}
const btCollisionObject* m_hitCollisionObject;
LocalShapeInfo* m_localShapeInfo;
btVector3 m_hitNormalLocal;
btVector3 m_hitPointLocal;
btScalar m_hitFraction;
const btCollisionObject* m_hitCollisionObject;
LocalShapeInfo* m_localShapeInfo;
btVector3 m_hitNormalLocal;
btVector3 m_hitPointLocal;
btScalar m_hitFraction;
};
///RayResultCallback is used to report new raycast results
struct ConvexResultCallback
struct ConvexResultCallback
{
btScalar m_closestHitFraction;
int m_collisionFilterGroup;
int m_collisionFilterMask;
btScalar m_closestHitFraction;
int m_collisionFilterGroup;
int m_collisionFilterMask;
ConvexResultCallback()
:m_closestHitFraction(btScalar(1.)),
m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
m_collisionFilterMask(btBroadphaseProxy::AllFilter)
: m_closestHitFraction(btScalar(1.)),
m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
m_collisionFilterMask(btBroadphaseProxy::AllFilter)
{
}
virtual ~ConvexResultCallback()
{
}
bool hasHit() const
bool hasHit() const
{
return (m_closestHitFraction < btScalar(1.));
}
virtual bool needsCollision(btBroadphaseProxy* proxy0) const
{
bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
@@ -371,39 +356,40 @@ public:
return collides;
}
virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) = 0;
virtual btScalar addSingleResult(LocalConvexResult& convexResult, bool normalInWorldSpace) = 0;
};
struct ClosestConvexResultCallback : public ConvexResultCallback
struct ClosestConvexResultCallback : public ConvexResultCallback
{
ClosestConvexResultCallback(const btVector3& convexFromWorld,const btVector3& convexToWorld)
:m_convexFromWorld(convexFromWorld),
m_convexToWorld(convexToWorld),
m_hitCollisionObject(0)
ClosestConvexResultCallback(const btVector3& convexFromWorld, const btVector3& convexToWorld)
: m_convexFromWorld(convexFromWorld),
m_convexToWorld(convexToWorld),
m_hitCollisionObject(0)
{
}
btVector3 m_convexFromWorld;//used to calculate hitPointWorld from hitFraction
btVector3 m_convexToWorld;
btVector3 m_convexFromWorld; //used to calculate hitPointWorld from hitFraction
btVector3 m_convexToWorld;
btVector3 m_hitNormalWorld;
btVector3 m_hitPointWorld;
const btCollisionObject* m_hitCollisionObject;
virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace)
btVector3 m_hitNormalWorld;
btVector3 m_hitPointWorld;
const btCollisionObject* m_hitCollisionObject;
virtual btScalar addSingleResult(LocalConvexResult& convexResult, bool normalInWorldSpace)
{
//caller already does the filter on the m_closestHitFraction
//caller already does the filter on the m_closestHitFraction
btAssert(convexResult.m_hitFraction <= m_closestHitFraction);
m_closestHitFraction = convexResult.m_hitFraction;
m_hitCollisionObject = convexResult.m_hitCollisionObject;
if (normalInWorldSpace)
{
m_hitNormalWorld = convexResult.m_hitNormalLocal;
} else
}
else
{
///need to transform normal into worldspace
m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis() * convexResult.m_hitNormalLocal;
}
m_hitPointWorld = convexResult.m_hitPointLocal;
return convexResult.m_hitFraction;
@@ -411,23 +397,23 @@ public:
};
///ContactResultCallback is used to report contact points
struct ContactResultCallback
struct ContactResultCallback
{
int m_collisionFilterGroup;
int m_collisionFilterMask;
btScalar m_closestDistanceThreshold;
int m_collisionFilterGroup;
int m_collisionFilterMask;
btScalar m_closestDistanceThreshold;
ContactResultCallback()
:m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
m_collisionFilterMask(btBroadphaseProxy::AllFilter),
m_closestDistanceThreshold(0)
: m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
m_collisionFilterMask(btBroadphaseProxy::AllFilter),
m_closestDistanceThreshold(0)
{
}
virtual ~ContactResultCallback()
{
}
virtual bool needsCollision(btBroadphaseProxy* proxy0) const
{
bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
@@ -435,61 +421,57 @@ public:
return collides;
}
virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) = 0;
virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) = 0;
};
int getNumCollisionObjects() const
int getNumCollisionObjects() const
{
return int(m_collisionObjects.size());
}
/// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback
/// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const;
virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const;
/// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
/// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
void convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = btScalar(0.)) const;
void convexSweepTest(const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = btScalar(0.)) const;
///contactTest performs a discrete collision test between colObj against all objects in the btCollisionWorld, and calls the resultCallback.
///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
void contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback);
void contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback);
///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
///it reports one or more contact points (including the one with deepest penetration)
void contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback);
void contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback);
/// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
/// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape.
/// This allows more customization.
static void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
btCollisionObject* collisionObject,
const btCollisionShape* collisionShape,
const btTransform& colObjWorldTransform,
RayResultCallback& resultCallback);
static void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
btCollisionObject* collisionObject,
const btCollisionShape* collisionShape,
const btTransform& colObjWorldTransform,
RayResultCallback& resultCallback);
static void rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans,
const btCollisionObjectWrapper* collisionObjectWrap,
RayResultCallback& resultCallback);
static void rayTestSingleInternal(const btTransform& rayFromTrans, const btTransform& rayToTrans,
const btCollisionObjectWrapper* collisionObjectWrap,
RayResultCallback& resultCallback);
/// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
static void objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans,const btTransform& rayToTrans,
btCollisionObject* collisionObject,
const btCollisionShape* collisionShape,
const btTransform& colObjWorldTransform,
ConvexResultCallback& resultCallback, btScalar allowedPenetration);
static void objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans, const btTransform& rayToTrans,
btCollisionObject* collisionObject,
const btCollisionShape* collisionShape,
const btTransform& colObjWorldTransform,
ConvexResultCallback& resultCallback, btScalar allowedPenetration);
static void objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
const btCollisionObjectWrapper* colObjWrap,
ConvexResultCallback& resultCallback, btScalar allowedPenetration);
static void objectQuerySingleInternal(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans,
const btCollisionObjectWrapper* colObjWrap,
ConvexResultCallback& resultCallback, btScalar allowedPenetration);
virtual void addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter);
virtual void refreshBroadphaseProxy(btCollisionObject* collisionObject);
virtual void addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
virtual void refreshBroadphaseProxy(btCollisionObject* collisionObject);
btCollisionObjectArray& getCollisionObjectArray()
{
@@ -501,10 +483,9 @@ public:
return m_collisionObjects;
}
virtual void removeCollisionObject(btCollisionObject* collisionObject);
virtual void removeCollisionObject(btCollisionObject* collisionObject);
virtual void performDiscreteCollisionDetection();
virtual void performDiscreteCollisionDetection();
btDispatcherInfo& getDispatchInfo()
{
@@ -515,20 +496,18 @@ public:
{
return m_dispatchInfo;
}
bool getForceUpdateAllAabbs() const
bool getForceUpdateAllAabbs() const
{
return m_forceUpdateAllAabbs;
}
void setForceUpdateAllAabbs( bool forceUpdateAllAabbs)
void setForceUpdateAllAabbs(bool forceUpdateAllAabbs)
{
m_forceUpdateAllAabbs = forceUpdateAllAabbs;
}
///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (Bullet/Demos/SerializeDemo)
virtual void serialize(btSerializer* serializer);
virtual void serialize(btSerializer* serializer);
};
#endif //BT_COLLISION_WORLD_H
#endif //BT_COLLISION_WORLD_H

866
src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp
View File

File diff suppressed because it is too large Load Diff

98
src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_COLLISION_WORLD_IMPORTER_H
#define BT_COLLISION_WORLD_IMPORTER_H
@@ -26,7 +25,6 @@ class btCollisionShape;
class btCollisionObject;
struct btBulletSerializedArrays;
struct ConstraintInput;
class btCollisionWorld;
struct btCollisionShapeData;
@@ -46,9 +44,6 @@ class btSliderConstraint;
class btGearConstraint;
struct btContactSolverInfo;
class btCollisionWorldImporter
{
protected:
@@ -56,60 +51,53 @@ protected:
int m_verboseMode;
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
btAlignedObjectArray<btCollisionObject*> m_allocatedRigidBodies;
btAlignedObjectArray<btOptimizedBvh*> m_allocatedBvhs;
btAlignedObjectArray<btOptimizedBvh*> m_allocatedBvhs;
btAlignedObjectArray<btTriangleInfoMap*> m_allocatedTriangleInfoMaps;
btAlignedObjectArray<btTriangleIndexVertexArray*> m_allocatedTriangleIndexArrays;
btAlignedObjectArray<btStridingMeshInterfaceData*> m_allocatedbtStridingMeshInterfaceDatas;
btAlignedObjectArray<btCollisionObject*> m_allocatedCollisionObjects;
btAlignedObjectArray<char*> m_allocatedNames;
btAlignedObjectArray<char*> m_allocatedNames;
btAlignedObjectArray<int*> m_indexArrays;
btAlignedObjectArray<short int*> m_shortIndexArrays;
btAlignedObjectArray<unsigned char*> m_charIndexArrays;
btAlignedObjectArray<int*> m_indexArrays;
btAlignedObjectArray<short int*> m_shortIndexArrays;
btAlignedObjectArray<unsigned char*> m_charIndexArrays;
btAlignedObjectArray<btVector3FloatData*> m_floatVertexArrays;
btAlignedObjectArray<btVector3DoubleData*> m_doubleVertexArrays;
btAlignedObjectArray<btVector3FloatData*> m_floatVertexArrays;
btAlignedObjectArray<btVector3DoubleData*> m_doubleVertexArrays;
btHashMap<btHashPtr, btOptimizedBvh*> m_bvhMap;
btHashMap<btHashPtr, btTriangleInfoMap*> m_timMap;
btHashMap<btHashString, btCollisionShape*> m_nameShapeMap;
btHashMap<btHashString, btCollisionObject*> m_nameColObjMap;
btHashMap<btHashPtr,btOptimizedBvh*> m_bvhMap;
btHashMap<btHashPtr,btTriangleInfoMap*> m_timMap;
btHashMap<btHashString,btCollisionShape*> m_nameShapeMap;
btHashMap<btHashString,btCollisionObject*> m_nameColObjMap;
btHashMap<btHashPtr,const char*> m_objectNameMap;
btHashMap<btHashPtr,btCollisionShape*> m_shapeMap;
btHashMap<btHashPtr,btCollisionObject*> m_bodyMap;
btHashMap<btHashPtr, const char*> m_objectNameMap;
btHashMap<btHashPtr, btCollisionShape*> m_shapeMap;
btHashMap<btHashPtr, btCollisionObject*> m_bodyMap;
//methods
char* duplicateName(const char* name);
char* duplicateName(const char* name);
btCollisionShape* convertCollisionShape( btCollisionShapeData* shapeData );
btCollisionShape* convertCollisionShape(btCollisionShapeData* shapeData);
public:
btCollisionWorldImporter(btCollisionWorld* world);
virtual ~btCollisionWorldImporter();
bool convertAllObjects( btBulletSerializedArrays* arrays);
bool convertAllObjects(btBulletSerializedArrays* arrays);
///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load.
///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load.
///make sure you don't use the dynamics world containing objects after you call this method
virtual void deleteAllData();
void setVerboseMode(int verboseMode)
void setVerboseMode(int verboseMode)
{
m_verboseMode = verboseMode;
}
@@ -119,14 +107,14 @@ public:
return m_verboseMode;
}
// query for data
int getNumCollisionShapes() const;
// query for data
int getNumCollisionShapes() const;
btCollisionShape* getCollisionShapeByIndex(int index);
int getNumRigidBodies() const;
btCollisionObject* getRigidBodyByIndex(int index) const;
int getNumBvhs() const;
btOptimizedBvh* getBvhByIndex(int index) const;
btOptimizedBvh* getBvhByIndex(int index) const;
int getNumTriangleInfoMaps() const;
btTriangleInfoMap* getTriangleInfoMapByIndex(int index) const;
@@ -134,56 +122,48 @@ public:
btCollisionShape* getCollisionShapeByName(const char* name);
btCollisionObject* getCollisionObjectByName(const char* name);
const char* getNameForPointer(const void* ptr) const;
const char* getNameForPointer(const void* ptr) const;
///those virtuals are called by load and can be overridden by the user
//bodies
virtual btCollisionObject* createCollisionObject( const btTransform& startTransform, btCollisionShape* shape,const char* bodyName);
virtual btCollisionObject* createCollisionObject(const btTransform& startTransform, btCollisionShape* shape, const char* bodyName);
///shapes
virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal,btScalar planeConstant);
virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal, btScalar planeConstant);
virtual btCollisionShape* createBoxShape(const btVector3& halfExtents);
virtual btCollisionShape* createSphereShape(btScalar radius);
virtual btCollisionShape* createCapsuleShapeX(btScalar radius, btScalar height);
virtual btCollisionShape* createCapsuleShapeY(btScalar radius, btScalar height);
virtual btCollisionShape* createCapsuleShapeZ(btScalar radius, btScalar height);
virtual btCollisionShape* createCylinderShapeX(btScalar radius,btScalar height);
virtual btCollisionShape* createCylinderShapeY(btScalar radius,btScalar height);
virtual btCollisionShape* createCylinderShapeZ(btScalar radius,btScalar height);
virtual btCollisionShape* createConeShapeX(btScalar radius,btScalar height);
virtual btCollisionShape* createConeShapeY(btScalar radius,btScalar height);
virtual btCollisionShape* createConeShapeZ(btScalar radius,btScalar height);
virtual class btTriangleIndexVertexArray* createTriangleMeshContainer();
virtual btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh);
virtual btCollisionShape* createCylinderShapeX(btScalar radius, btScalar height);
virtual btCollisionShape* createCylinderShapeY(btScalar radius, btScalar height);
virtual btCollisionShape* createCylinderShapeZ(btScalar radius, btScalar height);
virtual btCollisionShape* createConeShapeX(btScalar radius, btScalar height);
virtual btCollisionShape* createConeShapeY(btScalar radius, btScalar height);
virtual btCollisionShape* createConeShapeZ(btScalar radius, btScalar height);
virtual class btTriangleIndexVertexArray* createTriangleMeshContainer();
virtual btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh);
virtual btCollisionShape* createConvexTriangleMeshShape(btStridingMeshInterface* trimesh);
#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT
virtual btGImpactMeshShape* createGimpactShape(btStridingMeshInterface* trimesh);
#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
virtual btStridingMeshInterfaceData* createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData);
virtual class btConvexHullShape* createConvexHullShape();
virtual class btCompoundShape* createCompoundShape();
virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScalingbtBvhTriangleMeshShape);
virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape, const btVector3& localScalingbtBvhTriangleMeshShape);
virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres);
virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres);
virtual btTriangleIndexVertexArray* createMeshInterface(btStridingMeshInterfaceData& meshData);
///acceleration and connectivity structures
virtual btOptimizedBvh* createOptimizedBvh();
virtual btOptimizedBvh* createOptimizedBvh();
virtual btTriangleInfoMap* createTriangleInfoMap();
};
#endif //BT_WORLD_IMPORTER_H
#endif //BT_WORLD_IMPORTER_H

222
src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
View File

@@ -25,62 +25,58 @@ subject to the following restrictions:
btShapePairCallback gCompoundChildShapePairCallback = 0;
btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
m_isSwapped(isSwapped),
m_sharedManifold(ci.m_manifold)
btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
m_isSwapped(isSwapped),
m_sharedManifold(ci.m_manifold)
{
m_ownsManifold = false;
const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
btAssert (colObjWrap->getCollisionShape()->isCompound());
const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
btAssert(colObjWrap->getCollisionShape()->isCompound());
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
m_compoundShapeRevision = compoundShape->getUpdateRevision();
preallocateChildAlgorithms(body0Wrap,body1Wrap);
preallocateChildAlgorithms(body0Wrap, body1Wrap);
}
void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap;
btAssert (colObjWrap->getCollisionShape()->isCompound());
const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
btAssert(colObjWrap->getCollisionShape()->isCompound());
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
int numChildren = compoundShape->getNumChildShapes();
int i;
m_childCollisionAlgorithms.resize(numChildren);
for (i=0;i<numChildren;i++)
for (i = 0; i < numChildren; i++)
{
if (compoundShape->getDynamicAabbTree())
{
m_childCollisionAlgorithms[i] = 0;
} else
}
else
{
const btCollisionShape* childShape = compoundShape->getChildShape(i);
btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully)
m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
btCollisionObjectWrapper childWrap(colObjWrap, childShape, colObjWrap->getCollisionObject(), colObjWrap->getWorldTransform(), -1, i); //wrong child trans, but unused (hopefully)
m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap, otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsContact;
btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsClosestPoints;
}
}
}
void btCompoundCollisionAlgorithm::removeChildAlgorithms()
void btCompoundCollisionAlgorithm::removeChildAlgorithms()
{
int numChildren = m_childCollisionAlgorithms.size();
int i;
for (i=0;i<numChildren;i++)
for (i = 0; i < numChildren; i++)
{
if (m_childCollisionAlgorithms[i])
{
@@ -95,54 +91,44 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
removeChildAlgorithms();
}
struct btCompoundLeafCallback : btDbvt::ICollide
struct btCompoundLeafCallback : btDbvt::ICollide
{
public:
const btCollisionObjectWrapper* m_compoundColObjWrap;
const btCollisionObjectWrapper* m_otherObjWrap;
btDispatcher* m_dispatcher;
const btDispatcherInfo& m_dispatchInfo;
btManifoldResult* m_resultOut;
btCollisionAlgorithm** m_childCollisionAlgorithms;
btPersistentManifold* m_sharedManifold;
btCompoundLeafCallback (const btCollisionObjectWrapper* compoundObjWrap,const btCollisionObjectWrapper* otherObjWrap,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut,btCollisionAlgorithm** childCollisionAlgorithms,btPersistentManifold* sharedManifold)
:m_compoundColObjWrap(compoundObjWrap),m_otherObjWrap(otherObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
m_childCollisionAlgorithms(childCollisionAlgorithms),
m_sharedManifold(sharedManifold)
{
btManifoldResult* m_resultOut;
btCollisionAlgorithm** m_childCollisionAlgorithms;
btPersistentManifold* m_sharedManifold;
btCompoundLeafCallback(const btCollisionObjectWrapper* compoundObjWrap, const btCollisionObjectWrapper* otherObjWrap, btDispatcher* dispatcher, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut, btCollisionAlgorithm** childCollisionAlgorithms, btPersistentManifold* sharedManifold)
: m_compoundColObjWrap(compoundObjWrap), m_otherObjWrap(otherObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithms(childCollisionAlgorithms), m_sharedManifold(sharedManifold)
{
}
void ProcessChildShape(const btCollisionShape* childShape,int index)
void ProcessChildShape(const btCollisionShape* childShape, int index)
{
btAssert(index>=0);
btAssert(index >= 0);
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
btAssert(index<compoundShape->getNumChildShapes());
btAssert(index < compoundShape->getNumChildShapes());
//backup
btTransform orgTrans = m_compoundColObjWrap->getWorldTransform();
btTransform orgTrans = m_compoundColObjWrap->getWorldTransform();
const btTransform& childTrans = compoundShape->getChildTransform(index);
btTransform newChildWorldTrans = orgTrans*childTrans ;
btTransform newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
btVector3 aabbMin0,aabbMax0;
childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
btVector3 aabbMin0, aabbMax0;
childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0);
btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
aabbMin0 -= extendAabb;
aabbMax0 += extendAabb;
btVector3 aabbMin1, aabbMax1;
m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1);
if (gCompoundChildShapePairCallback)
{
@@ -150,11 +136,10 @@ public:
return;
}
if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
{
btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, -1, index);
btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index);
btCollisionAlgorithm* algo = 0;
bool allocatedAlgorithm = false;
@@ -172,7 +157,7 @@ public:
}
algo = m_childCollisionAlgorithms[index];
}
const btCollisionObjectWrapper* tmpWrap = 0;
///detect swapping case
@@ -180,15 +165,16 @@ public:
{
tmpWrap = m_resultOut->getBody0Wrap();
m_resultOut->setBody0Wrap(&compoundWrap);
m_resultOut->setShapeIdentifiersA(-1,index);
} else
m_resultOut->setShapeIdentifiersA(-1, index);
}
else
{
tmpWrap = m_resultOut->getBody1Wrap();
m_resultOut->setBody1Wrap(&compoundWrap);
m_resultOut->setShapeIdentifiersB(-1,index);
m_resultOut->setShapeIdentifiersB(-1, index);
}
algo->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
algo->processCollision(&compoundWrap, m_otherObjWrap, m_dispatchInfo, m_resultOut);
#if 0
if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
@@ -202,18 +188,19 @@ public:
if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
{
m_resultOut->setBody0Wrap(tmpWrap);
} else
}
else
{
m_resultOut->setBody1Wrap(tmpWrap);
}
if(allocatedAlgorithm)
if (allocatedAlgorithm)
{
algo->~btCollisionAlgorithm();
m_dispatcher->freeCollisionAlgorithm(algo);
}
}
}
}
void Process(const btDbvtNode* leaf)
void Process(const btDbvtNode* leaf)
{
int index = leaf->dataAsInt;
@@ -230,22 +217,16 @@ public:
}
#endif
ProcessChildShape(childShape,index);
ProcessChildShape(childShape, index);
}
};
void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
void btCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap;
const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
btAssert (colObjWrap->getCollisionShape()->isCompound());
btAssert(colObjWrap->getCollisionShape()->isCompound());
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
///btCompoundShape might have changed:
@@ -254,17 +235,17 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
{
///clear and update all
removeChildAlgorithms();
preallocateChildAlgorithms(body0Wrap,body1Wrap);
preallocateChildAlgorithms(body0Wrap, body1Wrap);
m_compoundShapeRevision = compoundShape->getUpdateRevision();
}
if (m_childCollisionAlgorithms.size()==0)
if (m_childCollisionAlgorithms.size() == 0)
return;
const btDbvt* tree = compoundShape->getDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
btCompoundLeafCallback callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
btCompoundLeafCallback callback(colObjWrap, otherObjWrap, m_dispatcher, dispatchInfo, resultOut, &m_childCollisionAlgorithms[0], m_sharedManifold);
///we need to refresh all contact manifolds
///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
@@ -272,18 +253,18 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
{
int i;
manifoldArray.resize(0);
for (i=0;i<m_childCollisionAlgorithms.size();i++)
for (i = 0; i < m_childCollisionAlgorithms.size(); i++)
{
if (m_childCollisionAlgorithms[i])
{
m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
for (int m=0;m<manifoldArray.size();m++)
for (int m = 0; m < manifoldArray.size(); m++)
{
if (manifoldArray[m]->getNumContacts())
{
resultOut->setPersistentManifold(manifoldArray[m]);
resultOut->refreshContactPoints();
resultOut->setPersistentManifold(0);//??necessary?
resultOut->setPersistentManifold(0); //??necessary?
}
}
manifoldArray.resize(0);
@@ -293,57 +274,56 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
if (tree)
{
btVector3 localAabbMin,localAabbMax;
btVector3 localAabbMin, localAabbMax;
btTransform otherInCompoundSpace;
otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace, localAabbMin, localAabbMax);
btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
localAabbMin -= extraExtends;
localAabbMax += extraExtends;
const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
//process all children, that overlap with the given AABB bounds
tree->collideTVNoStackAlloc(tree->m_root,bounds,stack2,callback);
} else
tree->collideTVNoStackAlloc(tree->m_root, bounds, stack2, callback);
}
else
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.size();
int i;
for (i=0;i<numChildren;i++)
for (i = 0; i < numChildren; i++)
{
callback.ProcessChildShape(compoundShape->getChildShape(i),i);
callback.ProcessChildShape(compoundShape->getChildShape(i), i);
}
}
{
//iterate over all children, perform an AABB check inside ProcessChildShape
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.size();
int i;
manifoldArray.resize(0);
const btCollisionShape* childShape = 0;
btTransform orgTrans;
btTransform newChildWorldTrans;
btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
for (i=0;i<numChildren;i++)
const btCollisionShape* childShape = 0;
btTransform orgTrans;
btTransform newChildWorldTrans;
btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
for (i = 0; i < numChildren; i++)
{
if (m_childCollisionAlgorithms[i])
{
childShape = compoundShape->getChildShape(i);
//if not longer overlapping, remove the algorithm
//if not longer overlapping, remove the algorithm
orgTrans = colObjWrap->getWorldTransform();
const btTransform& childTrans = compoundShape->getChildTransform(i);
newChildWorldTrans = orgTrans*childTrans ;
newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0);
otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1);
if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
{
m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
@@ -354,15 +334,15 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
}
}
btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
btAssert(0);
//needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures
btCollisionObject* colObj = m_isSwapped? body1 : body0;
btCollisionObject* otherObj = m_isSwapped? body0 : body1;
btCollisionObject* colObj = m_isSwapped ? body1 : body0;
btCollisionObject* otherObj = m_isSwapped ? body0 : body1;
btAssert(colObj->getCollisionShape()->isCompound());
btAssert (colObj->getCollisionShape()->isCompound());
btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
@@ -376,33 +356,29 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject*
int numChildren = m_childCollisionAlgorithms.size();
int i;
btTransform orgTrans;
btScalar frac;
for (i=0;i<numChildren;i++)
btTransform orgTrans;
btScalar frac;
for (i = 0; i < numChildren; i++)
{
//btCollisionShape* childShape = compoundShape->getChildShape(i);
//backup
orgTrans = colObj->getWorldTransform();
orgTrans = colObj->getWorldTransform();
const btTransform& childTrans = compoundShape->getChildTransform(i);
//btTransform newChildWorldTrans = orgTrans*childTrans ;
colObj->setWorldTransform( orgTrans*childTrans );
colObj->setWorldTransform(orgTrans * childTrans);
//btCollisionShape* tmpShape = colObj->getCollisionShape();
//colObj->internalSetTemporaryCollisionShape( childShape );
frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
if (frac<hitFraction)
frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut);
if (frac < hitFraction)
{
hitFraction = frac;
}
//revert back
//colObj->internalSetTemporaryCollisionShape( tmpShape);
colObj->setWorldTransform( orgTrans);
colObj->setWorldTransform(orgTrans);
}
return hitFraction;
}

45
src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
View File

@@ -35,7 +35,7 @@ typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCol
extern btShapePairCallback gCompoundChildShapePairCallback;
/// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes
class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm
class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
btNodeStack stack2;
btManifoldArray manifoldArray;
@@ -44,61 +44,56 @@ protected:
btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
bool m_isSwapped;
class btPersistentManifold* m_sharedManifold;
bool m_ownsManifold;
class btPersistentManifold* m_sharedManifold;
bool m_ownsManifold;
int m_compoundShapeRevision; //to keep track of changes, so that childAlgorithm array can be updated
int m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated
void removeChildAlgorithms();
void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
void removeChildAlgorithms();
void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap);
public:
btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
virtual ~btCompoundCollisionAlgorithm();
btCollisionAlgorithm* getChildAlgorithm (int n) const
btCollisionAlgorithm* getChildAlgorithm(int n) const
{
return m_childCollisionAlgorithms[n];
}
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
int i;
for (i=0;i<m_childCollisionAlgorithms.size();i++)
for (i = 0; i < m_childCollisionAlgorithms.size(); i++)
{
if (m_childCollisionAlgorithms[i])
m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
}
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
struct CreateFunc : public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false);
return new (mem) btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, false);
}
};
struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc
struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true);
return new (mem) btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, true);
}
};
};
#endif //BT_COMPOUND_COLLISION_ALGORITHM_H
#endif //BT_COMPOUND_COLLISION_ALGORITHM_H

312
src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
View File

@@ -29,29 +29,25 @@ subject to the following restrictions:
btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped)
btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
: btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, isSwapped)
{
void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache();
void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache), 16);
m_childCollisionAlgorithmCache = new (ptr) btHashedSimplePairCache();
const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
btAssert (col0ObjWrap->getCollisionShape()->isCompound());
btAssert(col0ObjWrap->getCollisionShape()->isCompound());
const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
btAssert (col1ObjWrap->getCollisionShape()->isCompound());
btAssert(col1ObjWrap->getCollisionShape()->isCompound());
const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
}
btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm()
{
removeChildAlgorithms();
@@ -59,32 +55,30 @@ btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm()
btAlignedFree(m_childCollisionAlgorithmCache);
}
void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
{
int i;
btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
for (i=0;i<pairs.size();i++)
for (i = 0; i < pairs.size(); i++)
{
if (pairs[i].m_userPointer)
{
((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray);
}
}
}
void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
{
btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
int numChildren = pairs.size();
int i;
for (i=0;i<numChildren;i++)
for (i = 0; i < numChildren; i++)
{
if (pairs[i].m_userPointer)
{
btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
algo->~btCollisionAlgorithm();
m_dispatcher->freeCollisionAlgorithm(algo);
}
@@ -92,77 +86,65 @@ void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
m_childCollisionAlgorithmCache->removeAllPairs();
}
struct btCompoundCompoundLeafCallback : btDbvt::ICollide
struct btCompoundCompoundLeafCallback : btDbvt::ICollide
{
int m_numOverlapPairs;
const btCollisionObjectWrapper* m_compound0ColObjWrap;
const btCollisionObjectWrapper* m_compound1ColObjWrap;
btDispatcher* m_dispatcher;
const btDispatcherInfo& m_dispatchInfo;
btManifoldResult* m_resultOut;
class btHashedSimplePairCache* m_childCollisionAlgorithmCache;
btPersistentManifold* m_sharedManifold;
btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap,
const btCollisionObjectWrapper* compound0ObjWrap,
btDispatcher* dispatcher,
const btDispatcherInfo& dispatchInfo,
btManifoldResult* resultOut,
btHashedSimplePairCache* childAlgorithmsCache,
btPersistentManifold* sharedManifold)
:m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
m_childCollisionAlgorithmCache(childAlgorithmsCache),
m_sharedManifold(sharedManifold)
{
btManifoldResult* m_resultOut;
class btHashedSimplePairCache* m_childCollisionAlgorithmCache;
btPersistentManifold* m_sharedManifold;
btCompoundCompoundLeafCallback(const btCollisionObjectWrapper* compound1ObjWrap,
const btCollisionObjectWrapper* compound0ObjWrap,
btDispatcher* dispatcher,
const btDispatcherInfo& dispatchInfo,
btManifoldResult* resultOut,
btHashedSimplePairCache* childAlgorithmsCache,
btPersistentManifold* sharedManifold)
: m_numOverlapPairs(0), m_compound0ColObjWrap(compound1ObjWrap), m_compound1ColObjWrap(compound0ObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithmCache(childAlgorithmsCache), m_sharedManifold(sharedManifold)
{
}
void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
void Process(const btDbvtNode* leaf0, const btDbvtNode* leaf1)
{
BT_PROFILE("btCompoundCompoundLeafCallback::Process");
m_numOverlapPairs++;
int childIndex0 = leaf0->dataAsInt;
int childIndex1 = leaf1->dataAsInt;
btAssert(childIndex0>=0);
btAssert(childIndex1>=0);
btAssert(childIndex0 >= 0);
btAssert(childIndex1 >= 0);
const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape());
btAssert(childIndex0<compoundShape0->getNumChildShapes());
btAssert(childIndex0 < compoundShape0->getNumChildShapes());
const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape());
btAssert(childIndex1<compoundShape1->getNumChildShapes());
btAssert(childIndex1 < compoundShape1->getNumChildShapes());
const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0);
const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1);
//backup
btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ;
btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
btTransform newChildWorldTrans0 = orgTrans0 * childTrans0;
btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1);
btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ;
btTransform newChildWorldTrans1 = orgTrans1 * childTrans1;
//perform an AABB check first
btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
childShape0->getAabb(newChildWorldTrans0, aabbMin0, aabbMax0);
childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1);
btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
aabbMin0 -= thresholdVec;
@@ -170,17 +152,16 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
if (gCompoundCompoundChildShapePairCallback)
{
if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
if (!gCompoundCompoundChildShapePairCallback(childShape0, childShape1))
return;
}
if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
{
btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0);
btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1);
btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap, childShape0, m_compound0ColObjWrap->getCollisionObject(), newChildWorldTrans0, -1, childIndex0);
btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap, childShape1, m_compound1ColObjWrap->getCollisionObject(), newChildWorldTrans1, -1, childIndex1);
btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0, childIndex1);
bool removePair = false;
btCollisionAlgorithm* colAlgo = 0;
if (m_resultOut->m_closestPointDistanceThreshold > 0)
@@ -193,7 +174,6 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
if (pair)
{
colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
}
else
{
@@ -205,7 +185,7 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
}
btAssert(colAlgo);
const btCollisionObjectWrapper* tmpWrap0 = 0;
const btCollisionObjectWrapper* tmpWrap1 = 0;
@@ -215,105 +195,100 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
m_resultOut->setBody0Wrap(&compoundWrap0);
m_resultOut->setBody1Wrap(&compoundWrap1);
m_resultOut->setShapeIdentifiersA(-1,childIndex0);
m_resultOut->setShapeIdentifiersB(-1,childIndex1);
m_resultOut->setShapeIdentifiersA(-1, childIndex0);
m_resultOut->setShapeIdentifiersB(-1, childIndex1);
colAlgo->processCollision(&compoundWrap0, &compoundWrap1, m_dispatchInfo, m_resultOut);
colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut);
m_resultOut->setBody0Wrap(tmpWrap0);
m_resultOut->setBody1Wrap(tmpWrap1);
if (removePair)
{
colAlgo->~btCollisionAlgorithm();
m_dispatcher->freeCollisionAlgorithm(colAlgo);
}
}
}
};
static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold)
static DBVT_INLINE bool MyIntersect(const btDbvtAabbMm& a,
const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold)
{
btVector3 newmin,newmax;
btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
btVector3 newmin, newmax;
btTransformAabb(b.Mins(), b.Maxs(), 0.f, xform, newmin, newmax);
newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
return Intersect(a,newb);
btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin, newmax);
return Intersect(a, newb);
}
static inline void MycollideTT( const btDbvtNode* root0,
const btDbvtNode* root1,
const btTransform& xform,
btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold)
static inline void MycollideTT(const btDbvtNode* root0,
const btDbvtNode* root1,
const btTransform& xform,
btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold)
{
if(root0&&root1)
{
int depth=1;
int treshold=btDbvt::DOUBLE_STACKSIZE-4;
btAlignedObjectArray<btDbvt::sStkNN> stkStack;
if (root0 && root1)
{
int depth = 1;
int treshold = btDbvt::DOUBLE_STACKSIZE - 4;
btAlignedObjectArray<btDbvt::sStkNN> stkStack;
#ifdef USE_LOCAL_STACK
ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]);
stkStack.initializeFromBuffer(&localStack,btDbvt::DOUBLE_STACKSIZE,btDbvt::DOUBLE_STACKSIZE);
ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]);
stkStack.initializeFromBuffer(&localStack, btDbvt::DOUBLE_STACKSIZE, btDbvt::DOUBLE_STACKSIZE);
#else
stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
#endif
stkStack[0]=btDbvt::sStkNN(root0,root1);
do {
btDbvt::sStkNN p=stkStack[--depth];
if(MyIntersect(p.a->volume,p.b->volume,xform, distanceThreshold))
stkStack[0] = btDbvt::sStkNN(root0, root1);
do
{
btDbvt::sStkNN p = stkStack[--depth];
if (MyIntersect(p.a->volume, p.b->volume, xform, distanceThreshold))
{
if (depth > treshold)
{
if(depth>treshold)
stkStack.resize(stkStack.size() * 2);
treshold = stkStack.size() - 4;
}
if (p.a->isinternal())
{
if (p.b->isinternal())
{
stkStack.resize(stkStack.size()*2);
treshold=stkStack.size()-4;
}
if(p.a->isinternal())
{
if(p.b->isinternal())
{
stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]);
stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]);
stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]);
stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]);
}
else
{
stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b);
stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b);
}
stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[0]);
stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[0]);
stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[1]);
stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[1]);
}
else
{
if(p.b->isinternal())
{
stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]);
stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]);
}
else
{
callback->Process(p.a,p.b);
}
stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b);
stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b);
}
}
} while(depth);
}
else
{
if (p.b->isinternal())
{
stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[0]);
stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[1]);
}
else
{
callback->Process(p.a, p.b);
}
}
}
} while (depth);
}
}
void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
void btCompoundCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
const btCollisionObjectWrapper* col1ObjWrap= body1Wrap;
const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
btAssert (col0ObjWrap->getCollisionShape()->isCompound());
btAssert (col1ObjWrap->getCollisionShape()->isCompound());
btAssert(col0ObjWrap->getCollisionShape()->isCompound());
btAssert(col1ObjWrap->getCollisionShape()->isCompound());
const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
@@ -321,7 +296,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
if (!tree0 || !tree1)
{
return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut);
return btCompoundCollisionAlgorithm::processCollision(body0Wrap, body1Wrap, dispatchInfo, resultOut);
}
///btCompoundShape might have changed:
////make sure the internal child collision algorithm caches are still valid
@@ -331,28 +306,26 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
removeChildAlgorithms();
m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
}
///we need to refresh all contact manifolds
///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
{
int i;
btManifoldArray manifoldArray;
#ifdef USE_LOCAL_STACK
#ifdef USE_LOCAL_STACK
btPersistentManifold localManifolds[4];
manifoldArray.initializeFromBuffer(&localManifolds,0,4);
manifoldArray.initializeFromBuffer(&localManifolds, 0, 4);
#endif
btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
for (i=0;i<pairs.size();i++)
for (i = 0; i < pairs.size(); i++)
{
if (pairs[i].m_userPointer)
{
btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
algo->getAllContactManifolds(manifoldArray);
for (int m=0;m<manifoldArray.size();m++)
for (int m = 0; m < manifoldArray.size(); m++)
{
if (manifoldArray[m]->getNumContacts())
{
@@ -366,35 +339,27 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
}
}
btCompoundCompoundLeafCallback callback(col0ObjWrap, col1ObjWrap, this->m_dispatcher, dispatchInfo, resultOut, this->m_childCollisionAlgorithmCache, m_sharedManifold);
btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
MycollideTT(tree0->m_root,tree1->m_root,xform,&callback, resultOut->m_closestPointDistanceThreshold);
const btTransform xform = col0ObjWrap->getWorldTransform().inverse() * col1ObjWrap->getWorldTransform();
MycollideTT(tree0->m_root, tree1->m_root, xform, &callback, resultOut->m_closestPointDistanceThreshold);
//printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs);
//remove non-overlapping child pairs
{
btAssert(m_removePairs.size()==0);
btAssert(m_removePairs.size() == 0);
//iterate over all children, perform an AABB check inside ProcessChildShape
btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
int i;
btManifoldArray manifoldArray;
btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
for (i=0;i<pairs.size();i++)
int i;
btManifoldArray manifoldArray;
btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
for (i = 0; i < pairs.size(); i++)
{
if (pairs[i].m_userPointer)
{
@@ -402,52 +367,47 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
{
const btCollisionShape* childShape0 = 0;
btTransform newChildWorldTrans0;
btTransform newChildWorldTrans0;
childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
newChildWorldTrans0 = col0ObjWrap->getWorldTransform()*childTrans0 ;
childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
newChildWorldTrans0 = col0ObjWrap->getWorldTransform() * childTrans0;
childShape0->getAabb(newChildWorldTrans0, aabbMin0, aabbMax0);
}
btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
aabbMin0 -= thresholdVec;
aabbMax0 += thresholdVec;
{
const btCollisionShape* childShape1 = 0;
btTransform newChildWorldTrans1;
btTransform newChildWorldTrans1;
childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
newChildWorldTrans1 = col1ObjWrap->getWorldTransform()*childTrans1 ;
childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
newChildWorldTrans1 = col1ObjWrap->getWorldTransform() * childTrans1;
childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1);
}
aabbMin1 -= thresholdVec;
aabbMax1 += thresholdVec;
if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
{
algo->~btCollisionAlgorithm();
m_dispatcher->freeCollisionAlgorithm(algo);
m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB));
m_removePairs.push_back(btSimplePair(pairs[i].m_indexA, pairs[i].m_indexB));
}
}
}
for (int i=0;i<m_removePairs.size();i++)
for (int i = 0; i < m_removePairs.size(); i++)
{
m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB);
m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA, m_removePairs[i].m_indexB);
}
m_removePairs.clear();
}
}
btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
btAssert(0);
return 0.f;
}

43
src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h
View File

@@ -35,53 +35,46 @@ class btCollisionObject;
class btCollisionShape;
/// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes
class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm
class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm
{
class btHashedSimplePairCache* m_childCollisionAlgorithmCache;
class btHashedSimplePairCache* m_childCollisionAlgorithmCache;
btSimplePairArray m_removePairs;
int m_compoundShapeRevision0; //to keep track of changes, so that childAlgorithm array can be updated
int m_compoundShapeRevision1;
int m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated
int m_compoundShapeRevision1;
void removeChildAlgorithms();
// void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
void removeChildAlgorithms();
// void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
public:
btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
virtual ~btCompoundCompoundCollisionAlgorithm();
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray);
struct CreateFunc :public btCollisionAlgorithmCreateFunc
struct CreateFunc : public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm));
return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false);
return new (mem) btCompoundCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, false);
}
};
struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc
struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm));
return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true);
return new (mem) btCompoundCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, true);
}
};
};
#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H

99
src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp
View File

@@ -22,7 +22,6 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
@@ -34,8 +33,6 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
@@ -45,31 +42,28 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
{
m_simplexSolver = simplexSolver;
m_pdSolver = pdSolver;
}
btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc()
{
btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc()
{
}
btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */)
: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
m_simplexSolver(simplexSolver),
m_pdSolver(pdSolver),
m_ownManifold (false),
m_manifoldPtr(mf),
m_lowLevelOfDetail(false)
btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */)
: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
m_simplexSolver(simplexSolver),
m_pdSolver(pdSolver),
m_ownManifold(false),
m_manifoldPtr(mf),
m_lowLevelOfDetail(false)
{
(void)body0Wrap;
(void)body1Wrap;
}
btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
{
if (m_ownManifold)
@@ -79,26 +73,22 @@ btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
}
}
void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
{
m_lowLevelOfDetail = useLowLevel;
}
extern btScalar gContactBreakingThreshold;
//
// Convex-Convex collision algorithm
//
void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
void btConvex2dConvex2dAlgorithm ::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
if (!m_manifoldPtr)
{
//swapped?
m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
m_ownManifold = true;
}
resultOut->setPersistentManifold(m_manifoldPtr);
@@ -106,49 +96,41 @@ void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrap
//comment-out next line to test multi-contact generation
//resultOut->getPersistentManifold()->clearManifold();
const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
btVector3 normalOnB;
btVector3 pointOnBWorld;
btVector3 normalOnB;
btVector3 pointOnBWorld;
{
btGjkPairDetector::ClosestPointInput input;
btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
btGjkPairDetector gjkPairDetector(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.setMinkowskiA(min0);
gjkPairDetector.setMinkowskiB(min1);
{
input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
input.m_transformA = body0Wrap->getWorldTransform();
input.m_transformB = body1Wrap->getWorldTransform();
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw);
btVector3 v0,v1;
btVector3 v0, v1;
btVector3 sepNormalWorldSpace;
}
if (m_ownManifold)
{
resultOut->refreshContactPoints();
}
}
btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
(void)resultOut;
(void)dispatchInfo;
@@ -158,7 +140,6 @@ btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* c
///col0->m_worldTransform,
btScalar resultFraction = btScalar(1.);
btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
@@ -166,77 +147,65 @@ btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* c
squareMot1 < col1->getCcdSquareMotionThreshold())
return resultFraction;
//An adhoc way of testing the Continuous Collision Detection algorithms
//One object is approximated as a sphere, to simplify things
//Starting in penetration should report no time of impact
//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
/// Convex0 against sphere for Convex1
{
btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
btConvexCast::CastResult result;
btVoronoiSimplexSolver voronoiSimplex;
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
btGjkConvexCast ccd1(convex0, &sphere1, &voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(),
col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (col0->getHitFraction()> result.m_fraction)
col0->setHitFraction( result.m_fraction );
if (col0->getHitFraction() > result.m_fraction)
col0->setHitFraction(result.m_fraction);
if (col1->getHitFraction() > result.m_fraction)
col1->setHitFraction( result.m_fraction);
col1->setHitFraction(result.m_fraction);
if (resultFraction > result.m_fraction)
resultFraction = result.m_fraction;
}
}
/// Sphere (for convex0) against Convex1
{
btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
btConvexCast::CastResult result;
btVoronoiSimplexSolver voronoiSimplex;
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
btGjkConvexCast ccd1(&sphere0, convex1, &voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(),
col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (col0->getHitFraction() > result.m_fraction)
col0->setHitFraction( result.m_fraction);
if (col0->getHitFraction() > result.m_fraction)
col0->setHitFraction(result.m_fraction);
if (col1->getHitFraction() > result.m_fraction)
col1->setHitFraction( result.m_fraction);
col1->setHitFraction(result.m_fraction);
if (resultFraction > result.m_fraction)
resultFraction = result.m_fraction;
}
}
return resultFraction;
}

49
src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h
View File

@@ -23,70 +23,61 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
class btConvexPenetrationDepthSolver;
///The convex2dConvex2dAlgorithm collision algorithm support 2d collision detection for btConvex2dShape
///Currently it requires the btMinkowskiPenetrationDepthSolver, it has support for 2d penetration depth computation
class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm
{
btSimplexSolverInterface* m_simplexSolver;
btSimplexSolverInterface* m_simplexSolver;
btConvexPenetrationDepthSolver* m_pdSolver;
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_lowLevelOfDetail;
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_lowLevelOfDetail;
public:
btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
btConvex2dConvex2dAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
virtual ~btConvex2dConvex2dAlgorithm();
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
///should we use m_ownManifold to avoid adding duplicates?
if (m_manifoldPtr && m_ownManifold)
manifoldArray.push_back(m_manifoldPtr);
}
void setLowLevelOfDetail(bool useLowLevel);
void setLowLevelOfDetail(bool useLowLevel);
const btPersistentManifold* getManifold()
const btPersistentManifold* getManifold()
{
return m_manifoldPtr;
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
struct CreateFunc : public btCollisionAlgorithmCreateFunc
{
btConvexPenetrationDepthSolver* m_pdSolver;
btSimplexSolverInterface* m_simplexSolver;
btConvexPenetrationDepthSolver* m_pdSolver;
btSimplexSolverInterface* m_simplexSolver;
int m_numPerturbationIterations;
int m_minimumPointsPerturbationThreshold;
CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
virtual ~CreateFunc();
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm));
return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
return new (mem) btConvex2dConvex2dAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_simplexSolver, m_pdSolver, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold);
}
};
};
#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H
#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H

183
src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btConvexConcaveCollisionAlgorithm.h"
#include "LinearMath/btQuickprof.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
@@ -29,10 +28,10 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped),
m_isSwapped(isSwapped)
btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
m_btConvexTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped),
m_isSwapped(isSwapped)
{
}
@@ -40,7 +39,7 @@ btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
{
}
void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_btConvexTriangleCallback.m_manifoldPtr)
{
@@ -48,38 +47,32 @@ void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&
}
}
btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped):
m_dispatcher(dispatcher),
m_dispatchInfoPtr(0)
btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher),
m_dispatchInfoPtr(0)
{
m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap;
m_triBodyWrap = isSwapped? body0Wrap:body1Wrap;
//
// create the manifold from the dispatcher 'manifold pool'
//
m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject());
m_convexBodyWrap = isSwapped ? body1Wrap : body0Wrap;
m_triBodyWrap = isSwapped ? body0Wrap : body1Wrap;
clearCache();
//
// create the manifold from the dispatcher 'manifold pool'
//
m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(), m_triBodyWrap->getCollisionObject());
clearCache();
}
btConvexTriangleCallback::~btConvexTriangleCallback()
{
clearCache();
m_dispatcher->releaseManifold( m_manifoldPtr );
m_dispatcher->releaseManifold(m_manifoldPtr);
}
void btConvexTriangleCallback::clearCache()
void btConvexTriangleCallback::clearCache()
{
m_dispatcher->clearManifold(m_manifoldPtr);
}
void btConvexTriangleCallback::processTriangle(btVector3* triangle,int
partId, int triangleIndex)
void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex)
{
BT_PROFILE("btConvexTriangleCallback::processTriangle");
@@ -88,16 +81,12 @@ partId, int triangleIndex)
return;
}
//just for debugging purposes
//printf("triangle %d",m_triangleCount++);
//just for debugging purposes
//printf("triangle %d",m_triangleCount++);
btCollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher1 = m_dispatcher;
#if 0
///debug drawing of the overlapping triangles
@@ -111,16 +100,15 @@ partId, int triangleIndex)
m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
}
#endif
if (m_convexBodyWrap->getCollisionShape()->isConvex())
{
btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
btTriangleShape tm(triangle[0], triangle[1], triangle[2]);
tm.setMargin(m_collisionMarginTriangle);
btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
btCollisionObjectWrapper triObWrap(m_triBodyWrap, &tm, m_triBodyWrap->getCollisionObject(), m_triBodyWrap->getWorldTransform(), partId, triangleIndex); //correct transform?
btCollisionAlgorithm* colAlgo = 0;
if (m_resultOut->m_closestPointDistanceThreshold > 0)
{
colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
@@ -135,36 +123,32 @@ partId, int triangleIndex)
{
tmpWrap = m_resultOut->getBody0Wrap();
m_resultOut->setBody0Wrap(&triObWrap);
m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
m_resultOut->setShapeIdentifiersA(partId, triangleIndex);
}
else
{
tmpWrap = m_resultOut->getBody1Wrap();
m_resultOut->setBody1Wrap(&triObWrap);
m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
m_resultOut->setShapeIdentifiersB(partId, triangleIndex);
}
colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut);
colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut);
if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
{
m_resultOut->setBody0Wrap(tmpWrap);
} else
}
else
{
m_resultOut->setBody1Wrap(tmpWrap);
}
colAlgo->~btCollisionAlgorithm();
ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
}
}
void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
{
m_convexBodyWrap = convexBodyWrap;
m_triBodyWrap = triBodyWrap;
@@ -185,16 +169,14 @@ void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr
m_aabbMax += extra;
m_aabbMin -= extra;
}
void btConvexConcaveCollisionAlgorithm::clearCache()
{
m_btConvexTriangleCallback.clearCache();
}
void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
void btConvexConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
@@ -208,7 +190,6 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape();
if (convexBodyWrap->getCollisionShape()->isConvex())
{
btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape();
btAlignedObjectArray<btVector3> queryVertices;
@@ -229,7 +210,6 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
queryVertices.push_back(btVector3(0, 0, 0));
btSphereShape* sphere = (btSphereShape*)convex;
maxDist = sphere->getRadius() + SIMD_EPSILON;
}
if (queryVertices.size())
{
@@ -240,7 +220,7 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
for (int v = 0; v < queryVertices.size(); v++)
{
const btVector3& vtx = queryVertices[v];
btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform()*vtx;
btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform() * vtx;
btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace);
btVector3 normalLocal;
@@ -250,58 +230,52 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
if (dist <= maxDist)
{
normalLocal.safeNormalize();
btVector3 normal = triBodyWrap->getWorldTransform().getBasis()*normalLocal;
btVector3 normal = triBodyWrap->getWorldTransform().getBasis() * normalLocal;
if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
{
btSphereShape* sphere = (btSphereShape*)convex;
dist -= sphere->getRadius();
vtxWorldSpace -= sphere->getRadius()*normal;
vtxWorldSpace -= sphere->getRadius() * normal;
}
resultOut->addContactPoint(normal,vtxWorldSpace-normal*dist, dist);
resultOut->addContactPoint(normal, vtxWorldSpace - normal * dist, dist);
}
}
}
resultOut->refreshContactPoints();
}
}
} else
}
else
{
const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triBodyWrap->getCollisionShape());
if (convexBodyWrap->getCollisionShape()->isConvex())
{
btScalar collisionMarginTriangle = concaveShape->getMargin();
resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, convexBodyWrap, triBodyWrap, resultOut);
m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(), triBodyWrap->getCollisionObject());
concaveShape->processAllTriangles(&m_btConvexTriangleCallback, m_btConvexTriangleCallback.getAabbMin(), m_btConvexTriangleCallback.getAabbMax());
concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
resultOut->refreshContactPoints();
m_btConvexTriangleCallback.clearWrapperData();
}
}
}
}
btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
(void)resultOut;
(void)dispatchInfo;
btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
btCollisionObject* triBody = m_isSwapped ? body0 : body1;
//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
//only perform CCD above a certain threshold, this prevents blocking on the long run
@@ -320,25 +294,23 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
struct LocalTriangleSphereCastCallback : public btTriangleCallback
struct LocalTriangleSphereCastCallback : public btTriangleCallback
{
btTransform m_ccdSphereFromTrans;
btTransform m_ccdSphereToTrans;
btTransform m_meshTransform;
btTransform m_meshTransform;
btScalar m_ccdSphereRadius;
btScalar m_hitFraction;
btScalar m_ccdSphereRadius;
btScalar m_hitFraction;
LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
:m_ccdSphereFromTrans(from),
m_ccdSphereToTrans(to),
m_ccdSphereRadius(ccdSphereRadius),
m_hitFraction(hitFraction)
{
LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction)
: m_ccdSphereFromTrans(from),
m_ccdSphereToTrans(to),
m_ccdSphereRadius(ccdSphereRadius),
m_hitFraction(hitFraction)
{
}
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
{
BT_PROFILE("processTriangle");
@@ -349,29 +321,23 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
ident.setIdentity();
btConvexCast::CastResult castResult;
castResult.m_fraction = m_hitFraction;
btSphereShape pointShape(m_ccdSphereRadius);
btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
btVoronoiSimplexSolver simplexSolver;
btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
btSphereShape pointShape(m_ccdSphereRadius);
btTriangleShape triShape(triangle[0], triangle[1], triangle[2]);
btVoronoiSimplexSolver simplexSolver;
btSubsimplexConvexCast 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 (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans,
ident, ident, castResult))
{
if (m_hitFraction > castResult.m_fraction)
m_hitFraction = castResult.m_fraction;
}
}
};
if (triBody->getCollisionShape()->isConcave())
{
btVector3 rayAabbMin = convexFromLocal.getOrigin();
@@ -379,33 +345,30 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
btVector3 rayAabbMax = convexFromLocal.getOrigin();
rayAabbMax.setMax(convexToLocal.getOrigin());
btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
btScalar curHitFraction = btScalar(1.); //is this available?
LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
convexbody->getCcdSweptSphereRadius(),curHitFraction);
btScalar curHitFraction = btScalar(1.); //is this available?
LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal,
convexbody->getCcdSweptSphereRadius(), curHitFraction);
raycastCallback.m_hitFraction = convexbody->getHitFraction();
btCollisionObject* concavebody = triBody;
btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape();
if (triangleMesh)
{
triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax);
}
if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
{
convexbody->setHitFraction( raycastCallback.m_hitFraction);
convexbody->setHitFraction(raycastCallback.m_hitFraction);
return raycastCallback.m_hitFraction;
}
}
return btScalar(1.);
}

74
src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
View File

@@ -26,42 +26,40 @@ class btDispatcher;
#include "btCollisionCreateFunc.h"
///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called.
ATTRIBUTE_ALIGNED16(class) btConvexTriangleCallback : public btTriangleCallback
ATTRIBUTE_ALIGNED16(class)
btConvexTriangleCallback : public btTriangleCallback
{
btVector3 m_aabbMin;
btVector3 m_aabbMax ;
btVector3 m_aabbMin;
btVector3 m_aabbMax;
const btCollisionObjectWrapper* m_convexBodyWrap;
const btCollisionObjectWrapper* m_triBodyWrap;
btManifoldResult* m_resultOut;
btDispatcher* m_dispatcher;
btDispatcher* m_dispatcher;
const btDispatcherInfo* m_dispatchInfoPtr;
btScalar m_collisionMarginTriangle;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
int m_triangleCount;
btPersistentManifold* m_manifoldPtr;
btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
int m_triangleCount;
void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut);
btPersistentManifold* m_manifoldPtr;
void clearWrapperData()
btConvexTriangleCallback(btDispatcher * dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
void setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut);
void clearWrapperData()
{
m_convexBodyWrap = 0;
m_triBodyWrap = 0;
}
virtual ~btConvexTriangleCallback();
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
virtual void processTriangle(btVector3 * triangle, int partId, int triangleIndex);
void clearCache();
SIMD_FORCE_INLINE const btVector3& getAabbMin() const
@@ -72,56 +70,48 @@ int m_triangleCount;
{
return m_aabbMax;
}
};
/// btConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes.
ATTRIBUTE_ALIGNED16(class) btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm
ATTRIBUTE_ALIGNED16(class)
btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
btConvexTriangleCallback m_btConvexTriangleCallback;
bool m_isSwapped;
bool m_isSwapped;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
virtual ~btConvexConcaveCollisionAlgorithm();
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
btScalar calculateTimeOfImpact(btCollisionObject * body0, btCollisionObject * body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray);
void clearCache();
virtual void getAllContactManifolds(btManifoldArray & manifoldArray);
struct CreateFunc :public btCollisionAlgorithmCreateFunc
void clearCache();
struct CreateFunc : public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,false);
return new (mem) btConvexConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, false);
}
};
struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc
struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,true);
return new (mem) btConvexConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, true);
}
};
};
#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H
#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H

1009
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
View File

File diff suppressed because it is too large Load Diff

46
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
View File

@@ -23,7 +23,7 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "btCollisionCreateFunc.h"
#include "btCollisionDispatcher.h"
#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
class btConvexPenetrationDepthSolver;
@@ -41,69 +41,61 @@ class btConvexPenetrationDepthSolver;
class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
{
#ifdef USE_SEPDISTANCE_UTIL2
btConvexSeparatingDistanceUtil m_sepDistance;
btConvexSeparatingDistanceUtil m_sepDistance;
#endif
btConvexPenetrationDepthSolver* m_pdSolver;
btVertexArray worldVertsB1;
btVertexArray worldVertsB2;
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_lowLevelOfDetail;
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_lowLevelOfDetail;
int m_numPerturbationIterations;
int m_minimumPointsPerturbationThreshold;
///cache separating vector to speedup collision detection
public:
btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
btConvexConvexAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
virtual ~btConvexConvexAlgorithm();
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
///should we use m_ownManifold to avoid adding duplicates?
if (m_manifoldPtr && m_ownManifold)
manifoldArray.push_back(m_manifoldPtr);
}
void setLowLevelOfDetail(bool useLowLevel);
void setLowLevelOfDetail(bool useLowLevel);
const btPersistentManifold* getManifold()
const btPersistentManifold* getManifold()
{
return m_manifoldPtr;
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
struct CreateFunc : public btCollisionAlgorithmCreateFunc
{
btConvexPenetrationDepthSolver* m_pdSolver;
btConvexPenetrationDepthSolver* m_pdSolver;
int m_numPerturbationIterations;
int m_minimumPointsPerturbationThreshold;
CreateFunc(btConvexPenetrationDepthSolver* pdSolver);
virtual ~CreateFunc();
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
return new (mem) btConvexConvexAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_pdSolver, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold);
}
};
};
#endif //BT_CONVEX_CONVEX_ALGORITHM_H
#endif //BT_CONVEX_CONVEX_ALGORITHM_H

94
src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
View File

@@ -23,25 +23,24 @@ subject to the following restrictions:
//#include <stdio.h>
btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold)
: btCollisionAlgorithm(ci),
m_ownManifold(false),
m_manifoldPtr(mf),
m_isSwapped(isSwapped),
m_numPerturbationIterations(numPerturbationIterations),
m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
: btCollisionAlgorithm(ci),
m_ownManifold(false),
m_manifoldPtr(mf),
m_isSwapped(isSwapped),
m_numPerturbationIterations(numPerturbationIterations),
m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
{
const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? col1Wrap : col0Wrap;
const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? col0Wrap : col1Wrap;
const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? col1Wrap : col0Wrap;
const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? col0Wrap : col1Wrap;
if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject()))
if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject()))
{
m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject());
m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject());
m_ownManifold = true;
}
}
btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()
{
if (m_ownManifold)
@@ -51,32 +50,32 @@ btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()
}
}
void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
void btConvexPlaneCollisionAlgorithm::collideSingleContact(const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap;
const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape();
btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape();
btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape();
btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape();
bool hasCollision = false;
bool hasCollision = false;
const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant();
btTransform convexWorldTransform = convexObjWrap->getWorldTransform();
btTransform convexInPlaneTrans;
convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexWorldTransform;
convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexWorldTransform;
//now perturbe the convex-world transform
convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot);
convexWorldTransform.getBasis() *= btMatrix3x3(perturbeRot);
btTransform planeInConvex;
planeInConvex= convexWorldTransform.inverse() * planeObjWrap->getWorldTransform();
btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
planeInConvex = convexWorldTransform.inverse() * planeObjWrap->getWorldTransform();
btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal);
btVector3 vtxInPlane = convexInPlaneTrans(vtx);
btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
@@ -86,36 +85,35 @@ void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion&
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
btVector3 pOnB = vtxInPlaneWorld;
resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance);
}
}
void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
void btConvexPlaneCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
(void)dispatchInfo;
if (!m_manifoldPtr)
return;
const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap;
const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape();
btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape();
btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape();
btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape();
bool hasCollision = false;
const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant();
btTransform planeInConvex;
planeInConvex= convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform();
planeInConvex = convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform();
btTransform convexInPlaneTrans;
convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform();
convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform();
btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal);
btVector3 vtxInPlane = convexInPlaneTrans(vtx);
btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
@@ -125,31 +123,31 @@ void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectW
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
btVector3 pOnB = vtxInPlaneWorld;
resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance);
}
//the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones:
//they keep on rolling forever because of the additional off-center contact points
//so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc)
if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold)
if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
{
btVector3 v0,v1;
btPlaneSpace1(planeNormal,v0,v1);
btVector3 v0, v1;
btPlaneSpace1(planeNormal, v0, v1);
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
const btScalar angleLimit = 0.125f * SIMD_PI;
btScalar perturbeAngle;
btScalar radius = convexShape->getAngularMotionDisc();
perturbeAngle = gContactBreakingThreshold / radius;
if ( perturbeAngle > angleLimit )
perturbeAngle = angleLimit;
if (perturbeAngle > angleLimit)
perturbeAngle = angleLimit;
btQuaternion perturbeRot(v0,perturbeAngle);
for (int i=0;i<m_numPerturbationIterations;i++)
btQuaternion perturbeRot(v0, perturbeAngle);
for (int i = 0; i < m_numPerturbationIterations; i++)
{
btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
btQuaternion rotq(planeNormal,iterationAngle);
collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0Wrap,body1Wrap,dispatchInfo,resultOut);
btScalar iterationAngle = i * (SIMD_2_PI / btScalar(m_numPerturbationIterations));
btQuaternion rotq(planeNormal, iterationAngle);
collideSingleContact(rotq.inverse() * perturbeRot * rotq, body0Wrap, body1Wrap, dispatchInfo, resultOut);
}
}
@@ -162,7 +160,7 @@ void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectW
}
}
btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
(void)resultOut;
(void)dispatchInfo;

46
src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
View File

@@ -28,25 +28,24 @@ class btPersistentManifold;
/// Other features are frame-coherency (persistent data) and collision response.
class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_isSwapped;
int m_numPerturbationIterations;
int m_minimumPointsPerturbationThreshold;
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_isSwapped;
int m_numPerturbationIterations;
int m_minimumPointsPerturbationThreshold;
public:
btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold);
btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
virtual ~btConvexPlaneCollisionAlgorithm();
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
void collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
void collideSingleContact(const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
@@ -54,31 +53,30 @@ public:
}
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
struct CreateFunc : public btCollisionAlgorithmCreateFunc
{
int m_numPerturbationIterations;
int m_numPerturbationIterations;
int m_minimumPointsPerturbationThreshold;
CreateFunc()
CreateFunc()
: m_numPerturbationIterations(1),
m_minimumPointsPerturbationThreshold(0)
m_minimumPointsPerturbationThreshold(0)
{
}
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm));
if (!m_swapped)
{
return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
} else
return new (mem) btConvexPlaneCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, false, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold);
}
else
{
return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
return new (mem) btConvexPlaneCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, true, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold);
}
}
};
};
#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H
#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H

217
src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
View File

@@ -26,114 +26,108 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
#include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "LinearMath/btPoolAllocator.h"
btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo)
//btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* stackAlloc,btPoolAllocator* persistentManifoldPool,btPoolAllocator* collisionAlgorithmPool)
{
void* mem = NULL;
void* mem = NULL;
if (constructionInfo.m_useEpaPenetrationAlgorithm)
{
mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16);
m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver;
}else
{
mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16);
m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver;
mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver), 16);
m_pdSolver = new (mem) btGjkEpaPenetrationDepthSolver;
}
else
{
mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver), 16);
m_pdSolver = new (mem) btMinkowskiPenetrationDepthSolver;
}
//default CreationFunctions, filling the m_doubleDispatch table
mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16);
m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_pdSolver);
mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16);
m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc), 16);
m_convexConvexCreateFunc = new (mem) btConvexConvexAlgorithm::CreateFunc(m_pdSolver);
mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc), 16);
m_convexConcaveCreateFunc = new (mem) btConvexConcaveCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc), 16);
m_swappedConvexConcaveCreateFunc = new (mem) btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc), 16);
m_compoundCreateFunc = new (mem) btCompoundCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc),16);
m_compoundCompoundCreateFunc = new (mem)btCompoundCompoundCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc), 16);
m_compoundCompoundCreateFunc = new (mem) btCompoundCompoundCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16);
m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc;
mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16);
m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16);
m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc), 16);
m_swappedCompoundCreateFunc = new (mem) btCompoundCollisionAlgorithm::SwappedCreateFunc;
mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc), 16);
m_emptyCreateFunc = new (mem) btEmptyAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc), 16);
m_sphereSphereCF = new (mem) btSphereSphereCollisionAlgorithm::CreateFunc;
#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc), 16);
m_sphereBoxCF = new (mem) btSphereBoxCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc), 16);
m_boxSphereCF = new (mem) btSphereBoxCollisionAlgorithm::CreateFunc;
m_boxSphereCF->m_swapped = true;
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc), 16);
m_sphereTriangleCF = new (mem) btSphereTriangleCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc), 16);
m_triangleSphereCF = new (mem) btSphereTriangleCollisionAlgorithm::CreateFunc;
m_triangleSphereCF->m_swapped = true;
mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc),16);
m_boxBoxCF = new(mem)btBoxBoxCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc), 16);
m_boxBoxCF = new (mem) btBoxBoxCollisionAlgorithm::CreateFunc;
//convex versus plane
mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
mem = btAlignedAlloc(sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc), 16);
m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
mem = btAlignedAlloc(sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc), 16);
m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
m_planeConvexCF->m_swapped = true;
///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
int maxSize = sizeof(btConvexConvexAlgorithm);
int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm);
int maxSize3 = sizeof(btCompoundCollisionAlgorithm);
int maxSize4 = sizeof(btCompoundCompoundCollisionAlgorithm);
int collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize);
collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4);
int collisionAlgorithmMaxElementSize = btMax(maxSize, constructionInfo.m_customCollisionAlgorithmMaxElementSize);
collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize2);
collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize3);
collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize4);
if (constructionInfo.m_persistentManifoldPool)
{
m_ownsPersistentManifoldPool = false;
m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool;
} else
}
else
{
m_ownsPersistentManifoldPool = true;
void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize);
void* mem = btAlignedAlloc(sizeof(btPoolAllocator), 16);
m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold), constructionInfo.m_defaultMaxPersistentManifoldPoolSize);
}
collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize+16)&0xffffffffffff0;
collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize + 16) & 0xffffffffffff0;
if (constructionInfo.m_collisionAlgorithmPool)
{
m_ownsCollisionAlgorithmPool = false;
m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool;
} else
}
else
{
m_ownsCollisionAlgorithmPool = true;
void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize);
void* mem = btAlignedAlloc(sizeof(btPoolAllocator), 16);
m_collisionAlgorithmPool = new (mem) btPoolAllocator(collisionAlgorithmMaxElementSize, constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize);
}
}
btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
@@ -150,83 +144,78 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
}
m_convexConvexCreateFunc->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_convexConvexCreateFunc);
btAlignedFree(m_convexConvexCreateFunc);
m_convexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_convexConcaveCreateFunc);
btAlignedFree(m_convexConcaveCreateFunc);
m_swappedConvexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_swappedConvexConcaveCreateFunc);
btAlignedFree(m_swappedConvexConcaveCreateFunc);
m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_compoundCreateFunc);
btAlignedFree(m_compoundCreateFunc);
m_compoundCompoundCreateFunc->~btCollisionAlgorithmCreateFunc();
btAlignedFree(m_compoundCompoundCreateFunc);
m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_swappedCompoundCreateFunc);
btAlignedFree(m_swappedCompoundCreateFunc);
m_emptyCreateFunc->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_emptyCreateFunc);
btAlignedFree(m_emptyCreateFunc);
m_sphereSphereCF->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_sphereSphereCF);
btAlignedFree(m_sphereSphereCF);
#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
m_sphereBoxCF->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_sphereBoxCF);
btAlignedFree(m_sphereBoxCF);
m_boxSphereCF->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_boxSphereCF);
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
btAlignedFree(m_boxSphereCF);
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_sphereTriangleCF);
btAlignedFree(m_sphereTriangleCF);
m_triangleSphereCF->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_triangleSphereCF);
btAlignedFree(m_triangleSphereCF);
m_boxBoxCF->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_boxBoxCF);
btAlignedFree(m_boxBoxCF);
m_convexPlaneCF->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_convexPlaneCF);
btAlignedFree(m_convexPlaneCF);
m_planeConvexCF->~btCollisionAlgorithmCreateFunc();
btAlignedFree( m_planeConvexCF);
btAlignedFree(m_planeConvexCF);
m_pdSolver->~btConvexPenetrationDepthSolver();
btAlignedFree(m_pdSolver);
}
btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1)
{
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
{
return m_sphereSphereCF;
return m_sphereSphereCF;
}
#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE))
{
return m_sphereBoxCF;
return m_sphereBoxCF;
}
if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
{
return m_boxSphereCF;
return m_boxSphereCF;
}
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE))
{
return m_sphereTriangleCF;
return m_sphereTriangleCF;
}
if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
{
return m_triangleSphereCF;
return m_triangleSphereCF;
}
if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE))
@@ -239,8 +228,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPoint
return m_planeConvexCF;
}
if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
{
return m_convexConvexCreateFunc;
@@ -256,7 +243,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPoint
return m_swappedConvexConcaveCreateFunc;
}
if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1))
{
return m_compoundCompoundCreateFunc;
@@ -276,46 +262,41 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPoint
//failed to find an algorithm
return m_emptyCreateFunc;
}
btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1)
{
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
{
return m_sphereSphereCF;
return m_sphereSphereCF;
}
#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE))
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE))
{
return m_sphereBoxCF;
return m_sphereBoxCF;
}
if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
{
return m_boxSphereCF;
return m_boxSphereCF;
}
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE))
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE))
{
return m_sphereTriangleCF;
return m_sphereTriangleCF;
}
if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
{
return m_triangleSphereCF;
}
return m_triangleSphereCF;
}
if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE))
{
return m_boxBoxCF;
}
if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE))
{
return m_convexPlaneCF;
@@ -325,8 +306,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
{
return m_planeConvexCF;
}
if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
{
@@ -343,7 +322,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
return m_swappedConvexConcaveCreateFunc;
}
if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1))
{
return m_compoundCompoundCreateFunc;
@@ -352,7 +330,8 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
if (btBroadphaseProxy::isCompound(proxyType0))
{
return m_compoundCreateFunc;
} else
}
else
{
if (btBroadphaseProxy::isCompound(proxyType1))
{
@@ -366,17 +345,17 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold)
{
btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc;
btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*)m_convexConvexCreateFunc;
convexConvex->m_numPerturbationIterations = numPerturbationIterations;
convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
}
void btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold)
void btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold)
{
btConvexPlaneCollisionAlgorithm::CreateFunc* cpCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_convexPlaneCF;
cpCF->m_numPerturbationIterations = numPerturbationIterations;
cpCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
btConvexPlaneCollisionAlgorithm::CreateFunc* pcCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_planeConvexCF;
pcCF->m_numPerturbationIterations = numPerturbationIterations;
pcCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;

87
src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
View File

@@ -20,76 +20,68 @@ subject to the following restrictions:
class btVoronoiSimplexSolver;
class btConvexPenetrationDepthSolver;
struct btDefaultCollisionConstructionInfo
struct btDefaultCollisionConstructionInfo
{
btPoolAllocator* m_persistentManifoldPool;
btPoolAllocator* m_collisionAlgorithmPool;
int m_defaultMaxPersistentManifoldPoolSize;
int m_defaultMaxCollisionAlgorithmPoolSize;
int m_customCollisionAlgorithmMaxElementSize;
int m_useEpaPenetrationAlgorithm;
btPoolAllocator* m_persistentManifoldPool;
btPoolAllocator* m_collisionAlgorithmPool;
int m_defaultMaxPersistentManifoldPoolSize;
int m_defaultMaxCollisionAlgorithmPoolSize;
int m_customCollisionAlgorithmMaxElementSize;
int m_useEpaPenetrationAlgorithm;
btDefaultCollisionConstructionInfo()
:m_persistentManifoldPool(0),
m_collisionAlgorithmPool(0),
m_defaultMaxPersistentManifoldPoolSize(4096),
m_defaultMaxCollisionAlgorithmPoolSize(4096),
m_customCollisionAlgorithmMaxElementSize(0),
m_useEpaPenetrationAlgorithm(true)
: m_persistentManifoldPool(0),
m_collisionAlgorithmPool(0),
m_defaultMaxPersistentManifoldPoolSize(4096),
m_defaultMaxCollisionAlgorithmPoolSize(4096),
m_customCollisionAlgorithmMaxElementSize(0),
m_useEpaPenetrationAlgorithm(true)
{
}
};
///btCollisionConfiguration allows to configure Bullet collision detection
///stack allocator, pool memory allocators
///@todo: describe the meaning
class btDefaultCollisionConfiguration : public btCollisionConfiguration
class btDefaultCollisionConfiguration : public btCollisionConfiguration
{
protected:
int m_persistentManifoldPoolSize;
int m_persistentManifoldPoolSize;
btPoolAllocator* m_persistentManifoldPool;
bool m_ownsPersistentManifoldPool;
btPoolAllocator* m_persistentManifoldPool;
bool m_ownsPersistentManifoldPool;
btPoolAllocator* m_collisionAlgorithmPool;
bool m_ownsCollisionAlgorithmPool;
btPoolAllocator* m_collisionAlgorithmPool;
bool m_ownsCollisionAlgorithmPool;
//default penetration depth solver
btConvexPenetrationDepthSolver* m_pdSolver;
btConvexPenetrationDepthSolver* m_pdSolver;
//default CreationFunctions, filling the m_doubleDispatch table
btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc;
btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc;
btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc;
btCollisionAlgorithmCreateFunc* m_compoundCreateFunc;
btCollisionAlgorithmCreateFunc* m_compoundCompoundCreateFunc;
btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc;
btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc;
btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc;
btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc;
btCollisionAlgorithmCreateFunc* m_compoundCreateFunc;
btCollisionAlgorithmCreateFunc* m_compoundCompoundCreateFunc;
btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc;
btCollisionAlgorithmCreateFunc* m_emptyCreateFunc;
btCollisionAlgorithmCreateFunc* m_sphereSphereCF;
btCollisionAlgorithmCreateFunc* m_sphereBoxCF;
btCollisionAlgorithmCreateFunc* m_boxSphereCF;
btCollisionAlgorithmCreateFunc* m_boxBoxCF;
btCollisionAlgorithmCreateFunc* m_sphereTriangleCF;
btCollisionAlgorithmCreateFunc* m_triangleSphereCF;
btCollisionAlgorithmCreateFunc* m_planeConvexCF;
btCollisionAlgorithmCreateFunc* m_convexPlaneCF;
btCollisionAlgorithmCreateFunc* m_sphereTriangleCF;
btCollisionAlgorithmCreateFunc* m_triangleSphereCF;
btCollisionAlgorithmCreateFunc* m_planeConvexCF;
btCollisionAlgorithmCreateFunc* m_convexPlaneCF;
public:
btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo());
virtual ~btDefaultCollisionConfiguration();
///memory pools
///memory pools
virtual btPoolAllocator* getPersistentManifoldPool()
{
return m_persistentManifoldPool;
@@ -100,8 +92,7 @@ public:
return m_collisionAlgorithmPool;
}
virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1);
virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1);
@@ -112,11 +103,9 @@ public:
///3 is a good value for both params, if you want to enable the feature. This is because the default contact cache contains a maximum of 4 points, and one collision query at the unperturbed orientation is performed first.
///See Bullet/Demos/CollisionDemo for an example how this feature gathers multiple points.
///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection.
void setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3);
void setPlaneConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3);
void setConvexConvexMultipointIterations(int numPerturbationIterations = 3, int minimumPointsPerturbationThreshold = 3);
void setPlaneConvexMultipointIterations(int numPerturbationIterations = 3, int minimumPointsPerturbationThreshold = 3);
};
#endif //BT_DEFAULT_COLLISION_CONFIGURATION
#endif //BT_DEFAULT_COLLISION_CONFIGURATION

8
src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp
View File

@@ -15,20 +15,16 @@ subject to the following restrictions:
#include "btEmptyCollisionAlgorithm.h"
btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btCollisionAlgorithm(ci)
{
}
void btEmptyAlgorithm::processCollision (const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* ,const btDispatcherInfo& ,btManifoldResult* )
void btEmptyAlgorithm::processCollision(const btCollisionObjectWrapper*, const btCollisionObjectWrapper*, const btDispatcherInfo&, btManifoldResult*)
{
}
btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* )
btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject*, btCollisionObject*, const btDispatcherInfo&, btManifoldResult*)
{
return btScalar(1.);
}

18
src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
View File

@@ -25,30 +25,28 @@ subject to the following restrictions:
///The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame.
class btEmptyAlgorithm : public btCollisionAlgorithm
{
public:
btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
struct CreateFunc : public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
(void)body0Wrap;
(void)body1Wrap;
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm));
return new(mem) btEmptyAlgorithm(ci);
return new (mem) btEmptyAlgorithm(ci);
}
};
} ATTRIBUTE_ALIGNED(16);
#endif //BT_EMPTY_ALGORITH
#endif //BT_EMPTY_ALGORITH

97
src/BulletCollision/CollisionDispatch/btGhostObject.cpp
View File

@@ -29,60 +29,58 @@ btGhostObject::~btGhostObject()
btAssert(!m_overlappingObjects.size());
}
void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy)
void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy)
{
btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
btAssert(otherObject);
///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure
int index = m_overlappingObjects.findLinearSearch(otherObject);
if (index==m_overlappingObjects.size())
if (index == m_overlappingObjects.size())
{
//not found
m_overlappingObjects.push_back(otherObject);
}
}
void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy)
void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy)
{
btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
btAssert(otherObject);
int index = m_overlappingObjects.findLinearSearch(otherObject);
if (index<m_overlappingObjects.size())
if (index < m_overlappingObjects.size())
{
m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1];
m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size() - 1];
m_overlappingObjects.pop_back();
}
}
btPairCachingGhostObject::btPairCachingGhostObject()
{
m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16)) btHashedOverlappingPairCache();
}
btPairCachingGhostObject::~btPairCachingGhostObject()
{
m_hashPairCache->~btHashedOverlappingPairCache();
btAlignedFree( m_hashPairCache );
btAlignedFree(m_hashPairCache);
}
void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy)
void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy)
{
btBroadphaseProxy*actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle();
btBroadphaseProxy* actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle();
btAssert(actualThisProxy);
btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
btAssert(otherObject);
int index = m_overlappingObjects.findLinearSearch(otherObject);
if (index==m_overlappingObjects.size())
if (index == m_overlappingObjects.size())
{
m_overlappingObjects.push_back(otherObject);
m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy);
m_hashPairCache->addOverlappingPair(actualThisProxy, otherProxy);
}
}
void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1)
void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy1)
{
btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle();
@@ -90,82 +88,79 @@ void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy
btAssert(otherObject);
int index = m_overlappingObjects.findLinearSearch(otherObject);
if (index<m_overlappingObjects.size())
if (index < m_overlappingObjects.size())
{
m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1];
m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size() - 1];
m_overlappingObjects.pop_back();
m_hashPairCache->removeOverlappingPair(actualThisProxy,otherProxy,dispatcher);
m_hashPairCache->removeOverlappingPair(actualThisProxy, otherProxy, dispatcher);
}
}
void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
{
btTransform convexFromTrans,convexToTrans;
btTransform convexFromTrans, convexToTrans;
convexFromTrans = convexFromWorld;
convexToTrans = convexToWorld;
btVector3 castShapeAabbMin, castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
{
btVector3 linVel, angVel;
btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
btTransformUtil::calculateVelocity(convexFromTrans, convexToTrans, 1.0, linVel, angVel);
btTransform R;
R.setIdentity ();
R.setRotation (convexFromTrans.getRotation());
castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
R.setIdentity();
R.setRotation(convexFromTrans.getRotation());
castShape->calculateTemporalAabb(R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
}
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
int i;
for (i=0;i<m_overlappingObjects.size();i++)
for (i = 0; i < m_overlappingObjects.size(); i++)
{
btCollisionObject* collisionObject= m_overlappingObjects[i];
btCollisionObject* collisionObject = m_overlappingObjects[i];
//only perform raycast if filterMask matches
if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
if (resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
{
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
btVector3 collisionObjectAabbMin, collisionObjectAabbMax;
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax);
AabbExpand(collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
btVector3 hitNormal;
if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
if (btRayAabb(convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal))
{
btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans,
collisionObject,
collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(),
resultCallback,
allowedCcdPenetration);
btCollisionWorld::objectQuerySingle(castShape, convexFromTrans, convexToTrans,
collisionObject,
collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(),
resultCallback,
allowedCcdPenetration);
}
}
}
}
void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const
void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const
{
btTransform rayFromTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFromWorld);
btTransform rayToTrans;
btTransform rayToTrans;
rayToTrans.setIdentity();
rayToTrans.setOrigin(rayToWorld);
int i;
for (i=0;i<m_overlappingObjects.size();i++)
for (i = 0; i < m_overlappingObjects.size(); i++)
{
btCollisionObject* collisionObject= m_overlappingObjects[i];
btCollisionObject* collisionObject = m_overlappingObjects[i];
//only perform raycast if filterMask matches
if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
if (resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
{
btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,
collisionObject,
collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(),
resultCallback);
btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans,
collisionObject,
collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(),
resultCallback);
}
}
}

79
src/BulletCollision/CollisionDispatch/btGhostObject.h
View File

@@ -16,7 +16,6 @@ subject to the following restrictions:
#ifndef BT_GHOST_OBJECT_H
#define BT_GHOST_OBJECT_H
#include "btCollisionObject.h"
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h"
#include "LinearMath/btAlignedAllocator.h"
@@ -31,48 +30,47 @@ class btDispatcher;
///By default, this overlap is based on the AABB
///This is useful for creating a character controller, collision sensors/triggers, explosions etc.
///We plan on adding rayTest and other queries for the btGhostObject
ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject
ATTRIBUTE_ALIGNED16(class)
btGhostObject : public btCollisionObject
{
protected:
btAlignedObjectArray<btCollisionObject*> m_overlappingObjects;
public:
btGhostObject();
virtual ~btGhostObject();
void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const;
void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const;
void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const;
void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const;
///this method is mainly for expert/internal use only.
virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0);
virtual void addOverlappingObjectInternal(btBroadphaseProxy * otherProxy, btBroadphaseProxy* thisProxy = 0);
///this method is mainly for expert/internal use only.
virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0);
virtual void removeOverlappingObjectInternal(btBroadphaseProxy * otherProxy, btDispatcher * dispatcher, btBroadphaseProxy* thisProxy = 0);
int getNumOverlappingObjects() const
int getNumOverlappingObjects() const
{
return m_overlappingObjects.size();
}
btCollisionObject* getOverlappingObject(int index)
btCollisionObject* getOverlappingObject(int index)
{
return m_overlappingObjects[index];
}
const btCollisionObject* getOverlappingObject(int index) const
const btCollisionObject* getOverlappingObject(int index) const
{
return m_overlappingObjects[index];
}
btAlignedObjectArray<btCollisionObject*>& getOverlappingPairs()
btAlignedObjectArray<btCollisionObject*>& getOverlappingPairs()
{
return m_overlappingObjects;
}
const btAlignedObjectArray<btCollisionObject*> getOverlappingPairs() const
const btAlignedObjectArray<btCollisionObject*> getOverlappingPairs() const
{
return m_overlappingObjects;
}
@@ -81,49 +79,43 @@ public:
// internal cast
//
static const btGhostObject* upcast(const btCollisionObject* colObj)
static const btGhostObject* upcast(const btCollisionObject* colObj)
{
if (colObj->getInternalType()==CO_GHOST_OBJECT)
if (colObj->getInternalType() == CO_GHOST_OBJECT)
return (const btGhostObject*)colObj;
return 0;
}
static btGhostObject* upcast(btCollisionObject* colObj)
static btGhostObject* upcast(btCollisionObject * colObj)
{
if (colObj->getInternalType()==CO_GHOST_OBJECT)
if (colObj->getInternalType() == CO_GHOST_OBJECT)
return (btGhostObject*)colObj;
return 0;
}
};
class btPairCachingGhostObject : public btGhostObject
class btPairCachingGhostObject : public btGhostObject
{
btHashedOverlappingPairCache* m_hashPairCache;
btHashedOverlappingPairCache* m_hashPairCache;
public:
btPairCachingGhostObject();
virtual ~btPairCachingGhostObject();
///this method is mainly for expert/internal use only.
virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0);
virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy = 0);
virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0);
virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy = 0);
btHashedOverlappingPairCache* getOverlappingPairCache()
btHashedOverlappingPairCache* getOverlappingPairCache()
{
return m_hashPairCache;
}
};
///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject.
class btGhostPairCallback : public btOverlappingPairCallback
{
public:
btGhostPairCallback()
{
@@ -131,15 +123,14 @@ public:
virtual ~btGhostPairCallback()
{
}
virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
{
btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject;
btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject;
btGhostObject* ghost0 = btGhostObject::upcast(colObj0);
btGhostObject* ghost1 = btGhostObject::upcast(colObj1);
btCollisionObject* colObj0 = (btCollisionObject*)proxy0->m_clientObject;
btCollisionObject* colObj1 = (btCollisionObject*)proxy1->m_clientObject;
btGhostObject* ghost0 = btGhostObject::upcast(colObj0);
btGhostObject* ghost1 = btGhostObject::upcast(colObj1);
if (ghost0)
ghost0->addOverlappingObjectInternal(proxy1, proxy0);
if (ghost1)
@@ -147,29 +138,25 @@ public:
return 0;
}
virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher)
{
btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject;
btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject;
btGhostObject* ghost0 = btGhostObject::upcast(colObj0);
btGhostObject* ghost1 = btGhostObject::upcast(colObj1);
btCollisionObject* colObj0 = (btCollisionObject*)proxy0->m_clientObject;
btCollisionObject* colObj1 = (btCollisionObject*)proxy1->m_clientObject;
btGhostObject* ghost0 = btGhostObject::upcast(colObj0);
btGhostObject* ghost1 = btGhostObject::upcast(colObj1);
if (ghost0)
ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0);
ghost0->removeOverlappingObjectInternal(proxy1, dispatcher, proxy0);
if (ghost1)
ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1);
ghost1->removeOverlappingObjectInternal(proxy0, dispatcher, proxy1);
return 0;
}
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/)
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/, btDispatcher* /*dispatcher*/)
{
btAssert(0);
//need to keep track of all ghost objects and call them here
//m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher);
}
};
#endif

85
src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp
View File

@@ -13,64 +13,49 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btHashedSimplePairCache.h"
#include <stdio.h>
#ifdef BT_DEBUG_COLLISION_PAIRS
int gOverlappingSimplePairs = 0;
int gRemoveSimplePairs =0;
int gAddedSimplePairs =0;
int gFindSimplePairs =0;
#endif //BT_DEBUG_COLLISION_PAIRS
int gOverlappingSimplePairs = 0;
int gRemoveSimplePairs = 0;
int gAddedSimplePairs = 0;
int gFindSimplePairs = 0;
#endif //BT_DEBUG_COLLISION_PAIRS
btHashedSimplePairCache::btHashedSimplePairCache() {
int initialAllocatedSize= 2;
btHashedSimplePairCache::btHashedSimplePairCache()
{
int initialAllocatedSize = 2;
m_overlappingPairArray.reserve(initialAllocatedSize);
growTables();
}
btHashedSimplePairCache::~btHashedSimplePairCache()
{
}
void btHashedSimplePairCache::removeAllPairs()
{
m_overlappingPairArray.clear();
m_hashTable.clear();
m_next.clear();
int initialAllocatedSize= 2;
int initialAllocatedSize = 2;
m_overlappingPairArray.reserve(initialAllocatedSize);
growTables();
}
btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB)
{
#ifdef BT_DEBUG_COLLISION_PAIRS
gFindSimplePairs++;
#endif
/*if (indexA > indexB)
btSwap(indexA, indexB);*/
int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1));
if (hash >= m_hashTable.size())
{
@@ -95,9 +80,8 @@ btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB)
//#include <stdio.h>
void btHashedSimplePairCache::growTables()
void btHashedSimplePairCache::growTables()
{
int newCapacity = m_overlappingPairArray.capacity();
if (m_hashTable.size() < newCapacity)
@@ -108,10 +92,9 @@ void btHashedSimplePairCache::growTables()
m_hashTable.resize(newCapacity);
m_next.resize(newCapacity);
int i;
for (i= 0; i < newCapacity; ++i)
for (i = 0; i < newCapacity; ++i)
{
m_hashTable[i] = BT_SIMPLE_NULL_PAIR;
}
@@ -120,27 +103,22 @@ void btHashedSimplePairCache::growTables()
m_next[i] = BT_SIMPLE_NULL_PAIR;
}
for(i=0;i<curHashtableSize;i++)
for (i = 0; i < curHashtableSize; i++)
{
const btSimplePair& pair = m_overlappingPairArray[i];
int indexA = pair.m_indexA;
int indexB = pair.m_indexB;
int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask
int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask
m_next[i] = m_hashTable[hashValue];
m_hashTable[hashValue] = i;
}
}
}
btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB)
{
int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask
int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask
btSimplePair* pair = internalFindPair(indexA, indexB, hash);
if (pair != NULL)
@@ -158,32 +136,29 @@ btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB)
{
growTables();
//hash with new capacity
hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1));
}
pair = new (mem) btSimplePair(indexA,indexB);
pair = new (mem) btSimplePair(indexA, indexB);
pair->m_userPointer = 0;
m_next[count] = m_hashTable[hash];
m_hashTable[hash] = count;
return pair;
}
void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
{
#ifdef BT_DEBUG_COLLISION_PAIRS
gRemoveSimplePairs++;
#endif
/*if (indexA > indexB)
btSwap(indexA, indexB);*/
int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1));
btSimplePair* pair = internalFindPair(indexA, indexB, hash);
if (pair == NULL)
@@ -191,10 +166,8 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
return 0;
}
void* userData = pair->m_userPointer;
int pairIndex = int(pair - &m_overlappingPairArray[0]);
btAssert(pairIndex < m_overlappingPairArray.size());
@@ -234,8 +207,8 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
// Remove the last pair from the hash table.
const btSimplePair* last = &m_overlappingPairArray[lastPairIndex];
/* missing swap here too, Nat. */
int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity()-1));
/* missing swap here too, Nat. */
int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity() - 1));
index = m_hashTable[lastHash];
btAssert(index != BT_SIMPLE_NULL_PAIR);
@@ -269,13 +242,3 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
return userData;
}
//#include <stdio.h>

101
src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h
View File

@@ -16,144 +16,126 @@ subject to the following restrictions:
#ifndef BT_HASHED_SIMPLE_PAIR_CACHE_H
#define BT_HASHED_SIMPLE_PAIR_CACHE_H
#include "LinearMath/btAlignedObjectArray.h"
const int BT_SIMPLE_NULL_PAIR=0xffffffff;
const int BT_SIMPLE_NULL_PAIR = 0xffffffff;
struct btSimplePair
{
btSimplePair(int indexA,int indexB)
:m_indexA(indexA),
m_indexB(indexB),
m_userPointer(0)
btSimplePair(int indexA, int indexB)
: m_indexA(indexA),
m_indexB(indexB),
m_userPointer(0)
{
}
int m_indexA;
int m_indexB;
union
{
void* m_userPointer;
int m_userValue;
union {
void* m_userPointer;
int m_userValue;
};
};
typedef btAlignedObjectArray<btSimplePair> btSimplePairArray;
typedef btAlignedObjectArray<btSimplePair> btSimplePairArray;
#ifdef BT_DEBUG_COLLISION_PAIRS
extern int gOverlappingSimplePairs;
extern int gRemoveSimplePairs;
extern int gAddedSimplePairs;
extern int gFindSimplePairs;
#endif //BT_DEBUG_COLLISION_PAIRS
#endif //BT_DEBUG_COLLISION_PAIRS
class btHashedSimplePairCache
{
btSimplePairArray m_overlappingPairArray;
btSimplePairArray m_overlappingPairArray;
protected:
btAlignedObjectArray<int> m_hashTable;
btAlignedObjectArray<int> m_next;
btAlignedObjectArray<int> m_hashTable;
btAlignedObjectArray<int> m_next;
public:
btHashedSimplePairCache();
virtual ~btHashedSimplePairCache();
void removeAllPairs();
virtual void* removeOverlappingPair(int indexA,int indexB);
virtual void* removeOverlappingPair(int indexA, int indexB);
// Add a pair and return the new pair. If the pair already exists,
// no new pair is created and the old one is returned.
virtual btSimplePair* addOverlappingPair(int indexA,int indexB)
virtual btSimplePair* addOverlappingPair(int indexA, int indexB)
{
#ifdef BT_DEBUG_COLLISION_PAIRS
gAddedSimplePairs++;
#endif
return internalAddPair(indexA,indexB);
return internalAddPair(indexA, indexB);
}
virtual btSimplePair* getOverlappingPairArrayPtr()
virtual btSimplePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
}
const btSimplePair* getOverlappingPairArrayPtr() const
const btSimplePair* getOverlappingPairArrayPtr() const
{
return &m_overlappingPairArray[0];
}
btSimplePairArray& getOverlappingPairArray()
btSimplePairArray& getOverlappingPairArray()
{
return m_overlappingPairArray;
}
const btSimplePairArray& getOverlappingPairArray() const
const btSimplePairArray& getOverlappingPairArray() const
{
return m_overlappingPairArray;
}
btSimplePair* findPair(int indexA,int indexB);
btSimplePair* findPair(int indexA, int indexB);
int GetCount() const { return m_overlappingPairArray.size(); }
int getNumOverlappingPairs() const
int getNumOverlappingPairs() const
{
return m_overlappingPairArray.size();
}
private:
btSimplePair* internalAddPair(int indexA, int indexB);
void growTables();
private:
btSimplePair* internalAddPair(int indexA, int indexB);
void growTables();
SIMD_FORCE_INLINE bool equalsPair(const btSimplePair& pair, int indexA, int indexB)
{
{
return pair.m_indexA == indexA && pair.m_indexB == indexB;
}
SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB)
{
unsigned int key = indexA | (indexB << 16);
// Thomas Wang's hash
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
key ^= (key >> 16);
return key;
}
SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA , int proxyIdB, int hash)
SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA, int proxyIdB, int hash)
{
int index = m_hashTable[hash];
while( index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false)
while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false)
{
index = m_next[index];
}
if ( index == BT_SIMPLE_NULL_PAIR )
if (index == BT_SIMPLE_NULL_PAIR)
{
return NULL;
}
@@ -162,13 +144,6 @@ private:
return &m_overlappingPairArray[index];
}
};
#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H
#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H

754
src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
View File

File diff suppressed because it is too large Load Diff

18
src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h
View File

@@ -16,32 +16,26 @@ struct btCollisionObjectWrapper;
class btManifoldPoint;
class btIDebugDraw;
enum btInternalEdgeAdjustFlags
{
BT_TRIANGLE_CONVEX_BACKFACE_MODE = 1,
BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended
BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended
BT_TRIANGLE_CONVEX_DOUBLE_SIDED = 4
};
///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo'
void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap);
void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap);
///Call the btFixMeshNormal to adjust the collision normal, using the triangle info map (generated using btGenerateInternalEdgeInfo)
///If this info map is missing, or the triangle is not store in this map, nothing will be done
void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap,const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0);
void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap, const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0);
///Enable the BT_INTERNAL_EDGE_DEBUG_DRAW define and call btSetDebugDrawer, to get visual info to see if the internal edge utility works properly.
///If the utility doesn't work properly, you might have to adjust the threshold values in btTriangleInfoMap
//#define BT_INTERNAL_EDGE_DEBUG_DRAW
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
void btSetDebugDrawer(btIDebugDraw* debugDrawer);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#endif //BT_INTERNAL_EDGE_UTILITY_H
void btSetDebugDrawer(btIDebugDraw* debugDrawer);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#endif //BT_INTERNAL_EDGE_UTILITY_H

176
src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
View File

@@ -13,106 +13,102 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btManifoldResult.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
///This is to allow MaterialCombiner/Custom Friction/Restitution values
ContactAddedCallback gContactAddedCallback=0;
ContactAddedCallback gContactAddedCallback = 0;
CalculateCombinedCallback gCalculateCombinedRestitutionCallback = &btManifoldResult::calculateCombinedRestitution;
CalculateCombinedCallback gCalculateCombinedFrictionCallback = &btManifoldResult::calculateCombinedFriction;
CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback = &btManifoldResult::calculateCombinedRollingFriction;
CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback = &btManifoldResult::calculateCombinedSpinningFriction;
CalculateCombinedCallback gCalculateCombinedContactDampingCallback = &btManifoldResult::calculateCombinedContactDamping;
CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback = &btManifoldResult::calculateCombinedContactStiffness;
CalculateCombinedCallback gCalculateCombinedRestitutionCallback = &btManifoldResult::calculateCombinedRestitution;
CalculateCombinedCallback gCalculateCombinedFrictionCallback = &btManifoldResult::calculateCombinedFriction;
CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback = &btManifoldResult::calculateCombinedRollingFriction;
CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback = &btManifoldResult::calculateCombinedSpinningFriction;
CalculateCombinedCallback gCalculateCombinedContactDampingCallback = &btManifoldResult::calculateCombinedContactDamping;
CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback = &btManifoldResult::calculateCombinedContactStiffness;
btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0, const btCollisionObject* body1)
{
btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction();
const btScalar MAX_FRICTION = btScalar(10.);
const btScalar MAX_FRICTION = btScalar(10.);
if (friction < -MAX_FRICTION)
friction = -MAX_FRICTION;
if (friction > MAX_FRICTION)
friction = MAX_FRICTION;
return friction;
}
btScalar btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1)
btScalar btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0, const btCollisionObject* body1)
{
btScalar friction = body0->getSpinningFriction() * body1->getFriction() + body1->getSpinningFriction() * body0->getFriction();
const btScalar MAX_FRICTION = btScalar(10.);
if (friction < -MAX_FRICTION)
friction = -MAX_FRICTION;
if (friction > MAX_FRICTION)
friction = MAX_FRICTION;
return friction;
btScalar friction = body0->getSpinningFriction() * body1->getFriction() + body1->getSpinningFriction() * body0->getFriction();
const btScalar MAX_FRICTION = btScalar(10.);
if (friction < -MAX_FRICTION)
friction = -MAX_FRICTION;
if (friction > MAX_FRICTION)
friction = MAX_FRICTION;
return friction;
}
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
btScalar btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1)
btScalar btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0, const btCollisionObject* body1)
{
btScalar friction = body0->getFriction() * body1->getFriction();
const btScalar MAX_FRICTION = btScalar(10.);
const btScalar MAX_FRICTION = btScalar(10.);
if (friction < -MAX_FRICTION)
friction = -MAX_FRICTION;
if (friction > MAX_FRICTION)
friction = MAX_FRICTION;
return friction;
}
btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1)
btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0, const btCollisionObject* body1)
{
return body0->getRestitution() * body1->getRestitution();
}
btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1)
btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0, const btCollisionObject* body1)
{
return body0->getContactDamping() + body1->getContactDamping();
return body0->getContactDamping() + body1->getContactDamping();
}
btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1)
btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0, const btCollisionObject* body1)
{
btScalar s0 = body0->getContactStiffness();
btScalar s1 = body1->getContactStiffness();
btScalar tmp0 = btScalar(1)/s0;
btScalar tmp1 = btScalar(1)/s1;
btScalar combinedStiffness = btScalar(1) / (tmp0+tmp1);
return combinedStiffness;
btScalar s0 = body0->getContactStiffness();
btScalar s1 = body1->getContactStiffness();
btScalar tmp0 = btScalar(1) / s0;
btScalar tmp1 = btScalar(1) / s1;
btScalar combinedStiffness = btScalar(1) / (tmp0 + tmp1);
return combinedStiffness;
}
btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
:m_manifoldPtr(0),
m_body0Wrap(body0Wrap),
m_body1Wrap(body1Wrap)
btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
: m_manifoldPtr(0),
m_body0Wrap(body0Wrap),
m_body1Wrap(body1Wrap)
#ifdef DEBUG_PART_INDEX
,m_partId0(-1),
m_partId1(-1),
m_index0(-1),
m_index1(-1)
#endif //DEBUG_PART_INDEX
, m_closestPointDistanceThreshold(0)
,
m_partId0(-1),
m_partId1(-1),
m_index0(-1),
m_index1(-1)
#endif //DEBUG_PART_INDEX
,
m_closestPointDistanceThreshold(0)
{
}
void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
{
btAssert(m_manifoldPtr);
//order in manifold needs to match
if (depth > m_manifoldPtr->getContactBreakingThreshold())
// if (depth > m_manifoldPtr->getContactProcessingThreshold())
// if (depth > m_manifoldPtr->getContactProcessingThreshold())
return;
bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
@@ -122,81 +118,82 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
btVector3 localA;
btVector3 localB;
if (isSwapped)
{
localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA);
localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
} else
}
else
{
localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA);
localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
}
btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth);
newPt.m_positionWorldOnA = pointA;
newPt.m_positionWorldOnB = pointInWorld;
int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
newPt.m_combinedFriction = gCalculateCombinedFrictionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_combinedRestitution = gCalculateCombinedRestitutionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_combinedRollingFriction = gCalculateCombinedRollingFrictionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_combinedSpinningFriction = gCalculateCombinedSpinningFrictionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
(m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))
{
newPt.m_combinedContactDamping1 = gCalculateCombinedContactDampingCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_combinedContactStiffness1 = gCalculateCombinedContactStiffnessCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
}
newPt.m_combinedFriction = gCalculateCombinedFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
newPt.m_combinedRestitution = gCalculateCombinedRestitutionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
newPt.m_combinedRollingFriction = gCalculateCombinedRollingFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
newPt.m_combinedSpinningFriction = gCalculateCombinedSpinningFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR) ||
(m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR))
{
newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR;
}
if ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
(m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))
{
newPt.m_combinedContactDamping1 = gCalculateCombinedContactDampingCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
newPt.m_combinedContactStiffness1 = gCalculateCombinedContactStiffnessCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
}
btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);
if ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR) ||
(m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR))
{
newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR;
}
//BP mod, store contact triangles.
btPlaneSpace1(newPt.m_normalWorldOnB, newPt.m_lateralFrictionDir1, newPt.m_lateralFrictionDir2);
//BP mod, store contact triangles.
if (isSwapped)
{
newPt.m_partId0 = m_partId1;
newPt.m_partId1 = m_partId0;
newPt.m_index0 = m_index1;
newPt.m_index1 = m_index0;
} else
newPt.m_index0 = m_index1;
newPt.m_index1 = m_index0;
}
else
{
newPt.m_partId0 = m_partId0;
newPt.m_partId1 = m_partId1;
newPt.m_index0 = m_index0;
newPt.m_index1 = m_index1;
newPt.m_index0 = m_index0;
newPt.m_index1 = m_index1;
}
//printf("depth=%f\n",depth);
///@todo, check this for any side effects
if (insertIndex >= 0)
{
//const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
m_manifoldPtr->replaceContactPoint(newPt,insertIndex);
} else
m_manifoldPtr->replaceContactPoint(newPt, insertIndex);
}
else
{
insertIndex = m_manifoldPtr->addManifoldPoint(newPt);
}
//User can override friction and/or restitution
if (gContactAddedCallback &&
//and if either of the two bodies requires custom material
((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
(m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
(m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
{
//experimental feature info, for per-triangle material etc.
const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap;
const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap;
(*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1);
const btCollisionObjectWrapper* obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap;
const btCollisionObjectWrapper* obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap;
(*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex), obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1);
}
if (gContactStartedCallback && isNewCollision)
@@ -204,4 +201,3 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
gContactStartedCallback(m_manifoldPtr);
}
}

90
src/BulletCollision/CollisionDispatch/btManifoldResult.h
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_MANIFOLD_RESULT_H
#define BT_MANIFOLD_RESULT_H
@@ -29,85 +28,81 @@ class btManifoldPoint;
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1);
extern ContactAddedCallback gContactAddedCallback;
typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1);
extern ContactAddedCallback gContactAddedCallback;
//#define DEBUG_PART_INDEX 1
/// These callbacks are used to customize the algorith that combine restitution, friction, damping, Stiffness
typedef btScalar (*CalculateCombinedCallback)(const btCollisionObject* body0,const btCollisionObject* body1);
typedef btScalar (*CalculateCombinedCallback)(const btCollisionObject* body0, const btCollisionObject* body1);
extern CalculateCombinedCallback gCalculateCombinedRestitutionCallback;
extern CalculateCombinedCallback gCalculateCombinedFrictionCallback;
extern CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback;
extern CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback;
extern CalculateCombinedCallback gCalculateCombinedContactDampingCallback;
extern CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback;
extern CalculateCombinedCallback gCalculateCombinedRestitutionCallback;
extern CalculateCombinedCallback gCalculateCombinedFrictionCallback;
extern CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback;
extern CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback;
extern CalculateCombinedCallback gCalculateCombinedContactDampingCallback;
extern CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback;
///btManifoldResult is a helper class to manage contact results.
class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result
{
protected:
btPersistentManifold* m_manifoldPtr;
const btCollisionObjectWrapper* m_body0Wrap;
const btCollisionObjectWrapper* m_body1Wrap;
int m_partId0;
int m_partId0;
int m_partId1;
int m_index0;
int m_index1;
public:
public:
btManifoldResult()
:
#ifdef DEBUG_PART_INDEX
m_partId0(-1),
m_partId1(-1),
m_index0(-1),
m_index1(-1)
#endif //DEBUG_PART_INDEX
m_closestPointDistanceThreshold(0)
m_partId0(-1),
m_partId1(-1),
m_index0(-1),
m_index1(-1)
#endif //DEBUG_PART_INDEX
m_closestPointDistanceThreshold(0)
{
}
btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
btManifoldResult(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap);
virtual ~btManifoldResult() {};
virtual ~btManifoldResult(){};
void setPersistentManifold(btPersistentManifold* manifoldPtr)
void setPersistentManifold(btPersistentManifold* manifoldPtr)
{
m_manifoldPtr = manifoldPtr;
}
const btPersistentManifold* getPersistentManifold() const
const btPersistentManifold* getPersistentManifold() const
{
return m_manifoldPtr;
}
btPersistentManifold* getPersistentManifold()
btPersistentManifold* getPersistentManifold()
{
return m_manifoldPtr;
}
virtual void setShapeIdentifiersA(int partId0,int index0)
virtual void setShapeIdentifiersA(int partId0, int index0)
{
m_partId0=partId0;
m_index0=index0;
m_partId0 = partId0;
m_index0 = index0;
}
virtual void setShapeIdentifiersB( int partId1,int index1)
virtual void setShapeIdentifiersB(int partId1, int index1)
{
m_partId1=partId1;
m_index1=index1;
m_partId1 = partId1;
m_index1 = index1;
}
virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth);
virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth);
SIMD_FORCE_INLINE void refreshContactPoints()
SIMD_FORCE_INLINE void refreshContactPoints()
{
btAssert(m_manifoldPtr);
if (!m_manifoldPtr->getNumContacts())
@@ -117,10 +112,11 @@ public:
if (isSwapped)
{
m_manifoldPtr->refreshContactPoints(m_body1Wrap->getCollisionObject()->getWorldTransform(),m_body0Wrap->getCollisionObject()->getWorldTransform());
} else
m_manifoldPtr->refreshContactPoints(m_body1Wrap->getCollisionObject()->getWorldTransform(), m_body0Wrap->getCollisionObject()->getWorldTransform());
}
else
{
m_manifoldPtr->refreshContactPoints(m_body0Wrap->getCollisionObject()->getWorldTransform(),m_body1Wrap->getCollisionObject()->getWorldTransform());
m_manifoldPtr->refreshContactPoints(m_body0Wrap->getCollisionObject()->getWorldTransform(), m_body1Wrap->getCollisionObject()->getWorldTransform());
}
}
@@ -153,15 +149,15 @@ public:
return m_body1Wrap->getCollisionObject();
}
btScalar m_closestPointDistanceThreshold;
btScalar m_closestPointDistanceThreshold;
/// in the future we can let the user override the methods to combine restitution and friction
static btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedRestitution(const btCollisionObject* body0, const btCollisionObject* body1);
static btScalar calculateCombinedFriction(const btCollisionObject* body0, const btCollisionObject* body1);
static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0, const btCollisionObject* body1);
static btScalar calculateCombinedSpinningFriction(const btCollisionObject* body0, const btCollisionObject* body1);
static btScalar calculateCombinedContactDamping(const btCollisionObject* body0, const btCollisionObject* body1);
static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0, const btCollisionObject* body1);
};
#endif //BT_MANIFOLD_RESULT_H
#endif //BT_MANIFOLD_RESULT_H

264
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
View File

@@ -14,7 +14,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "LinearMath/btScalar.h"
#include "btSimulationIslandManager.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
@@ -25,8 +24,7 @@ subject to the following restrictions:
//#include <stdio.h>
#include "LinearMath/btQuickprof.h"
btSimulationIslandManager::btSimulationIslandManager():
m_splitIslands(true)
btSimulationIslandManager::btSimulationIslandManager() : m_splitIslands(true)
{
}
@@ -34,53 +32,47 @@ btSimulationIslandManager::~btSimulationIslandManager()
{
}
void btSimulationIslandManager::initUnionFind(int n)
{
m_unionFind.reset(n);
m_unionFind.reset(n);
}
void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld)
void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */, btCollisionWorld* colWorld)
{
{
btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
if (numOverlappingPairs)
{
btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
for (int i=0;i<numOverlappingPairs;i++)
{
const btBroadphasePair& collisionPair = pairPtr[i];
btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
((colObj1) && ((colObj1)->mergesSimulationIslands())))
for (int i = 0; i < numOverlappingPairs; i++)
{
const btBroadphasePair& collisionPair = pairPtr[i];
btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
m_unionFind.unite((colObj0)->getIslandTag(),
(colObj1)->getIslandTag());
if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
((colObj1) && ((colObj1)->mergesSimulationIslands())))
{
m_unionFind.unite((colObj0)->getIslandTag(),
(colObj1)->getIslandTag());
}
}
}
}
}
}
#ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher)
{
// put the index into m_controllers into m_tag
// put the index into m_controllers into m_tag
int index = 0;
{
int i;
for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
{
btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
//Adding filtering here
if (!collisionObject->isStaticOrKinematicObject())
{
@@ -92,28 +84,29 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWor
}
// do the union find
initUnionFind( index );
initUnionFind(index);
findUnions(dispatcher,colWorld);
findUnions(dispatcher, colWorld);
}
void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
{
// put the islandId ('find' value) into m_tag
// put the islandId ('find' value) into m_tag
{
int index = 0;
int i;
for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
{
btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
if (!collisionObject->isStaticOrKinematicObject())
{
collisionObject->setIslandTag( m_unionFind.find(index) );
collisionObject->setIslandTag(m_unionFind.find(index));
//Set the correct object offset in Collision Object Array
m_unionFind.getElement(index).m_sz = i;
collisionObject->setCompanionId(-1);
index++;
} else
}
else
{
collisionObject->setIslandTag(-1);
collisionObject->setCompanionId(-2);
@@ -122,49 +115,44 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* c
}
}
#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher)
{
initUnionFind(int(colWorld->getCollisionObjectArray().size()));
initUnionFind( int (colWorld->getCollisionObjectArray().size()));
// put the index into m_controllers into m_tag
// put the index into m_controllers into m_tag
{
int index = 0;
int i;
for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
{
btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
collisionObject->setIslandTag(index);
collisionObject->setCompanionId(-1);
collisionObject->setHitFraction(btScalar(1.));
index++;
}
}
// do the union find
findUnions(dispatcher,colWorld);
findUnions(dispatcher, colWorld);
}
void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
{
// put the islandId ('find' value) into m_tag
// put the islandId ('find' value) into m_tag
{
int index = 0;
int i;
for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
{
btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
if (!collisionObject->isStaticOrKinematicObject())
{
collisionObject->setIslandTag( m_unionFind.find(index) );
collisionObject->setIslandTag(m_unionFind.find(index));
collisionObject->setCompanionId(-1);
} else
}
else
{
collisionObject->setIslandTag(-1);
collisionObject->setCompanionId(-2);
@@ -174,72 +162,59 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col
}
}
#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
inline int getIslandId(const btPersistentManifold* lhs)
inline int getIslandId(const btPersistentManifold* lhs)
{
int islandId;
const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag();
return islandId;
}
/// function object that routes calls to operator<
class btPersistentManifoldSortPredicate
{
public:
SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const
{
return getIslandId(lhs) < getIslandId(rhs);
}
public:
SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
{
return getIslandId(lhs) < getIslandId(rhs);
}
};
class btPersistentManifoldSortPredicateDeterministic
{
public:
SIMD_FORCE_INLINE bool operator() (const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
{
return (
(getIslandId(lhs) < getIslandId(rhs))
|| ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId)
||((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) &&
(lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId))
);
(getIslandId(lhs) < getIslandId(rhs)) || ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) || ((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) && (lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId)));
}
};
void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld)
{
BT_PROFILE("islandUnionFindAndQuickSort");
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
m_islandmanifold.resize(0);
//we are going to sort the unionfind array, and store the element id in the size
//afterwards, we clean unionfind, to make sure no-one uses it anymore
getUnionFind().sortIslands();
int numElem = getUnionFind().getNumElements();
int endIslandIndex=1;
int endIslandIndex = 1;
int startIslandIndex;
//update the sleeping state for bodies, if all are sleeping
for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
{
int islandId = getUnionFind().getElement(startIslandIndex).m_id;
for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
{
}
@@ -248,69 +223,68 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
bool allSleeping = true;
int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++)
for (idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{
// printf("error in island management\n");
// printf("error in island management\n");
}
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
if (colObj0->getActivationState()== ACTIVE_TAG ||
colObj0->getActivationState()== DISABLE_DEACTIVATION)
if (colObj0->getActivationState() == ACTIVE_TAG ||
colObj0->getActivationState() == DISABLE_DEACTIVATION)
{
allSleeping = false;
break;
}
}
}
if (allSleeping)
{
int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++)
for (idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{
// printf("error in island management\n");
// printf("error in island management\n");
}
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
colObj0->setActivationState( ISLAND_SLEEPING );
colObj0->setActivationState(ISLAND_SLEEPING);
}
}
} else
}
else
{
int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++)
for (idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{
// printf("error in island management\n");
// printf("error in island management\n");
}
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
if ( colObj0->getActivationState() == ISLAND_SLEEPING)
if (colObj0->getActivationState() == ISLAND_SLEEPING)
{
colObj0->setActivationState( WANTS_DEACTIVATION);
colObj0->setActivationState(WANTS_DEACTIVATION);
colObj0->setDeactivationTime(0.f);
}
}
@@ -318,34 +292,30 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
}
}
int i;
int maxNumManifolds = dispatcher->getNumManifolds();
//#define SPLIT_ISLANDS 1
//#ifdef SPLIT_ISLANDS
//#define SPLIT_ISLANDS 1
//#ifdef SPLIT_ISLANDS
//#endif //SPLIT_ISLANDS
//#endif //SPLIT_ISLANDS
for (i=0;i<maxNumManifolds ;i++)
for (i = 0; i < maxNumManifolds; i++)
{
btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
{
btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
{
if (manifold->getNumContacts() == 0)
continue;
}
}
const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
///@todo: check sleeping conditions!
if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
///@todo: check sleeping conditions!
if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
{
//kinematic objects don't merge islands, but wake up all connected objects
if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
{
@@ -357,36 +327,34 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
if (colObj1->hasContactResponse())
colObj0->activate();
}
if(m_splitIslands)
{
if (m_splitIslands)
{
//filtering for response
if (dispatcher->needsResponse(colObj0,colObj1))
if (dispatcher->needsResponse(colObj0, colObj1))
m_islandmanifold.push_back(manifold);
}
}
}
}
///@todo: this is random access, it can be walked 'cache friendly'!
void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback)
{
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
buildIslands(dispatcher,collisionWorld);
buildIslands(dispatcher, collisionWorld);
int endIslandIndex=1;
int endIslandIndex = 1;
int startIslandIndex;
int numElem = getUnionFind().getNumElements();
BT_PROFILE("processIslands");
if(!m_splitIslands)
if (!m_splitIslands)
{
btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
int maxNumManifolds = dispatcher->getNumManifolds();
callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
callback->processIsland(&collisionObjects[0], collisionObjects.size(), manifold, maxNumManifolds, -1);
}
else
{
@@ -394,7 +362,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
// Sort the vector using predicate and std::sort
//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
int numManifolds = int (m_islandmanifold.size());
int numManifolds = int(m_islandmanifold.size());
//tried a radix sort, but quicksort/heapsort seems still faster
//@todo rewrite island management
@@ -403,7 +371,8 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
{
m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic());
} else
}
else
{
m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
}
@@ -417,55 +386,49 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
//int islandId;
// printf("Start Islands\n");
// printf("Start Islands\n");
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
{
int islandId = getUnionFind().getElement(startIslandIndex).m_id;
bool islandSleeping = true;
bool islandSleeping = true;
for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
{
int i = getUnionFind().getElement(endIslandIndex).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
m_islandBodies.push_back(colObj0);
if (colObj0->isActive())
islandSleeping = false;
}
for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
{
int i = getUnionFind().getElement(endIslandIndex).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
m_islandBodies.push_back(colObj0);
if (colObj0->isActive())
islandSleeping = false;
}
//find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
btPersistentManifold** startManifold = 0;
if (startManifoldIndex<numManifolds)
if (startManifoldIndex < numManifolds)
{
int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
if (curIslandId == islandId)
{
startManifold = &m_islandmanifold[startManifoldIndex];
for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
{
for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex])); endManifoldIndex++)
{
}
/// Process the actual simulation, only if not sleeping/deactivated
numIslandManifolds = endManifoldIndex-startManifoldIndex;
numIslandManifolds = endManifoldIndex - startManifoldIndex;
}
}
if (!islandSleeping)
{
callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
// printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
callback->processIsland(&m_islandBodies[0], m_islandBodies.size(), startManifold, numIslandManifolds, islandId);
// printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
}
if (numIslandManifolds)
{
startManifoldIndex = endManifoldIndex;
@@ -473,6 +436,5 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
m_islandBodies.resize(0);
}
} // else if(!splitIslands)
} // else if(!splitIslands)
}

38
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
View File

@@ -26,45 +26,39 @@ class btCollisionWorld;
class btDispatcher;
class btPersistentManifold;
///SimulationIslandManager creates and handles simulation islands, using btUnionFind
class btSimulationIslandManager
{
btUnionFind m_unionFind;
btAlignedObjectArray<btPersistentManifold*> m_islandmanifold;
btAlignedObjectArray<btCollisionObject* > m_islandBodies;
btAlignedObjectArray<btPersistentManifold*> m_islandmanifold;
btAlignedObjectArray<btCollisionObject*> m_islandBodies;
bool m_splitIslands;
public:
btSimulationIslandManager();
virtual ~btSimulationIslandManager();
void initUnionFind(int n);
void initUnionFind(int n);
btUnionFind& getUnionFind() { return m_unionFind;}
btUnionFind& getUnionFind() { return m_unionFind; }
virtual void updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher);
virtual void storeIslandActivationState(btCollisionWorld* world);
virtual void updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher);
virtual void storeIslandActivationState(btCollisionWorld* world);
void findUnions(btDispatcher* dispatcher, btCollisionWorld* colWorld);
void findUnions(btDispatcher* dispatcher,btCollisionWorld* colWorld);
struct IslandCallback
struct IslandCallback
{
virtual ~IslandCallback() {};
virtual ~IslandCallback(){};
virtual void processIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0;
virtual void processIsland(btCollisionObject** bodies, int numBodies, class btPersistentManifold** manifolds, int numManifolds, int islandId) = 0;
};
void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback);
void buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback);
void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld);
bool getSplitIslands()
{
@@ -74,8 +68,6 @@ public:
{
m_splitIslands = doSplitIslands;
}
};
#endif //BT_SIMULATION_ISLAND_MANAGER_H
#endif //BT_SIMULATION_ISLAND_MANAGER_H

89
src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
View File

@@ -21,23 +21,22 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
//#include <stdio.h>
btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped)
: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap),
m_ownManifold(false),
m_manifoldPtr(mf),
m_isSwapped(isSwapped)
btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped)
: btActivatingCollisionAlgorithm(ci, col0Wrap, col1Wrap),
m_ownManifold(false),
m_manifoldPtr(mf),
m_isSwapped(isSwapped)
{
const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? col1Wrap : col0Wrap;
const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? col0Wrap : col1Wrap;
if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject()))
const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? col1Wrap : col0Wrap;
const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? col0Wrap : col1Wrap;
if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject()))
{
m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject());
m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject());
m_ownManifold = true;
}
}
btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
{
if (m_ownManifold)
@@ -47,17 +46,15 @@ btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
}
}
void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
void btSphereBoxCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
(void)dispatchInfo;
(void)resultOut;
if (!m_manifoldPtr)
return;
const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? body0Wrap : body1Wrap;
const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
btVector3 pOnBox;
@@ -83,10 +80,9 @@ void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWra
resultOut->refreshContactPoints();
}
}
}
btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
(void)resultOut;
(void)dispatchInfo;
@@ -97,27 +93,26 @@ btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject*
return btScalar(1.);
}
bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance )
bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance)
{
const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape();
btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
const btBoxShape* boxShape = (const btBoxShape*)boxObjWrap->getCollisionShape();
btVector3 const& boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
btScalar boxMargin = boxShape->getMargin();
penetrationDepth = 1.0f;
// convert the sphere position to the box's local space
btTransform const &m44T = boxObjWrap->getWorldTransform();
btTransform const& m44T = boxObjWrap->getWorldTransform();
btVector3 sphereRelPos = m44T.invXform(sphereCenter);
// Determine the closest point to the sphere center in the box
btVector3 closestPoint = sphereRelPos;
closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) );
closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) );
closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) );
closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) );
closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) );
closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) );
closestPoint.setX(btMin(boxHalfExtent.getX(), closestPoint.getX()));
closestPoint.setX(btMax(-boxHalfExtent.getX(), closestPoint.getX()));
closestPoint.setY(btMin(boxHalfExtent.getY(), closestPoint.getY()));
closestPoint.setY(btMax(-boxHalfExtent.getY(), closestPoint.getY()));
closestPoint.setZ(btMin(boxHalfExtent.getZ(), closestPoint.getZ()));
closestPoint.setZ(btMax(-boxHalfExtent.getZ(), closestPoint.getZ()));
btScalar intersectionDist = fRadius + boxMargin;
btScalar contactDist = intersectionDist + maxContactDistance;
normal = sphereRelPos - closestPoint;
@@ -136,42 +131,42 @@ bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWra
{
distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal);
}
else //compute the penetration details
else //compute the penetration details
{
distance = normal.length();
normal /= distance;
}
pointOnBox = closestPoint + normal * boxMargin;
// v3PointOnSphere = sphereRelPos - (normal * fRadius);
// v3PointOnSphere = sphereRelPos - (normal * fRadius);
penetrationDepth = distance - intersectionDist;
// transform back in world space
btVector3 tmp = m44T(pointOnBox);
pointOnBox = tmp;
// tmp = m44T(v3PointOnSphere);
// v3PointOnSphere = tmp;
// tmp = m44T(v3PointOnSphere);
// v3PointOnSphere = tmp;
tmp = m44T.getBasis() * normal;
normal = tmp;
return true;
}
btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal )
btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration(btVector3 const& boxHalfExtent, btVector3 const& sphereRelPos, btVector3& closestPoint, btVector3& normal)
{
//project the center of the sphere on the closest face of the box
btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX();
btScalar minDist = faceDist;
closestPoint.setX( boxHalfExtent.getX() );
normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f));
closestPoint.setX(boxHalfExtent.getX());
normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f));
faceDist = boxHalfExtent.getX() + sphereRelPos.getX();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setX( -boxHalfExtent.getX() );
normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f));
closestPoint.setX(-boxHalfExtent.getX());
normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f));
}
faceDist = boxHalfExtent.getY() - sphereRelPos.getY();
@@ -179,8 +174,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setY( boxHalfExtent.getY() );
normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f));
closestPoint.setY(boxHalfExtent.getY());
normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f));
}
faceDist = boxHalfExtent.getY() + sphereRelPos.getY();
@@ -188,8 +183,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setY( -boxHalfExtent.getY() );
normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f));
closestPoint.setY(-boxHalfExtent.getY());
normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f));
}
faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ();
@@ -197,8 +192,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setZ( boxHalfExtent.getZ() );
normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f));
closestPoint.setZ(boxHalfExtent.getZ());
normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f));
}
faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ();
@@ -206,8 +201,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setZ( -boxHalfExtent.getZ() );
normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f));
closestPoint.setZ(-boxHalfExtent.getZ());
normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f));
}
return minDist;

38
src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
View File

@@ -28,21 +28,20 @@ class btPersistentManifold;
/// Other features are frame-coherency (persistent data) and collision response.
class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_isSwapped;
public:
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_isSwapped;
btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
public:
btSphereBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
virtual ~btSphereBoxCollisionAlgorithm();
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
@@ -50,26 +49,25 @@ public:
}
}
bool getSphereDistance( const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& v3SphereCenter, btScalar fRadius, btScalar maxContactDistance );
bool getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& v3SphereCenter, btScalar fRadius, btScalar maxContactDistance);
btScalar getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal );
struct CreateFunc :public btCollisionAlgorithmCreateFunc
btScalar getSpherePenetration(btVector3 const& boxHalfExtent, btVector3 const& sphereRelPos, btVector3& closestPoint, btVector3& normal);
struct CreateFunc : public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm));
if (!m_swapped)
{
return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false);
} else
return new (mem) btSphereBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, false);
}
else
{
return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true);
return new (mem) btSphereBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, true);
}
}
};
};
#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H
#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H

36
src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
View File

@@ -20,14 +20,14 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap)
: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap),
m_ownManifold(false),
m_manifoldPtr(mf)
btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap)
: btActivatingCollisionAlgorithm(ci, col0Wrap, col1Wrap),
m_ownManifold(false),
m_manifoldPtr(mf)
{
if (!m_manifoldPtr)
{
m_manifoldPtr = m_dispatcher->getNewManifold(col0Wrap->getCollisionObject(),col1Wrap->getCollisionObject());
m_manifoldPtr = m_dispatcher->getNewManifold(col0Wrap->getCollisionObject(), col1Wrap->getCollisionObject());
m_ownManifold = true;
}
}
@@ -41,7 +41,7 @@ btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm()
}
}
void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
void btSphereSphereCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
(void)dispatchInfo;
@@ -53,27 +53,27 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject
btSphereShape* sphere0 = (btSphereShape*)col0Wrap->getCollisionShape();
btSphereShape* sphere1 = (btSphereShape*)col1Wrap->getCollisionShape();
btVector3 diff = col0Wrap->getWorldTransform().getOrigin()- col1Wrap->getWorldTransform().getOrigin();
btVector3 diff = col0Wrap->getWorldTransform().getOrigin() - col1Wrap->getWorldTransform().getOrigin();
btScalar len = diff.length();
btScalar radius0 = sphere0->getRadius();
btScalar radius1 = sphere1->getRadius();
#ifdef CLEAR_MANIFOLD
m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting
m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting
#endif
///iff distance positive, don't generate a new contact
if ( len > (radius0+radius1+resultOut->m_closestPointDistanceThreshold))
if (len > (radius0 + radius1 + resultOut->m_closestPointDistanceThreshold))
{
#ifndef CLEAR_MANIFOLD
resultOut->refreshContactPoints();
#endif //CLEAR_MANIFOLD
#endif //CLEAR_MANIFOLD
return;
}
///distance (negative means penetration)
btScalar dist = len - (radius0+radius1);
btScalar dist = len - (radius0 + radius1);
btVector3 normalOnSurfaceB(1,0,0);
btVector3 normalOnSurfaceB(1, 0, 0);
if (len > SIMD_EPSILON)
{
normalOnSurfaceB = diff / len;
@@ -82,20 +82,18 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject
///point on A (worldspace)
///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
btVector3 pos1 = col1Wrap->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
btVector3 pos1 = col1Wrap->getWorldTransform().getOrigin() + radius1 * normalOnSurfaceB;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->addContactPoint(normalOnSurfaceB,pos1,dist);
resultOut->addContactPoint(normalOnSurfaceB, pos1, dist);
#ifndef CLEAR_MANIFOLD
resultOut->refreshContactPoints();
#endif //CLEAR_MANIFOLD
#endif //CLEAR_MANIFOLD
}
btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
(void)col0;
(void)col1;

26
src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
View File

@@ -28,39 +28,37 @@ class btPersistentManifold;
/// Also provides the most basic sample for custom/user btCollisionAlgorithm
class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
public:
btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap);
btSphereSphereCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap);
btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btActivatingCollisionAlgorithm(ci) {}
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
manifoldArray.push_back(m_manifoldPtr);
}
}
virtual ~btSphereSphereCollisionAlgorithm();
struct CreateFunc :public btCollisionAlgorithmCreateFunc
struct CreateFunc : public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm));
return new(mem) btSphereSphereCollisionAlgorithm(0,ci,col0Wrap,col1Wrap);
return new (mem) btSphereSphereCollisionAlgorithm(0, ci, col0Wrap, col1Wrap);
}
};
};
#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H
#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H

32
src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btSphereTriangleCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
@@ -21,15 +20,15 @@ subject to the following restrictions:
#include "SphereTriangleDetector.h"
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped)
: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
m_ownManifold(false),
m_manifoldPtr(mf),
m_swapped(swapped)
btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool swapped)
: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
m_ownManifold(false),
m_manifoldPtr(mf),
m_swapped(swapped)
{
if (!m_manifoldPtr)
{
m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
m_ownManifold = true;
}
}
@@ -43,36 +42,35 @@ btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm()
}
}
void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
void btSphereTriangleCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
if (!m_manifoldPtr)
return;
const btCollisionObjectWrapper* sphereObjWrap = m_swapped? col1Wrap : col0Wrap;
const btCollisionObjectWrapper* triObjWrap = m_swapped? col0Wrap : col1Wrap;
const btCollisionObjectWrapper* sphereObjWrap = m_swapped ? col1Wrap : col0Wrap;
const btCollisionObjectWrapper* triObjWrap = m_swapped ? col0Wrap : col1Wrap;
btSphereShape* sphere = (btSphereShape*)sphereObjWrap->getCollisionShape();
btTriangleShape* triangle = (btTriangleShape*)triObjWrap->getCollisionShape();
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr);
SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold);
SphereTriangleDetector detector(sphere, triangle, m_manifoldPtr->getContactBreakingThreshold() + resultOut->m_closestPointDistanceThreshold);
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds
input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT); ///@todo: tighter bounds
input.m_transformA = sphereObjWrap->getWorldTransform();
input.m_transformB = triObjWrap->getWorldTransform();
bool swapResults = m_swapped;
detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults);
detector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw, swapResults);
if (m_ownManifold)
resultOut->refreshContactPoints();
}
btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
(void)resultOut;
(void)dispatchInfo;

30
src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
View File

@@ -27,43 +27,39 @@ class btPersistentManifold;
/// Also provides the most basic sample for custom/user btCollisionAlgorithm
class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_swapped;
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_swapped;
public:
btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped);
btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool swapped);
btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btActivatingCollisionAlgorithm(ci) {}
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
manifoldArray.push_back(m_manifoldPtr);
}
}
virtual ~btSphereTriangleCollisionAlgorithm();
struct CreateFunc :public btCollisionAlgorithmCreateFunc
struct CreateFunc : public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm));
return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_swapped);
return new (mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_swapped);
}
};
};
#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H

50
src/BulletCollision/CollisionDispatch/btUnionFind.cpp
View File

@@ -15,68 +15,60 @@ subject to the following restrictions:
#include "btUnionFind.h"
btUnionFind::~btUnionFind()
{
Free();
}
btUnionFind::btUnionFind()
{
{
}
void btUnionFind::allocate(int N)
void btUnionFind::allocate(int N)
{
m_elements.resize(N);
}
void btUnionFind::Free()
void btUnionFind::Free()
{
m_elements.clear();
}
void btUnionFind::reset(int N)
void btUnionFind::reset(int N)
{
allocate(N);
for (int i = 0; i < N; i++)
{
m_elements[i].m_id = i; m_elements[i].m_sz = 1;
}
for (int i = 0; i < N; i++)
{
m_elements[i].m_id = i;
m_elements[i].m_sz = 1;
}
}
class btUnionFindElementSortPredicate
{
public:
bool operator() ( const btElement& lhs, const btElement& rhs ) const
{
return lhs.m_id < rhs.m_id;
}
public:
bool operator()(const btElement& lhs, const btElement& rhs) const
{
return lhs.m_id < rhs.m_id;
}
};
///this is a special operation, destroying the content of btUnionFind.
///it sorts the elements, based on island id, in order to make it easy to iterate over islands
void btUnionFind::sortIslands()
void btUnionFind::sortIslands()
{
//first store the original body index, and islandId
int numElements = m_elements.size();
for (int i=0;i<numElements;i++)
for (int i = 0; i < numElements; i++)
{
m_elements[i].m_id = find(i);
#ifndef STATIC_SIMULATION_ISLAND_OPTIMIZATION
m_elements[i].m_sz = i;
#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
}
// Sort the vector using predicate and std::sort
//std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate);
m_elements.quickSort(btUnionFindElementSortPredicate());
// Sort the vector using predicate and std::sort
//std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate);
m_elements.quickSort(btUnionFindElementSortPredicate());
}

156
src/BulletCollision/CollisionDispatch/btUnionFind.h
View File

@@ -23,107 +23,101 @@ subject to the following restrictions:
///see for discussion of static island optimizations by Vroonsh here: http://code.google.com/p/bullet/issues/detail?id=406
#define STATIC_SIMULATION_ISLAND_OPTIMIZATION 1
struct btElement
struct btElement
{
int m_id;
int m_sz;
int m_id;
int m_sz;
};
///UnionFind calculates connected subsets
// Implements weighted Quick Union with path compression
// optimization: could use short ints instead of ints (halving memory, would limit the number of rigid bodies to 64k, sounds reasonable)
class btUnionFind
{
private:
btAlignedObjectArray<btElement> m_elements;
{
private:
btAlignedObjectArray<btElement> m_elements;
public:
btUnionFind();
~btUnionFind();
public:
btUnionFind();
~btUnionFind();
//this is a special operation, destroying the content of btUnionFind.
//it sorts the elements, based on island id, in order to make it easy to iterate over islands
void sortIslands();
//this is a special operation, destroying the content of btUnionFind.
//it sorts the elements, based on island id, in order to make it easy to iterate over islands
void sortIslands();
void reset(int N);
void reset(int N);
SIMD_FORCE_INLINE int getNumElements() const
{
return int(m_elements.size());
}
SIMD_FORCE_INLINE bool isRoot(int x) const
{
return (x == m_elements[x].m_id);
}
SIMD_FORCE_INLINE int getNumElements() const
{
return int(m_elements.size());
}
SIMD_FORCE_INLINE bool isRoot(int x) const
{
return (x == m_elements[x].m_id);
}
btElement& getElement(int index)
{
return m_elements[index];
}
const btElement& getElement(int index) const
{
return m_elements[index];
}
void allocate(int N);
void Free();
btElement& getElement(int index)
{
return m_elements[index];
}
const btElement& getElement(int index) const
{
return m_elements[index];
}
void allocate(int N);
void Free();
int find(int p, int q)
{
return (find(p) == find(q));
}
int find(int p, int q)
{
return (find(p) == find(q));
}
void unite(int p, int q)
{
int i = find(p), j = find(q);
if (i == j)
return;
void unite(int p, int q)
{
int i = find(p), j = find(q);
if (i == j)
return;
#ifndef USE_PATH_COMPRESSION
//weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) )
if (m_elements[i].m_sz < m_elements[j].m_sz)
{
m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz;
}
else
{
m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz;
}
#else
m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz;
#endif //USE_PATH_COMPRESSION
//weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) )
if (m_elements[i].m_sz < m_elements[j].m_sz)
{
m_elements[i].m_id = j;
m_elements[j].m_sz += m_elements[i].m_sz;
}
else
{
m_elements[j].m_id = i;
m_elements[i].m_sz += m_elements[j].m_sz;
}
#else
m_elements[i].m_id = j;
m_elements[j].m_sz += m_elements[i].m_sz;
#endif //USE_PATH_COMPRESSION
}
int find(int x)
{
int find(int x)
{
//btAssert(x < m_N);
//btAssert(x >= 0);
while (x != m_elements[x].m_id)
{
//not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically
#ifdef USE_PATH_COMPRESSION
const btElement* elementPtr = &m_elements[m_elements[x].m_id];
m_elements[x].m_id = elementPtr->m_id;
x = elementPtr->m_id;
#else //
x = m_elements[x].m_id;
#endif
//btAssert(x < m_N);
//btAssert(x >= 0);
while (x != m_elements[x].m_id)
{
//not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically
#ifdef USE_PATH_COMPRESSION
const btElement* elementPtr = &m_elements[m_elements[x].m_id];
m_elements[x].m_id = elementPtr->m_id;
x = elementPtr->m_id;
#else//
x = m_elements[x].m_id;
#endif
//btAssert(x < m_N);
//btAssert(x >= 0);
}
return x;
}
return x;
}
};
};
#endif //BT_UNION_FIND_H
#endif //BT_UNION_FIND_H

25
src/BulletCollision/CollisionShapes/btBox2dShape.cpp
View File

@@ -15,28 +15,23 @@ subject to the following restrictions:
#include "btBox2dShape.h"
//{
//{
void btBox2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
void btBox2dShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
{
btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax);
}
void btBox2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
void btBox2dShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
{
//btScalar margin = btScalar(0.);
btVector3 halfExtents = getHalfExtentsWithMargin();
btScalar lx=btScalar(2.)*(halfExtents.x());
btScalar ly=btScalar(2.)*(halfExtents.y());
btScalar lz=btScalar(2.)*(halfExtents.z());
inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
mass/(btScalar(12.0)) * (lx*lx + lz*lz),
mass/(btScalar(12.0)) * (lx*lx + ly*ly));
btScalar lx = btScalar(2.) * (halfExtents.x());
btScalar ly = btScalar(2.) * (halfExtents.y());
btScalar lz = btScalar(2.) * (halfExtents.z());
inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
mass / (btScalar(12.0)) * (lx * lx + lz * lz),
mass / (btScalar(12.0)) * (lx * lx + ly * ly));
}

347
src/BulletCollision/CollisionShapes/btBox2dShape.h
View File

@@ -23,9 +23,9 @@ subject to the following restrictions:
#include "LinearMath/btMinMax.h"
///The btBox2dShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space.
ATTRIBUTE_ALIGNED16(class) btBox2dShape: public btPolyhedralConvexShape
ATTRIBUTE_ALIGNED16(class)
btBox2dShape : public btPolyhedralConvexShape
{
//btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead
btVector3 m_centroid;
@@ -33,79 +33,75 @@ ATTRIBUTE_ALIGNED16(class) btBox2dShape: public btPolyhedralConvexShape
btVector3 m_normals[4];
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 getHalfExtentsWithMargin() const
{
btVector3 halfExtents = getHalfExtentsWithoutMargin();
btVector3 margin(getMargin(),getMargin(),getMargin());
btVector3 margin(getMargin(), getMargin(), getMargin());
halfExtents += margin;
return halfExtents;
}
const btVector3& getHalfExtentsWithoutMargin() const
{
return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included
return m_implicitShapeDimensions; //changed in Bullet 2.63: assume the scaling and margin are included
}
virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
{
btVector3 halfExtents = getHalfExtentsWithoutMargin();
btVector3 margin(getMargin(),getMargin(),getMargin());
btVector3 margin(getMargin(), getMargin(), getMargin());
halfExtents += margin;
return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
}
SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const
{
const btVector3& halfExtents = getHalfExtentsWithoutMargin();
return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
}
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
{
const btVector3& halfExtents = getHalfExtentsWithoutMargin();
for (int i=0;i<numVectors;i++)
for (int i = 0; i < numVectors; i++)
{
const btVector3& vec = vectors[i];
supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
}
}
///a btBox2dShape is a flat 2D box in the X-Y plane (Z extents are zero)
btBox2dShape( const btVector3& boxHalfExtents)
btBox2dShape(const btVector3& boxHalfExtents)
: btPolyhedralConvexShape(),
m_centroid(0,0,0)
m_centroid(0, 0, 0)
{
m_vertices[0].setValue(-boxHalfExtents.getX(),-boxHalfExtents.getY(),0);
m_vertices[1].setValue(boxHalfExtents.getX(),-boxHalfExtents.getY(),0);
m_vertices[2].setValue(boxHalfExtents.getX(),boxHalfExtents.getY(),0);
m_vertices[3].setValue(-boxHalfExtents.getX(),boxHalfExtents.getY(),0);
m_vertices[0].setValue(-boxHalfExtents.getX(), -boxHalfExtents.getY(), 0);
m_vertices[1].setValue(boxHalfExtents.getX(), -boxHalfExtents.getY(), 0);
m_vertices[2].setValue(boxHalfExtents.getX(), boxHalfExtents.getY(), 0);
m_vertices[3].setValue(-boxHalfExtents.getX(), boxHalfExtents.getY(), 0);
m_normals[0].setValue(0,-1,0);
m_normals[1].setValue(1,0,0);
m_normals[2].setValue(0,1,0);
m_normals[3].setValue(-1,0,0);
m_normals[0].setValue(0, -1, 0);
m_normals[1].setValue(1, 0, 0);
m_normals[2].setValue(0, 1, 0);
m_normals[3].setValue(-1, 0, 0);
btScalar minDimension = boxHalfExtents.getX();
if (minDimension>boxHalfExtents.getY())
if (minDimension > boxHalfExtents.getY())
minDimension = boxHalfExtents.getY();
m_shapeType = BOX_2D_SHAPE_PROXYTYPE;
btVector3 margin(getMargin(),getMargin(),getMargin());
btVector3 margin(getMargin(), getMargin(), getMargin());
m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
setSafeMargin(minDimension);
@@ -114,42 +110,34 @@ public:
virtual void setMargin(btScalar collisionMargin)
{
//correct the m_implicitShapeDimensions for the margin
btVector3 oldMargin(getMargin(),getMargin(),getMargin());
btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
btConvexInternalShape::setMargin(collisionMargin);
btVector3 newMargin(getMargin(),getMargin(),getMargin());
m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
btVector3 oldMargin(getMargin(), getMargin(), getMargin());
btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin;
btConvexInternalShape::setMargin(collisionMargin);
btVector3 newMargin(getMargin(), getMargin(), getMargin());
m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
}
virtual void setLocalScaling(const btVector3& scaling)
virtual void setLocalScaling(const btVector3& scaling)
{
btVector3 oldMargin(getMargin(),getMargin(),getMargin());
btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
btVector3 oldMargin(getMargin(), getMargin(), getMargin());
btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin;
btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
btConvexInternalShape::setLocalScaling(scaling);
m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
}
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
int getVertexCount() const
int getVertexCount() const
{
return 4;
}
virtual int getNumVertices()const
virtual int getNumVertices() const
{
return 4;
}
@@ -164,82 +152,70 @@ public:
return &m_normals[0];
}
virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const
virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const
{
//this plane might not be aligned...
btVector4 plane ;
getPlaneEquation(plane,i);
planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ());
btVector4 plane;
getPlaneEquation(plane, i);
planeNormal = btVector3(plane.getX(), plane.getY(), plane.getZ());
planeSupport = localGetSupportingVertex(-planeNormal);
}
const btVector3& getCentroid() const
{
return m_centroid;
}
virtual int getNumPlanes() const
{
return 6;
}
}
virtual int getNumEdges() const
{
return 12;
}
virtual void getVertex(int i,btVector3& vtx) const
virtual void getVertex(int i, btVector3& vtx) const
{
btVector3 halfExtents = getHalfExtentsWithoutMargin();
vtx = btVector3(
halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1),
halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1),
halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2));
halfExtents.x() * (1 - (i & 1)) - halfExtents.x() * (i & 1),
halfExtents.y() * (1 - ((i & 2) >> 1)) - halfExtents.y() * ((i & 2) >> 1),
halfExtents.z() * (1 - ((i & 4) >> 2)) - halfExtents.z() * ((i & 4) >> 2));
}
virtual void getPlaneEquation(btVector4& plane,int i) const
virtual void getPlaneEquation(btVector4 & plane, int i) const
{
btVector3 halfExtents = getHalfExtentsWithoutMargin();
switch (i)
{
case 0:
plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x());
break;
case 1:
plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
break;
case 2:
plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
break;
case 3:
plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
break;
case 4:
plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
break;
case 5:
plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
break;
default:
btAssert(0);
case 0:
plane.setValue(btScalar(1.), btScalar(0.), btScalar(0.), -halfExtents.x());
break;
case 1:
plane.setValue(btScalar(-1.), btScalar(0.), btScalar(0.), -halfExtents.x());
break;
case 2:
plane.setValue(btScalar(0.), btScalar(1.), btScalar(0.), -halfExtents.y());
break;
case 3:
plane.setValue(btScalar(0.), btScalar(-1.), btScalar(0.), -halfExtents.y());
break;
case 4:
plane.setValue(btScalar(0.), btScalar(0.), btScalar(1.), -halfExtents.z());
break;
case 5:
plane.setValue(btScalar(0.), btScalar(0.), btScalar(-1.), -halfExtents.z());
break;
default:
btAssert(0);
}
}
virtual void getEdge(int i,btVector3& pa,btVector3& pb) const
virtual void getEdge(int i, btVector3& pa, btVector3& pb) const
//virtual void getEdge(int i,Edge& edge) const
{
int edgeVert0 = 0;
@@ -247,126 +223,117 @@ public:
switch (i)
{
case 0:
case 0:
edgeVert0 = 0;
edgeVert1 = 1;
break;
case 1:
break;
case 1:
edgeVert0 = 0;
edgeVert1 = 2;
break;
case 2:
edgeVert0 = 1;
edgeVert1 = 3;
break;
case 2:
edgeVert0 = 1;
edgeVert1 = 3;
break;
case 3:
edgeVert0 = 2;
edgeVert1 = 3;
break;
case 4:
edgeVert0 = 0;
edgeVert1 = 4;
break;
case 5:
edgeVert0 = 1;
edgeVert1 = 5;
break;
case 6:
edgeVert0 = 2;
edgeVert1 = 6;
break;
case 7:
edgeVert0 = 3;
edgeVert1 = 7;
break;
case 8:
edgeVert0 = 4;
edgeVert1 = 5;
break;
case 9:
edgeVert0 = 4;
edgeVert1 = 6;
break;
case 10:
edgeVert0 = 5;
edgeVert1 = 7;
break;
case 11:
edgeVert0 = 6;
edgeVert1 = 7;
break;
default:
btAssert(0);
break;
case 3:
edgeVert0 = 2;
edgeVert1 = 3;
break;
case 4:
edgeVert0 = 0;
edgeVert1 = 4;
break;
case 5:
edgeVert0 = 1;
edgeVert1 = 5;
break;
case 6:
edgeVert0 = 2;
edgeVert1 = 6;
break;
case 7:
edgeVert0 = 3;
edgeVert1 = 7;
break;
case 8:
edgeVert0 = 4;
edgeVert1 = 5;
break;
case 9:
edgeVert0 = 4;
edgeVert1 = 6;
break;
case 10:
edgeVert0 = 5;
edgeVert1 = 7;
break;
case 11:
edgeVert0 = 6;
edgeVert1 = 7;
break;
default:
btAssert(0);
}
getVertex(edgeVert0,pa );
getVertex(edgeVert1,pb );
getVertex(edgeVert0, pa);
getVertex(edgeVert1, pb);
}
virtual bool isInside(const btVector3& pt,btScalar tolerance) const
virtual bool isInside(const btVector3& pt, btScalar tolerance) const
{
btVector3 halfExtents = getHalfExtentsWithoutMargin();
//btScalar minDist = 2*tolerance;
bool result = (pt.x() <= (halfExtents.x()+tolerance)) &&
(pt.x() >= (-halfExtents.x()-tolerance)) &&
(pt.y() <= (halfExtents.y()+tolerance)) &&
(pt.y() >= (-halfExtents.y()-tolerance)) &&
(pt.z() <= (halfExtents.z()+tolerance)) &&
(pt.z() >= (-halfExtents.z()-tolerance));
bool result = (pt.x() <= (halfExtents.x() + tolerance)) &&
(pt.x() >= (-halfExtents.x() - tolerance)) &&
(pt.y() <= (halfExtents.y() + tolerance)) &&
(pt.y() >= (-halfExtents.y() - tolerance)) &&
(pt.z() <= (halfExtents.z() + tolerance)) &&
(pt.z() >= (-halfExtents.z() - tolerance));
return result;
}
//debugging
virtual const char* getName()const
virtual const char* getName() const
{
return "Box2d";
}
virtual int getNumPreferredPenetrationDirections() const
virtual int getNumPreferredPenetrationDirections() const
{
return 6;
}
virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
{
switch (index)
{
case 0:
penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
break;
case 1:
penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
break;
case 2:
penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
break;
case 3:
penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
break;
case 4:
penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
break;
case 5:
penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
break;
default:
btAssert(0);
case 0:
penetrationVector.setValue(btScalar(1.), btScalar(0.), btScalar(0.));
break;
case 1:
penetrationVector.setValue(btScalar(-1.), btScalar(0.), btScalar(0.));
break;
case 2:
penetrationVector.setValue(btScalar(0.), btScalar(1.), btScalar(0.));
break;
case 3:
penetrationVector.setValue(btScalar(0.), btScalar(-1.), btScalar(0.));
break;
case 4:
penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(1.));
break;
case 5:
penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(-1.));
break;
default:
btAssert(0);
}
}
};
#endif //BT_OBB_BOX_2D_SHAPE_H
#endif //BT_OBB_BOX_2D_SHAPE_H

30
src/BulletCollision/CollisionShapes/btBoxShape.cpp
View File

@@ -14,38 +14,32 @@ subject to the following restrictions:
*/
#include "btBoxShape.h"
btBoxShape::btBoxShape( const btVector3& boxHalfExtents)
: btPolyhedralConvexShape()
btBoxShape::btBoxShape(const btVector3& boxHalfExtents)
: btPolyhedralConvexShape()
{
m_shapeType = BOX_SHAPE_PROXYTYPE;
btVector3 margin(getMargin(),getMargin(),getMargin());
btVector3 margin(getMargin(), getMargin(), getMargin());
m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
setSafeMargin(boxHalfExtents);
};
void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
void btBoxShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
{
btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax);
}
void btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
void btBoxShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
{
//btScalar margin = btScalar(0.);
btVector3 halfExtents = getHalfExtentsWithMargin();
btScalar lx=btScalar(2.)*(halfExtents.x());
btScalar ly=btScalar(2.)*(halfExtents.y());
btScalar lz=btScalar(2.)*(halfExtents.z());
inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
mass/(btScalar(12.0)) * (lx*lx + lz*lz),
mass/(btScalar(12.0)) * (lx*lx + ly*ly));
btScalar lx = btScalar(2.) * (halfExtents.x());
btScalar ly = btScalar(2.) * (halfExtents.y());
btScalar lz = btScalar(2.) * (halfExtents.z());
inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
mass / (btScalar(12.0)) * (lx * lx + lz * lz),
mass / (btScalar(12.0)) * (lx * lx + ly * ly));
}

315
src/BulletCollision/CollisionShapes/btBoxShape.h
View File

@@ -23,112 +23,102 @@ subject to the following restrictions:
#include "LinearMath/btMinMax.h"
///The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space.
ATTRIBUTE_ALIGNED16(class) btBoxShape: public btPolyhedralConvexShape
ATTRIBUTE_ALIGNED16(class)
btBoxShape : public btPolyhedralConvexShape
{
//btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 getHalfExtentsWithMargin() const
{
btVector3 halfExtents = getHalfExtentsWithoutMargin();
btVector3 margin(getMargin(),getMargin(),getMargin());
btVector3 margin(getMargin(), getMargin(), getMargin());
halfExtents += margin;
return halfExtents;
}
const btVector3& getHalfExtentsWithoutMargin() const
{
return m_implicitShapeDimensions;//scaling is included, margin is not
return m_implicitShapeDimensions; //scaling is included, margin is not
}
virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
{
btVector3 halfExtents = getHalfExtentsWithoutMargin();
btVector3 margin(getMargin(),getMargin(),getMargin());
btVector3 margin(getMargin(), getMargin(), getMargin());
halfExtents += margin;
return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
}
SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const
{
const btVector3& halfExtents = getHalfExtentsWithoutMargin();
return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
}
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
{
const btVector3& halfExtents = getHalfExtentsWithoutMargin();
for (int i=0;i<numVectors;i++)
for (int i = 0; i < numVectors; i++)
{
const btVector3& vec = vectors[i];
supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
}
}
btBoxShape( const btVector3& boxHalfExtents);
btBoxShape(const btVector3& boxHalfExtents);
virtual void setMargin(btScalar collisionMargin)
{
//correct the m_implicitShapeDimensions for the margin
btVector3 oldMargin(getMargin(),getMargin(),getMargin());
btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
btConvexInternalShape::setMargin(collisionMargin);
btVector3 newMargin(getMargin(),getMargin(),getMargin());
m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
btVector3 oldMargin(getMargin(), getMargin(), getMargin());
btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin;
btConvexInternalShape::setMargin(collisionMargin);
btVector3 newMargin(getMargin(), getMargin(), getMargin());
m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
}
virtual void setLocalScaling(const btVector3& scaling)
virtual void setLocalScaling(const btVector3& scaling)
{
btVector3 oldMargin(getMargin(),getMargin(),getMargin());
btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
btVector3 oldMargin(getMargin(), getMargin(), getMargin());
btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin;
btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
btConvexInternalShape::setLocalScaling(scaling);
m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
}
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const
virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const
{
//this plane might not be aligned...
btVector4 plane ;
getPlaneEquation(plane,i);
planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ());
btVector4 plane;
getPlaneEquation(plane, i);
planeNormal = btVector3(plane.getX(), plane.getY(), plane.getZ());
planeSupport = localGetSupportingVertex(-planeNormal);
}
virtual int getNumPlanes() const
{
return 6;
}
virtual int getNumVertices() const
}
virtual int getNumVertices() const
{
return 8;
}
@@ -138,49 +128,46 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
return 12;
}
virtual void getVertex(int i,btVector3& vtx) const
virtual void getVertex(int i, btVector3& vtx) const
{
btVector3 halfExtents = getHalfExtentsWithMargin();
vtx = btVector3(
halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1),
halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1),
halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2));
halfExtents.x() * (1 - (i & 1)) - halfExtents.x() * (i & 1),
halfExtents.y() * (1 - ((i & 2) >> 1)) - halfExtents.y() * ((i & 2) >> 1),
halfExtents.z() * (1 - ((i & 4) >> 2)) - halfExtents.z() * ((i & 4) >> 2));
}
virtual void getPlaneEquation(btVector4& plane,int i) const
virtual void getPlaneEquation(btVector4 & plane, int i) const
{
btVector3 halfExtents = getHalfExtentsWithoutMargin();
switch (i)
{
case 0:
plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x());
break;
case 1:
plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
break;
case 2:
plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
break;
case 3:
plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
break;
case 4:
plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
break;
case 5:
plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
break;
default:
btAssert(0);
case 0:
plane.setValue(btScalar(1.), btScalar(0.), btScalar(0.), -halfExtents.x());
break;
case 1:
plane.setValue(btScalar(-1.), btScalar(0.), btScalar(0.), -halfExtents.x());
break;
case 2:
plane.setValue(btScalar(0.), btScalar(1.), btScalar(0.), -halfExtents.y());
break;
case 3:
plane.setValue(btScalar(0.), btScalar(-1.), btScalar(0.), -halfExtents.y());
break;
case 4:
plane.setValue(btScalar(0.), btScalar(0.), btScalar(1.), -halfExtents.z());
break;
case 5:
plane.setValue(btScalar(0.), btScalar(0.), btScalar(-1.), -halfExtents.z());
break;
default:
btAssert(0);
}
}
virtual void getEdge(int i,btVector3& pa,btVector3& pb) const
virtual void getEdge(int i, btVector3& pa, btVector3& pb) const
//virtual void getEdge(int i,Edge& edge) const
{
int edgeVert0 = 0;
@@ -188,127 +175,117 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
switch (i)
{
case 0:
case 0:
edgeVert0 = 0;
edgeVert1 = 1;
break;
case 1:
break;
case 1:
edgeVert0 = 0;
edgeVert1 = 2;
break;
case 2:
edgeVert0 = 1;
edgeVert1 = 3;
break;
case 2:
edgeVert0 = 1;
edgeVert1 = 3;
break;
case 3:
edgeVert0 = 2;
edgeVert1 = 3;
break;
case 4:
edgeVert0 = 0;
edgeVert1 = 4;
break;
case 5:
edgeVert0 = 1;
edgeVert1 = 5;
break;
case 6:
edgeVert0 = 2;
edgeVert1 = 6;
break;
case 7:
edgeVert0 = 3;
edgeVert1 = 7;
break;
case 8:
edgeVert0 = 4;
edgeVert1 = 5;
break;
case 9:
edgeVert0 = 4;
edgeVert1 = 6;
break;
case 10:
edgeVert0 = 5;
edgeVert1 = 7;
break;
case 11:
edgeVert0 = 6;
edgeVert1 = 7;
break;
default:
btAssert(0);
break;
case 3:
edgeVert0 = 2;
edgeVert1 = 3;
break;
case 4:
edgeVert0 = 0;
edgeVert1 = 4;
break;
case 5:
edgeVert0 = 1;
edgeVert1 = 5;
break;
case 6:
edgeVert0 = 2;
edgeVert1 = 6;
break;
case 7:
edgeVert0 = 3;
edgeVert1 = 7;
break;
case 8:
edgeVert0 = 4;
edgeVert1 = 5;
break;
case 9:
edgeVert0 = 4;
edgeVert1 = 6;
break;
case 10:
edgeVert0 = 5;
edgeVert1 = 7;
break;
case 11:
edgeVert0 = 6;
edgeVert1 = 7;
break;
default:
btAssert(0);
}
getVertex(edgeVert0,pa );
getVertex(edgeVert1,pb );
getVertex(edgeVert0, pa);
getVertex(edgeVert1, pb);
}
virtual bool isInside(const btVector3& pt,btScalar tolerance) const
virtual bool isInside(const btVector3& pt, btScalar tolerance) const
{
btVector3 halfExtents = getHalfExtentsWithoutMargin();
//btScalar minDist = 2*tolerance;
bool result = (pt.x() <= (halfExtents.x()+tolerance)) &&
(pt.x() >= (-halfExtents.x()-tolerance)) &&
(pt.y() <= (halfExtents.y()+tolerance)) &&
(pt.y() >= (-halfExtents.y()-tolerance)) &&
(pt.z() <= (halfExtents.z()+tolerance)) &&
(pt.z() >= (-halfExtents.z()-tolerance));
bool result = (pt.x() <= (halfExtents.x() + tolerance)) &&
(pt.x() >= (-halfExtents.x() - tolerance)) &&
(pt.y() <= (halfExtents.y() + tolerance)) &&
(pt.y() >= (-halfExtents.y() - tolerance)) &&
(pt.z() <= (halfExtents.z() + tolerance)) &&
(pt.z() >= (-halfExtents.z() - tolerance));
return result;
}
//debugging
virtual const char* getName()const
virtual const char* getName() const
{
return "Box";
}
virtual int getNumPreferredPenetrationDirections() const
virtual int getNumPreferredPenetrationDirections() const
{
return 6;
}
virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
{
switch (index)
{
case 0:
penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
break;
case 1:
penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
break;
case 2:
penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
break;
case 3:
penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
break;
case 4:
penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
break;
case 5:
penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
break;
default:
btAssert(0);
case 0:
penetrationVector.setValue(btScalar(1.), btScalar(0.), btScalar(0.));
break;
case 1:
penetrationVector.setValue(btScalar(-1.), btScalar(0.), btScalar(0.));
break;
case 2:
penetrationVector.setValue(btScalar(0.), btScalar(1.), btScalar(0.));
break;
case 3:
penetrationVector.setValue(btScalar(0.), btScalar(-1.), btScalar(0.));
break;
case 4:
penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(1.));
break;
case 5:
penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(-1.));
break;
default:
btAssert(0);
}
}
};
#endif //BT_OBB_BOX_MINKOWSKI_H
#endif //BT_OBB_BOX_MINKOWSKI_H

328
src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
View File

@@ -22,11 +22,11 @@ subject to the following restrictions:
///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
///Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh)
:btTriangleMeshShape(meshInterface),
m_bvh(0),
m_triangleInfoMap(0),
m_useQuantizedAabbCompression(useQuantizedAabbCompression),
m_ownsBvh(false)
: btTriangleMeshShape(meshInterface),
m_bvh(0),
m_triangleInfoMap(0),
m_useQuantizedAabbCompression(useQuantizedAabbCompression),
m_ownsBvh(false)
{
m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
//construct bvh from meshInterface
@@ -37,16 +37,15 @@ m_ownsBvh(false)
buildOptimizedBvh();
}
#endif //DISABLE_BVH
#endif //DISABLE_BVH
}
btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,bool buildBvh)
:btTriangleMeshShape(meshInterface),
m_bvh(0),
m_triangleInfoMap(0),
m_useQuantizedAabbCompression(useQuantizedAabbCompression),
m_ownsBvh(false)
btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, bool buildBvh)
: btTriangleMeshShape(meshInterface),
m_bvh(0),
m_triangleInfoMap(0),
m_useQuantizedAabbCompression(useQuantizedAabbCompression),
m_ownsBvh(false)
{
m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
//construct bvh from meshInterface
@@ -54,30 +53,28 @@ m_ownsBvh(false)
if (buildBvh)
{
void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16);
void* mem = btAlignedAlloc(sizeof(btOptimizedBvh), 16);
m_bvh = new (mem) btOptimizedBvh();
m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax);
m_bvh->build(meshInterface, m_useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax);
m_ownsBvh = true;
}
#endif //DISABLE_BVH
#endif //DISABLE_BVH
}
void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax)
void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin, const btVector3& aabbMax)
{
m_bvh->refitPartial( m_meshInterface,aabbMin,aabbMax );
m_bvh->refitPartial(m_meshInterface, aabbMin, aabbMax);
m_localAabbMin.setMin(aabbMin);
m_localAabbMax.setMax(aabbMax);
}
void btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin,const btVector3& aabbMax)
void btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin, const btVector3& aabbMax)
{
m_bvh->refit( m_meshInterface, aabbMin,aabbMax );
m_bvh->refit(m_meshInterface, aabbMin, aabbMax);
recalcLocalAabb();
}
@@ -90,27 +87,27 @@ btBvhTriangleMeshShape::~btBvhTriangleMeshShape()
}
}
void btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget)
void btBvhTriangleMeshShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget)
{
struct MyNodeOverlapCallback : public btNodeOverlapCallback
struct MyNodeOverlapCallback : public btNodeOverlapCallback
{
btStridingMeshInterface* m_meshInterface;
btStridingMeshInterface* m_meshInterface;
btTriangleCallback* m_callback;
MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
:m_meshInterface(meshInterface),
m_callback(callback)
MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface)
: m_meshInterface(meshInterface),
m_callback(callback)
{
}
virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
{
btVector3 m_triangle[3];
const unsigned char *vertexbase;
const unsigned char* vertexbase;
int numverts;
PHY_ScalarType type;
int stride;
const unsigned char *indexbase;
const unsigned char* indexbase;
int indexstride;
int numfaces;
PHY_ScalarType indicestype;
@@ -126,60 +123,60 @@ void btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const
indicestype,
nodeSubPart);
unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT);
const btVector3& meshScaling = m_meshInterface->getScaling();
for (int j=2;j>=0;j--)
for (int j = 2; j >= 0; j--)
{
int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
if (type == PHY_FLOAT)
{
float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
m_triangle[j] = btVector3(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
}
else
{
double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ());
double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
m_triangle[j] = btVector3(btScalar(graphicsbase[0]) * meshScaling.getX(), btScalar(graphicsbase[1]) * meshScaling.getY(), btScalar(graphicsbase[2]) * meshScaling.getZ());
}
}
/* Perform ray vs. triangle collision here */
m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex);
m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
}
};
MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface);
MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface);
m_bvh->reportRayOverlappingNodex(&myNodeCallback,raySource,rayTarget);
m_bvh->reportRayOverlappingNodex(&myNodeCallback, raySource, rayTarget);
}
void btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax)
void btBvhTriangleMeshShape::performConvexcast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax)
{
struct MyNodeOverlapCallback : public btNodeOverlapCallback
struct MyNodeOverlapCallback : public btNodeOverlapCallback
{
btStridingMeshInterface* m_meshInterface;
btStridingMeshInterface* m_meshInterface;
btTriangleCallback* m_callback;
MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
:m_meshInterface(meshInterface),
m_callback(callback)
MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface)
: m_meshInterface(meshInterface),
m_callback(callback)
{
}
virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
{
btVector3 m_triangle[3];
const unsigned char *vertexbase;
const unsigned char* vertexbase;
int numverts;
PHY_ScalarType type;
int stride;
const unsigned char *indexbase;
const unsigned char* indexbase;
int indexstride;
int numfaces;
PHY_ScalarType indicestype;
@@ -195,77 +192,74 @@ void btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, co
indicestype,
nodeSubPart);
unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT);
const btVector3& meshScaling = m_meshInterface->getScaling();
for (int j=2;j>=0;j--)
for (int j = 2; j >= 0; j--)
{
int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
if (type == PHY_FLOAT)
{
float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
m_triangle[j] = btVector3(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
}
else
{
double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ());
double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
m_triangle[j] = btVector3(btScalar(graphicsbase[0]) * meshScaling.getX(), btScalar(graphicsbase[1]) * meshScaling.getY(), btScalar(graphicsbase[2]) * meshScaling.getZ());
}
}
/* Perform ray vs. triangle collision here */
m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex);
m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
}
};
MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface);
MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface);
m_bvh->reportBoxCastOverlappingNodex (&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax);
m_bvh->reportBoxCastOverlappingNodex(&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax);
}
//perform bvh tree traversal and report overlapping triangles to 'callback'
void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
{
#ifdef DISABLE_BVH
//brute force traverse all triangles
btTriangleMeshShape::processAllTriangles(callback,aabbMin,aabbMax);
btTriangleMeshShape::processAllTriangles(callback, aabbMin, aabbMax);
#else
//first get all the nodes
struct MyNodeOverlapCallback : public btNodeOverlapCallback
struct MyNodeOverlapCallback : public btNodeOverlapCallback
{
btStridingMeshInterface* m_meshInterface;
btTriangleCallback* m_callback;
btVector3 m_triangle[3];
btStridingMeshInterface* m_meshInterface;
btTriangleCallback* m_callback;
btVector3 m_triangle[3];
int m_numOverlap;
MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
:m_meshInterface(meshInterface),
m_callback(callback),
m_numOverlap(0)
MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface)
: m_meshInterface(meshInterface),
m_callback(callback),
m_numOverlap(0)
{
}
virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
{
m_numOverlap++;
const unsigned char *vertexbase;
const unsigned char* vertexbase;
int numverts;
PHY_ScalarType type;
int stride;
const unsigned char *indexbase;
const unsigned char* indexbase;
int indexstride;
int numfaces;
PHY_ScalarType indicestype;
m_meshInterface->getLockedReadOnlyVertexIndexBase(
&vertexbase,
@@ -278,67 +272,62 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
indicestype,
nodeSubPart);
unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT||indicestype==PHY_UCHAR);
const btVector3& meshScaling = m_meshInterface->getScaling();
for (int j=2;j>=0;j--)
{
int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:indicestype==PHY_INTEGER?gfxbase[j]:((unsigned char*)gfxbase)[j];
unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT || indicestype == PHY_UCHAR);
const btVector3& meshScaling = m_meshInterface->getScaling();
for (int j = 2; j >= 0; j--)
{
int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : indicestype == PHY_INTEGER ? gfxbase[j] : ((unsigned char*)gfxbase)[j];
#ifdef DEBUG_TRIANGLE_MESH
printf("%d ,",graphicsindex);
#endif //DEBUG_TRIANGLE_MESH
printf("%d ,", graphicsindex);
#endif //DEBUG_TRIANGLE_MESH
if (type == PHY_FLOAT)
{
float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
m_triangle[j] = btVector3(
graphicsbase[0]*meshScaling.getX(),
graphicsbase[1]*meshScaling.getY(),
graphicsbase[2]*meshScaling.getZ());
graphicsbase[0] * meshScaling.getX(),
graphicsbase[1] * meshScaling.getY(),
graphicsbase[2] * meshScaling.getZ());
}
else
{
double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
m_triangle[j] = btVector3(
btScalar(graphicsbase[0])*meshScaling.getX(),
btScalar(graphicsbase[1])*meshScaling.getY(),
btScalar(graphicsbase[2])*meshScaling.getZ());
btScalar(graphicsbase[0]) * meshScaling.getX(),
btScalar(graphicsbase[1]) * meshScaling.getY(),
btScalar(graphicsbase[2]) * meshScaling.getZ());
}
#ifdef DEBUG_TRIANGLE_MESH
printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z());
#endif //DEBUG_TRIANGLE_MESH
printf("triangle vertices:%f,%f,%f\n", triangle[j].x(), triangle[j].y(), triangle[j].z());
#endif //DEBUG_TRIANGLE_MESH
}
m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex);
m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
}
};
MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface);
m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
#endif//DISABLE_BVH
MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface);
m_bvh->reportAabbOverlappingNodex(&myNodeCallback, aabbMin, aabbMax);
#endif //DISABLE_BVH
}
void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
{
if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON)
{
btTriangleMeshShape::setLocalScaling(scaling);
buildOptimizedBvh();
}
if ((getLocalScaling() - scaling).length2() > SIMD_EPSILON)
{
btTriangleMeshShape::setLocalScaling(scaling);
buildOptimizedBvh();
}
}
void btBvhTriangleMeshShape::buildOptimizedBvh()
void btBvhTriangleMeshShape::buildOptimizedBvh()
{
if (m_ownsBvh)
{
@@ -346,43 +335,39 @@ void btBvhTriangleMeshShape::buildOptimizedBvh()
btAlignedFree(m_bvh);
}
///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work
void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16);
m_bvh = new(mem) btOptimizedBvh();
void* mem = btAlignedAlloc(sizeof(btOptimizedBvh), 16);
m_bvh = new (mem) btOptimizedBvh();
//rebuild the bvh...
m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression,m_localAabbMin,m_localAabbMax);
m_bvh->build(m_meshInterface, m_useQuantizedAabbCompression, m_localAabbMin, m_localAabbMax);
m_ownsBvh = true;
}
void btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling)
void btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling)
{
btAssert(!m_bvh);
btAssert(!m_ownsBvh);
btAssert(!m_bvh);
btAssert(!m_ownsBvh);
m_bvh = bvh;
m_ownsBvh = false;
// update the scaling without rebuilding the bvh
if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON)
{
btTriangleMeshShape::setLocalScaling(scaling);
}
m_bvh = bvh;
m_ownsBvh = false;
// update the scaling without rebuilding the bvh
if ((getLocalScaling() - scaling).length2() > SIMD_EPSILON)
{
btTriangleMeshShape::setLocalScaling(scaling);
}
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const
const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const
{
btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*) dataBuffer;
btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*)dataBuffer;
btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer);
btCollisionShape::serialize(&trimeshData->m_collisionShapeData, serializer);
m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer);
trimeshData->m_collisionMargin = float(m_collisionMargin);
if (m_bvh && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_BVH))
if (m_bvh && !(serializer->getSerializationFlags() & BT_SERIALIZE_NO_BVH))
{
void* chunk = serializer->findPointer(m_bvh);
if (chunk)
@@ -391,48 +376,49 @@ const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* se
trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)chunk;
trimeshData->m_quantizedFloatBvh = 0;
#else
trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)chunk;
trimeshData->m_quantizedDoubleBvh= 0;
#endif //BT_USE_DOUBLE_PRECISION
} else
trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)chunk;
trimeshData->m_quantizedDoubleBvh = 0;
#endif //BT_USE_DOUBLE_PRECISION
}
else
{
#ifdef BT_USE_DOUBLE_PRECISION
trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh);
trimeshData->m_quantizedFloatBvh = 0;
#else
trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh);
trimeshData->m_quantizedDoubleBvh= 0;
#endif //BT_USE_DOUBLE_PRECISION
trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh);
trimeshData->m_quantizedDoubleBvh = 0;
#endif //BT_USE_DOUBLE_PRECISION
int sz = m_bvh->calculateSerializeBufferSizeNew();
btChunk* chunk = serializer->allocate(sz,1);
btChunk* chunk = serializer->allocate(sz, 1);
const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,m_bvh);
serializer->finalizeChunk(chunk, structType, BT_QUANTIZED_BVH_CODE, m_bvh);
}
} else
}
else
{
trimeshData->m_quantizedFloatBvh = 0;
trimeshData->m_quantizedDoubleBvh = 0;
}
if (m_triangleInfoMap && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_TRIANGLEINFOMAP))
if (m_triangleInfoMap && !(serializer->getSerializationFlags() & BT_SERIALIZE_NO_TRIANGLEINFOMAP))
{
void* chunk = serializer->findPointer(m_triangleInfoMap);
if (chunk)
{
trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)chunk;
} else
}
else
{
trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)serializer->getUniquePointer(m_triangleInfoMap);
int sz = m_triangleInfoMap->calculateSerializeBufferSize();
btChunk* chunk = serializer->allocate(sz,1);
btChunk* chunk = serializer->allocate(sz, 1);
const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,m_triangleInfoMap);
serializer->finalizeChunk(chunk, structType, BT_TRIANLGE_INFO_MAP, m_triangleInfoMap);
}
} else
}
else
{
trimeshData->m_triangleInfoMap = 0;
}
@@ -443,28 +429,24 @@ const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* se
return "btTriangleMeshShapeData";
}
void btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const
void btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const
{
if (m_bvh)
{
int len = m_bvh->calculateSerializeBufferSizeNew(); //make sure not to use calculateSerializeBufferSize because it is used for in-place
btChunk* chunk = serializer->allocate(len,1);
int len = m_bvh->calculateSerializeBufferSizeNew(); //make sure not to use calculateSerializeBufferSize because it is used for in-place
btChunk* chunk = serializer->allocate(len, 1);
const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,(void*)m_bvh);
serializer->finalizeChunk(chunk, structType, BT_QUANTIZED_BVH_CODE, (void*)m_bvh);
}
}
void btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const
void btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const
{
if (m_triangleInfoMap)
{
int len = m_triangleInfoMap->calculateSerializeBufferSize();
btChunk* chunk = serializer->allocate(len,1);
btChunk* chunk = serializer->allocate(len, 1);
const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,(void*)m_triangleInfoMap);
serializer->finalizeChunk(chunk, structType, BT_TRIANLGE_INFO_MAP, (void*)m_triangleInfoMap);
}
}

87
src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h
View File

@@ -23,103 +23,99 @@ subject to the following restrictions:
///The btBvhTriangleMeshShape is a static-triangle mesh shape, it can only be used for fixed/non-moving objects.
///If you required moving concave triangle meshes, it is recommended to perform convex decomposition
///using HACD, see Bullet/Demos/ConvexDecompositionDemo.
///using HACD, see Bullet/Demos/ConvexDecompositionDemo.
///Alternatively, you can use btGimpactMeshShape for moving concave triangle meshes.
///btBvhTriangleMeshShape has several optimizations, such as bounding volume hierarchy and
///cache friendly traversal for PlayStation 3 Cell SPU.
///btBvhTriangleMeshShape has several optimizations, such as bounding volume hierarchy and
///cache friendly traversal for PlayStation 3 Cell SPU.
///It is recommended to enable useQuantizedAabbCompression for better memory usage.
///It takes a triangle mesh as input, for example a btTriangleMesh or btTriangleIndexVertexArray. The btBvhTriangleMeshShape class allows for triangle mesh deformations by a refit or partialRefit method.
///Instead of building the bounding volume hierarchy acceleration structure, it is also possible to serialize (save) and deserialize (load) the structure from disk.
///See Demos\ConcaveDemo\ConcavePhysicsDemo.cpp for an example.
ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape
ATTRIBUTE_ALIGNED16(class)
btBvhTriangleMeshShape : public btTriangleMeshShape
{
btOptimizedBvh* m_bvh;
btTriangleInfoMap* m_triangleInfoMap;
btOptimizedBvh* m_bvh;
btTriangleInfoMap* m_triangleInfoMap;
bool m_useQuantizedAabbCompression;
bool m_ownsBvh;
#ifdef __clang__
bool m_pad[11] __attribute__((unused));////need padding due to alignment
bool m_pad[11] __attribute__((unused)); ////need padding due to alignment
#else
bool m_pad[11];////need padding due to alignment
bool m_pad[11]; ////need padding due to alignment
#endif
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true);
btBvhTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true);
///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb
btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true);
btBvhTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, bool buildBvh = true);
virtual ~btBvhTriangleMeshShape();
bool getOwnsBvh () const
bool getOwnsBvh() const
{
return m_ownsBvh;
}
void performRaycast(btTriangleCallback * callback, const btVector3& raySource, const btVector3& rayTarget);
void performConvexcast(btTriangleCallback * callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax);
void performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget);
void performConvexcast (btTriangleCallback* callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax);
virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const;
virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
void refitTree(const btVector3& aabbMin,const btVector3& aabbMax);
void refitTree(const btVector3& aabbMin, const btVector3& aabbMax);
///for a fast incremental refit of parts of the tree. Note: the entire AABB of the tree will become more conservative, it never shrinks
void partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax);
void partialRefitTree(const btVector3& aabbMin, const btVector3& aabbMax);
//debugging
virtual const char* getName()const {return "BVHTRIANGLEMESH";}
virtual const char* getName() const { return "BVHTRIANGLEMESH"; }
virtual void setLocalScaling(const btVector3& scaling);
virtual void setLocalScaling(const btVector3& scaling);
btOptimizedBvh* getOptimizedBvh()
btOptimizedBvh* getOptimizedBvh()
{
return m_bvh;
}
void setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& localScaling=btVector3(1,1,1));
void setOptimizedBvh(btOptimizedBvh * bvh, const btVector3& localScaling = btVector3(1, 1, 1));
void buildOptimizedBvh();
void buildOptimizedBvh();
bool usesQuantizedAabbCompression() const
bool usesQuantizedAabbCompression() const
{
return m_useQuantizedAabbCompression;
return m_useQuantizedAabbCompression;
}
void setTriangleInfoMap(btTriangleInfoMap* triangleInfoMap)
void setTriangleInfoMap(btTriangleInfoMap * triangleInfoMap)
{
m_triangleInfoMap = triangleInfoMap;
}
const btTriangleInfoMap* getTriangleInfoMap() const
{
return m_triangleInfoMap;
}
btTriangleInfoMap* getTriangleInfoMap()
const btTriangleInfoMap* getTriangleInfoMap() const
{
return m_triangleInfoMap;
}
virtual int calculateSerializeBufferSize() const;
btTriangleInfoMap* getTriangleInfoMap()
{
return m_triangleInfoMap;
}
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual void serializeSingleBvh(btSerializer* serializer) const;
virtual void serializeSingleTriangleInfoMap(btSerializer* serializer) const;
virtual void serializeSingleBvh(btSerializer * serializer) const;
virtual void serializeSingleTriangleInfoMap(btSerializer * serializer) const;
};
// clang-format off
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btTriangleMeshShapeData
{
@@ -138,12 +134,11 @@ struct btTriangleMeshShapeData
};
// clang-format on
SIMD_FORCE_INLINE int btBvhTriangleMeshShape::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btBvhTriangleMeshShape::calculateSerializeBufferSize() const
{
return sizeof(btTriangleMeshShapeData);
}
#endif //BT_BVH_TRIANGLE_MESH_SHAPE_H
#endif //BT_BVH_TRIANGLE_MESH_SHAPE_H

81
src/BulletCollision/CollisionShapes/btCapsuleShape.cpp
View File

@@ -13,24 +13,21 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btCapsuleShape.h"
#include "LinearMath/btQuaternion.h"
btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape ()
btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape()
{
m_collisionMargin = radius;
m_shapeType = CAPSULE_SHAPE_PROXYTYPE;
m_upAxis = 1;
m_implicitShapeDimensions.setValue(radius,0.5f*height,radius);
m_implicitShapeDimensions.setValue(radius, 0.5f * height, radius);
}
btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const
{
btVector3 supVec(0,0,0);
btVector3 supVec(0, 0, 0);
btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
@@ -38,20 +35,19 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
vec.setValue(1, 0, 0);
}
else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
vec *= rlen;
}
btVector3 vtx;
btScalar newDot;
{
btVector3 pos(0,0,0);
btVector3 pos(0, 0, 0);
pos[getUpAxis()] = getHalfHeight();
vtx = pos;
@@ -63,7 +59,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
}
}
{
btVector3 pos(0,0,0);
btVector3 pos(0, 0, 0);
pos[getUpAxis()] = -getHalfHeight();
vtx = pos;
@@ -76,15 +72,11 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
}
return supVec;
}
void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
{
for (int j=0;j<numVectors;j++)
for (int j = 0; j < numVectors; j++)
{
btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
const btVector3& vec = vectors[j];
@@ -92,7 +84,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
btVector3 vtx;
btScalar newDot;
{
btVector3 pos(0,0,0);
btVector3 pos(0, 0, 0);
pos[getUpAxis()] = getHalfHeight();
vtx = pos;
newDot = vec.dot(vtx);
@@ -103,7 +95,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
}
}
{
btVector3 pos(0,0,0);
btVector3 pos(0, 0, 0);
pos[getUpAxis()] = -getHalfHeight();
vtx = pos;
newDot = vec.dot(vtx);
@@ -113,57 +105,44 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
supportVerticesOut[j] = vtx;
}
}
}
}
void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
void btCapsuleShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
{
//as an approximation, take the inertia of the box that bounds the spheres
btTransform ident;
ident.setIdentity();
btScalar radius = getRadius();
btVector3 halfExtents(radius,radius,radius);
halfExtents[getUpAxis()]+=getHalfHeight();
btVector3 halfExtents(radius, radius, radius);
halfExtents[getUpAxis()] += getHalfHeight();
btScalar lx=btScalar(2.)*(halfExtents[0]);
btScalar ly=btScalar(2.)*(halfExtents[1]);
btScalar lz=btScalar(2.)*(halfExtents[2]);
const btScalar x2 = lx*lx;
const btScalar y2 = ly*ly;
const btScalar z2 = lz*lz;
btScalar lx = btScalar(2.) * (halfExtents[0]);
btScalar ly = btScalar(2.) * (halfExtents[1]);
btScalar lz = btScalar(2.) * (halfExtents[2]);
const btScalar x2 = lx * lx;
const btScalar y2 = ly * ly;
const btScalar z2 = lz * lz;
const btScalar scaledmass = mass * btScalar(.08333333);
inertia[0] = scaledmass * (y2+z2);
inertia[1] = scaledmass * (x2+z2);
inertia[2] = scaledmass * (x2+y2);
inertia[0] = scaledmass * (y2 + z2);
inertia[1] = scaledmass * (x2 + z2);
inertia[2] = scaledmass * (x2 + y2);
}
btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height)
btCapsuleShapeX::btCapsuleShapeX(btScalar radius, btScalar height)
{
m_collisionMargin = radius;
m_upAxis = 0;
m_implicitShapeDimensions.setValue(0.5f*height, radius,radius);
m_implicitShapeDimensions.setValue(0.5f * height, radius, radius);
}
btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height)
btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius, btScalar height)
{
m_collisionMargin = radius;
m_upAxis = 2;
m_implicitShapeDimensions.setValue(radius,radius,0.5f*height);
m_implicitShapeDimensions.setValue(radius, radius, 0.5f * height);
}

109
src/BulletCollision/CollisionShapes/btCapsuleShape.h
View File

@@ -17,99 +17,96 @@ subject to the following restrictions:
#define BT_CAPSULE_SHAPE_H
#include "btConvexInternalShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
///The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned around the X axis and btCapsuleShapeZ around the Z axis.
///The total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
///The btCapsuleShape is a convex hull of two spheres. The btMultiSphereShape is a more general collision shape that takes the convex hull of multiple sphere, so it can also represent a capsule when just using two spheres.
ATTRIBUTE_ALIGNED16(class) btCapsuleShape : public btConvexInternalShape
ATTRIBUTE_ALIGNED16(class)
btCapsuleShape : public btConvexInternalShape
{
protected:
int m_upAxis;
int m_upAxis;
protected:
///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses.
btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;};
btCapsuleShape() : btConvexInternalShape() { m_shapeType = CAPSULE_SHAPE_PROXYTYPE; };
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btCapsuleShape(btScalar radius,btScalar height);
btCapsuleShape(btScalar radius, btScalar height);
///CollisionShape Interface
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
/// btConvexShape Interface
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
virtual void setMargin(btScalar collisionMargin)
{
//don't override the margin for capsules, their entire radius == margin
(void)collisionMargin;
}
virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
{
btVector3 halfExtents(getRadius(),getRadius(),getRadius());
halfExtents[m_upAxis] = getRadius() + getHalfHeight();
btMatrix3x3 abs_b = t.getBasis().absolute();
btVector3 center = t.getOrigin();
btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
aabbMin = center - extent;
aabbMax = center + extent;
btVector3 halfExtents(getRadius(), getRadius(), getRadius());
halfExtents[m_upAxis] = getRadius() + getHalfHeight();
btMatrix3x3 abs_b = t.getBasis().absolute();
btVector3 center = t.getOrigin();
btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
aabbMin = center - extent;
aabbMax = center + extent;
}
virtual const char* getName()const
virtual const char* getName() const
{
return "CapsuleShape";
}
int getUpAxis() const
int getUpAxis() const
{
return m_upAxis;
}
btScalar getRadius() const
btScalar getRadius() const
{
int radiusAxis = (m_upAxis+2)%3;
int radiusAxis = (m_upAxis + 2) % 3;
return m_implicitShapeDimensions[radiusAxis];
}
btScalar getHalfHeight() const
btScalar getHalfHeight() const
{
return m_implicitShapeDimensions[m_upAxis];
}
virtual void setLocalScaling(const btVector3& scaling)
virtual void setLocalScaling(const btVector3& scaling)
{
btVector3 unScaledImplicitShapeDimensions = m_implicitShapeDimensions / m_localScaling;
btConvexInternalShape::setLocalScaling(scaling);
btConvexInternalShape::setLocalScaling(scaling);
m_implicitShapeDimensions = (unScaledImplicitShapeDimensions * scaling);
//update m_collisionMargin, since entire radius==margin
int radiusAxis = (m_upAxis+2)%3;
int radiusAxis = (m_upAxis + 2) % 3;
m_collisionMargin = m_implicitShapeDimensions[radiusAxis];
}
virtual btVector3 getAnisotropicRollingFrictionDirection() const
virtual btVector3 getAnisotropicRollingFrictionDirection() const
{
btVector3 aniDir(0,0,0);
aniDir[getUpAxis()]=1;
btVector3 aniDir(0, 0, 0);
aniDir[getUpAxis()] = 1;
return aniDir;
}
virtual int calculateSerializeBufferSize() const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
SIMD_FORCE_INLINE void deSerializeFloat(struct btCapsuleShapeData* dataBuffer);
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
SIMD_FORCE_INLINE void deSerializeFloat(struct btCapsuleShapeData * dataBuffer);
};
///btCapsuleShapeX represents a capsule around the Z axis
@@ -117,17 +114,13 @@ public:
class btCapsuleShapeX : public btCapsuleShape
{
public:
btCapsuleShapeX(btScalar radius, btScalar height);
btCapsuleShapeX(btScalar radius,btScalar height);
//debugging
virtual const char* getName()const
virtual const char* getName() const
{
return "CapsuleX";
}
};
///btCapsuleShapeZ represents a capsule around the Z axis
@@ -135,38 +128,36 @@ public:
class btCapsuleShapeZ : public btCapsuleShape
{
public:
btCapsuleShapeZ(btScalar radius,btScalar height);
btCapsuleShapeZ(btScalar radius, btScalar height);
//debugging
virtual const char* getName()const
//debugging
virtual const char* getName() const
{
return "CapsuleZ";
}
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btCapsuleShapeData
struct btCapsuleShapeData
{
btConvexInternalShapeData m_convexInternalShapeData;
btConvexInternalShapeData m_convexInternalShapeData;
int m_upAxis;
int m_upAxis;
char m_padding[4];
char m_padding[4];
};
SIMD_FORCE_INLINE int btCapsuleShape::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btCapsuleShape::calculateSerializeBufferSize() const
{
return sizeof(btCapsuleShapeData);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const
{
btCapsuleShapeData* shapeData = (btCapsuleShapeData*) dataBuffer;
btCapsuleShapeData* shapeData = (btCapsuleShapeData*)dataBuffer;
btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
shapeData->m_upAxis = m_upAxis;
@@ -179,7 +170,7 @@ SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSeri
return "btCapsuleShapeData";
}
SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer)
SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer)
{
m_implicitShapeDimensions.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_implicitShapeDimensions);
m_collisionMargin = dataBuffer->m_convexInternalShapeData.m_collisionMargin;
@@ -188,4 +179,4 @@ SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* data
m_upAxis = dataBuffer->m_upAxis;
}
#endif //BT_CAPSULE_SHAPE_H
#endif //BT_CAPSULE_SHAPE_H

7
src/BulletCollision/CollisionShapes/btCollisionMargin.h
View File

@@ -19,9 +19,6 @@ subject to the following restrictions:
///The CONVEX_DISTANCE_MARGIN is a default collision margin for convex collision shapes derived from btConvexInternalShape.
///This collision margin is used by Gjk and some other algorithms
///Note that when creating small objects, you need to make sure to set a smaller collision margin, using the 'setMargin' API
#define CONVEX_DISTANCE_MARGIN btScalar(0.04)// btScalar(0.1)//;//btScalar(0.01)
#endif //BT_COLLISION_MARGIN_H
#define CONVEX_DISTANCE_MARGIN btScalar(0.04) // btScalar(0.1)//;//btScalar(0.01)
#endif //BT_COLLISION_MARGIN_H

57
src/BulletCollision/CollisionShapes/btCollisionShape.cpp
View File

@@ -20,47 +20,44 @@ subject to the following restrictions:
can be used by probes that are checking whether the
library is actually installed.
*/
extern "C"
extern "C"
{
void btBulletCollisionProbe ();
void btBulletCollisionProbe();
void btBulletCollisionProbe () {}
void btBulletCollisionProbe() {}
}
void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) const
void btCollisionShape::getBoundingSphere(btVector3& center, btScalar& radius) const
{
btTransform tr;
tr.setIdentity();
btVector3 aabbMin,aabbMax;
btVector3 aabbMin, aabbMax;
getAabb(tr,aabbMin,aabbMax);
getAabb(tr, aabbMin, aabbMax);
radius = (aabbMax-aabbMin).length()*btScalar(0.5);
center = (aabbMin+aabbMax)*btScalar(0.5);
radius = (aabbMax - aabbMin).length() * btScalar(0.5);
center = (aabbMin + aabbMax) * btScalar(0.5);
}
btScalar btCollisionShape::getContactBreakingThreshold(btScalar defaultContactThreshold) const
btScalar btCollisionShape::getContactBreakingThreshold(btScalar defaultContactThreshold) const
{
return getAngularMotionDisc() * defaultContactThreshold;
}
btScalar btCollisionShape::getAngularMotionDisc() const
btScalar btCollisionShape::getAngularMotionDisc() const
{
///@todo cache this value, to improve performance
btVector3 center;
btVector3 center;
btScalar disc;
getBoundingSphere(center,disc);
getBoundingSphere(center, disc);
disc += (center).length();
return disc;
}
void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const
void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans, const btVector3& linvel, const btVector3& angvel, btScalar timeStep, btVector3& temporalAabbMin, btVector3& temporalAabbMax) const
{
//start with static aabb
getAabb(curTrans,temporalAabbMin,temporalAabbMax);
getAabb(curTrans, temporalAabbMin, temporalAabbMax);
btScalar temporalAabbMaxx = temporalAabbMax.getX();
btScalar temporalAabbMaxy = temporalAabbMax.getY();
@@ -70,36 +67,36 @@ void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const b
btScalar temporalAabbMinz = temporalAabbMin.getZ();
// add linear motion
btVector3 linMotion = linvel*timeStep;
btVector3 linMotion = linvel * timeStep;
///@todo: simd would have a vector max/min operation, instead of per-element access
if (linMotion.x() > btScalar(0.))
temporalAabbMaxx += linMotion.x();
temporalAabbMaxx += linMotion.x();
else
temporalAabbMinx += linMotion.x();
if (linMotion.y() > btScalar(0.))
temporalAabbMaxy += linMotion.y();
temporalAabbMaxy += linMotion.y();
else
temporalAabbMiny += linMotion.y();
if (linMotion.z() > btScalar(0.))
temporalAabbMaxz += linMotion.z();
temporalAabbMaxz += linMotion.z();
else
temporalAabbMinz += linMotion.z();
//add conservative angular motion
btScalar angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
btVector3 angularMotion3d(angularMotion, angularMotion, angularMotion);
temporalAabbMin = btVector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz);
temporalAabbMax = btVector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz);
temporalAabbMin -= angularMotion3d;
temporalAabbMax += angularMotion3d;
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const
const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const
{
btCollisionShapeData* shapeData = (btCollisionShapeData*) dataBuffer;
char* name = (char*) serializer->findNameForPointer(this);
btCollisionShapeData* shapeData = (btCollisionShapeData*)dataBuffer;
char* name = (char*)serializer->findNameForPointer(this);
shapeData->m_name = (char*)serializer->getUniquePointer(name);
if (shapeData->m_name)
{
@@ -113,10 +110,10 @@ const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializ
return "btCollisionShapeData";
}
void btCollisionShape::serializeSingleShape(btSerializer* serializer) const
void btCollisionShape::serializeSingleShape(btSerializer* serializer) const
{
int len = calculateSerializeBufferSize();
btChunk* chunk = serializer->allocate(len,1);
btChunk* chunk = serializer->allocate(len, 1);
const char* structType = serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,(void*)this);
serializer->finalizeChunk(chunk, structType, BT_SHAPE_CODE, (void*)this);
}

89
src/BulletCollision/CollisionShapes/btCollisionShape.h
View File

@@ -19,12 +19,12 @@ subject to the following restrictions:
#include "LinearMath/btTransform.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types
class btSerializer;
///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects.
ATTRIBUTE_ALIGNED16(class) btCollisionShape
ATTRIBUTE_ALIGNED16(class)
btCollisionShape
{
protected:
int m_shapeType;
@@ -32,10 +32,9 @@ protected:
int m_userIndex;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1)
btCollisionShape() : m_shapeType(INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1)
{
}
@@ -44,50 +43,47 @@ public:
}
///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0;
virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0;
virtual void getBoundingSphere(btVector3& center,btScalar& radius) const;
virtual void getBoundingSphere(btVector3 & center, btScalar & radius) const;
///getAngularMotionDisc returns the maximum radius needed for Conservative Advancement to handle time-of-impact with rotations.
virtual btScalar getAngularMotionDisc() const;
virtual btScalar getContactBreakingThreshold(btScalar defaultContactThresholdFactor) const;
virtual btScalar getAngularMotionDisc() const;
virtual btScalar getContactBreakingThreshold(btScalar defaultContactThresholdFactor) const;
///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
///result is conservative
void calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const;
void calculateTemporalAabb(const btTransform& curTrans, const btVector3& linvel, const btVector3& angvel, btScalar timeStep, btVector3& temporalAabbMin, btVector3& temporalAabbMax) const;
SIMD_FORCE_INLINE bool isPolyhedral() const
SIMD_FORCE_INLINE bool isPolyhedral() const
{
return btBroadphaseProxy::isPolyhedral(getShapeType());
}
SIMD_FORCE_INLINE bool isConvex2d() const
SIMD_FORCE_INLINE bool isConvex2d() const
{
return btBroadphaseProxy::isConvex2d(getShapeType());
}
SIMD_FORCE_INLINE bool isConvex() const
SIMD_FORCE_INLINE bool isConvex() const
{
return btBroadphaseProxy::isConvex(getShapeType());
}
SIMD_FORCE_INLINE bool isNonMoving() const
SIMD_FORCE_INLINE bool isNonMoving() const
{
return btBroadphaseProxy::isNonMoving(getShapeType());
}
SIMD_FORCE_INLINE bool isConcave() const
SIMD_FORCE_INLINE bool isConcave() const
{
return btBroadphaseProxy::isConcave(getShapeType());
}
SIMD_FORCE_INLINE bool isCompound() const
SIMD_FORCE_INLINE bool isCompound() const
{
return btBroadphaseProxy::isCompound(getShapeType());
}
SIMD_FORCE_INLINE bool isSoftBody() const
SIMD_FORCE_INLINE bool isSoftBody() const
{
return btBroadphaseProxy::isSoftBody(getShapeType());
}
@@ -99,35 +95,35 @@ public:
}
#ifndef __SPU__
virtual void setLocalScaling(const btVector3& scaling) =0;
virtual const btVector3& getLocalScaling() const =0;
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const = 0;
virtual void setLocalScaling(const btVector3& scaling) = 0;
virtual const btVector3& getLocalScaling() const = 0;
virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const = 0;
//debugging support
virtual const char* getName() const = 0;
#endif //__SPU__
//debugging support
virtual const char* getName()const =0 ;
#endif //__SPU__
int getShapeType() const { return m_shapeType; }
int getShapeType() const
{
return m_shapeType;
}
///the getAnisotropicRollingFrictionDirection can be used in combination with setAnisotropicFriction
///See Bullet/Demos/RollingFrictionDemo for an example
virtual btVector3 getAnisotropicRollingFrictionDirection() const
virtual btVector3 getAnisotropicRollingFrictionDirection() const
{
return btVector3(1,1,1);
return btVector3(1, 1, 1);
}
virtual void setMargin(btScalar margin) = 0;
virtual btScalar getMargin() const = 0;
virtual void setMargin(btScalar margin) = 0;
virtual btScalar getMargin() const = 0;
///optional user data pointer
void setUserPointer(void* userPtr)
void setUserPointer(void* userPtr)
{
m_userPointer = userPtr;
}
void* getUserPointer() const
void* getUserPointer() const
{
return m_userPointer;
}
@@ -141,16 +137,16 @@ public:
return m_userIndex;
}
virtual int calculateSerializeBufferSize() const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual void serializeSingleShape(btSerializer* serializer) const;
};
virtual void serializeSingleShape(btSerializer * serializer) const;
};
// clang-format off
// parser needs * with the name
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btCollisionShapeData
{
@@ -158,13 +154,10 @@ struct btCollisionShapeData
int m_shapeType;
char m_padding[4];
};
SIMD_FORCE_INLINE int btCollisionShape::calculateSerializeBufferSize() const
// clang-format on
SIMD_FORCE_INLINE int btCollisionShape::calculateSerializeBufferSize() const
{
return sizeof(btCollisionShapeData);
}
#endif //BT_COLLISION_SHAPE_H
#endif //BT_COLLISION_SHAPE_H

192
src/BulletCollision/CollisionShapes/btCompoundShape.cpp
View File

@@ -19,26 +19,25 @@ subject to the following restrictions:
#include "LinearMath/btSerializer.h"
btCompoundShape::btCompoundShape(bool enableDynamicAabbTree, const int initialChildCapacity)
: m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)),
m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)),
m_dynamicAabbTree(0),
m_updateRevision(1),
m_collisionMargin(btScalar(0.)),
m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
: m_localAabbMin(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)),
m_localAabbMax(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)),
m_dynamicAabbTree(0),
m_updateRevision(1),
m_collisionMargin(btScalar(0.)),
m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.))
{
m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
if (enableDynamicAabbTree)
{
void* mem = btAlignedAlloc(sizeof(btDbvt),16);
m_dynamicAabbTree = new(mem) btDbvt();
btAssert(mem==m_dynamicAabbTree);
void* mem = btAlignedAlloc(sizeof(btDbvt), 16);
m_dynamicAabbTree = new (mem) btDbvt();
btAssert(mem == m_dynamicAabbTree);
}
m_children.reserve(initialChildCapacity);
}
btCompoundShape::~btCompoundShape()
{
if (m_dynamicAabbTree)
@@ -48,7 +47,7 @@ btCompoundShape::~btCompoundShape()
}
}
void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
void btCompoundShape::addChildShape(const btTransform& localTransform, btCollisionShape* shape)
{
m_updateRevision++;
//m_childTransforms.push_back(localTransform);
@@ -60,11 +59,10 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio
child.m_childShapeType = shape->getShapeType();
child.m_childMargin = shape->getMargin();
//extend the local aabbMin/aabbMax
btVector3 localAabbMin,localAabbMax;
shape->getAabb(localTransform,localAabbMin,localAabbMax);
for (int i=0;i<3;i++)
btVector3 localAabbMin, localAabbMax;
shape->getAabb(localTransform, localAabbMin, localAabbMax);
for (int i = 0; i < 3; i++)
{
if (m_localAabbMin[i] > localAabbMin[i])
{
@@ -74,31 +72,30 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio
{
m_localAabbMax[i] = localAabbMax[i];
}
}
if (m_dynamicAabbTree)
{
const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
size_t index = m_children.size();
child.m_node = m_dynamicAabbTree->insert(bounds,reinterpret_cast<void*>(index) );
child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index));
}
m_children.push_back(child);
}
void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform,bool shouldRecalculateLocalAabb)
void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb)
{
m_children[childIndex].m_transform = newChildTransform;
if (m_dynamicAabbTree)
{
///update the dynamic aabb tree
btVector3 localAabbMin,localAabbMax;
m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax);
ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
btVector3 localAabbMin, localAabbMax;
m_children[childIndex].m_childShape->getAabb(newChildTransform, localAabbMin, localAabbMax);
ATTRIBUTE_ALIGNED16(btDbvtVolume)
bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
//int index = m_children.size()-1;
m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds);
m_dynamicAabbTree->update(m_children[childIndex].m_node, bounds);
}
if (shouldRecalculateLocalAabb)
@@ -110,35 +107,30 @@ void btCompoundShape::updateChildTransform(int childIndex, const btTransform& ne
void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
{
m_updateRevision++;
btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
btAssert(childShapeIndex >= 0 && childShapeIndex < m_children.size());
if (m_dynamicAabbTree)
{
m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node);
}
m_children.swap(childShapeIndex,m_children.size()-1);
if (m_dynamicAabbTree)
m_children.swap(childShapeIndex, m_children.size() - 1);
if (m_dynamicAabbTree)
m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex;
m_children.pop_back();
}
void btCompoundShape::removeChildShape(btCollisionShape* shape)
{
m_updateRevision++;
// Find the children containing the shape specified, and remove those children.
//note: there might be multiple children using the same shape!
for(int i = m_children.size()-1; i >= 0 ; i--)
for (int i = m_children.size() - 1; i >= 0; i--)
{
if(m_children[i].m_childShape == shape)
if (m_children[i].m_childShape == shape)
{
removeChildShapeByIndex(i);
}
}
recalculateLocalAabb();
}
@@ -147,15 +139,15 @@ void btCompoundShape::recalculateLocalAabb()
// Recalculate the local aabb
// Brute force, it iterates over all the shapes left.
m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
//extend the local aabbMin/aabbMax
for (int j = 0; j < m_children.size(); j++)
{
btVector3 localAabbMin,localAabbMax;
btVector3 localAabbMin, localAabbMax;
m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax);
for (int i=0;i<3;i++)
for (int i = 0; i < 3; i++)
{
if (m_localAabbMin[i] > localAabbMin[i])
m_localAabbMin[i] = localAabbMin[i];
@@ -166,53 +158,47 @@ void btCompoundShape::recalculateLocalAabb()
}
///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
void btCompoundShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const
{
btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
btVector3 localHalfExtents = btScalar(0.5) * (m_localAabbMax - m_localAabbMin);
btVector3 localCenter = btScalar(0.5) * (m_localAabbMax + m_localAabbMin);
//avoid an illegal AABB when there are no children
if (!m_children.size())
{
localHalfExtents.setValue(0,0,0);
localCenter.setValue(0,0,0);
localHalfExtents.setValue(0, 0, 0);
localCenter.setValue(0, 0, 0);
}
localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
localHalfExtents += btVector3(getMargin(), getMargin(), getMargin());
btMatrix3x3 abs_b = trans.getBasis().absolute();
btMatrix3x3 abs_b = trans.getBasis().absolute();
btVector3 center = trans(localCenter);
btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
aabbMin = center-extent;
aabbMax = center+extent;
btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
aabbMin = center - extent;
aabbMax = center + extent;
}
void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
void btCompoundShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
{
//approximation: take the inertia from the aabb for now
btTransform ident;
ident.setIdentity();
btVector3 aabbMin,aabbMax;
getAabb(ident,aabbMin,aabbMax);
btVector3 aabbMin, aabbMax;
getAabb(ident, aabbMin, aabbMax);
btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
btScalar lx=btScalar(2.)*(halfExtents.x());
btScalar ly=btScalar(2.)*(halfExtents.y());
btScalar lz=btScalar(2.)*(halfExtents.z());
inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz);
inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz);
inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly);
btScalar lx = btScalar(2.) * (halfExtents.x());
btScalar ly = btScalar(2.) * (halfExtents.y());
btScalar lz = btScalar(2.) * (halfExtents.z());
inertia[0] = mass / (btScalar(12.0)) * (ly * ly + lz * lz);
inertia[1] = mass / (btScalar(12.0)) * (lx * lx + lz * lz);
inertia[2] = mass / (btScalar(12.0)) * (lx * lx + ly * ly);
}
void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const
{
int n = m_children.size();
@@ -223,18 +209,18 @@ void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, bt
for (k = 0; k < n; k++)
{
btAssert(masses[k]>0);
btAssert(masses[k] > 0);
center += m_children[k].m_transform.getOrigin() * masses[k];
totalMass += masses[k];
}
btAssert(totalMass>0);
btAssert(totalMass > 0);
center /= totalMass;
principal.setOrigin(center);
btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
for ( k = 0; k < n; k++)
for (k = 0; k < n; k++)
{
btVector3 i;
m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
@@ -259,8 +245,8 @@ void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, bt
j[0].setValue(o2, 0, 0);
j[1].setValue(0, o2, 0);
j[2].setValue(0, 0, o2);
j[0] += o * -o.x();
j[1] += o * -o.y();
j[0] += o * -o.x();
j[1] += o * -o.y();
j[2] += o * -o.z();
//add inertia tensor of pointmass
@@ -273,59 +259,50 @@ void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, bt
inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
}
void btCompoundShape::setLocalScaling(const btVector3& scaling)
{
for(int i = 0; i < m_children.size(); i++)
for (int i = 0; i < m_children.size(); i++)
{
btTransform childTrans = getChildTransform(i);
btVector3 childScale = m_children[i].m_childShape->getLocalScaling();
// childScale = childScale * (childTrans.getBasis() * scaling);
// childScale = childScale * (childTrans.getBasis() * scaling);
childScale = childScale * scaling / m_localScaling;
m_children[i].m_childShape->setLocalScaling(childScale);
childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling);
updateChildTransform(i, childTrans,false);
updateChildTransform(i, childTrans, false);
}
m_localScaling = scaling;
recalculateLocalAabb();
}
void btCompoundShape::createAabbTreeFromChildren()
{
if ( !m_dynamicAabbTree )
{
void* mem = btAlignedAlloc(sizeof(btDbvt),16);
m_dynamicAabbTree = new(mem) btDbvt();
btAssert(mem==m_dynamicAabbTree);
if (!m_dynamicAabbTree)
{
void* mem = btAlignedAlloc(sizeof(btDbvt), 16);
m_dynamicAabbTree = new (mem) btDbvt();
btAssert(mem == m_dynamicAabbTree);
for ( int index = 0; index < m_children.size(); index++ )
{
btCompoundShapeChild &child = m_children[index];
for (int index = 0; index < m_children.size(); index++)
{
btCompoundShapeChild& child = m_children[index];
//extend the local aabbMin/aabbMax
btVector3 localAabbMin,localAabbMax;
child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax);
//extend the local aabbMin/aabbMax
btVector3 localAabbMin, localAabbMax;
child.m_childShape->getAabb(child.m_transform, localAabbMin, localAabbMax);
const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
size_t index2 = index;
child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2) );
}
}
child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2));
}
}
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const
const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const
{
btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer;
btCompoundShapeData* shapeData = (btCompoundShapeData*)dataBuffer;
btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
shapeData->m_collisionMargin = float(m_collisionMargin);
@@ -333,27 +310,26 @@ const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serialize
shapeData->m_childShapePtr = 0;
if (shapeData->m_numChildShapes)
{
btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes);
btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData), shapeData->m_numChildShapes);
btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr;
shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr);
for (int i=0;i<shapeData->m_numChildShapes;i++,memPtr++)
for (int i = 0; i < shapeData->m_numChildShapes; i++, memPtr++)
{
memPtr->m_childMargin = float(m_children[i].m_childMargin);
memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape);
//don't serialize shapes that already have been serialized
if (!serializer->findPointer(m_children[i].m_childShape))
{
btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1);
const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer);
serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape);
}
btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(), 1);
const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk, structType, BT_SHAPE_CODE, m_children[i].m_childShape);
}
memPtr->m_childShapeType = m_children[i].m_childShapeType;
m_children[i].m_transform.serializeFloat(memPtr->m_transform);
}
serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr);
serializer->finalizeChunk(chunk, "btCompoundShapeChildData", BT_ARRAY_CODE, chunk->m_oldPtr);
}
return "btCompoundShapeData";
}

97
src/BulletCollision/CollisionShapes/btCompoundShape.h
View File

@@ -27,45 +27,47 @@ subject to the following restrictions:
//class btOptimizedBvh;
struct btDbvt;
ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild
ATTRIBUTE_ALIGNED16(struct)
btCompoundShapeChild
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btTransform m_transform;
btCollisionShape* m_childShape;
int m_childShapeType;
btScalar m_childMargin;
struct btDbvtNode* m_node;
btTransform m_transform;
btCollisionShape* m_childShape;
int m_childShapeType;
btScalar m_childMargin;
struct btDbvtNode* m_node;
};
SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompoundShapeChild& c2)
{
return ( c1.m_transform == c2.m_transform &&
c1.m_childShape == c2.m_childShape &&
c1.m_childShapeType == c2.m_childShapeType &&
c1.m_childMargin == c2.m_childMargin );
return (c1.m_transform == c2.m_transform &&
c1.m_childShape == c2.m_childShape &&
c1.m_childShapeType == c2.m_childShapeType &&
c1.m_childMargin == c2.m_childMargin);
}
/// The btCompoundShape allows to store multiple other btCollisionShapes
/// This allows for moving concave collision objects. This is more general then the static concave btBvhTriangleMeshShape.
/// It has an (optional) dynamic aabb tree to accelerate early rejection tests.
/// It has an (optional) dynamic aabb tree to accelerate early rejection tests.
/// @todo: This aabb tree can also be use to speed up ray tests on btCompoundShape, see http://code.google.com/p/bullet/issues/detail?id=25
/// Currently, removal of child shapes is only supported when disabling the aabb tree (pass 'false' in the constructor of btCompoundShape)
ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape
ATTRIBUTE_ALIGNED16(class)
btCompoundShape : public btCollisionShape
{
protected:
btAlignedObjectArray<btCompoundShapeChild> m_children;
btVector3 m_localAabbMin;
btVector3 m_localAabbMax;
btVector3 m_localAabbMin;
btVector3 m_localAabbMax;
btDbvt* m_dynamicAabbTree;
btDbvt* m_dynamicAabbTree;
///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated
int m_updateRevision;
int m_updateRevision;
btScalar m_collisionMargin;
btScalar m_collisionMargin;
btVector3 m_localScaling;
btVector3 m_localScaling;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -74,17 +76,16 @@ public:
virtual ~btCompoundShape();
void addChildShape(const btTransform& localTransform,btCollisionShape* shape);
void addChildShape(const btTransform& localTransform, btCollisionShape* shape);
/// Remove all children shapes that contain the specified shape
virtual void removeChildShape(btCollisionShape* shape);
virtual void removeChildShape(btCollisionShape * shape);
void removeChildShapeByIndex(int childShapeindex);
int getNumChildShapes() const
int getNumChildShapes() const
{
return int (m_children.size());
return int(m_children.size());
}
btCollisionShape* getChildShape(int index)
@@ -96,18 +97,17 @@ public:
return m_children[index].m_childShape;
}
btTransform& getChildTransform(int index)
btTransform& getChildTransform(int index)
{
return m_children[index].m_transform;
}
const btTransform& getChildTransform(int index) const
const btTransform& getChildTransform(int index) const
{
return m_children[index].m_transform;
}
///set a new transform for a child, and update internal data structures (local aabb and dynamic tree)
void updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb = true);
void updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb = true);
btCompoundShapeChild* getChildList()
{
@@ -115,40 +115,40 @@ public:
}
///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
/** Re-calculate the local Aabb. Is called at the end of removeChildShapes.
Use this yourself if you modify the children or their transforms. */
virtual void recalculateLocalAabb();
virtual void recalculateLocalAabb();
virtual void setLocalScaling(const btVector3& scaling);
virtual void setLocalScaling(const btVector3& scaling);
virtual const btVector3& getLocalScaling() const
virtual const btVector3& getLocalScaling() const
{
return m_localScaling;
}
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
virtual void setMargin(btScalar margin)
virtual void setMargin(btScalar margin)
{
m_collisionMargin = margin;
}
virtual btScalar getMargin() const
virtual btScalar getMargin() const
{
return m_collisionMargin;
}
virtual const char* getName()const
virtual const char* getName() const
{
return "Compound";
}
const btDbvt* getDynamicAabbTree() const
const btDbvt* getDynamicAabbTree() const
{
return m_dynamicAabbTree;
}
btDbvt* getDynamicAabbTree()
btDbvt* getDynamicAabbTree()
{
return m_dynamicAabbTree;
}
@@ -162,19 +162,19 @@ public:
///of the collision object by the principal transform.
void calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const;
int getUpdateRevision() const
int getUpdateRevision() const
{
return m_updateRevision;
}
virtual int calculateSerializeBufferSize() const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
};
// clang-format off
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btCompoundShapeChildData
{
@@ -197,16 +197,11 @@ struct btCompoundShapeData
};
// clang-format on
SIMD_FORCE_INLINE int btCompoundShape::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btCompoundShape::calculateSerializeBufferSize() const
{
return sizeof(btCompoundShapeData);
}
#endif //BT_COMPOUND_SHAPE_H
#endif //BT_COMPOUND_SHAPE_H

3
src/BulletCollision/CollisionShapes/btConcaveShape.cpp
View File

@@ -13,15 +13,12 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btConcaveShape.h"
btConcaveShape::btConcaveShape() : m_collisionMargin(btScalar(0.))
{
}
btConcaveShape::~btConcaveShape()
{
}

20
src/BulletCollision/CollisionShapes/btConcaveShape.h
View File

@@ -17,12 +17,13 @@ subject to the following restrictions:
#define BT_CONCAVE_SHAPE_H
#include "btCollisionShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
#include "btTriangleCallback.h"
/// PHY_ScalarType enumerates possible scalar types.
/// See the btStridingMeshInterface or btHeightfieldTerrainShape for its use
typedef enum PHY_ScalarType {
typedef enum PHY_ScalarType
{
PHY_FLOAT,
PHY_DOUBLE,
PHY_INTEGER,
@@ -33,30 +34,29 @@ typedef enum PHY_ScalarType {
///The btConcaveShape class provides an interface for non-moving (static) concave shapes.
///It has been implemented by the btStaticPlaneShape, btBvhTriangleMeshShape and btHeightfieldTerrainShape.
ATTRIBUTE_ALIGNED16(class) btConcaveShape : public btCollisionShape
ATTRIBUTE_ALIGNED16(class)
btConcaveShape : public btCollisionShape
{
protected:
btScalar m_collisionMargin;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btConcaveShape();
virtual ~btConcaveShape();
virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const = 0;
virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const = 0;
virtual btScalar getMargin() const {
virtual btScalar getMargin() const
{
return m_collisionMargin;
}
virtual void setMargin(btScalar collisionMargin)
{
m_collisionMargin = collisionMargin;
}
};
#endif //BT_CONCAVE_SHAPE_H
#endif //BT_CONCAVE_SHAPE_H

107
src/BulletCollision/CollisionShapes/btConeShape.cpp
View File

@@ -15,11 +15,9 @@ subject to the following restrictions:
#include "btConeShape.h"
btConeShape::btConeShape (btScalar radius,btScalar height): btConvexInternalShape (),
m_radius (radius),
m_height(height)
btConeShape::btConeShape(btScalar radius, btScalar height) : btConvexInternalShape(),
m_radius(radius),
m_height(height)
{
m_shapeType = CONE_SHAPE_PROXYTYPE;
setConeUpIndex(1);
@@ -27,42 +25,40 @@ m_height(height)
m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height));
}
btConeShapeZ::btConeShapeZ (btScalar radius,btScalar height):
btConeShape(radius,height)
btConeShapeZ::btConeShapeZ(btScalar radius, btScalar height) : btConeShape(radius, height)
{
setConeUpIndex(2);
}
btConeShapeX::btConeShapeX (btScalar radius,btScalar height):
btConeShape(radius,height)
btConeShapeX::btConeShapeX(btScalar radius, btScalar height) : btConeShape(radius, height)
{
setConeUpIndex(0);
}
///choose upAxis index
void btConeShape::setConeUpIndex(int upIndex)
void btConeShape::setConeUpIndex(int upIndex)
{
switch (upIndex)
{
case 0:
case 0:
m_coneIndices[0] = 1;
m_coneIndices[1] = 0;
m_coneIndices[2] = 2;
break;
case 1:
break;
case 1:
m_coneIndices[0] = 0;
m_coneIndices[1] = 1;
m_coneIndices[2] = 2;
break;
case 2:
break;
case 2:
m_coneIndices[0] = 0;
m_coneIndices[1] = 2;
m_coneIndices[2] = 1;
break;
default:
btAssert(0);
break;
default:
btAssert(0);
};
m_implicitShapeDimensions[m_coneIndices[0]] = m_radius;
m_implicitShapeDimensions[m_coneIndices[1]] = m_height;
m_implicitShapeDimensions[m_coneIndices[2]] = m_radius;
@@ -70,72 +66,71 @@ void btConeShape::setConeUpIndex(int upIndex)
btVector3 btConeShape::coneLocalSupport(const btVector3& v) const
{
btScalar halfHeight = m_height * btScalar(0.5);
if (v[m_coneIndices[1]] > v.length() * m_sinAngle)
{
btVector3 tmp;
tmp[m_coneIndices[0]] = btScalar(0.);
tmp[m_coneIndices[1]] = halfHeight;
tmp[m_coneIndices[2]] = btScalar(0.);
return tmp;
}
else {
btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]);
if (s > SIMD_EPSILON) {
btScalar d = m_radius / s;
btVector3 tmp;
tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d;
tmp[m_coneIndices[1]] = -halfHeight;
tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d;
return tmp;
}
else {
if (v[m_coneIndices[1]] > v.length() * m_sinAngle)
{
btVector3 tmp;
tmp[m_coneIndices[0]] = btScalar(0.);
tmp[m_coneIndices[1]] = -halfHeight;
tmp[m_coneIndices[1]] = halfHeight;
tmp[m_coneIndices[2]] = btScalar(0.);
return tmp;
}
}
else
{
btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]);
if (s > SIMD_EPSILON)
{
btScalar d = m_radius / s;
btVector3 tmp;
tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d;
tmp[m_coneIndices[1]] = -halfHeight;
tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d;
return tmp;
}
else
{
btVector3 tmp;
tmp[m_coneIndices[0]] = btScalar(0.);
tmp[m_coneIndices[1]] = -halfHeight;
tmp[m_coneIndices[2]] = btScalar(0.);
return tmp;
}
}
}
btVector3 btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
btVector3 btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
{
return coneLocalSupport(vec);
return coneLocalSupport(vec);
}
void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
{
for (int i=0;i<numVectors;i++)
for (int i = 0; i < numVectors; i++)
{
const btVector3& vec = vectors[i];
supportVerticesOut[i] = coneLocalSupport(vec);
}
}
btVector3 btConeShape::localGetSupportingVertex(const btVector3& vec) const
btVector3 btConeShape::localGetSupportingVertex(const btVector3& vec) const
{
btVector3 supVertex = coneLocalSupport(vec);
if ( getMargin()!=btScalar(0.) )
if (getMargin() != btScalar(0.))
{
btVector3 vecnorm = vec;
if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
{
vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
}
vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
}
vecnorm.normalize();
supVertex+= getMargin() * vecnorm;
supVertex += getMargin() * vecnorm;
}
return supVertex;
}
void btConeShape::setLocalScaling(const btVector3& scaling)
void btConeShape::setLocalScaling(const btVector3& scaling)
{
int axis = m_coneIndices[1];
int r1 = m_coneIndices[0];

143
src/BulletCollision/CollisionShapes/btConeShape.h
View File

@@ -17,31 +17,30 @@ subject to the following restrictions:
#define BT_CONE_MINKOWSKI_H
#include "btConvexInternalShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
///The btConeShape implements a cone shape primitive, centered around the origin and aligned with the Y axis. The btConeShapeX is aligned around the X axis and btConeShapeZ around the Z axis.
ATTRIBUTE_ALIGNED16(class) btConeShape : public btConvexInternalShape
ATTRIBUTE_ALIGNED16(class)
btConeShape : public btConvexInternalShape
{
btScalar m_sinAngle;
btScalar m_radius;
btScalar m_height;
int m_coneIndices[3];
int m_coneIndices[3];
btVector3 coneLocalSupport(const btVector3& v) const;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btConeShape (btScalar radius,btScalar height);
virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
btScalar getRadius() const { return m_radius;}
btScalar getHeight() const { return m_height;}
btConeShape(btScalar radius, btScalar height);
virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
btScalar getRadius() const { return m_radius; }
btScalar getHeight() const { return m_height; }
void setRadius(const btScalar radius)
{
@@ -52,124 +51,115 @@ public:
m_height = height;
}
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const
virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const
{
btTransform identity;
identity.setIdentity();
btVector3 aabbMin,aabbMax;
getAabb(identity,aabbMin,aabbMax);
btVector3 aabbMin, aabbMax;
getAabb(identity, aabbMin, aabbMax);
btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
btScalar margin = getMargin();
btScalar lx=btScalar(2.)*(halfExtents.x()+margin);
btScalar ly=btScalar(2.)*(halfExtents.y()+margin);
btScalar lz=btScalar(2.)*(halfExtents.z()+margin);
const btScalar x2 = lx*lx;
const btScalar y2 = ly*ly;
const btScalar z2 = lz*lz;
btScalar lx = btScalar(2.) * (halfExtents.x() + margin);
btScalar ly = btScalar(2.) * (halfExtents.y() + margin);
btScalar lz = btScalar(2.) * (halfExtents.z() + margin);
const btScalar x2 = lx * lx;
const btScalar y2 = ly * ly;
const btScalar z2 = lz * lz;
const btScalar scaledmass = mass * btScalar(0.08333333);
inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2));
// inertia.x() = scaledmass * (y2+z2);
// inertia.y() = scaledmass * (x2+z2);
// inertia.z() = scaledmass * (x2+y2);
// inertia.x() = scaledmass * (y2+z2);
// inertia.y() = scaledmass * (x2+z2);
// inertia.z() = scaledmass * (x2+y2);
}
virtual const char* getName()const
{
return "Cone";
}
///choose upAxis index
void setConeUpIndex(int upIndex);
int getConeUpIndex() const
{
return m_coneIndices[1];
}
virtual btVector3 getAnisotropicRollingFrictionDirection() const
virtual const char* getName() const
{
return btVector3 (0,1,0);
return "Cone";
}
virtual void setLocalScaling(const btVector3& scaling);
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
///choose upAxis index
void setConeUpIndex(int upIndex);
int getConeUpIndex() const
{
return m_coneIndices[1];
}
virtual btVector3 getAnisotropicRollingFrictionDirection() const
{
return btVector3(0, 1, 0);
}
virtual void setLocalScaling(const btVector3& scaling);
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
};
///btConeShape implements a Cone shape, around the X axis
class btConeShapeX : public btConeShape
{
public:
btConeShapeX(btScalar radius,btScalar height);
public:
btConeShapeX(btScalar radius, btScalar height);
virtual btVector3 getAnisotropicRollingFrictionDirection() const
virtual btVector3 getAnisotropicRollingFrictionDirection() const
{
return btVector3 (1,0,0);
return btVector3(1, 0, 0);
}
//debugging
virtual const char* getName()const
virtual const char* getName() const
{
return "ConeX";
}
};
///btConeShapeZ implements a Cone shape, around the Z axis
class btConeShapeZ : public btConeShape
{
public:
btConeShapeZ(btScalar radius,btScalar height);
btConeShapeZ(btScalar radius, btScalar height);
virtual btVector3 getAnisotropicRollingFrictionDirection() const
virtual btVector3 getAnisotropicRollingFrictionDirection() const
{
return btVector3 (0,0,1);
return btVector3(0, 0, 1);
}
//debugging
virtual const char* getName()const
virtual const char* getName() const
{
return "ConeZ";
}
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btConeShapeData
struct btConeShapeData
{
btConvexInternalShapeData m_convexInternalShapeData;
int m_upIndex;
char m_padding[4];
btConvexInternalShapeData m_convexInternalShapeData;
int m_upIndex;
char m_padding[4];
};
SIMD_FORCE_INLINE int btConeShape::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btConeShape::calculateSerializeBufferSize() const
{
return sizeof(btConeShapeData);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const
SIMD_FORCE_INLINE const char* btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const
{
btConeShapeData* shapeData = (btConeShapeData*) dataBuffer;
btConeShapeData* shapeData = (btConeShapeData*)dataBuffer;
btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
shapeData->m_upIndex = m_coneIndices[1];
@@ -182,5 +172,4 @@ SIMD_FORCE_INLINE const char* btConeShape::serialize(void* dataBuffer, btSeriali
return "btConeShapeData";
}
#endif //BT_CONE_MINKOWSKI_H
#endif //BT_CONE_MINKOWSKI_H

46
src/BulletCollision/CollisionShapes/btConvex2dShape.cpp
View File

@@ -15,54 +15,48 @@ subject to the following restrictions:
#include "btConvex2dShape.h"
btConvex2dShape::btConvex2dShape( btConvexShape* convexChildShape):
btConvexShape (), m_childConvexShape(convexChildShape)
btConvex2dShape::btConvex2dShape(btConvexShape* convexChildShape) : btConvexShape(), m_childConvexShape(convexChildShape)
{
m_shapeType = CONVEX_2D_SHAPE_PROXYTYPE;
}
btConvex2dShape::~btConvex2dShape()
{
}
btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
{
return m_childConvexShape->localGetSupportingVertexWithoutMargin(vec);
}
void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
{
m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors);
m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors, supportVerticesOut, numVectors);
}
btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec)const
btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec) const
{
return m_childConvexShape->localGetSupportingVertex(vec);
}
void btConvex2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
void btConvex2dShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
{
///this linear upscaling is not realistic, but we don't deal with large mass ratios...
m_childConvexShape->calculateLocalInertia(mass,inertia);
m_childConvexShape->calculateLocalInertia(mass, inertia);
}
///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
void btConvex2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
void btConvex2dShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
{
m_childConvexShape->getAabb(t,aabbMin,aabbMax);
m_childConvexShape->getAabb(t, aabbMin, aabbMax);
}
void btConvex2dShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
void btConvex2dShape::getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
{
m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax);
m_childConvexShape->getAabbSlow(t, aabbMin, aabbMax);
}
void btConvex2dShape::setLocalScaling(const btVector3& scaling)
void btConvex2dShape::setLocalScaling(const btVector3& scaling)
{
m_childConvexShape->setLocalScaling(scaling);
}
@@ -72,21 +66,21 @@ const btVector3& btConvex2dShape::getLocalScaling() const
return m_childConvexShape->getLocalScaling();
}
void btConvex2dShape::setMargin(btScalar margin)
void btConvex2dShape::setMargin(btScalar margin)
{
m_childConvexShape->setMargin(margin);
}
btScalar btConvex2dShape::getMargin() const
btScalar btConvex2dShape::getMargin() const
{
return m_childConvexShape->getMargin();
}
int btConvex2dShape::getNumPreferredPenetrationDirections() const
int btConvex2dShape::getNumPreferredPenetrationDirections() const
{
return m_childConvexShape->getNumPreferredPenetrationDirections();
}
void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
{
m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector);
m_childConvexShape->getPreferredPenetrationDirection(index, penetrationVector);
}

55
src/BulletCollision/CollisionShapes/btConvex2dShape.h
View File

@@ -17,66 +17,61 @@ subject to the following restrictions:
#define BT_CONVEX_2D_SHAPE_H
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
///The btConvex2dShape allows to use arbitrary convex shapes as 2d convex shapes, with the Z component assumed to be 0.
///For 2d boxes, the btBox2dShape is recommended.
ATTRIBUTE_ALIGNED16(class) btConvex2dShape : public btConvexShape
ATTRIBUTE_ALIGNED16(class)
btConvex2dShape : public btConvexShape
{
btConvexShape* m_childConvexShape;
btConvexShape* m_childConvexShape;
public:
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btConvex2dShape( btConvexShape* convexChildShape);
btConvex2dShape(btConvexShape * convexChildShape);
virtual ~btConvex2dShape();
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
btConvexShape* getChildShape()
virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
btConvexShape* getChildShape()
{
return m_childConvexShape;
}
const btConvexShape* getChildShape() const
const btConvexShape* getChildShape() const
{
return m_childConvexShape;
}
virtual const char* getName()const
virtual const char* getName() const
{
return "Convex2dShape";
}
///////////////////////////
///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
virtual void setLocalScaling(const btVector3& scaling) ;
virtual const btVector3& getLocalScaling() const ;
virtual void setLocalScaling(const btVector3& scaling);
virtual const btVector3& getLocalScaling() const;
virtual void setMargin(btScalar margin);
virtual btScalar getMargin() const;
virtual int getNumPreferredPenetrationDirections() const;
virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const;
virtual void setMargin(btScalar margin);
virtual btScalar getMargin() const;
virtual int getNumPreferredPenetrationDirections() const;
virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const;
};
#endif //BT_CONVEX_2D_SHAPE_H
#endif //BT_CONVEX_2D_SHAPE_H

150
src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
View File

@@ -13,7 +13,7 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#if defined (_WIN32) || defined (__i386__)
#if defined(_WIN32) || defined(__i386__)
#define BT_USE_SSE_IN_API
#endif
@@ -25,14 +25,14 @@ subject to the following restrictions:
#include "btConvexPolyhedron.h"
#include "LinearMath/btConvexHullComputer.h"
btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexAabbCachingShape ()
btConvexHullShape ::btConvexHullShape(const btScalar* points, int numPoints, int stride) : btPolyhedralConvexAabbCachingShape()
{
m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE;
m_unscaledPoints.resize(numPoints);
unsigned char* pointsAddress = (unsigned char*)points;
for (int i=0;i<numPoints;i++)
for (int i = 0; i < numPoints; i++)
{
btScalar* point = (btScalar*)pointsAddress;
m_unscaledPoints[i] = btVector3(point[0], point[1], point[2]);
@@ -40,11 +40,8 @@ btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int
}
recalcLocalAabb();
}
void btConvexHullShape::setLocalScaling(const btVector3& scaling)
{
m_localScaling = scaling;
@@ -56,90 +53,81 @@ void btConvexHullShape::addPoint(const btVector3& point, bool recalculateLocalAa
m_unscaledPoints.push_back(point);
if (recalculateLocalAabb)
recalcLocalAabb();
}
btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
{
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.));
btScalar maxDot = btScalar(-BT_LARGE_FLOAT);
// Here we take advantage of dot(a, b*c) = dot(a*b, c). Note: This is true mathematically, but not numerically.
if( 0 < m_unscaledPoints.size() )
{
btVector3 scaled = vec * m_localScaling;
int index = (int) scaled.maxDot( &m_unscaledPoints[0], m_unscaledPoints.size(), maxDot); // FIXME: may violate encapsulation of m_unscaledPoints
return m_unscaledPoints[index] * m_localScaling;
}
// Here we take advantage of dot(a, b*c) = dot(a*b, c). Note: This is true mathematically, but not numerically.
if (0 < m_unscaledPoints.size())
{
btVector3 scaled = vec * m_localScaling;
int index = (int)scaled.maxDot(&m_unscaledPoints[0], m_unscaledPoints.size(), maxDot); // FIXME: may violate encapsulation of m_unscaledPoints
return m_unscaledPoints[index] * m_localScaling;
}
return supVec;
return supVec;
}
void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
{
btScalar newDot;
//use 'w' component of supportVerticesOut?
{
for (int i=0;i<numVectors;i++)
for (int i = 0; i < numVectors; i++)
{
supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
}
}
for (int j=0;j<numVectors;j++)
{
btVector3 vec = vectors[j] * m_localScaling; // dot(a*b,c) = dot(a,b*c)
if( 0 < m_unscaledPoints.size() )
{
int i = (int) vec.maxDot( &m_unscaledPoints[0], m_unscaledPoints.size(), newDot);
supportVerticesOut[j] = getScaledPoint(i);
supportVerticesOut[j][3] = newDot;
}
else
supportVerticesOut[j][3] = -BT_LARGE_FLOAT;
}
for (int j = 0; j < numVectors; j++)
{
btVector3 vec = vectors[j] * m_localScaling; // dot(a*b,c) = dot(a,b*c)
if (0 < m_unscaledPoints.size())
{
int i = (int)vec.maxDot(&m_unscaledPoints[0], m_unscaledPoints.size(), newDot);
supportVerticesOut[j] = getScaledPoint(i);
supportVerticesOut[j][3] = newDot;
}
else
supportVerticesOut[j][3] = -BT_LARGE_FLOAT;
}
}
btVector3 btConvexHullShape::localGetSupportingVertex(const btVector3& vec)const
btVector3 btConvexHullShape::localGetSupportingVertex(const btVector3& vec) const
{
btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
if ( getMargin()!=btScalar(0.) )
if (getMargin() != btScalar(0.))
{
btVector3 vecnorm = vec;
if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
{
vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
}
vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
}
vecnorm.normalize();
supVertex+= getMargin() * vecnorm;
supVertex += getMargin() * vecnorm;
}
return supVertex;
}
void btConvexHullShape::optimizeConvexHull()
{
btConvexHullComputer conv;
conv.compute(&m_unscaledPoints[0].getX(), sizeof(btVector3),m_unscaledPoints.size(),0.f,0.f);
conv.compute(&m_unscaledPoints[0].getX(), sizeof(btVector3), m_unscaledPoints.size(), 0.f, 0.f);
int numVerts = conv.vertices.size();
m_unscaledPoints.resize(0);
for (int i=0;i<numVerts;i++)
{
m_unscaledPoints.push_back(conv.vertices[i]);
}
for (int i = 0; i < numVerts; i++)
{
m_unscaledPoints.push_back(conv.vertices[i]);
}
}
//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
//Please note that you can debug-draw btConvexHullShape with the Raytracer Demo
int btConvexHullShape::getNumVertices() const
int btConvexHullShape::getNumVertices() const
{
return m_unscaledPoints.size();
}
@@ -149,67 +137,65 @@ int btConvexHullShape::getNumEdges() const
return m_unscaledPoints.size();
}
void btConvexHullShape::getEdge(int i,btVector3& pa,btVector3& pb) const
void btConvexHullShape::getEdge(int i, btVector3& pa, btVector3& pb) const
{
int index0 = i%m_unscaledPoints.size();
int index1 = (i+1)%m_unscaledPoints.size();
int index0 = i % m_unscaledPoints.size();
int index1 = (i + 1) % m_unscaledPoints.size();
pa = getScaledPoint(index0);
pb = getScaledPoint(index1);
}
void btConvexHullShape::getVertex(int i,btVector3& vtx) const
void btConvexHullShape::getVertex(int i, btVector3& vtx) const
{
vtx = getScaledPoint(i);
}
int btConvexHullShape::getNumPlanes() const
int btConvexHullShape::getNumPlanes() const
{
return 0;
}
void btConvexHullShape::getPlane(btVector3& ,btVector3& ,int ) const
void btConvexHullShape::getPlane(btVector3&, btVector3&, int) const
{
btAssert(0);
}
//not yet
bool btConvexHullShape::isInside(const btVector3& ,btScalar ) const
bool btConvexHullShape::isInside(const btVector3&, btScalar) const
{
btAssert(0);
return false;
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* serializer) const
const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* serializer) const
{
//int szc = sizeof(btConvexHullShapeData);
btConvexHullShapeData* shapeData = (btConvexHullShapeData*) dataBuffer;
btConvexHullShapeData* shapeData = (btConvexHullShapeData*)dataBuffer;
btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
int numElem = m_unscaledPoints.size();
shapeData->m_numUnscaledPoints = numElem;
#ifdef BT_USE_DOUBLE_PRECISION
shapeData->m_unscaledPointsFloatPtr = 0;
shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]): 0;
shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]) : 0;
#else
shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]): 0;
shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]) : 0;
shapeData->m_unscaledPointsDoublePtr = 0;
#endif
if (numElem)
{
int sz = sizeof(btVector3Data);
// int sz2 = sizeof(btVector3DoubleData);
// int sz3 = sizeof(btVector3FloatData);
btChunk* chunk = serializer->allocate(sz,numElem);
// int sz2 = sizeof(btVector3DoubleData);
// int sz3 = sizeof(btVector3FloatData);
btChunk* chunk = serializer->allocate(sz, numElem);
btVector3Data* memPtr = (btVector3Data*)chunk->m_oldPtr;
for (int i=0;i<numElem;i++,memPtr++)
for (int i = 0; i < numElem; i++, memPtr++)
{
m_unscaledPoints[i].serialize(*memPtr);
}
serializer->finalizeChunk(chunk,btVector3DataName,BT_ARRAY_CODE,(void*)&m_unscaledPoints[0]);
serializer->finalizeChunk(chunk, btVector3DataName, BT_ARRAY_CODE, (void*)&m_unscaledPoints[0]);
}
// Fill padding with zeros to appease msan.
@@ -218,45 +204,41 @@ const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* seriali
return "btConvexHullShapeData";
}
void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const
{
#if 1
minProj = FLT_MAX;
maxProj = -FLT_MAX;
int numVerts = m_unscaledPoints.size();
for(int i=0;i<numVerts;i++)
for (int i = 0; i < numVerts; i++)
{
btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
btVector3 pt = trans * vtx;
btScalar dp = pt.dot(dir);
if(dp < minProj)
if (dp < minProj)
{
minProj = dp;
witnesPtMin = pt;
}
if(dp > maxProj)
if (dp > maxProj)
{
maxProj = dp;
witnesPtMax=pt;
witnesPtMax = pt;
}
}
#else
btVector3 localAxis = dir*trans.getBasis();
witnesPtMin = trans(localGetSupportingVertex(localAxis));
btVector3 localAxis = dir * trans.getBasis();
witnesPtMin = trans(localGetSupportingVertex(localAxis));
witnesPtMax = trans(localGetSupportingVertex(-localAxis));
minProj = witnesPtMin.dot(dir);
maxProj = witnesPtMax.dot(dir);
#endif
if(minProj>maxProj)
if (minProj > maxProj)
{
btSwap(minProj,maxProj);
btSwap(witnesPtMin,witnesPtMax);
btSwap(minProj, maxProj);
btSwap(witnesPtMin, witnesPtMax);
}
}

61
src/BulletCollision/CollisionShapes/btConvexHullShape.h
View File

@@ -17,28 +17,26 @@ subject to the following restrictions:
#define BT_CONVEX_HULL_SHAPE_H
#include "btPolyhedralConvexShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
#include "LinearMath/btAlignedObjectArray.h"
///The btConvexHullShape implements an implicit convex hull of an array of vertices.
///Bullet provides a general and fast collision detector for convex shapes based on GJK and EPA using localGetSupportingVertex.
ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexAabbCachingShape
ATTRIBUTE_ALIGNED16(class)
btConvexHullShape : public btPolyhedralConvexAabbCachingShape
{
btAlignedObjectArray<btVector3> m_unscaledPoints;
btAlignedObjectArray<btVector3> m_unscaledPoints;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive btScalar (x,y,z), the striding defines the number of bytes between each point, in memory.
///It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint.
///btConvexHullShape make an internal copy of the points.
btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btVector3));
btConvexHullShape(const btScalar* points = 0, int numPoints = 0, int stride = sizeof(btVector3));
void addPoint(const btVector3& point, bool recalculateLocalAabb = true);
btVector3* getUnscaledPoints()
{
return &m_unscaledPoints[0];
@@ -55,48 +53,46 @@ public:
return getUnscaledPoints();
}
void optimizeConvexHull();
SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const
void optimizeConvexHull();
SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const
{
return m_unscaledPoints[i] * m_localScaling;
}
SIMD_FORCE_INLINE int getNumPoints() const
SIMD_FORCE_INLINE int getNumPoints() const
{
return m_unscaledPoints.size();
}
virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const;
//debugging
virtual const char* getName()const {return "Convex";}
virtual const char* getName() const { return "Convex"; }
virtual int getNumVertices() const;
virtual int getNumVertices() const;
virtual int getNumEdges() const;
virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
virtual void getVertex(int i,btVector3& vtx) const;
virtual int getNumPlanes() const;
virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const;
virtual bool isInside(const btVector3& pt,btScalar tolerance) const;
virtual void getEdge(int i, btVector3& pa, btVector3& pb) const;
virtual void getVertex(int i, btVector3& vtx) const;
virtual int getNumPlanes() const;
virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const;
virtual bool isInside(const btVector3& pt, btScalar tolerance) const;
///in case we receive negative scaling
virtual void setLocalScaling(const btVector3& scaling);
virtual void setLocalScaling(const btVector3& scaling);
virtual int calculateSerializeBufferSize() const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
};
// clang-format off
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btConvexHullShapeData
{
@@ -110,12 +106,11 @@ struct btConvexHullShapeData
};
// clang-format on
SIMD_FORCE_INLINE int btConvexHullShape::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btConvexHullShape::calculateSerializeBufferSize() const
{
return sizeof(btConvexHullShapeData);
}
#endif //BT_CONVEX_HULL_SHAPE_H
#endif //BT_CONVEX_HULL_SHAPE_H

124
src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp
View File

@@ -13,139 +13,125 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btConvexInternalShape.h"
btConvexInternalShape::btConvexInternalShape()
: m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
m_collisionMargin(CONVEX_DISTANCE_MARGIN)
: m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.)),
m_collisionMargin(CONVEX_DISTANCE_MARGIN)
{
}
void btConvexInternalShape::setLocalScaling(const btVector3& scaling)
void btConvexInternalShape::setLocalScaling(const btVector3& scaling)
{
m_localScaling = scaling.absolute();
}
void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const
void btConvexInternalShape::getAabbSlow(const btTransform& trans, btVector3& minAabb, btVector3& maxAabb) const
{
#ifndef __SPU__
//use localGetSupportingVertexWithoutMargin?
btScalar margin = getMargin();
for (int i=0;i<3;i++)
for (int i = 0; i < 3; i++)
{
btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.));
vec[i] = btScalar(1.);
btVector3 sv = localGetSupportingVertex(vec*trans.getBasis());
btVector3 sv = localGetSupportingVertex(vec * trans.getBasis());
btVector3 tmp = trans(sv);
maxAabb[i] = tmp[i]+margin;
maxAabb[i] = tmp[i] + margin;
vec[i] = btScalar(-1.);
tmp = trans(localGetSupportingVertex(vec*trans.getBasis()));
minAabb[i] = tmp[i]-margin;
tmp = trans(localGetSupportingVertex(vec * trans.getBasis()));
minAabb[i] = tmp[i] - margin;
}
#endif
}
btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)const
btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec) const
{
#ifndef __SPU__
btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
if ( getMargin()!=btScalar(0.) )
if (getMargin() != btScalar(0.))
{
btVector3 vecnorm = vec;
if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
{
vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
}
vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
}
vecnorm.normalize();
supVertex+= getMargin() * vecnorm;
supVertex += getMargin() * vecnorm;
}
return supVertex;
#else
btAssert(0);
return btVector3(0,0,0);
#endif //__SPU__
}
return btVector3(0, 0, 0);
#endif //__SPU__
}
btConvexInternalAabbCachingShape::btConvexInternalAabbCachingShape()
: btConvexInternalShape(),
m_localAabbMin(1,1,1),
m_localAabbMax(-1,-1,-1),
m_isLocalAabbValid(false)
: btConvexInternalShape(),
m_localAabbMin(1, 1, 1),
m_localAabbMax(-1, -1, -1),
m_isLocalAabbValid(false)
{
}
void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const
{
getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin());
}
void btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling)
void btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling)
{
btConvexInternalShape::setLocalScaling(scaling);
recalcLocalAabb();
}
void btConvexInternalAabbCachingShape::recalcLocalAabb()
void btConvexInternalAabbCachingShape::recalcLocalAabb()
{
m_isLocalAabbValid = true;
#if 1
#if 1
static const btVector3 _directions[] =
{
btVector3( 1., 0., 0.),
btVector3( 0., 1., 0.),
btVector3( 0., 0., 1.),
btVector3( -1., 0., 0.),
btVector3( 0., -1., 0.),
btVector3( 0., 0., -1.)
};
{
btVector3(1., 0., 0.),
btVector3(0., 1., 0.),
btVector3(0., 0., 1.),
btVector3(-1., 0., 0.),
btVector3(0., -1., 0.),
btVector3(0., 0., -1.)};
btVector3 _supporting[] =
{
btVector3( 0., 0., 0.),
btVector3( 0., 0., 0.),
btVector3( 0., 0., 0.),
btVector3( 0., 0., 0.),
btVector3( 0., 0., 0.),
btVector3( 0., 0., 0.)
};
{
btVector3(0., 0., 0.),
btVector3(0., 0., 0.),
btVector3(0., 0., 0.),
btVector3(0., 0., 0.),
btVector3(0., 0., 0.),
btVector3(0., 0., 0.)};
batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
for ( int i = 0; i < 3; ++i )
for (int i = 0; i < 3; ++i)
{
m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
}
#else
for (int i=0;i<3;i++)
#else
for (int i = 0; i < 3; i++)
{
btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.));
vec[i] = btScalar(1.);
btVector3 tmp = localGetSupportingVertex(vec);
m_localAabbMax[i] = tmp[i]+m_collisionMargin;
m_localAabbMax[i] = tmp[i] + m_collisionMargin;
vec[i] = btScalar(-1.);
tmp = localGetSupportingVertex(vec);
m_localAabbMin[i] = tmp[i]-m_collisionMargin;
m_localAabbMin[i] = tmp[i] - m_collisionMargin;
}
#endif
#endif
}

Some files were not shown because too many files have changed in this diff Show More