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

16
src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h
View File

@@ -19,22 +19,20 @@ subject to the following restrictions:
#include "Bullet3Common/b3Vector3.h"
struct b3BroadphaseProxy;
struct b3BroadphaseAabbCallback
struct b3BroadphaseAabbCallback
{
virtual ~b3BroadphaseAabbCallback() {}
virtual bool process(const b3BroadphaseProxy* proxy) = 0;
virtual bool process(const b3BroadphaseProxy* proxy) = 0;
};
struct b3BroadphaseRayCallback : public b3BroadphaseAabbCallback
struct b3BroadphaseRayCallback : public b3BroadphaseAabbCallback
{
///added some cached data to accelerate ray-AABB tests
b3Vector3 m_rayDirectionInverse;
unsigned int m_signs[3];
b3Scalar m_lambda_max;
b3Vector3 m_rayDirectionInverse;
unsigned int m_signs[3];
b3Scalar m_lambda_max;
virtual ~b3BroadphaseRayCallback() {}
};
#endif //B3_BROADPHASE_CALLBACK_H
#endif //B3_BROADPHASE_CALLBACK_H

1419
src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp
View File

File diff suppressed because it is too large Load Diff

1593
src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h
View File

File diff suppressed because it is too large Load Diff

818
src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp
View File

File diff suppressed because it is too large Load Diff

193
src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h
View File

@@ -27,46 +27,43 @@ subject to the following restrictions:
// Compile time config
//
#define B3_DBVT_BP_PROFILE 0
#define B3_DBVT_BP_PROFILE 0
//#define B3_DBVT_BP_SORTPAIRS 1
#define B3_DBVT_BP_PREVENTFALSEUPDATE 0
#define B3_DBVT_BP_ACCURATESLEEPING 0
#define B3_DBVT_BP_ENABLE_BENCHMARK 0
#define B3_DBVT_BP_MARGIN (b3Scalar)0.05
#define B3_DBVT_BP_PREVENTFALSEUPDATE 0
#define B3_DBVT_BP_ACCURATESLEEPING 0
#define B3_DBVT_BP_ENABLE_BENCHMARK 0
#define B3_DBVT_BP_MARGIN (b3Scalar)0.05
#if B3_DBVT_BP_PROFILE
#define B3_DBVT_BP_PROFILING_RATE 256
#define B3_DBVT_BP_PROFILING_RATE 256
#endif
B3_ATTRIBUTE_ALIGNED16(struct) b3BroadphaseProxy
B3_ATTRIBUTE_ALIGNED16(struct)
b3BroadphaseProxy
{
B3_DECLARE_ALIGNED_ALLOCATOR();
B3_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 b3CollisionObject or Rigidbody class
void* m_clientObject;
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.
b3Vector3 m_aabbMin;
b3Vector3 m_aabbMax;
b3Vector3 m_aabbMin;
b3Vector3 m_aabbMax;
B3_FORCE_INLINE int getUid() const
{
@@ -74,116 +71,112 @@ B3_DECLARE_ALIGNED_ALLOCATOR();
}
//used for memory pools
b3BroadphaseProxy() :m_clientObject(0)
b3BroadphaseProxy() : m_clientObject(0)
{
}
b3BroadphaseProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask)
:m_clientObject(userPtr),
m_collisionFilterGroup(collisionFilterGroup),
m_collisionFilterMask(collisionFilterMask),
m_aabbMin(aabbMin),
m_aabbMax(aabbMax)
b3BroadphaseProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask)
: m_clientObject(userPtr),
m_collisionFilterGroup(collisionFilterGroup),
m_collisionFilterMask(collisionFilterMask),
m_aabbMin(aabbMin),
m_aabbMax(aabbMax)
{
}
};
//
// b3DbvtProxy
//
struct b3DbvtProxy : b3BroadphaseProxy
{
/* Fields */
/* Fields */
//b3DbvtAabbMm aabb;
b3DbvtNode* leaf;
b3DbvtProxy* links[2];
int stage;
/* ctor */
b3DbvtNode* leaf;
b3DbvtProxy* links[2];
int stage;
/* ctor */
explicit b3DbvtProxy() {}
b3DbvtProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask) :
b3BroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
b3DbvtProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask) : b3BroadphaseProxy(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask)
{
links[0]=links[1]=0;
links[0] = links[1] = 0;
}
};
typedef b3AlignedObjectArray<b3DbvtProxy*> b3DbvtProxyArray;
typedef b3AlignedObjectArray<b3DbvtProxy*> b3DbvtProxyArray;
///The b3DynamicBvhBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see b3DynamicBvh).
///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 b3AxisSweep3 and b332BitAxisSweep3.
struct b3DynamicBvhBroadphase
struct b3DynamicBvhBroadphase
{
/* 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 */
b3DynamicBvh m_sets[2]; // Dbvt sets
b3DbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list
/* Fields */
b3DynamicBvh m_sets[2]; // Dbvt sets
b3DbvtProxy* m_stageRoots[STAGECOUNT + 1]; // Stages list
b3AlignedObjectArray<b3DbvtProxy> m_proxies;
b3OverlappingPairCache* m_paircache; // Pair cache
b3Scalar 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
b3Scalar m_updates_ratio; // m_updates_done/m_updates_call
int m_pid; // Parse id
int m_cid; // Cleanup index
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?
b3AlignedObjectArray<b3DbvtProxy> m_proxies;
b3OverlappingPairCache* m_paircache; // Pair cache
b3Scalar 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
b3Scalar m_updates_ratio; // m_updates_done/m_updates_call
int m_pid; // Parse id
int m_cid; // Cleanup index
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?
#if B3_DBVT_BP_PROFILE
b3Clock 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;
b3Clock 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 */
b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache=0);
/* Methods */
b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache = 0);
virtual ~b3DynamicBvhBroadphase();
void collide(b3Dispatcher* dispatcher);
void optimize();
void collide(b3Dispatcher* dispatcher);
void optimize();
/* b3BroadphaseInterface Implementation */
b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int objectIndex,void* userPtr, int collisionFilterGroup, int collisionFilterMask);
virtual void destroyProxy(b3BroadphaseProxy* proxy,b3Dispatcher* dispatcher);
virtual void setAabb(int objectId,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* dispatcher);
virtual void rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin=b3MakeVector3(0,0,0), const b3Vector3& aabbMax = b3MakeVector3(0,0,0));
virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int objectIndex, void* userPtr, int collisionFilterGroup, int collisionFilterMask);
virtual void destroyProxy(b3BroadphaseProxy* proxy, b3Dispatcher* dispatcher);
virtual void setAabb(int objectId, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* dispatcher);
virtual void rayTest(const b3Vector3& rayFrom, const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin = b3MakeVector3(0, 0, 0), const b3Vector3& aabbMax = b3MakeVector3(0, 0, 0));
virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
//virtual void getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
virtual void getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
virtual void calculateOverlappingPairs(b3Dispatcher* dispatcher=0);
virtual b3OverlappingPairCache* getOverlappingPairCache();
virtual const b3OverlappingPairCache* getOverlappingPairCache() const;
virtual void getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const;
virtual void printStats();
virtual void getAabb(int objectId, b3Vector3& aabbMin, b3Vector3& aabbMax) const;
virtual void calculateOverlappingPairs(b3Dispatcher* dispatcher = 0);
virtual b3OverlappingPairCache* getOverlappingPairCache();
virtual const b3OverlappingPairCache* getOverlappingPairCache() const;
virtual void getBroadphaseAabb(b3Vector3& aabbMin, b3Vector3& aabbMax) const;
virtual void printStats();
///reset broadphase internal structures, to ensure determinism/reproducability
virtual void resetPool(b3Dispatcher* dispatcher);
void performDeferredRemoval(b3Dispatcher* dispatcher);
void setVelocityPrediction(b3Scalar prediction)
void performDeferredRemoval(b3Dispatcher* dispatcher);
void setVelocityPrediction(b3Scalar prediction)
{
m_prediction = prediction;
}
@@ -192,15 +185,13 @@ struct b3DynamicBvhBroadphase
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 b3BroadphaseInterface but specific to b3DynamicBvhBroadphase.
///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( b3BroadphaseProxy* absproxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* /*dispatcher*/);
void setAabbForceUpdate(b3BroadphaseProxy* absproxy, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* /*dispatcher*/);
//static void benchmark(b3BroadphaseInterface*);
};
#endif

44
src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h
View File

@@ -23,20 +23,20 @@ subject to the following restrictions:
typedef b3Int4 b3BroadphasePair;
inline b3Int4 b3MakeBroadphasePair(int xx,int yy)
inline b3Int4 b3MakeBroadphasePair(int xx, int yy)
{
b3Int4 pair;
if (xx < yy)
{
pair.x = xx;
pair.y = yy;
}
else
{
{
pair.x = xx;
pair.y = yy;
}
else
{
pair.x = yy;
pair.y = xx;
}
pair.y = xx;
}
pair.z = B3_NEW_PAIR_MARKER;
pair.w = B3_NEW_PAIR_MARKER;
return pair;
@@ -51,22 +51,20 @@ inline b3Int4 b3MakeBroadphasePair(int xx,int yy)
class b3BroadphasePairSortPredicate
{
public:
bool operator() ( const b3BroadphasePair& a, const b3BroadphasePair& b ) const
{
const int uidA0 = a.x;
const int uidB0 = b.x;
const int uidA1 = a.y;
const int uidB1 = b.y;
return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
}
public:
bool operator()(const b3BroadphasePair& a, const b3BroadphasePair& b) const
{
const int uidA0 = a.x;
const int uidB0 = b.x;
const int uidA1 = a.y;
const int uidB1 = b.y;
return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
}
};
B3_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePair& b)
B3_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePair& b)
{
return (a.x == b.x ) && (a.y == b.y );
return (a.x == b.x) && (a.y == b.y);
}
#endif //B3_OVERLAPPING_PAIR_H
#endif //B3_OVERLAPPING_PAIR_H

301
src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.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 "b3OverlappingPairCache.h"
//#include "b3Dispatcher.h"
@@ -23,35 +21,26 @@ subject to the following restrictions:
#include <stdio.h>
int b3g_overlappingPairs = 0;
int b3g_removePairs =0;
int b3g_addedPairs =0;
int b3g_findPairs =0;
int b3g_overlappingPairs = 0;
int b3g_removePairs = 0;
int b3g_addedPairs = 0;
int b3g_findPairs = 0;
b3HashedOverlappingPairCache::b3HashedOverlappingPairCache():
m_overlapFilterCallback(0)
b3HashedOverlappingPairCache::b3HashedOverlappingPairCache() : m_overlapFilterCallback(0)
//, m_blockedForChanges(false)
{
int initialAllocatedSize= 2;
int initialAllocatedSize = 2;
m_overlappingPairArray.reserve(initialAllocatedSize);
growTables();
}
b3HashedOverlappingPairCache::~b3HashedOverlappingPairCache()
{
}
void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher)
void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher)
{
/* if (pair.m_algorithm)
/* if (pair.m_algorithm)
{
{
pair.m_algorithm->~b3CollisionAlgorithm();
@@ -60,91 +49,74 @@ void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b
}
}
*/
}
void b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher)
void b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
{
class CleanPairCallback : public b3OverlapCallback
class CleanPairCallback : public b3OverlapCallback
{
int m_cleanProxy;
b3OverlappingPairCache* m_pairCache;
b3OverlappingPairCache* m_pairCache;
b3Dispatcher* m_dispatcher;
public:
CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher)
:m_cleanProxy(cleanProxy),
m_pairCache(pairCache),
m_dispatcher(dispatcher)
CleanPairCallback(int cleanProxy, b3OverlappingPairCache* pairCache, b3Dispatcher* dispatcher)
: m_cleanProxy(cleanProxy),
m_pairCache(pairCache),
m_dispatcher(dispatcher)
{
}
virtual bool processOverlap(b3BroadphasePair& pair)
virtual bool processOverlap(b3BroadphasePair& pair)
{
if ((pair.x == m_cleanProxy) ||
(pair.y == 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 b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher)
void b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
{
class RemovePairCallback : public b3OverlapCallback
class RemovePairCallback : public b3OverlapCallback
{
int m_obsoleteProxy;
public:
RemovePairCallback(int obsoleteProxy)
:m_obsoleteProxy(obsoleteProxy)
: m_obsoleteProxy(obsoleteProxy)
{
}
virtual bool processOverlap(b3BroadphasePair& pair)
virtual bool processOverlap(b3BroadphasePair& pair)
{
return ((pair.x == m_obsoleteProxy) ||
(pair.y == m_obsoleteProxy));
(pair.y == m_obsoleteProxy));
}
};
RemovePairCallback removeCallback(proxy);
processAllOverlappingPairs(&removeCallback,dispatcher);
processAllOverlappingPairs(&removeCallback, dispatcher);
}
b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1)
{
b3g_findPairs++;
if(proxy0 >proxy1)
b3Swap(proxy0,proxy1);
if (proxy0 > proxy1)
b3Swap(proxy0, proxy1);
int proxyId1 = proxy0;
int proxyId2 = proxy1;
/*if (proxyId1 > proxyId2)
b3Swap(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())
{
@@ -169,9 +141,8 @@ b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1)
//#include <stdio.h>
void b3HashedOverlappingPairCache::growTables()
void b3HashedOverlappingPairCache::growTables()
{
int newCapacity = m_overlappingPairArray.capacity();
if (m_hashTable.size() < newCapacity)
@@ -182,10 +153,9 @@ void b3HashedOverlappingPairCache::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] = B3_NULL_PAIR;
}
@@ -194,35 +164,31 @@ void b3HashedOverlappingPairCache::growTables()
m_next[i] = B3_NULL_PAIR;
}
for(i=0;i<curHashtableSize;i++)
for (i = 0; i < curHashtableSize; i++)
{
const b3BroadphasePair& pair = m_overlappingPairArray[i];
int proxyId1 = pair.x;
int proxyId2 = pair.y;
/*if (proxyId1 > proxyId2)
b3Swap(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;
}
}
}
b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1)
{
if(proxy0>proxy1)
b3Swap(proxy0,proxy1);
if (proxy0 > proxy1)
b3Swap(proxy0, proxy1);
int proxyId1 = proxy0;
int proxyId2 = proxy1;
/*if (proxyId1 > proxyId2)
b3Swap(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
b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
if (pair != NULL)
@@ -243,8 +209,8 @@ b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int
pair = &m_overlappingPairArray.expandNonInitializing();
//this is where we add an actual pair, so also call the 'ghost'
// if (m_ghostPairCallback)
// m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
// if (m_ghostPairCallback)
// m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
int newCapacity = m_overlappingPairArray.capacity();
@@ -252,16 +218,15 @@ b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int
{
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 = b3MakeBroadphasePair(proxy0,proxy1);
// pair->m_pProxy0 = proxy0;
// pair->m_pProxy1 = proxy1;
*pair = b3MakeBroadphasePair(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;
@@ -269,20 +234,18 @@ b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int
return pair;
}
void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1,b3Dispatcher* dispatcher)
void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
{
b3g_removePairs++;
if(proxy0>proxy1)
b3Swap(proxy0,proxy1);
if (proxy0 > proxy1)
b3Swap(proxy0, proxy1);
int proxyId1 = proxy0;
int proxyId2 = proxy1;
/*if (proxyId1 > proxyId2)
b3Swap(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));
b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
if (pair == NULL)
@@ -290,9 +253,7 @@ void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1
return 0;
}
cleanOverlappingPair(*pair,dispatcher);
cleanOverlappingPair(*pair, dispatcher);
int pairIndex = int(pair - &m_overlappingPairArray[0]);
b3Assert(pairIndex < m_overlappingPairArray.size());
@@ -336,8 +297,8 @@ void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1
// Remove the last pair from the hash table.
const b3BroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
/* missing swap here too, Nat. */
int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->x), static_cast<unsigned int>(last->y)) & (m_overlappingPairArray.capacity()-1));
/* missing swap here too, Nat. */
int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->x), static_cast<unsigned int>(last->y)) & (m_overlappingPairArray.capacity() - 1));
index = m_hashTable[lastHash];
b3Assert(index != B3_NULL_PAIR);
@@ -372,47 +333,42 @@ void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1
}
//#include <stdio.h>
void b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher)
void b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
{
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();)
{
b3BroadphasePair* pair = &m_overlappingPairArray[i];
if (callback->processOverlap(*pair))
{
removeOverlappingPair(pair->x,pair->y,dispatcher);
removeOverlappingPair(pair->x, pair->y, dispatcher);
b3g_overlappingPairs--;
} else
}
else
{
i++;
}
}
}
void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
{
///need to keep hashmap in sync with pair address, so rebuild all
b3BroadphasePairArray 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].x,tmpPairs[i].y,dispatcher);
removeOverlappingPair(tmpPairs[i].x, tmpPairs[i].y, dispatcher);
}
for (i = 0; i < m_next.size(); i++)
{
m_next[i] = B3_NULL_PAIR;
@@ -420,33 +376,29 @@ void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher
tmpPairs.quickSort(b3BroadphasePairSortPredicate());
for (i=0;i<tmpPairs.size();i++)
for (i = 0; i < tmpPairs.size(); i++)
{
addOverlappingPair(tmpPairs[i].x ,tmpPairs[i].y);
addOverlappingPair(tmpPairs[i].x, tmpPairs[i].y);
}
}
void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0,int proxy1, b3Dispatcher* dispatcher )
void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
{
if (!hasDeferredRemoval())
{
b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0,proxy1);
b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0, proxy1);
int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
if (findIndex < m_overlappingPairArray.size())
{
b3g_overlappingPairs--;
b3BroadphasePair& pair = m_overlappingPairArray[findIndex];
cleanOverlappingPair(pair,dispatcher);
cleanOverlappingPair(pair, dispatcher);
//if (m_ghostPairCallback)
// m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1);
m_overlappingPairArray.swap(findIndex, m_overlappingPairArray.capacity() - 1);
m_overlappingPairArray.pop_back();
return 0;
}
@@ -455,100 +407,77 @@ void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0,int proxy1,
return 0;
}
b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0,int proxy1)
b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0, int proxy1)
{
//don't add overlap with own
b3Assert(proxy0 != proxy1);
if (!needsBroadphaseCollision(proxy0,proxy1))
if (!needsBroadphaseCollision(proxy0, proxy1))
return 0;
b3BroadphasePair* pair = &m_overlappingPairArray.expandNonInitializing();
*pair = b3MakeBroadphasePair(proxy0,proxy1);
*pair = b3MakeBroadphasePair(proxy0, proxy1);
b3g_overlappingPairs++;
b3g_addedPairs++;
// if (m_ghostPairCallback)
// m_ghostPairCallback->addOverlappingPair(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
b3BroadphasePair* b3SortedOverlappingPairCache::findPair(int proxy0,int proxy1)
b3BroadphasePair* b3SortedOverlappingPairCache::findPair(int proxy0, int proxy1)
{
if (!needsBroadphaseCollision(proxy0,proxy1))
if (!needsBroadphaseCollision(proxy0, proxy1))
return 0;
b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0,proxy1);
b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0, proxy1);
int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
if (findIndex < m_overlappingPairArray.size())
{
//b3Assert(it != m_overlappingPairSet.end());
b3BroadphasePair* pair = &m_overlappingPairArray[findIndex];
b3BroadphasePair* pair = &m_overlappingPairArray[findIndex];
return pair;
}
return 0;
}
//#include <stdio.h>
void b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher)
void b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
{
int i;
for (i=0;i<m_overlappingPairArray.size();)
for (i = 0; i < m_overlappingPairArray.size();)
{
b3BroadphasePair* pair = &m_overlappingPairArray[i];
if (callback->processOverlap(*pair))
{
cleanOverlappingPair(*pair,dispatcher);
cleanOverlappingPair(*pair, dispatcher);
pair->x = -1;
pair->y = -1;
m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1);
m_overlappingPairArray.pop_back();
b3g_overlappingPairs--;
} else
}
else
{
i++;
}
}
}
b3SortedOverlappingPairCache::b3SortedOverlappingPairCache():
m_blockedForChanges(false),
m_hasDeferredRemoval(true),
m_overlapFilterCallback(0)
b3SortedOverlappingPairCache::b3SortedOverlappingPairCache() : m_blockedForChanges(false),
m_hasDeferredRemoval(true),
m_overlapFilterCallback(0)
{
int initialAllocatedSize= 2;
int initialAllocatedSize = 2;
m_overlappingPairArray.reserve(initialAllocatedSize);
}
@@ -556,9 +485,9 @@ b3SortedOverlappingPairCache::~b3SortedOverlappingPairCache()
{
}
void b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher)
void b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher)
{
/* if (pair.m_algorithm)
/* if (pair.m_algorithm)
{
{
pair.m_algorithm->~b3CollisionAlgorithm();
@@ -570,69 +499,61 @@ void b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b
*/
}
void b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher)
void b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
{
class CleanPairCallback : public b3OverlapCallback
class CleanPairCallback : public b3OverlapCallback
{
int m_cleanProxy;
b3OverlappingPairCache* m_pairCache;
b3OverlappingPairCache* m_pairCache;
b3Dispatcher* m_dispatcher;
public:
CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher)
:m_cleanProxy(cleanProxy),
m_pairCache(pairCache),
m_dispatcher(dispatcher)
CleanPairCallback(int cleanProxy, b3OverlappingPairCache* pairCache, b3Dispatcher* dispatcher)
: m_cleanProxy(cleanProxy),
m_pairCache(pairCache),
m_dispatcher(dispatcher)
{
}
virtual bool processOverlap(b3BroadphasePair& pair)
virtual bool processOverlap(b3BroadphasePair& pair)
{
if ((pair.x == m_cleanProxy) ||
(pair.y == 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 b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher)
void b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
{
class RemovePairCallback : public b3OverlapCallback
class RemovePairCallback : public b3OverlapCallback
{
int m_obsoleteProxy;
public:
RemovePairCallback(int obsoleteProxy)
:m_obsoleteProxy(obsoleteProxy)
: m_obsoleteProxy(obsoleteProxy)
{
}
virtual bool processOverlap(b3BroadphasePair& pair)
virtual bool processOverlap(b3BroadphasePair& pair)
{
return ((pair.x == m_obsoleteProxy) ||
(pair.y == m_obsoleteProxy));
(pair.y == m_obsoleteProxy));
}
};
RemovePairCallback removeCallback(proxy);
processAllOverlappingPairs(&removeCallback,dispatcher);
processAllOverlappingPairs(&removeCallback, dispatcher);
}
void b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
void b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
{
//should already be sorted
}

363
src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h
View File

@@ -22,152 +22,136 @@ subject to the following restrictions:
class b3Dispatcher;
#include "b3OverlappingPair.h"
typedef b3AlignedObjectArray<b3BroadphasePair> b3BroadphasePairArray;
typedef b3AlignedObjectArray<b3BroadphasePair> b3BroadphasePairArray;
struct b3OverlapCallback
struct b3OverlapCallback
{
virtual ~b3OverlapCallback()
{}
{
}
//return true for deletion of the pair
virtual bool processOverlap(b3BroadphasePair& pair) = 0;
virtual bool processOverlap(b3BroadphasePair& pair) = 0;
};
struct b3OverlapFilterCallback
{
virtual ~b3OverlapFilterCallback()
{}
{
}
// return true when pairs need collision
virtual bool needBroadphaseCollision(int proxy0,int proxy1) const = 0;
virtual bool needBroadphaseCollision(int proxy0, int proxy1) const = 0;
};
extern int b3g_removePairs;
extern int b3g_addedPairs;
extern int b3g_findPairs;
const int B3_NULL_PAIR=0xffffffff;
const int B3_NULL_PAIR = 0xffffffff;
///The b3OverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the b3BroadphaseInterface broadphases.
///The b3HashedOverlappingPairCache and b3SortedOverlappingPairCache classes are two implementations.
class b3OverlappingPairCache
class b3OverlappingPairCache
{
public:
virtual ~b3OverlappingPairCache() {} // this is needed so we can get to the derived class destructor
virtual ~b3OverlappingPairCache() {} // this is needed so we can get to the derived class destructor
virtual b3BroadphasePair* getOverlappingPairArrayPtr() = 0;
virtual const b3BroadphasePair* getOverlappingPairArrayPtr() const = 0;
virtual b3BroadphasePair* getOverlappingPairArrayPtr() = 0;
virtual b3BroadphasePairArray& getOverlappingPairArray() = 0;
virtual const b3BroadphasePair* getOverlappingPairArrayPtr() const = 0;
virtual void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher) = 0;
virtual b3BroadphasePairArray& getOverlappingPairArray() = 0;
virtual void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher) = 0;
virtual int getNumOverlappingPairs() const = 0;
virtual void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher) = 0;
virtual void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher) = 0;
virtual void setOverlapFilterCallback(b3OverlapFilterCallback* callback) = 0;
virtual void setOverlapFilterCallback(b3OverlapFilterCallback* callback) = 0;
virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher) = 0;
virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher) = 0;
virtual b3BroadphasePair* findPair(int proxy0, int proxy1) = 0;
virtual bool hasDeferredRemoval() = 0;
virtual bool hasDeferredRemoval() = 0;
//virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)=0;
virtual b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1)=0;
virtual void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher)=0;
virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/)=0;
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher) = 0;
virtual b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1) = 0;
virtual void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher) = 0;
virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/, b3Dispatcher* /*dispatcher*/) = 0;
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher) = 0;
};
/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
class b3HashedOverlappingPairCache : public b3OverlappingPairCache
{
b3BroadphasePairArray m_overlappingPairArray;
b3BroadphasePairArray m_overlappingPairArray;
b3OverlapFilterCallback* m_overlapFilterCallback;
// bool m_blockedForChanges;
// bool m_blockedForChanges;
public:
b3HashedOverlappingPairCache();
virtual ~b3HashedOverlappingPairCache();
virtual void removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher);
virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
virtual void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher);
B3_FORCE_INLINE bool needsBroadphaseCollision(int proxy0,int proxy1) const
virtual void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher);
B3_FORCE_INLINE bool needsBroadphaseCollision(int proxy0, int proxy1) const
{
if (m_overlapFilterCallback)
return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
bool collides = true; //(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 b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1)
virtual b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1)
{
b3g_addedPairs++;
if (!needsBroadphaseCollision(proxy0,proxy1))
if (!needsBroadphaseCollision(proxy0, proxy1))
return 0;
return internalAddPair(proxy0,proxy1);
return internalAddPair(proxy0, proxy1);
}
void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher);
virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher);
virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher);
virtual b3BroadphasePair* getOverlappingPairArrayPtr()
virtual b3BroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
}
const b3BroadphasePair* getOverlappingPairArrayPtr() const
const b3BroadphasePair* getOverlappingPairArrayPtr() const
{
return &m_overlappingPairArray[0];
}
b3BroadphasePairArray& getOverlappingPairArray()
b3BroadphasePairArray& getOverlappingPairArray()
{
return m_overlappingPairArray;
}
const b3BroadphasePairArray& getOverlappingPairArray() const
const b3BroadphasePairArray& getOverlappingPairArray() const
{
return m_overlappingPairArray;
}
void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher);
void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher);
b3BroadphasePair* findPair(int proxy0, int proxy1);
int GetCount() const { return m_overlappingPairArray.size(); }
// b3BroadphasePair* GetPairs() { return m_pairs; }
// b3BroadphasePair* GetPairs() { return m_pairs; }
b3OverlapFilterCallback* getOverlapFilterCallback()
{
@@ -179,19 +163,19 @@ public:
m_overlapFilterCallback = callback;
}
int getNumOverlappingPairs() const
int getNumOverlappingPairs() const
{
return m_overlappingPairArray.size();
}
private:
b3BroadphasePair* internalAddPair(int proxy0,int proxy1);
void growTables();
private:
b3BroadphasePair* internalAddPair(int proxy0, int proxy1);
void growTables();
B3_FORCE_INLINE bool equalsPair(const b3BroadphasePair& pair, int proxyId1, int proxyId2)
{
return pair.x == proxyId1 && pair.y == proxyId2;
{
return pair.x == proxyId1 && pair.y == proxyId2;
}
/*
@@ -210,43 +194,37 @@ private:
}
*/
B3_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
B3_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
{
int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16));
int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)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 static_cast<unsigned int>(key);
}
B3_FORCE_INLINE b3BroadphasePair* internalFindPair(int proxy0, int proxy1, int hash)
{
int proxyId1 = proxy0;
int proxyId2 = proxy1;
#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)
b3Swap(proxyId1, proxyId2);
#endif
#endif
int index = m_hashTable[hash];
while( index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
{
index = m_next[index];
}
if ( index == B3_NULL_PAIR )
if (index == B3_NULL_PAIR)
{
return NULL;
}
@@ -256,161 +234,142 @@ private:
return &m_overlappingPairArray[index];
}
virtual bool hasDeferredRemoval()
virtual bool hasDeferredRemoval()
{
return false;
}
/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
{
m_ghostPairCallback = ghostPairCallback;
}
*/
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
protected:
b3AlignedObjectArray<int> m_hashTable;
b3AlignedObjectArray<int> m_next;
// b3OverlappingPairCallback* m_ghostPairCallback;
b3AlignedObjectArray<int> m_hashTable;
b3AlignedObjectArray<int> m_next;
// b3OverlappingPairCallback* m_ghostPairCallback;
};
///b3SortedOverlappingPairCache maintains the objects with overlapping AABB
///Typically managed by the Broadphase, Axis3Sweep or b3SimpleBroadphase
class b3SortedOverlappingPairCache : public b3OverlappingPairCache
class b3SortedOverlappingPairCache : public b3OverlappingPairCache
{
protected:
//avoid brute-force finding all the time
b3BroadphasePairArray m_overlappingPairArray;
protected:
//avoid brute-force finding all the time
b3BroadphasePairArray 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
b3OverlapFilterCallback* m_overlapFilterCallback;
///by default, do the removal during the pair traversal
bool m_hasDeferredRemoval;
// b3OverlappingPairCallback* m_ghostPairCallback;
//if set, use the callback instead of the built in filter in needBroadphaseCollision
b3OverlapFilterCallback* m_overlapFilterCallback;
public:
b3SortedOverlappingPairCache();
virtual ~b3SortedOverlappingPairCache();
// b3OverlappingPairCallback* m_ghostPairCallback;
virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher);
public:
b3SortedOverlappingPairCache();
virtual ~b3SortedOverlappingPairCache();
void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher);
virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher);
void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher);
b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1);
void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher);
b3BroadphasePair* findPair(int proxy0,int proxy1);
void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher);
void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher);
virtual void removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher);
b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1);
b3BroadphasePair* findPair(int proxy0, int proxy1);
inline bool needsBroadphaseCollision(int proxy0,int proxy1) const
{
if (m_overlapFilterCallback)
return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
return collides;
}
b3BroadphasePairArray& getOverlappingPairArray()
{
return m_overlappingPairArray;
}
virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
const b3BroadphasePairArray& getOverlappingPairArray() const
{
return m_overlappingPairArray;
}
inline bool needsBroadphaseCollision(int proxy0, int proxy1) const
{
if (m_overlapFilterCallback)
return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
bool collides = true; //(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
return collides;
}
b3BroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
}
b3BroadphasePairArray& getOverlappingPairArray()
{
return m_overlappingPairArray;
}
const b3BroadphasePair* getOverlappingPairArrayPtr() const
{
return &m_overlappingPairArray[0];
}
const b3BroadphasePairArray& getOverlappingPairArray() const
{
return m_overlappingPairArray;
}
int getNumOverlappingPairs() const
{
return m_overlappingPairArray.size();
}
b3OverlapFilterCallback* getOverlapFilterCallback()
{
return m_overlapFilterCallback;
}
b3BroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
}
void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
{
m_overlapFilterCallback = callback;
}
const b3BroadphasePair* getOverlappingPairArrayPtr() const
{
return &m_overlappingPairArray[0];
}
virtual bool hasDeferredRemoval()
{
return m_hasDeferredRemoval;
}
int getNumOverlappingPairs() const
{
return m_overlappingPairArray.size();
}
/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
b3OverlapFilterCallback* getOverlapFilterCallback()
{
return m_overlapFilterCallback;
}
void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
{
m_overlapFilterCallback = callback;
}
virtual bool hasDeferredRemoval()
{
return m_hasDeferredRemoval;
}
/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
{
m_ghostPairCallback = ghostPairCallback;
}
*/
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
};
///b3NullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
class b3NullPairCache : public b3OverlappingPairCache
{
b3BroadphasePairArray m_overlappingPairArray;
b3BroadphasePairArray m_overlappingPairArray;
public:
virtual b3BroadphasePair* getOverlappingPairArrayPtr()
virtual b3BroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
}
const b3BroadphasePair* getOverlappingPairArrayPtr() const
const b3BroadphasePair* getOverlappingPairArrayPtr() const
{
return &m_overlappingPairArray[0];
}
b3BroadphasePairArray& getOverlappingPairArray()
b3BroadphasePairArray& getOverlappingPairArray()
{
return m_overlappingPairArray;
}
virtual void cleanOverlappingPair(b3BroadphasePair& /*pair*/,b3Dispatcher* /*dispatcher*/)
{
virtual void cleanOverlappingPair(b3BroadphasePair& /*pair*/, b3Dispatcher* /*dispatcher*/)
{
}
virtual int getNumOverlappingPairs() const
@@ -418,16 +377,15 @@ public:
return 0;
}
virtual void cleanProxyFromPairs(int /*proxy*/,b3Dispatcher* /*dispatcher*/)
{
}
virtual void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/)
virtual void cleanProxyFromPairs(int /*proxy*/, b3Dispatcher* /*dispatcher*/)
{
}
virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* /*dispatcher*/)
virtual void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/)
{
}
virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* /*dispatcher*/)
{
}
@@ -436,39 +394,34 @@ public:
return 0;
}
virtual bool hasDeferredRemoval()
virtual bool hasDeferredRemoval()
{
return true;
}
// virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* /* ghostPairCallback */)
// {
//
// }
// virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* /* ghostPairCallback */)
// {
//
// }
virtual b3BroadphasePair* addOverlappingPair(int /*proxy0*/,int /*proxy1*/)
virtual b3BroadphasePair* addOverlappingPair(int /*proxy0*/, int /*proxy1*/)
{
return 0;
}
virtual void* removeOverlappingPair(int /*proxy0*/,int /*proxy1*/,b3Dispatcher* /*dispatcher*/)
virtual void* removeOverlappingPair(int /*proxy0*/, int /*proxy1*/, b3Dispatcher* /*dispatcher*/)
{
return 0;
}
virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/)
virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/, b3Dispatcher* /*dispatcher*/)
{
}
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher)
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher)
{
(void) dispatcher;
(void)dispatcher;
}
};
#endif //B3_OVERLAPPING_PAIR_CACHE_H
#endif //B3_OVERLAPPING_PAIR_CACHE_H

51
src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h
View File

@@ -2,7 +2,6 @@
#ifndef B3_AABB_H
#define B3_AABB_H
#include "Bullet3Common/shared/b3Float4.h"
#include "Bullet3Common/shared/b3Mat3x3.h"
@@ -10,44 +9,42 @@ typedef struct b3Aabb b3Aabb_t;
struct b3Aabb
{
union
{
union {
float m_min[4];
b3Float4 m_minVec;
int m_minIndices[4];
};
union
{
float m_max[4];
union {
float m_max[4];
b3Float4 m_maxVec;
int m_signedMaxIndices[4];
};
};
inline void b3TransformAabb2(b3Float4ConstArg localAabbMin,b3Float4ConstArg localAabbMax, float margin,
b3Float4ConstArg pos,
b3QuatConstArg orn,
b3Float4* aabbMinOut,b3Float4* aabbMaxOut)
inline void b3TransformAabb2(b3Float4ConstArg localAabbMin, b3Float4ConstArg localAabbMax, float margin,
b3Float4ConstArg pos,
b3QuatConstArg orn,
b3Float4* aabbMinOut, b3Float4* aabbMaxOut)
{
b3Float4 localHalfExtents = 0.5f*(localAabbMax-localAabbMin);
localHalfExtents+=b3MakeFloat4(margin,margin,margin,0.f);
b3Float4 localCenter = 0.5f*(localAabbMax+localAabbMin);
b3Mat3x3 m;
m = b3QuatGetRotationMatrix(orn);
b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);
b3Float4 center = b3TransformPoint(localCenter,pos,orn);
b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents,b3GetRow(abs_b,0)),
b3Dot3F4(localHalfExtents,b3GetRow(abs_b,1)),
b3Dot3F4(localHalfExtents,b3GetRow(abs_b,2)),
0.f);
*aabbMinOut = center-extent;
*aabbMaxOut = center+extent;
b3Float4 localHalfExtents = 0.5f * (localAabbMax - localAabbMin);
localHalfExtents += b3MakeFloat4(margin, margin, margin, 0.f);
b3Float4 localCenter = 0.5f * (localAabbMax + localAabbMin);
b3Mat3x3 m;
m = b3QuatGetRotationMatrix(orn);
b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);
b3Float4 center = b3TransformPoint(localCenter, pos, orn);
b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 0)),
b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 1)),
b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 2)),
0.f);
*aabbMinOut = center - extent;
*aabbMaxOut = center + extent;
}
/// conservative test for overlap between two aabbs
inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1,b3Float4ConstArg aabbMax1,
b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)
inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1, b3Float4ConstArg aabbMax1,
b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)
{
bool overlap = true;
overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap;
@@ -56,4 +53,4 @@ inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1,b3Float4ConstArg aab
return overlap;
}
#endif //B3_AABB_H
#endif //B3_AABB_H

40
src/Bullet3Collision/NarrowPhaseCollision/b3Config.h
View File

@@ -1,41 +1,39 @@
#ifndef B3_CONFIG_H
#define B3_CONFIG_H
struct b3Config
struct b3Config
{
int m_maxConvexBodies;
int m_maxConvexShapes;
int m_maxBroadphasePairs;
int m_maxConvexBodies;
int m_maxConvexShapes;
int m_maxBroadphasePairs;
int m_maxContactCapacity;
int m_compoundPairCapacity;
int m_maxVerticesPerFace;
int m_maxFacesPerShape;
int m_maxConvexVertices;
int m_maxConvexVertices;
int m_maxConvexIndices;
int m_maxConvexUniqueEdges;
int m_maxCompoundChildShapes;
int m_maxCompoundChildShapes;
int m_maxTriConvexPairCapacity;
b3Config()
:m_maxConvexBodies(128*1024),
m_maxVerticesPerFace(64),
m_maxFacesPerShape(12),
m_maxConvexVertices(8192),
m_maxConvexIndices(81920),
m_maxConvexUniqueEdges(8192),
m_maxCompoundChildShapes(8192),
m_maxTriConvexPairCapacity(256*1024)
: m_maxConvexBodies(128 * 1024),
m_maxVerticesPerFace(64),
m_maxFacesPerShape(12),
m_maxConvexVertices(8192),
m_maxConvexIndices(81920),
m_maxConvexUniqueEdges(8192),
m_maxCompoundChildShapes(8192),
m_maxTriConvexPairCapacity(256 * 1024)
{
m_maxConvexShapes = m_maxConvexBodies;
m_maxBroadphasePairs = 16*m_maxConvexBodies;
m_maxBroadphasePairs = 16 * m_maxConvexBodies;
m_maxContactCapacity = m_maxBroadphasePairs;
m_compoundPairCapacity = 1024*1024;
m_compoundPairCapacity = 1024 * 1024;
}
};
#endif//B3_CONFIG_H
#endif //B3_CONFIG_H

33
src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h
View File

@@ -19,28 +19,37 @@ subject to the following restrictions:
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
B3_ATTRIBUTE_ALIGNED16(struct) b3Contact4 : public b3Contact4Data
B3_ATTRIBUTE_ALIGNED16(struct)
b3Contact4 : public b3Contact4Data
{
B3_DECLARE_ALIGNED_ALLOCATOR();
int getBodyA()const {return abs(m_bodyAPtrAndSignBit);}
int getBodyB()const {return abs(m_bodyBPtrAndSignBit);}
bool isBodyAFixed()const { return m_bodyAPtrAndSignBit<0;}
bool isBodyBFixed()const { return m_bodyBPtrAndSignBit<0;}
int getBodyA() const { return abs(m_bodyAPtrAndSignBit); }
int getBodyB() const { return abs(m_bodyBPtrAndSignBit); }
bool isBodyAFixed() const { return m_bodyAPtrAndSignBit < 0; }
bool isBodyBFixed() const { return m_bodyBPtrAndSignBit < 0; }
// todo. make it safer
int& getBatchIdx() { return m_batchIdx; }
const int& getBatchIdx() const { return m_batchIdx; }
float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp/(float)0xffff); }
void setRestituitionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_restituitionCoeffCmp = (unsigned short)(c*0xffff); }
float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp/(float)0xffff); }
void setFrictionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_frictionCoeffCmp = (unsigned short)(c*0xffff); }
float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp / (float)0xffff); }
void setRestituitionCoeff(float c)
{
b3Assert(c >= 0.f && c <= 1.f);
m_restituitionCoeffCmp = (unsigned short)(c * 0xffff);
}
float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp / (float)0xffff); }
void setFrictionCoeff(float c)
{
b3Assert(c >= 0.f && c <= 1.f);
m_frictionCoeffCmp = (unsigned short)(c * 0xffff);
}
//float& getNPoints() { return m_worldNormal[3]; }
int getNPoints() const { return (int) m_worldNormalOnB.w; }
int getNPoints() const { return (int)m_worldNormalOnB.w; }
float getPenetration(int idx) const { return m_worldPosB[idx].w; }
bool isInvalid() const { return (getBodyA()==0 || getBodyB()==0); }
bool isInvalid() const { return (getBodyA() == 0 || getBodyB() == 0); }
};
#endif //B3_CONTACT4_H
#endif //B3_CONTACT4_H

312
src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp
View File

@@ -13,52 +13,42 @@ subject to the following restrictions:
*/
//Originally written by Erwin Coumans
#include "b3ConvexUtility.h"
#include "Bullet3Geometry/b3ConvexHullComputer.h"
#include "Bullet3Geometry/b3GrahamScan2dConvexHull.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3HashMap.h"
b3ConvexUtility::~b3ConvexUtility()
{
}
bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
{
b3ConvexHullComputer conv;
conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3),numPoints,0.f,0.f);
conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3), numPoints, 0.f, 0.f);
b3AlignedObjectArray<b3Vector3> faceNormals;
int numFaces = conv.faces.size();
faceNormals.resize(numFaces);
b3ConvexHullComputer* convexUtil = &conv;
b3AlignedObjectArray<b3MyFace> tmpFaces;
b3AlignedObjectArray<b3MyFace> tmpFaces;
tmpFaces.resize(numFaces);
int numVertices = convexUtil->vertices.size();
m_vertices.resize(numVertices);
for (int p=0;p<numVertices;p++)
for (int p = 0; p < numVertices; p++)
{
m_vertices[p] = convexUtil->vertices[p];
}
for (int i=0;i<numFaces;i++)
for (int i = 0; i < numFaces; i++)
{
int face = convexUtil->faces[i];
//printf("face=%d\n",face);
const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
const b3ConvexHullComputer::Edge* edge = firstEdge;
const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
const b3ConvexHullComputer::Edge* edge = firstEdge;
b3Vector3 edges[3];
int numEdges = 0;
@@ -66,25 +56,23 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
do
{
int src = edge->getSourceVertex();
tmpFaces[i].m_indices.push_back(src);
int targ = edge->getTargetVertex();
b3Vector3 wa = convexUtil->vertices[src];
b3Vector3 wb = convexUtil->vertices[targ];
b3Vector3 newEdge = wb-wa;
b3Vector3 newEdge = wb - wa;
newEdge.normalize();
if (numEdges<2)
if (numEdges < 2)
edges[numEdges++] = newEdge;
edge = edge->getNextEdgeOfFace();
} while (edge!=firstEdge);
} while (edge != firstEdge);
b3Scalar planeEq = 1e30f;
if (numEdges==2)
if (numEdges == 2)
{
faceNormals[i] = edges[0].cross(edges[1]);
faceNormals[i].normalize();
@@ -92,20 +80,19 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
tmpFaces[i].m_plane[1] = faceNormals[i].getY();
tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
tmpFaces[i].m_plane[3] = planeEq;
}
else
{
b3Assert(0);//degenerate?
b3Assert(0); //degenerate?
faceNormals[i].setZero();
}
for (int v=0;v<tmpFaces[i].m_indices.size();v++)
for (int v = 0; v < tmpFaces[i].m_indices.size(); v++)
{
b3Scalar eq = m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
if (planeEq>eq)
if (planeEq > eq)
{
planeEq=eq;
planeEq = eq;
}
}
tmpFaces[i].m_plane[3] = -planeEq;
@@ -113,89 +100,86 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
//merge coplanar faces and copy them to m_polyhedron
b3Scalar faceWeldThreshold= 0.999f;
b3Scalar faceWeldThreshold = 0.999f;
b3AlignedObjectArray<int> todoFaces;
for (int i=0;i<tmpFaces.size();i++)
for (int i = 0; i < tmpFaces.size(); i++)
todoFaces.push_back(i);
while (todoFaces.size())
{
b3AlignedObjectArray<int> coplanarFaceGroup;
int refFace = todoFaces[todoFaces.size()-1];
int refFace = todoFaces[todoFaces.size() - 1];
coplanarFaceGroup.push_back(refFace);
b3MyFace& faceA = tmpFaces[refFace];
todoFaces.pop_back();
b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
for (int j=todoFaces.size()-1;j>=0;j--)
b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2]);
for (int j = todoFaces.size() - 1; j >= 0; j--)
{
int i = todoFaces[j];
b3MyFace& faceB = tmpFaces[i];
b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)
b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2]);
if (faceNormalA.dot(faceNormalB) > faceWeldThreshold)
{
coplanarFaceGroup.push_back(i);
todoFaces.remove(i);
}
}
bool did_merge = false;
if (coplanarFaceGroup.size()>1)
if (coplanarFaceGroup.size() > 1)
{
//do the merge: use Graham Scan 2d convex hull
b3AlignedObjectArray<b3GrahamVector3> orgpoints;
b3Vector3 averageFaceNormal = b3MakeVector3(0,0,0);
b3Vector3 averageFaceNormal = b3MakeVector3(0, 0, 0);
for (int i=0;i<coplanarFaceGroup.size();i++)
for (int i = 0; i < coplanarFaceGroup.size(); i++)
{
// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
b3MyFace& face = tmpFaces[coplanarFaceGroup[i]];
b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
averageFaceNormal+=faceNormal;
for (int f=0;f<face.m_indices.size();f++)
b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0], face.m_plane[1], face.m_plane[2]);
averageFaceNormal += faceNormal;
for (int f = 0; f < face.m_indices.size(); f++)
{
int orgIndex = face.m_indices[f];
b3Vector3 pt = m_vertices[orgIndex];
bool found = false;
for (int i=0;i<orgpoints.size();i++)
for (int i = 0; i < orgpoints.size(); i++)
{
//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
if (orgpoints[i].m_orgIndex == orgIndex)
{
found=true;
found = true;
break;
}
}
if (!found)
orgpoints.push_back(b3GrahamVector3(pt,orgIndex));
orgpoints.push_back(b3GrahamVector3(pt, orgIndex));
}
}
b3MyFace combinedFace;
for (int i=0;i<4;i++)
for (int i = 0; i < 4; i++)
combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
b3AlignedObjectArray<b3GrahamVector3> hull;
averageFaceNormal.normalize();
b3GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal);
b3GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal);
for (int i=0;i<hull.size();i++)
for (int i = 0; i < hull.size(); i++)
{
combinedFace.m_indices.push_back(hull[i].m_orgIndex);
for(int k = 0; k < orgpoints.size(); k++)
for (int k = 0; k < orgpoints.size(); k++)
{
if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
{
orgpoints[k].m_orgIndex = -1; // invalidate...
orgpoints[k].m_orgIndex = -1; // invalidate...
break;
}
}
@@ -203,38 +187,41 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
// are there rejected vertices?
bool reject_merge = false;
for(int i = 0; i < orgpoints.size(); i++) {
if(orgpoints[i].m_orgIndex == -1)
continue; // this is in the hull...
for (int i = 0; i < orgpoints.size(); i++)
{
if (orgpoints[i].m_orgIndex == -1)
continue; // this is in the hull...
// this vertex is rejected -- is anybody else using this vertex?
for(int j = 0; j < tmpFaces.size(); j++) {
for (int j = 0; j < tmpFaces.size(); j++)
{
b3MyFace& face = tmpFaces[j];
// is this a face of the current coplanar group?
bool is_in_current_group = false;
for(int k = 0; k < coplanarFaceGroup.size(); k++) {
if(coplanarFaceGroup[k] == j) {
for (int k = 0; k < coplanarFaceGroup.size(); k++)
{
if (coplanarFaceGroup[k] == j)
{
is_in_current_group = true;
break;
}
}
if(is_in_current_group) // ignore this face...
if (is_in_current_group) // ignore this face...
continue;
// does this face use this rejected vertex?
for(int v = 0; v < face.m_indices.size(); v++) {
if(face.m_indices[v] == orgpoints[i].m_orgIndex) {
for (int v = 0; v < face.m_indices.size(); v++)
{
if (face.m_indices[v] == orgpoints[i].m_orgIndex)
{
// this rejected vertex is used in another face -- reject merge
reject_merge = true;
break;
}
}
if(reject_merge)
if (reject_merge)
break;
}
if(reject_merge)
if (reject_merge)
break;
}
@@ -245,18 +232,14 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
m_faces.push_back(combinedFace);
}
}
if(!did_merge)
if (!did_merge)
{
for (int i=0;i<coplanarFaceGroup.size();i++)
for (int i = 0; i < coplanarFaceGroup.size(); i++)
{
b3MyFace face = tmpFaces[coplanarFaceGroup[i]];
m_faces.push_back(face);
}
}
}
}
initialize();
@@ -264,43 +247,38 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
return true;
}
inline bool IsAlmostZero(const b3Vector3& v)
{
if(fabsf(v.getX())>1e-6 || fabsf(v.getY())>1e-6 || fabsf(v.getZ())>1e-6) return false;
if (fabsf(v.getX()) > 1e-6 || fabsf(v.getY()) > 1e-6 || fabsf(v.getZ()) > 1e-6) return false;
return true;
}
struct b3InternalVertexPair
{
b3InternalVertexPair(short int v0,short int v1)
:m_v0(v0),
m_v1(v1)
b3InternalVertexPair(short int v0, short int v1)
: m_v0(v0),
m_v1(v1)
{
if (m_v1>m_v0)
b3Swap(m_v0,m_v1);
if (m_v1 > m_v0)
b3Swap(m_v0, m_v1);
}
short int m_v0;
short int m_v1;
int getHash() const
{
return m_v0+(m_v1<<16);
return m_v0 + (m_v1 << 16);
}
bool equals(const b3InternalVertexPair& other) const
{
return m_v0==other.m_v0 && m_v1==other.m_v1;
return m_v0 == other.m_v0 && m_v1 == other.m_v1;
}
};
struct b3InternalEdge
{
b3InternalEdge()
:m_face0(-1),
m_face1(-1)
: m_face0(-1),
m_face1(-1)
{
}
short int m_face0;
@@ -312,23 +290,31 @@ struct b3InternalEdge
#ifdef TEST_INTERNAL_OBJECTS
bool b3ConvexUtility::testContainment() const
{
for(int p=0;p<8;p++)
for (int p = 0; p < 8; p++)
{
b3Vector3 LocalPt;
if(p==0) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
else if(p==1) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
else if(p==2) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
else if(p==3) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
else if(p==4) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
else if(p==5) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
else if(p==6) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
else if(p==7) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
if (p == 0)
LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
else if (p == 1)
LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
else if (p == 2)
LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
else if (p == 3)
LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
else if (p == 4)
LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
else if (p == 5)
LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
else if (p == 6)
LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
else if (p == 7)
LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
for(int i=0;i<m_faces.size();i++)
for (int i = 0; i < m_faces.size(); i++)
{
const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
const b3Scalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
if(d>0.0f)
if (d > 0.0f)
return false;
}
}
@@ -336,39 +322,38 @@ bool b3ConvexUtility::testContainment() const
}
#endif
void b3ConvexUtility::initialize()
void b3ConvexUtility::initialize()
{
b3HashMap<b3InternalVertexPair,b3InternalEdge> edges;
b3HashMap<b3InternalVertexPair, b3InternalEdge> edges;
b3Scalar TotalArea = 0.0f;
m_localCenter.setValue(0, 0, 0);
for(int i=0;i<m_faces.size();i++)
for (int i = 0; i < m_faces.size(); i++)
{
int numVertices = m_faces[i].m_indices.size();
int NbTris = numVertices;
for(int j=0;j<NbTris;j++)
for (int j = 0; j < NbTris; j++)
{
int k = (j+1)%numVertices;
b3InternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
int k = (j + 1) % numVertices;
b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
b3InternalEdge* edptr = edges.find(vp);
b3Vector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0];
b3Vector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0];
edge.normalize();
bool found = false;
b3Vector3 diff,diff2;
b3Vector3 diff, diff2;
for (int p=0;p<m_uniqueEdges.size();p++)
for (int p = 0; p < m_uniqueEdges.size(); p++)
{
diff = m_uniqueEdges[p]-edge;
diff2 = m_uniqueEdges[p]+edge;
diff = m_uniqueEdges[p] - edge;
diff2 = m_uniqueEdges[p] + edge;
// if ((diff.length2()==0.f) ||
// if ((diff.length2()==0.f) ||
// (diff2.length2()==0.f))
if (IsAlmostZero(diff) ||
IsAlmostZero(diff2))
if (IsAlmostZero(diff) ||
IsAlmostZero(diff2))
{
found = true;
break;
@@ -382,106 +367,101 @@ void b3ConvexUtility::initialize()
if (edptr)
{
//TBD: figure out why I added this assert
// b3Assert(edptr->m_face0>=0);
// b3Assert(edptr->m_face1<0);
//TBD: figure out why I added this assert
// b3Assert(edptr->m_face0>=0);
// b3Assert(edptr->m_face1<0);
edptr->m_face1 = i;
} else
}
else
{
b3InternalEdge ed;
ed.m_face0 = i;
edges.insert(vp,ed);
edges.insert(vp, ed);
}
}
}
#ifdef USE_CONNECTED_FACES
for(int i=0;i<m_faces.size();i++)
for (int i = 0; i < m_faces.size(); i++)
{
int numVertices = m_faces[i].m_indices.size();
m_faces[i].m_connectedFaces.resize(numVertices);
for(int j=0;j<numVertices;j++)
for (int j = 0; j < numVertices; j++)
{
int k = (j+1)%numVertices;
b3InternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
int k = (j + 1) % numVertices;
b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
b3InternalEdge* edptr = edges.find(vp);
b3Assert(edptr);
b3Assert(edptr->m_face0>=0);
b3Assert(edptr->m_face1>=0);
b3Assert(edptr->m_face0 >= 0);
b3Assert(edptr->m_face1 >= 0);
int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0;
int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0;
m_faces[i].m_connectedFaces[j] = connectedFace;
}
}
#endif//USE_CONNECTED_FACES
#endif //USE_CONNECTED_FACES
for(int i=0;i<m_faces.size();i++)
for (int i = 0; i < m_faces.size(); i++)
{
int numVertices = m_faces[i].m_indices.size();
int NbTris = numVertices-2;
int NbTris = numVertices - 2;
const b3Vector3& p0 = m_vertices[m_faces[i].m_indices[0]];
for(int j=1;j<=NbTris;j++)
for (int j = 1; j <= NbTris; j++)
{
int k = (j+1)%numVertices;
int k = (j + 1) % numVertices;
const b3Vector3& p1 = m_vertices[m_faces[i].m_indices[j]];
const b3Vector3& p2 = m_vertices[m_faces[i].m_indices[k]];
b3Scalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
b3Vector3 Center = (p0+p1+p2)/3.0f;
b3Vector3 Center = (p0 + p1 + p2) / 3.0f;
m_localCenter += Area * Center;
TotalArea += Area;
}
}
m_localCenter /= TotalArea;
#ifdef TEST_INTERNAL_OBJECTS
if(1)
if (1)
{
m_radius = FLT_MAX;
for(int i=0;i<m_faces.size();i++)
for (int i = 0; i < m_faces.size(); i++)
{
const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
const b3Scalar dist = b3Fabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
if(dist<m_radius)
if (dist < m_radius)
m_radius = dist;
}
b3Scalar MinX = FLT_MAX;
b3Scalar MinY = FLT_MAX;
b3Scalar MinZ = FLT_MAX;
b3Scalar MaxX = -FLT_MAX;
b3Scalar MaxY = -FLT_MAX;
b3Scalar MaxZ = -FLT_MAX;
for(int i=0; i<m_vertices.size(); i++)
for (int i = 0; i < m_vertices.size(); i++)
{
const b3Vector3& pt = m_vertices[i];
if(pt.getX()<MinX) MinX = pt.getX();
if(pt.getX()>MaxX) MaxX = pt.getX();
if(pt.getY()<MinY) MinY = pt.getY();
if(pt.getY()>MaxY) MaxY = pt.getY();
if(pt.getZ()<MinZ) MinZ = pt.getZ();
if(pt.getZ()>MaxZ) MaxZ = pt.getZ();
if (pt.getX() < MinX) MinX = pt.getX();
if (pt.getX() > MaxX) MaxX = pt.getX();
if (pt.getY() < MinY) MinY = pt.getY();
if (pt.getY() > MaxY) MaxY = pt.getY();
if (pt.getZ() < MinZ) MinZ = pt.getZ();
if (pt.getZ() > MaxZ) MaxZ = pt.getZ();
}
mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);
mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ);
mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ);
// const b3Scalar r = m_radius / sqrtf(2.0f);
// const b3Scalar r = m_radius / sqrtf(2.0f);
const b3Scalar r = m_radius / sqrtf(3.0f);
const int LargestExtent = mE.maxAxis();
const b3Scalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f;
const b3Scalar Step = (mE[LargestExtent] * 0.5f - r) / 1024.0f;
m_extents[0] = m_extents[1] = m_extents[2] = r;
m_extents[LargestExtent] = mE[LargestExtent]*0.5f;
m_extents[LargestExtent] = mE[LargestExtent] * 0.5f;
bool FoundBox = false;
for(int j=0;j<1024;j++)
for (int j = 0; j < 1024; j++)
{
if(testContainment())
if (testContainment())
{
FoundBox = true;
break;
@@ -489,25 +469,25 @@ void b3ConvexUtility::initialize()
m_extents[LargestExtent] -= Step;
}
if(!FoundBox)
if (!FoundBox)
{
m_extents[0] = m_extents[1] = m_extents[2] = r;
}
else
{
// Refine the box
const b3Scalar Step = (m_radius - r)/1024.0f;
const int e0 = (1<<LargestExtent) & 3;
const int e1 = (1<<e0) & 3;
const b3Scalar Step = (m_radius - r) / 1024.0f;
const int e0 = (1 << LargestExtent) & 3;
const int e1 = (1 << e0) & 3;
for(int j=0;j<1024;j++)
for (int j = 0; j < 1024; j++)
{
const b3Scalar Saved0 = m_extents[e0];
const b3Scalar Saved1 = m_extents[e1];
m_extents[e0] += Step;
m_extents[e1] += Step;
if(!testContainment())
if (!testContainment())
{
m_extents[e0] = Saved0;
m_extents[e1] = Saved1;

39
src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h
View File

@@ -20,43 +20,36 @@ subject to the following restrictions:
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "Bullet3Common/b3Transform.h"
struct b3MyFace
{
b3AlignedObjectArray<int> m_indices;
b3Scalar m_plane[4];
b3AlignedObjectArray<int> m_indices;
b3Scalar m_plane[4];
};
B3_ATTRIBUTE_ALIGNED16(class) b3ConvexUtility
B3_ATTRIBUTE_ALIGNED16(class)
b3ConvexUtility
{
public:
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Vector3 m_localCenter;
b3Vector3 m_extents;
b3Vector3 mC;
b3Vector3 mE;
b3Scalar m_radius;
b3AlignedObjectArray<b3Vector3> m_vertices;
b3AlignedObjectArray<b3MyFace> m_faces;
b3Vector3 m_localCenter;
b3Vector3 m_extents;
b3Vector3 mC;
b3Vector3 mE;
b3Scalar m_radius;
b3AlignedObjectArray<b3Vector3> m_vertices;
b3AlignedObjectArray<b3MyFace> m_faces;
b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
b3ConvexUtility()
{
}
virtual ~b3ConvexUtility();
bool initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles=true);
void initialize();
bool initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles = true);
void initialize();
bool testContainment() const;
};
#endif

190
src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp
View File

@@ -5,15 +5,13 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h"
struct b3CpuNarrowPhaseInternalData
{
b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU;
b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
b3AlignedObjectArray<b3ConvexUtility*> m_convexData;
b3Config m_config;
b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
b3AlignedObjectArray<b3Vector3> m_convexVertices;
@@ -22,10 +20,9 @@ struct b3CpuNarrowPhaseInternalData
b3AlignedObjectArray<b3Contact4Data> m_contacts;
int m_numAcceleratedShapes;
int m_numAcceleratedShapes;
};
const b3AlignedObjectArray<b3Contact4Data>& b3CpuNarrowPhase::getContacts() const
{
return m_data->m_contacts;
@@ -41,7 +38,6 @@ const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) cons
return m_data->m_collidablesCPU[collidableIndex];
}
b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
{
m_data = new b3CpuNarrowPhaseInternalData;
@@ -61,7 +57,7 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
int maxContactCapacity = m_data->m_config.m_maxContactCapacity;
m_data->m_contacts.resize(maxContactCapacity);
for (int i=0;i<nPairs;i++)
for (int i = 0; i < nPairs; i++)
{
int bodyIndexA = pairs[i].x;
int bodyIndexB = pairs[i].y;
@@ -71,64 +67,57 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
{
// computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_SPHERE)
{
// computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
//printf("convex-sphere\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
{
// computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// printf("convex-plane\n");
// computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// printf("convex-plane\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
{
// computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// printf("plane-convex\n");
// computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// printf("plane-convex\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
{
// computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
// nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
// printf("convex-plane\n");
// computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
// nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
// printf("convex-plane\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
{
// computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// printf("convex-plane\n");
// computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// printf("convex-plane\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
{
// computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// printf("plane-convex\n");
// computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// printf("plane-convex\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
@@ -137,54 +126,48 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
//printf("pairs[i].z=%d\n",pairs[i].z);
//int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
// m_data->m_collidablesCPU,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
int contactIndex = b3ContactConvexConvexSAT(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
m_data->m_collidablesCPU,m_data->m_convexPolyhedra,m_data->m_convexVertices,m_data->m_uniqueEdges,m_data->m_convexIndices,m_data->m_convexFaces,m_data->m_contacts,numContacts,maxContactCapacity);
int contactIndex = b3ContactConvexConvexSAT(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, bodies,
m_data->m_collidablesCPU, m_data->m_convexPolyhedra, m_data->m_convexVertices, m_data->m_uniqueEdges, m_data->m_convexIndices, m_data->m_convexFaces, m_data->m_contacts, numContacts, maxContactCapacity);
if (contactIndex>=0)
if (contactIndex >= 0)
{
pairs[i].z = contactIndex;
}
// printf("plane-convex\n");
// printf("plane-convex\n");
}
}
m_data->m_contacts.resize(numContacts);
}
int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
{
int collidableIndex = allocateCollidable();
if (collidableIndex<0)
if (collidableIndex < 0)
return collidableIndex;
b3Collidable& col = m_data->m_collidablesCPU[collidableIndex];
col.m_shapeType = SHAPE_CONVEX_HULL;
col.m_shapeIndex = -1;
{
b3Vector3 localCenter=b3MakeVector3(0,0,0);
for (int i=0;i<utilPtr->m_vertices.size();i++)
localCenter+=utilPtr->m_vertices[i];
localCenter*= (1.f/utilPtr->m_vertices.size());
b3Vector3 localCenter = b3MakeVector3(0, 0, 0);
for (int i = 0; i < utilPtr->m_vertices.size(); i++)
localCenter += utilPtr->m_vertices[i];
localCenter *= (1.f / utilPtr->m_vertices.size());
utilPtr->m_localCenter = localCenter;
col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr,col);
col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr, col);
}
if (col.m_shapeIndex>=0)
if (col.m_shapeIndex >= 0)
{
b3Aabb aabb;
b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f);
b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f);
for (int i=0;i<utilPtr->m_vertices.size();i++)
b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f);
b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f);
for (int i = 0; i < utilPtr->m_vertices.size(); i++)
{
myAabbMin.setMin(utilPtr->m_vertices[i]);
myAabbMax.setMax(utilPtr->m_vertices[i]);
@@ -200,44 +183,42 @@ int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
aabb.m_signedMaxIndices[3] = 0;
m_data->m_localShapeAABBCPU.push_back(aabb);
}
return collidableIndex;
}
int b3CpuNarrowPhase::allocateCollidable()
int b3CpuNarrowPhase::allocateCollidable()
{
int curSize = m_data->m_collidablesCPU.size();
if (curSize<m_data->m_config.m_maxConvexShapes)
if (curSize < m_data->m_config.m_maxConvexShapes)
{
m_data->m_collidablesCPU.expand();
return curSize;
}
else
{
b3Error("allocateCollidable out-of-range %d\n",m_data->m_config.m_maxConvexShapes);
b3Error("allocateCollidable out-of-range %d\n", m_data->m_config.m_maxConvexShapes);
}
return -1;
}
int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
{
b3AlignedObjectArray<b3Vector3> verts;
unsigned char* vts = (unsigned char*) vertices;
for (int i=0;i<numVertices;i++)
unsigned char* vts = (unsigned char*)vertices;
for (int i = 0; i < numVertices; i++)
{
float* vertex = (float*) &vts[i*strideInBytes];
verts.push_back(b3MakeVector3(vertex[0]*scaling[0],vertex[1]*scaling[1],vertex[2]*scaling[2]));
float* vertex = (float*)&vts[i * strideInBytes];
verts.push_back(b3MakeVector3(vertex[0] * scaling[0], vertex[1] * scaling[1], vertex[2] * scaling[2]));
}
b3ConvexUtility* utilPtr = new b3ConvexUtility();
bool merge = true;
if (numVertices)
{
utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge);
utilPtr->initializePolyhedralFeatures(&verts[0], verts.size(), merge);
}
int collidableIndex = registerConvexHullShape(utilPtr);
@@ -246,74 +227,67 @@ int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideI
return collidableIndex;
}
int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col)
int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr, b3Collidable& col)
{
m_data->m_convexData.resize(m_data->m_numAcceleratedShapes + 1);
m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1);
m_data->m_convexData.resize(m_data->m_numAcceleratedShapes+1);
m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1);
b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1);
b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1);
convex.mC = convexPtr->mC;
convex.mE = convexPtr->mE;
convex.m_extents= convexPtr->m_extents;
convex.m_extents = convexPtr->m_extents;
convex.m_localCenter = convexPtr->m_localCenter;
convex.m_radius = convexPtr->m_radius;
convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size();
int edgeOffset = m_data->m_uniqueEdges.size();
convex.m_uniqueEdgesOffset = edgeOffset;
m_data->m_uniqueEdges.resize(edgeOffset+convex.m_numUniqueEdges);
m_data->m_uniqueEdges.resize(edgeOffset + convex.m_numUniqueEdges);
//convex data here
int i;
for ( i=0;i<convexPtr->m_uniqueEdges.size();i++)
for (i = 0; i < convexPtr->m_uniqueEdges.size(); i++)
{
m_data->m_uniqueEdges[edgeOffset+i] = convexPtr->m_uniqueEdges[i];
m_data->m_uniqueEdges[edgeOffset + i] = convexPtr->m_uniqueEdges[i];
}
int faceOffset = m_data->m_convexFaces.size();
convex.m_faceOffset = faceOffset;
convex.m_numFaces = convexPtr->m_faces.size();
m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces);
m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces);
for (i=0;i<convexPtr->m_faces.size();i++)
for (i = 0; i < convexPtr->m_faces.size(); i++)
{
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
convexPtr->m_faces[i].m_plane[1],
convexPtr->m_faces[i].m_plane[2],
convexPtr->m_faces[i].m_plane[3]);
m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
convexPtr->m_faces[i].m_plane[1],
convexPtr->m_faces[i].m_plane[2],
convexPtr->m_faces[i].m_plane[3]);
int indexOffset = m_data->m_convexIndices.size();
int numIndices = convexPtr->m_faces[i].m_indices.size();
m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices;
m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset;
m_data->m_convexIndices.resize(indexOffset+numIndices);
for (int p=0;p<numIndices;p++)
m_data->m_convexFaces[convex.m_faceOffset + i].m_numIndices = numIndices;
m_data->m_convexFaces[convex.m_faceOffset + i].m_indexOffset = indexOffset;
m_data->m_convexIndices.resize(indexOffset + numIndices);
for (int p = 0; p < numIndices; p++)
{
m_data->m_convexIndices[indexOffset+p] = convexPtr->m_faces[i].m_indices[p];
m_data->m_convexIndices[indexOffset + p] = convexPtr->m_faces[i].m_indices[p];
}
}
convex.m_numVertices = convexPtr->m_vertices.size();
int vertexOffset = m_data->m_convexVertices.size();
convex.m_vertexOffset =vertexOffset;
m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices);
for (int i=0;i<convexPtr->m_vertices.size();i++)
convex.m_vertexOffset = vertexOffset;
m_data->m_convexVertices.resize(vertexOffset + convex.m_numVertices);
for (int i = 0; i < convexPtr->m_vertices.size(); i++)
{
m_data->m_convexVertices[vertexOffset+i] = convexPtr->m_vertices[i];
m_data->m_convexVertices[vertexOffset + i] = convexPtr->m_vertices[i];
}
(m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
return m_data->m_numAcceleratedShapes++;
}

61
src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h
View File

@@ -12,66 +12,55 @@
class b3CpuNarrowPhase
{
protected:
struct b3CpuNarrowPhaseInternalData* m_data;
struct b3CpuNarrowPhaseInternalData* m_data;
int m_acceleratedCompanionShapeIndex;
int m_planeBodyIndex;
int m_static0Index;
int m_static0Index;
int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr,b3Collidable& col);
int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr, b3Collidable& col);
int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling);
public:
b3CpuNarrowPhase(const struct b3Config& config);
virtual ~b3CpuNarrowPhase(void);
int registerSphereShape(float radius);
int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant);
int registerSphereShape(float radius);
int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant);
int registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes);
int registerFace(const b3Vector3& faceNormal, float faceConstant);
int registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,const float* scaling);
int registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, const float* scaling);
//do they need to be merged?
int registerConvexHullShape(b3ConvexUtility* utilPtr);
int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling);
int registerConvexHullShape(b3ConvexUtility* utilPtr);
int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling);
//int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu);
void setObjectTransform(const float* position, const float* orientation , int bodyIndex);
void setObjectTransform(const float* position, const float* orientation, int bodyIndex);
void writeAllBodiesToGpu();
void reset();
void readbackAllBodiesToCpu();
bool getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const;
void writeAllBodiesToGpu();
void reset();
void readbackAllBodiesToCpu();
bool getObjectTransformFromCpu(float* position, float* orientation, int bodyIndex) const;
void setObjectTransformCpu(float* position, float* orientation , int bodyIndex);
void setObjectTransformCpu(float* position, float* orientation, int bodyIndex);
void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex);
//virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects);
virtual void computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies);
const struct b3RigidBodyData* getBodiesCpu() const;
//struct b3RigidBodyData* getBodiesCpu();
int getNumBodiesGpu() const;
int getNumBodiesGpu() const;
int getNumBodyInertiasGpu() const;
int getNumBodyInertiasGpu() const;
const struct b3Collidable* getCollidablesCpu() const;
int getNumCollidablesGpu() const;
int getNumCollidablesGpu() const;
/*const struct b3Contact4* getContactsCPU() const;
@@ -80,8 +69,7 @@ public:
*/
const b3AlignedObjectArray<b3Contact4Data>& getContacts() const;
int getNumRigidBodies() const;
int allocateCollidable();
@@ -93,13 +81,12 @@ public:
b3Collidable& getCollidableCpu(int collidableIndex);
const b3Collidable& getCollidableCpu(int collidableIndex) const;
const b3CpuNarrowPhaseInternalData* getInternalData() const
const b3CpuNarrowPhaseInternalData* getInternalData() const
{
return m_data;
return m_data;
}
const struct b3Aabb& getLocalSpaceAabb(int collidableIndex) const;
};
#endif //B3_CPU_NARROWPHASE_H
#endif //B3_CPU_NARROWPHASE_H

21
src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h
View File

@@ -4,21 +4,22 @@
#include "Bullet3Common/b3Vector3.h"
B3_ATTRIBUTE_ALIGNED16(struct) b3RayInfo
B3_ATTRIBUTE_ALIGNED16(struct)
b3RayInfo
{
b3Vector3 m_from;
b3Vector3 m_to;
};
B3_ATTRIBUTE_ALIGNED16(struct) b3RayHit
B3_ATTRIBUTE_ALIGNED16(struct)
b3RayHit
{
b3Scalar m_hitFraction;
int m_hitBody;
int m_hitResult1;
int m_hitResult2;
b3Vector3 m_hitPoint;
b3Vector3 m_hitNormal;
b3Scalar m_hitFraction;
int m_hitBody;
int m_hitResult1;
int m_hitResult2;
b3Vector3 m_hitPoint;
b3Vector3 m_hitNormal;
};
#endif //B3_RAYCAST_INFO_H
#endif //B3_RAYCAST_INFO_H

8
src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h
View File

@@ -20,11 +20,9 @@ subject to the following restrictions:
#include "Bullet3Common/b3Matrix3x3.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
inline float b3GetInvMass(const b3RigidBodyData& body)
inline float b3GetInvMass(const b3RigidBodyData& body)
{
return body.m_invMass;
return body.m_invMass;
}
#endif//B3_RIGID_BODY_CL
#endif //B3_RIGID_BODY_CL

13
src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h
View File

@@ -7,14 +7,13 @@ typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t;
struct b3BvhSubtreeInfoData
{
//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];
};
#endif //B3_BVH_SUBTREE_INFO_DATA_H
#endif //B3_BVH_SUBTREE_INFO_DATA_H

103
src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h
View File

@@ -7,69 +7,64 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
// work-in-progress
void b3BvhTraversal( __global const b3Int4* pairs,
__global const b3RigidBodyData* rigidBodies,
__global const b3Collidable* collidables,
__global b3Aabb* aabbs,
__global b3Int4* concavePairsOut,
__global volatile int* numConcavePairsOut,
__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
__global const b3QuantizedBvhNode* quantizedNodesRoot,
__global const b3BvhInfo* bvhInfos,
int numPairs,
int maxNumConcavePairsCapacity,
int id)
void b3BvhTraversal(__global const b3Int4* pairs,
__global const b3RigidBodyData* rigidBodies,
__global const b3Collidable* collidables,
__global b3Aabb* aabbs,
__global b3Int4* concavePairsOut,
__global volatile int* numConcavePairsOut,
__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
__global const b3QuantizedBvhNode* quantizedNodesRoot,
__global const b3BvhInfo* bvhInfos,
int numPairs,
int maxNumConcavePairsCapacity,
int id)
{
int bodyIndexA = pairs[id].x;
int bodyIndexB = pairs[id].y;
int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
//once the broadphase avoids static-static pairs, we can remove this test
if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))
if ((rigidBodies[bodyIndexA].m_invMass == 0) && (rigidBodies[bodyIndexB].m_invMass == 0))
{
return;
}
if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH)
if (collidables[collidableIndexA].m_shapeType != SHAPE_CONCAVE_TRIMESH)
return;
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
if (shapeTypeB!=SHAPE_CONVEX_HULL &&
shapeTypeB!=SHAPE_SPHERE &&
shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS
)
if (shapeTypeB != SHAPE_CONVEX_HULL &&
shapeTypeB != SHAPE_SPHERE &&
shapeTypeB != SHAPE_COMPOUND_OF_CONVEX_HULLS)
return;
b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
b3Float4 bvhAabbMin = bvhInfo.m_aabbMin;
b3Float4 bvhAabbMax = bvhInfo.m_aabbMax;
b3Float4 bvhQuantization = bvhInfo.m_quantization;
b3Float4 bvhAabbMin = bvhInfo.m_aabbMin;
b3Float4 bvhAabbMax = bvhInfo.m_aabbMax;
b3Float4 bvhQuantization = bvhInfo.m_quantization;
int numSubtreeHeaders = bvhInfo.m_numSubTrees;
__global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
__global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
unsigned short int quantizedQueryAabbMin[3];
unsigned short int quantizedQueryAabbMax[3];
b3QuantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_minVec,false,bvhAabbMin, bvhAabbMax,bvhQuantization);
b3QuantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_maxVec,true ,bvhAabbMin, bvhAabbMax,bvhQuantization);
for (int i=0;i<numSubtreeHeaders;i++)
b3QuantizeWithClamp(quantizedQueryAabbMin, aabbs[bodyIndexB].m_minVec, false, bvhAabbMin, bvhAabbMax, bvhQuantization);
b3QuantizeWithClamp(quantizedQueryAabbMax, aabbs[bodyIndexB].m_maxVec, true, bvhAabbMin, bvhAabbMax, bvhQuantization);
for (int i = 0; i < numSubtreeHeaders; i++)
{
b3BvhSubtreeInfoData subtree = subtreeHeaders[i];
int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax);
if (overlap != 0)
{
int startNodeIndex = subtree.m_rootNodeIndex;
int endNodeIndex = subtree.m_rootNodeIndex+subtree.m_subtreeSize;
int endNodeIndex = subtree.m_rootNodeIndex + subtree.m_subtreeSize;
int curIndex = startNodeIndex;
int escapeIndex;
int isLeafNode;
@@ -77,43 +72,46 @@ void b3BvhTraversal( __global const b3Int4* pairs,
while (curIndex < endNodeIndex)
{
b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex];
aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode.m_quantizedAabbMin,rootNode.m_quantizedAabbMax);
aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode.m_quantizedAabbMin, rootNode.m_quantizedAabbMax);
isLeafNode = b3IsLeaf(&rootNode);
if (aabbOverlap)
{
if (isLeafNode)
{
int triangleIndex = b3GetTriangleIndex(&rootNode);
if (shapeTypeB==SHAPE_COMPOUND_OF_CONVEX_HULLS)
if (shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS)
{
int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
int pairIdx = b3AtomicAdd (numConcavePairsOut,numChildrenB);
for (int b=0;b<numChildrenB;b++)
int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
int pairIdx = b3AtomicAdd(numConcavePairsOut, numChildrenB);
for (int b = 0; b < numChildrenB; b++)
{
if ((pairIdx + b) < maxNumConcavePairsCapacity)
{
if ((pairIdx+b)<maxNumConcavePairsCapacity)
{
int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+b;
b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,childShapeIndexB);
concavePairsOut[pairIdx+b] = newPair;
}
int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, childShapeIndexB);
concavePairsOut[pairIdx + b] = newPair;
}
} else
}
}
else
{
int pairIdx = b3AtomicInc(numConcavePairsOut);
if (pairIdx<maxNumConcavePairsCapacity)
if (pairIdx < maxNumConcavePairsCapacity)
{
b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,0);
b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, 0);
concavePairsOut[pairIdx] = newPair;
}
}
}
}
curIndex++;
} else
}
else
{
if (isLeafNode)
{
curIndex++;
} else
}
else
{
escapeIndex = b3GetEscapeIndex(&rootNode);
curIndex += escapeIndex;
@@ -122,5 +120,4 @@ void b3BvhTraversal( __global const b3Int4* pairs,
}
}
}
}

213
src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h
View File

@@ -1,7 +1,6 @@
#ifndef B3_CLIP_FACES_H
#define B3_CLIP_FACES_H
#include "Bullet3Common/shared/b3Int4.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
@@ -10,38 +9,36 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
inline b3Float4 b3Lerp3(b3Float4ConstArg a,b3Float4ConstArg b, float t)
inline b3Float4 b3Lerp3(b3Float4ConstArg a, b3Float4ConstArg b, float t)
{
return b3MakeFloat4( a.x + (b.x - a.x) * t,
return b3MakeFloat4(a.x + (b.x - a.x) * t,
a.y + (b.y - a.y) * t,
a.z + (b.z - a.z) * t,
0.f);
}
// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS,float planeEqWS, __global b3Float4* ppVtxOut)
int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS, float planeEqWS, __global b3Float4* ppVtxOut)
{
int ve;
float ds, de;
int numVertsOut = 0;
//double-check next test
// if (numVertsIn < 2)
// return 0;
b3Float4 firstVertex=pVtxIn[numVertsIn-1];
//double-check next test
// if (numVertsIn < 2)
// return 0;
b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
b3Float4 endVertex = pVtxIn[0];
ds = b3Dot(planeNormalWS,firstVertex)+planeEqWS;
ds = b3Dot(planeNormalWS, firstVertex) + planeEqWS;
for (ve = 0; ve < numVertsIn; ve++)
{
endVertex=pVtxIn[ve];
de = b3Dot(planeNormalWS,endVertex)+planeEqWS;
if (ds<0)
endVertex = pVtxIn[ve];
de = b3Dot(planeNormalWS, endVertex) + planeEqWS;
if (ds < 0)
{
if (de<0)
if (de < 0)
{
// Start < 0, end < 0, so output endVertex
ppVtxOut[numVertsOut++] = endVertex;
@@ -49,15 +46,15 @@ int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4Cons
else
{
// Start < 0, end >= 0, so output intersection
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
}
}
else
{
if (de<0)
if (de < 0)
{
// Start >= 0, end < 0 so output intersection and end
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
ppVtxOut[numVertsOut++] = endVertex;
}
}
@@ -67,90 +64,81 @@ int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4Cons
return numVertsOut;
}
__kernel void clipFacesAndFindContactsKernel( __global const b3Float4* separatingNormals,
__global const int* hasSeparatingAxis,
__global b3Int4* clippingFacesOut,
__global b3Float4* worldVertsA1,
__global b3Float4* worldNormalsA1,
__global b3Float4* worldVertsB1,
__global b3Float4* worldVertsB2,
int vertexFaceCapacity,
int pairIndex
)
__kernel void clipFacesAndFindContactsKernel(__global const b3Float4* separatingNormals,
__global const int* hasSeparatingAxis,
__global b3Int4* clippingFacesOut,
__global b3Float4* worldVertsA1,
__global b3Float4* worldNormalsA1,
__global b3Float4* worldVertsB1,
__global b3Float4* worldVertsB2,
int vertexFaceCapacity,
int pairIndex)
{
// int i = get_global_id(0);
// int i = get_global_id(0);
//int pairIndex = i;
int i = pairIndex;
float minDist = -1e30f;
float maxDist = 0.02f;
// if (i<numPairs)
// if (i<numPairs)
{
if (hasSeparatingAxis[i])
{
// int bodyIndexA = pairs[i].x;
// int bodyIndexB = pairs[i].y;
int numLocalContactsOut = 0;
// int bodyIndexA = pairs[i].x;
// int bodyIndexB = pairs[i].y;
int capacityWorldVertsB2 = vertexFaceCapacity;
__global b3Float4* pVtxIn = &worldVertsB1[pairIndex*capacityWorldVertsB2];
__global b3Float4* pVtxOut = &worldVertsB2[pairIndex*capacityWorldVertsB2];
int numLocalContactsOut = 0;
{
__global b3Int4* clippingFaces = clippingFacesOut;
int closestFaceA = clippingFaces[pairIndex].x;
// int closestFaceB = clippingFaces[pairIndex].y;
int numVertsInA = clippingFaces[pairIndex].z;
int numVertsInB = clippingFaces[pairIndex].w;
int numVertsOut = 0;
if (closestFaceA>=0)
{
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
for(int e0=0;e0<numVertsInA;e0++)
{
const b3Float4 aw = worldVertsA1[pairIndex*capacityWorldVertsB2+e0];
const b3Float4 bw = worldVertsA1[pairIndex*capacityWorldVertsB2+((e0+1)%numVertsInA)];
const b3Float4 WorldEdge0 = aw - bw;
b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0,worldPlaneAnormal1);
b3Float4 worldA1 = aw;
float planeEqWS1 = -b3Dot(worldA1,planeNormalWS1);
b3Float4 planeNormalWS = planeNormalWS1;
float planeEqWS=planeEqWS1;
numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS,planeEqWS, pVtxOut);
__global b3Float4* tmp = pVtxOut;
pVtxOut = pVtxIn;
pVtxIn = tmp;
numVertsInB = numVertsOut;
numVertsOut = 0;
}
b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
float planeEqWS=-b3Dot(planeNormalWS,worldVertsA1[pairIndex*capacityWorldVertsB2]);
for (int i=0;i<numVertsInB;i++)
{
float depth = b3Dot(planeNormalWS,pVtxIn[i])+planeEqWS;
if (depth <=minDist)
{
depth = minDist;
}
/*
int capacityWorldVertsB2 = vertexFaceCapacity;
__global b3Float4* pVtxIn = &worldVertsB1[pairIndex * capacityWorldVertsB2];
__global b3Float4* pVtxOut = &worldVertsB2[pairIndex * capacityWorldVertsB2];
{
__global b3Int4* clippingFaces = clippingFacesOut;
int closestFaceA = clippingFaces[pairIndex].x;
// int closestFaceB = clippingFaces[pairIndex].y;
int numVertsInA = clippingFaces[pairIndex].z;
int numVertsInB = clippingFaces[pairIndex].w;
int numVertsOut = 0;
if (closestFaceA >= 0)
{
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
for (int e0 = 0; e0 < numVertsInA; e0++)
{
const b3Float4 aw = worldVertsA1[pairIndex * capacityWorldVertsB2 + e0];
const b3Float4 bw = worldVertsA1[pairIndex * capacityWorldVertsB2 + ((e0 + 1) % numVertsInA)];
const b3Float4 WorldEdge0 = aw - bw;
b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0, worldPlaneAnormal1);
b3Float4 worldA1 = aw;
float planeEqWS1 = -b3Dot(worldA1, planeNormalWS1);
b3Float4 planeNormalWS = planeNormalWS1;
float planeEqWS = planeEqWS1;
numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS, planeEqWS, pVtxOut);
__global b3Float4* tmp = pVtxOut;
pVtxOut = pVtxIn;
pVtxIn = tmp;
numVertsInB = numVertsOut;
numVertsOut = 0;
}
b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
float planeEqWS = -b3Dot(planeNormalWS, worldVertsA1[pairIndex * capacityWorldVertsB2]);
for (int i = 0; i < numVertsInB; i++)
{
float depth = b3Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
if (depth <= minDist)
{
depth = minDist;
}
/*
static float maxDepth = 0.f;
if (depth < maxDepth)
{
@@ -163,26 +151,21 @@ __kernel void clipFacesAndFindContactsKernel( __global const b3Float4* sepa
}
*/
if (depth <=maxDist)
{
b3Float4 pointInWorld = pVtxIn[i];
pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
}
}
}
clippingFaces[pairIndex].w =numLocalContactsOut;
if (depth <= maxDist)
{
b3Float4 pointInWorld = pVtxIn[i];
pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
}
}
}
clippingFaces[pairIndex].w = numLocalContactsOut;
}
}
for (int i=0;i<numLocalContactsOut;i++)
pVtxIn[i] = pVtxOut[i];
}// if (hasSeparatingAxis[i])
}// if (i<numPairs)
for (int i = 0; i < numLocalContactsOut; i++)
pVtxIn[i] = pVtxOut[i];
} // if (hasSeparatingAxis[i])
} // if (i<numPairs)
}
#endif //B3_CLIP_FACES_H
#endif //B3_CLIP_FACES_H

53
src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h
View File

@@ -2,40 +2,36 @@
#ifndef B3_COLLIDABLE_H
#define B3_COLLIDABLE_H
#include "Bullet3Common/shared/b3Float4.h"
#include "Bullet3Common/shared/b3Quat.h"
enum b3ShapeTypes
{
SHAPE_HEIGHT_FIELD=1,
SHAPE_HEIGHT_FIELD = 1,
SHAPE_CONVEX_HULL=3,
SHAPE_PLANE=4,
SHAPE_CONCAVE_TRIMESH=5,
SHAPE_COMPOUND_OF_CONVEX_HULLS=6,
SHAPE_SPHERE=7,
SHAPE_CONVEX_HULL = 3,
SHAPE_PLANE = 4,
SHAPE_CONCAVE_TRIMESH = 5,
SHAPE_COMPOUND_OF_CONVEX_HULLS = 6,
SHAPE_SPHERE = 7,
MAX_NUM_SHAPE_TYPES,
};
typedef struct b3Collidable b3Collidable_t;
struct b3Collidable
{
union {
int m_numChildShapes;
int m_bvhIndex;
};
union
{
union {
float m_radius;
int m_compoundBvhIndex;
int m_compoundBvhIndex;
};
int m_shapeType;
union
{
union {
int m_shapeIndex;
float m_height;
};
@@ -44,33 +40,30 @@ struct b3Collidable
typedef struct b3GpuChildShape b3GpuChildShape_t;
struct b3GpuChildShape
{
b3Float4 m_childPosition;
b3Quat m_childOrientation;
union
{
int m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS
int m_capsuleAxis;
b3Float4 m_childPosition;
b3Quat m_childOrientation;
union {
int m_shapeIndex; //used for SHAPE_COMPOUND_OF_CONVEX_HULLS
int m_capsuleAxis;
};
union
{
float m_radius;//used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES
int m_numChildShapes;//used for compound shape
union {
float m_radius; //used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES
int m_numChildShapes; //used for compound shape
};
union
{
float m_height;//used for childshape of SHAPE_COMPOUND_OF_CAPSULES
int m_collidableShapeIndex;
union {
float m_height; //used for childshape of SHAPE_COMPOUND_OF_CAPSULES
int m_collidableShapeIndex;
};
int m_shapeType;
int m_shapeType;
};
struct b3CompoundOverlappingPair
{
int m_bodyIndexA;
int m_bodyIndexB;
// int m_pairType;
// int m_pairType;
int m_childShapeIndexA;
int m_childShapeIndexB;
};
#endif //B3_COLLIDABLE_H
#endif //B3_COLLIDABLE_H

26
src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h
View File

@@ -3,26 +3,24 @@
#include "Bullet3Common/shared/b3Float4.h"
typedef struct b3Contact4Data b3Contact4Data_t;
typedef struct b3Contact4Data b3Contact4Data_t;
struct b3Contact4Data
{
b3Float4 m_worldPosB[4];
// b3Float4 m_localPosA[4];
// b3Float4 m_localPosB[4];
b3Float4 m_worldNormalOnB; // w: m_nPoints
unsigned short m_restituitionCoeffCmp;
unsigned short m_frictionCoeffCmp;
b3Float4 m_worldPosB[4];
// b3Float4 m_localPosA[4];
// b3Float4 m_localPosB[4];
b3Float4 m_worldNormalOnB; // w: m_nPoints
unsigned short m_restituitionCoeffCmp;
unsigned short m_frictionCoeffCmp;
int m_batchIdx;
int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr
int m_bodyAPtrAndSignBit; //x:m_bodyAPtr, y:m_bodyBPtr
int m_bodyBPtrAndSignBit;
int m_childIndexA;
int m_childIndexB;
int m_childIndexA;
int m_childIndexB;
int m_unused1;
int m_unused2;
};
inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)
@@ -35,6 +33,4 @@ inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numP
contact->m_worldNormalOnB.w = (float)numPoints;
};
#endif //B3_CONTACT4DATA_H
#endif //B3_CONTACT4DATA_H

464
src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h
View File

@@ -2,48 +2,43 @@
#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H
#define B3_CONTACT_CONVEX_CONVEX_SAT_H
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h"
#define B3_MAX_VERTS 1024
inline b3Float4 b3Lerp3(const b3Float4& a,const b3Float4& b, float t)
inline b3Float4 b3Lerp3(const b3Float4& a, const b3Float4& b, float t)
{
return b3MakeVector3( a.x + (b.x - a.x) * t,
a.y + (b.y - a.y) * t,
a.z + (b.z - a.z) * t,
0.f);
return b3MakeVector3(a.x + (b.x - a.x) * t,
a.y + (b.y - a.y) * t,
a.z + (b.z - a.z) * t,
0.f);
}
// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS,float planeEqWS, b3Float4* ppVtxOut)
inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS, float planeEqWS, b3Float4* ppVtxOut)
{
int ve;
float ds, de;
int numVertsOut = 0;
if (numVertsIn < 2)
return 0;
b3Float4 firstVertex=pVtxIn[numVertsIn-1];
b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
b3Float4 endVertex = pVtxIn[0];
ds = b3Dot3F4(planeNormalWS,firstVertex)+planeEqWS;
ds = b3Dot3F4(planeNormalWS, firstVertex) + planeEqWS;
for (ve = 0; ve < numVertsIn; ve++)
{
endVertex=pVtxIn[ve];
endVertex = pVtxIn[ve];
de = b3Dot3F4(planeNormalWS,endVertex)+planeEqWS;
de = b3Dot3F4(planeNormalWS, endVertex) + planeEqWS;
if (ds<0)
if (ds < 0)
{
if (de<0)
if (de < 0)
{
// Start < 0, end < 0, so output endVertex
ppVtxOut[numVertsOut++] = endVertex;
@@ -51,15 +46,15 @@ inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNor
else
{
// Start < 0, end >= 0, so output intersection
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
}
}
else
{
if (de<0)
if (de < 0)
{
// Start >= 0, end < 0 so output intersection and end
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
ppVtxOut[numVertsOut++] = endVertex;
}
}
@@ -69,36 +64,35 @@ inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNor
return numVertsOut;
}
inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
b3Float4* worldVertsB2, int capacityWorldVertsB2,
const float minDist, float maxDist,
const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
//const b3Float4* verticesB, const b3GpuFace* facesB, const int* indicesB,
b3Float4* contactsOut,
int contactCapacity)
inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
b3Float4* worldVertsB2, int capacityWorldVertsB2,
const float minDist, float maxDist,
const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
//const b3Float4* verticesB, const b3GpuFace* facesB, const int* indicesB,
b3Float4* contactsOut,
int contactCapacity)
{
int numContactsOut = 0;
b3Float4* pVtxIn = worldVertsB1;
b3Float4* pVtxOut = worldVertsB2;
int numVertsIn = numWorldVertsB1;
int numVertsOut = 0;
int closestFaceA=-1;
int closestFaceA = -1;
{
float dmin = FLT_MAX;
for(int face=0;face<hullA->m_numFaces;face++)
for (int face = 0; face < hullA->m_numFaces; face++)
{
const b3Float4 Normal = b3MakeVector3(
facesA[hullA->m_faceOffset+face].m_plane.x,
facesA[hullA->m_faceOffset+face].m_plane.y,
facesA[hullA->m_faceOffset+face].m_plane.z,0.f);
const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal);
float d = b3Dot3F4(faceANormalWS,separatingNormal);
facesA[hullA->m_faceOffset + face].m_plane.x,
facesA[hullA->m_faceOffset + face].m_plane.y,
facesA[hullA->m_faceOffset + face].m_plane.z, 0.f);
const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
float d = b3Dot3F4(faceANormalWS, separatingNormal);
if (d < dmin)
{
dmin = d;
@@ -106,33 +100,33 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
}
}
}
if (closestFaceA<0)
if (closestFaceA < 0)
return numContactsOut;
b3GpuFace polyA = facesA[hullA->m_faceOffset+closestFaceA];
b3GpuFace polyA = facesA[hullA->m_faceOffset + closestFaceA];
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
//int numContacts = numWorldVertsB1;
int numVerticesA = polyA.m_numIndices;
for(int e0=0;e0<numVerticesA;e0++)
for (int e0 = 0; e0 < numVerticesA; e0++)
{
const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+e0]];
const b3Float4 b = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+((e0+1)%numVerticesA)]];
const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + e0]];
const b3Float4 b = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + ((e0 + 1) % numVerticesA)]];
const b3Float4 edge0 = a - b;
const b3Float4 WorldEdge0 = b3QuatRotate(ornA,edge0);
b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);
b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA,planeNormalA);
const b3Float4 WorldEdge0 = b3QuatRotate(ornA, edge0);
b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA, planeNormalA);
b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0, worldPlaneAnormal1);
b3Float4 worldA1 = b3TransformPoint(a, posA, ornA);
float planeEqWS1 = -b3Dot3F4(worldA1, planeNormalWS1);
b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0,worldPlaneAnormal1);
b3Float4 worldA1 = b3TransformPoint(a,posA,ornA);
float planeEqWS1 = -b3Dot3F4(worldA1,planeNormalWS1);
b3Float4 planeNormalWS = planeNormalWS1;
float planeEqWS=planeEqWS1;
float planeEqWS = planeEqWS1;
//clip face
//clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut);
numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS, planeEqWS, pVtxOut);
//btSwap(pVtxIn,pVtxOut);
b3Float4* tmp = pVtxOut;
@@ -142,32 +136,32 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
numVertsOut = 0;
}
// only keep points that are behind the witness face
{
b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);
b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
float localPlaneEq = polyA.m_plane.w;
b3Float4 planeNormalWS = b3QuatRotate(ornA,localPlaneNormal);
float planeEqWS=localPlaneEq-b3Dot3F4(planeNormalWS,posA);
for (int i=0;i<numVertsIn;i++)
b3Float4 planeNormalWS = b3QuatRotate(ornA, localPlaneNormal);
float planeEqWS = localPlaneEq - b3Dot3F4(planeNormalWS, posA);
for (int i = 0; i < numVertsIn; i++)
{
float depth = b3Dot3F4(planeNormalWS,pVtxIn[i])+planeEqWS;
if (depth <=minDist)
float depth = b3Dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS;
if (depth <= minDist)
{
depth = minDist;
}
if (numContactsOut<contactCapacity)
if (numContactsOut < contactCapacity)
{
if (depth <=maxDist)
if (depth <= maxDist)
{
b3Float4 pointInWorld = pVtxIn[i];
//resultOut.addContactPoint(separatingNormal,point,depth);
contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
//printf("depth=%f\n",depth);
}
} else
}
else
{
b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut,contactCapacity);
b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity);
}
}
}
@@ -175,62 +169,60 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
return numContactsOut;
}
inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
const b3Float4& posA, const b3Quaternion& ornA, const b3Float4& posB, const b3Quaternion& ornB,
b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
const float minDist, float maxDist,
const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
const b3Float4& posA, const b3Quaternion& ornA,const b3Float4& posB, const b3Quaternion& ornB,
b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
const float minDist, float maxDist,
const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
b3Float4* contactsOut,
int contactCapacity)
b3Float4* contactsOut,
int contactCapacity)
{
int numContactsOut = 0;
int numWorldVertsB1= 0;
int numWorldVertsB1 = 0;
B3_PROFILE("clipHullAgainstHull");
//float curMaxDist=maxDist;
int closestFaceB=-1;
int closestFaceB = -1;
float dmax = -FLT_MAX;
{
//B3_PROFILE("closestFaceB");
if (hullB.m_numFaces!=1)
if (hullB.m_numFaces != 1)
{
//printf("wtf\n");
}
static bool once = true;
//printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
for(int face=0;face<hullB.m_numFaces;face++)
for (int face = 0; face < hullB.m_numFaces; face++)
{
#ifdef BT_DEBUG_SAT_FACE
if (once)
printf("face %d\n",face);
const b3GpuFace* faceB = &facesB[hullB.m_faceOffset+face];
printf("face %d\n", face);
const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face];
if (once)
{
for (int i=0;i<faceB->m_numIndices;i++)
for (int i = 0; i < faceB->m_numIndices; i++)
{
b3Float4 vert = verticesB[hullB.m_vertexOffset+indicesB[faceB->m_indexOffset+i]];
printf("vert[%d] = %f,%f,%f\n",i,vert.x,vert.y,vert.z);
b3Float4 vert = verticesB[hullB.m_vertexOffset + indicesB[faceB->m_indexOffset + i]];
printf("vert[%d] = %f,%f,%f\n", i, vert.x, vert.y, vert.z);
}
}
#endif //BT_DEBUG_SAT_FACE
//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
#endif //BT_DEBUG_SAT_FACE \
//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
{
const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset+face].m_plane.x,
facesB[hullB.m_faceOffset+face].m_plane.y, facesB[hullB.m_faceOffset+face].m_plane.z,0.f);
const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset + face].m_plane.x,
facesB[hullB.m_faceOffset + face].m_plane.y, facesB[hullB.m_faceOffset + face].m_plane.z, 0.f);
const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
#ifdef BT_DEBUG_SAT_FACE
if (once)
printf("faceNormal = %f,%f,%f\n",Normal.x,Normal.y,Normal.z);
printf("faceNormal = %f,%f,%f\n", Normal.x, Normal.y, Normal.z);
#endif
float d = b3Dot3F4(WorldNormal,separatingNormal);
float d = b3Dot3F4(WorldNormal, separatingNormal);
if (d > dmax)
{
dmax = d;
@@ -241,79 +233,73 @@ inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
once = false;
}
b3Assert(closestFaceB>=0);
b3Assert(closestFaceB >= 0);
{
//B3_PROFILE("worldVertsB1");
const b3GpuFace& polyB = facesB[hullB.m_faceOffset+closestFaceB];
const b3GpuFace& polyB = facesB[hullB.m_faceOffset + closestFaceB];
const int numVertices = polyB.m_numIndices;
for(int e0=0;e0<numVertices;e0++)
for (int e0 = 0; e0 < numVertices; e0++)
{
const b3Float4& b = verticesB[hullB.m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];
worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b,posB,ornB);
const b3Float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
}
}
if (closestFaceB>=0)
if (closestFaceB >= 0)
{
//B3_PROFILE("clipFaceAgainstHull");
numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
posA,ornA,
worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist,
verticesA, facesA, indicesA,
contactsOut,contactCapacity);
numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
posA, ornA,
worldVertsB1, numWorldVertsB1, worldVertsB2, capacityWorldVerts, minDist, maxDist,
verticesA, facesA, indicesA,
contactsOut, contactCapacity);
}
return numContactsOut;
}
inline int b3ClipHullHullSingle(
int bodyIndexA, int bodyIndexB,
const b3Float4& posA,
const b3Quaternion& ornA,
const b3Float4& posB,
const b3Quaternion& ornB,
int bodyIndexA, int bodyIndexB,
const b3Float4& posA,
const b3Quaternion& ornA,
const b3Float4& posB,
const b3Quaternion& ornB,
int collidableIndexA, int collidableIndexB,
int collidableIndexA, int collidableIndexB,
const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
int& nContacts,
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
const b3AlignedObjectArray<b3GpuFace>& facesA,
const b3AlignedObjectArray<int>& indicesA,
const b3AlignedObjectArray<b3Vector3>& verticesB,
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
const b3AlignedObjectArray<b3GpuFace>& facesB,
const b3AlignedObjectArray<int>& indicesB,
const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
int& nContacts,
const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
const b3Vector3& sepNormalWorldSpace,
int maxContactCapacity )
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
const b3AlignedObjectArray<b3GpuFace>& facesA,
const b3AlignedObjectArray<int>& indicesA,
const b3AlignedObjectArray<b3Vector3>& verticesB,
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
const b3AlignedObjectArray<b3GpuFace>& facesB,
const b3AlignedObjectArray<int>& indicesB,
const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
const b3Vector3& sepNormalWorldSpace,
int maxContactCapacity)
{
int contactIndex = -1;
b3ConvexPolyhedronData hullA, hullB;
b3Collidable colA = hostCollidablesA[collidableIndexA];
hullA = hostConvexDataA[colA.m_shapeIndex];
//printf("numvertsA = %d\n",hullA.m_numVertices);
b3Collidable colB = hostCollidablesB[collidableIndexB];
hullB = hostConvexDataB[colB.m_shapeIndex];
//printf("numvertsB = %d\n",hullB.m_numVertices);
b3Collidable colA = hostCollidablesA[collidableIndexA];
hullA = hostConvexDataA[colA.m_shapeIndex];
//printf("numvertsA = %d\n",hullA.m_numVertices);
b3Collidable colB = hostCollidablesB[collidableIndexB];
hullB = hostConvexDataB[colB.m_shapeIndex];
//printf("numvertsB = %d\n",hullB.m_numVertices);
b3Float4 contactsOut[B3_MAX_VERTS];
int localContactCapacity = B3_MAX_VERTS;
@@ -321,187 +307,168 @@ inline int b3ClipHullHullSingle(
b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
#endif
{
b3Float4 worldVertsB1[B3_MAX_VERTS];
b3Float4 worldVertsB2[B3_MAX_VERTS];
int capacityWorldVerts = B3_MAX_VERTS;
b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z,0.f);
b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x, sepNormalWorldSpace.y, sepNormalWorldSpace.z, 0.f);
int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
b3Scalar minDist = -1;
b3Scalar maxDist = 0.;
b3Transform trA,trB;
b3Transform trA, trB;
{
//B3_PROFILE("b3TransformPoint computation");
//trA.setIdentity();
trA.setOrigin(b3MakeVector3(posA.x,posA.y,posA.z));
trA.setRotation(b3Quaternion(ornA.x,ornA.y,ornA.z,ornA.w));
//trB.setIdentity();
trB.setOrigin(b3MakeVector3(posB.x,posB.y,posB.z));
trB.setRotation(b3Quaternion(ornB.x,ornB.y,ornB.z,ornB.w));
//B3_PROFILE("b3TransformPoint computation");
//trA.setIdentity();
trA.setOrigin(b3MakeVector3(posA.x, posA.y, posA.z));
trA.setRotation(b3Quaternion(ornA.x, ornA.y, ornA.z, ornA.w));
//trB.setIdentity();
trB.setOrigin(b3MakeVector3(posB.x, posB.y, posB.z));
trB.setRotation(b3Quaternion(ornB.x, ornB.y, ornB.z, ornB.w));
}
b3Quaternion trAorn = trA.getRotation();
b3Quaternion trBorn = trB.getRotation();
int numContactsOut = b3ClipHullAgainstHull(hostNormal,
hostConvexDataA.at(shapeA),
hostConvexDataB.at(shapeB),
(b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
(b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
worldVertsB1,worldVertsB2,capacityWorldVerts,
minDist, maxDist,
verticesA, facesA,indicesA,
verticesB, facesB,indicesB,
contactsOut,localContactCapacity);
b3Quaternion trBorn = trB.getRotation();
if (numContactsOut>0)
int numContactsOut = b3ClipHullAgainstHull(hostNormal,
hostConvexDataA.at(shapeA),
hostConvexDataB.at(shapeB),
(b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
(b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
worldVertsB1, worldVertsB2, capacityWorldVerts,
minDist, maxDist,
verticesA, facesA, indicesA,
verticesB, facesB, indicesB,
contactsOut, localContactCapacity);
if (numContactsOut > 0)
{
B3_PROFILE("overlap");
b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal;
// b3Float4 centerOut;
// b3Float4 centerOut;
b3Int4 contactIdx;
contactIdx.x = 0;
contactIdx.y = 1;
contactIdx.z = 2;
contactIdx.w = 3;
int numPoints = 0;
{
B3_PROFILE("extractManifold");
numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
}
b3Assert(numPoints);
if (nContacts<maxContactCapacity)
if (nContacts < maxContactCapacity)
{
contactIndex = nContacts;
globalContactOut->expand();
b3Contact4Data& contact = globalContactOut->at(nContacts);
contact.m_batchIdx = 0;//i;
contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA;
contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB;
contact.m_batchIdx = 0; //i;
contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
contact.m_frictionCoeffCmp = 45874;
contact.m_restituitionCoeffCmp = 0;
// float distance = 0.f;
for (int p=0;p<numPoints;p++)
// float distance = 0.f;
for (int p = 0; p < numPoints; p++)
{
contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]];//check if it is actually on B
contact.m_worldNormalOnB = normalOnSurfaceB;
contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]]; //check if it is actually on B
contact.m_worldNormalOnB = normalOnSurfaceB;
}
//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
nContacts++;
} else
}
else
{
b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts,maxContactCapacity);
b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
}
}
}
return contactIndex;
}
inline int b3ContactConvexConvexSAT(
int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
const b3AlignedObjectArray<b3Collidable>& collidables,
const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
const b3AlignedObjectArray<b3Float4>& convexVertices,
const b3AlignedObjectArray<b3Float4>& uniqueEdges,
const b3AlignedObjectArray<int>& convexIndices,
const b3AlignedObjectArray<b3GpuFace>& faces,
b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
int& nGlobalContactsOut,
int maxContactCapacity)
int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
const b3AlignedObjectArray<b3Collidable>& collidables,
const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
const b3AlignedObjectArray<b3Float4>& convexVertices,
const b3AlignedObjectArray<b3Float4>& uniqueEdges,
const b3AlignedObjectArray<int>& convexIndices,
const b3AlignedObjectArray<b3GpuFace>& faces,
b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
int& nGlobalContactsOut,
int maxContactCapacity)
{
int contactIndex = -1;
b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
b3ConvexPolyhedronData hullA, hullB;
b3Float4 sepNormalWorldSpace;
b3Collidable colA = collidables[collidableIndexA];
hullA = convexShapes[colA.m_shapeIndex];
//printf("numvertsA = %d\n",hullA.m_numVertices);
b3Collidable colB = collidables[collidableIndexB];
hullB = convexShapes[colB.m_shapeIndex];
//printf("numvertsB = %d\n",hullB.m_numVertices);
b3Collidable colA = collidables[collidableIndexA];
hullA = convexShapes[colA.m_shapeIndex];
//printf("numvertsA = %d\n",hullA.m_numVertices);
b3Collidable colB = collidables[collidableIndexB];
hullB = convexShapes[colB.m_shapeIndex];
//printf("numvertsB = %d\n",hullB.m_numVertices);
#ifdef _WIN32
b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
#endif
bool foundSepAxis = b3FindSeparatingAxis(hullA,hullB,
posA,
ornA,
posB,
ornB,
convexVertices,uniqueEdges,faces,convexIndices,
convexVertices,uniqueEdges,faces,convexIndices,
sepNormalWorldSpace
);
bool foundSepAxis = b3FindSeparatingAxis(hullA, hullB,
posA,
ornA,
posB,
ornB,
convexVertices, uniqueEdges, faces, convexIndices,
convexVertices, uniqueEdges, faces, convexIndices,
sepNormalWorldSpace);
if (foundSepAxis)
{
contactIndex = b3ClipHullHullSingle(
bodyIndexA, bodyIndexB,
posA,ornA,
posB,ornB,
posA, ornA,
posB, ornB,
collidableIndexA, collidableIndexB,
&rigidBodies,
&rigidBodies,
&globalContactsOut,
nGlobalContactsOut,
convexShapes,
convexShapes,
convexVertices,
uniqueEdges,
convexVertices,
uniqueEdges,
faces,
convexIndices,
convexVertices,
uniqueEdges,
faces,
@@ -511,10 +478,9 @@ inline int b3ContactConvexConvexSAT(
collidables,
sepNormalWorldSpace,
maxContactCapacity);
}
return contactIndex;
}
#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H
#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H

137
src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h
View File

@@ -2,32 +2,24 @@
#ifndef B3_CONTACT_SPHERE_SPHERE_H
#define B3_CONTACT_SPHERE_SPHERE_H
void computeContactSphereConvex(int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
const b3RigidBodyData* rigidBodies,
const b3Collidable* collidables,
const b3ConvexPolyhedronData* convexShapes,
const b3Vector3* convexVertices,
const int* convexIndices,
const b3GpuFace* faces,
b3Contact4* globalContactsOut,
int& nGlobalContactsOut,
int maxContactCapacity)
void computeContactSphereConvex(int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
const b3RigidBodyData* rigidBodies,
const b3Collidable* collidables,
const b3ConvexPolyhedronData* convexShapes,
const b3Vector3* convexVertices,
const int* convexIndices,
const b3GpuFace* faces,
b3Contact4* globalContactsOut,
int& nGlobalContactsOut,
int maxContactCapacity)
{
float radius = collidables[collidableIndexA].m_radius;
float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
float4 pos = rigidBodies[bodyIndexB].m_pos;
b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
@@ -44,63 +36,64 @@ void computeContactSphereConvex(int pairIndex,
int numFaces = convexShapes[shapeIndex].m_numFaces;
float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
float minDist = -1000000.f; // TODO: What is the largest/smallest float?
float minDist = -1000000.f; // TODO: What is the largest/smallest float?
bool bCollide = true;
int region = -1;
float4 localHitNormal;
for ( int f = 0; f < numFaces; f++ )
for (int f = 0; f < numFaces; f++)
{
b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];
b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset + f];
float4 planeEqn;
float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal);
float4 localPlaneNormal = b3MakeVector3(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
float4 n1 = localPlaneNormal; //quatRotate(quat,localPlaneNormal);
planeEqn = n1;
planeEqn[3] = face.m_plane.w;
float4 pntReturn;
float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
if ( dist > radius)
if (dist > radius)
{
bCollide = false;
break;
}
if ( dist > 0 )
if (dist > 0)
{
//might hit an edge or vertex
b3Vector3 out;
bool isInPoly = IsPointInPolygon(spherePos,
&face,
&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
convexIndices,
&out);
&face,
&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
convexIndices,
&out);
if (isInPoly)
{
if (dist>minDist)
if (dist > minDist)
{
minDist = dist;
closestPnt = pntReturn;
localHitNormal = planeEqn;
region=1;
region = 1;
}
} else
}
else
{
b3Vector3 tmp = spherePos-out;
b3Vector3 tmp = spherePos - out;
b3Scalar l2 = tmp.length2();
if (l2<radius*radius)
if (l2 < radius * radius)
{
dist = b3Sqrt(l2);
if (dist>minDist)
dist = b3Sqrt(l2);
if (dist > minDist)
{
minDist = dist;
closestPnt = out;
localHitNormal = tmp/dist;
region=2;
localHitNormal = tmp / dist;
region = 2;
}
} else
}
else
{
bCollide = false;
break;
@@ -109,12 +102,12 @@ void computeContactSphereConvex(int pairIndex,
}
else
{
if ( dist > minDist )
if (dist > minDist)
{
minDist = dist;
closestPnt = pntReturn;
localHitNormal = planeEqn;
region=3;
region = 3;
}
}
}
@@ -123,40 +116,38 @@ void computeContactSphereConvex(int pairIndex,
if (bCollide && minDist > -10000)
{
float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld;
float4 normalOnSurfaceB1 = tr.getBasis() * localHitNormal; //-hitNormalWorld;
float4 pOnB1 = tr(closestPnt);
//printf("dist ,%f,",minDist);
float actualDepth = minDist-radius;
if (actualDepth<0)
float actualDepth = minDist - radius;
if (actualDepth < 0)
{
//printf("actualDepth = ,%f,", actualDepth);
//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
//printf("region=,%d,\n", region);
pOnB1[3] = actualDepth;
//printf("actualDepth = ,%f,", actualDepth);
//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
//printf("region=,%d,\n", region);
pOnB1[3] = actualDepth;
int dstIdx;
// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
if (nGlobalContactsOut < maxContactCapacity)
{
dstIdx=nGlobalContactsOut;
nGlobalContactsOut++;
int dstIdx;
// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
b3Contact4* c = &globalContactsOut[dstIdx];
c->m_worldNormalOnB = normalOnSurfaceB1;
c->setFrictionCoeff(0.7);
c->setRestituitionCoeff(0.f);
if (nGlobalContactsOut < maxContactCapacity)
{
dstIdx = nGlobalContactsOut;
nGlobalContactsOut++;
c->m_batchIdx = pairIndex;
c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
c->m_worldPosB[0] = pOnB1;
int numPoints = 1;
c->m_worldNormalOnB.w = (b3Scalar)numPoints;
}//if (dstIdx < numPairs)
b3Contact4* c = &globalContactsOut[dstIdx];
c->m_worldNormalOnB = normalOnSurfaceB1;
c->setFrictionCoeff(0.7);
c->setRestituitionCoeff(0.f);
c->m_batchIdx = pairIndex;
c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
c->m_worldPosB[0] = pOnB1;
int numPoints = 1;
c->m_worldNormalOnB.w = (b3Scalar)numPoints;
} //if (dstIdx < numPairs)
}
}//if (hasCollision)
} //if (hasCollision)
}
#endif //B3_CONTACT_SPHERE_SPHERE_H
#endif //B3_CONTACT_SPHERE_SPHERE_H

22
src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h
View File

@@ -2,8 +2,6 @@
#ifndef B3_CONVEX_POLYHEDRON_DATA_H
#define B3_CONVEX_POLYHEDRON_DATA_H
#include "Bullet3Common/shared/b3Float4.h"
#include "Bullet3Common/shared/b3Quat.h"
@@ -21,20 +19,20 @@ typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;
struct b3ConvexPolyhedronData
{
b3Float4 m_localCenter;
b3Float4 m_extents;
b3Float4 mC;
b3Float4 mE;
b3Float4 m_localCenter;
b3Float4 m_extents;
b3Float4 mC;
b3Float4 mE;
float m_radius;
int m_faceOffset;
float m_radius;
int m_faceOffset;
int m_numFaces;
int m_numVertices;
int m_numVertices;
int m_vertexOffset;
int m_uniqueEdgesOffset;
int m_numUniqueEdges;
int m_uniqueEdgesOffset;
int m_numUniqueEdges;
int m_unused;
};
#endif //B3_CONVEX_POLYHEDRON_DATA_H
#endif //B3_CONVEX_POLYHEDRON_DATA_H

925
src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
View File

File diff suppressed because it is too large Load Diff

147
src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h
View File

@@ -1,28 +1,27 @@
#ifndef B3_FIND_SEPARATING_AXIS_H
#define B3_FIND_SEPARATING_AXIS_H
inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
{
min = FLT_MAX;
max = -FLT_MAX;
int numVerts = hull.m_numVertices;
const b3Float4 localDir = b3QuatRotate(orn.inverse(),dir);
const b3Float4 localDir = b3QuatRotate(orn.inverse(), dir);
b3Scalar offset = b3Dot3F4(pos,dir);
b3Scalar offset = b3Dot3F4(pos, dir);
for(int i=0;i<numVerts;i++)
for (int i = 0; i < numVerts; i++)
{
//b3Vector3 pt = trans * vertices[m_vertexOffset+i];
//b3Scalar dp = pt.dot(dir);
//b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset+i],localDir);
b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset + i], localDir);
//b3Assert(dp==dpL);
if(dp < min) min = dp;
if(dp > max) max = dp;
if (dp < min) min = dp;
if (dp > max) max = dp;
}
if(min>max)
if (min > max)
{
b3Scalar tmp = min;
min = max;
@@ -32,44 +31,42 @@ inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& p
max += offset;
}
inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
const b3Float4& posA,const b3Quaternion& ornA,
const b3Float4& posB,const b3Quaternion& ornB,
const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& verticesB,b3Scalar& depth)
inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
const b3Float4& posA, const b3Quaternion& ornA,
const b3Float4& posB, const b3Quaternion& ornB,
const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB, b3Scalar& depth)
{
b3Scalar Min0,Max0;
b3Scalar Min1,Max1;
b3ProjectAxis(hullA,posA,ornA,sep_axis,verticesA, Min0, Max0);
b3ProjectAxis(hullB,posB,ornB, sep_axis,verticesB, Min1, Max1);
b3Scalar Min0, Max0;
b3Scalar Min1, Max1;
b3ProjectAxis(hullA, posA, ornA, sep_axis, verticesA, Min0, Max0);
b3ProjectAxis(hullB, posB, ornB, sep_axis, verticesB, Min1, Max1);
if(Max0<Min1 || Max1<Min0)
if (Max0 < Min1 || Max1 < Min0)
return false;
b3Scalar d0 = Max0 - Min1;
b3Assert(d0>=0.0f);
b3Assert(d0 >= 0.0f);
b3Scalar d1 = Max1 - Min0;
b3Assert(d1>=0.0f);
depth = d0<d1 ? d0:d1;
b3Assert(d1 >= 0.0f);
depth = d0 < d1 ? d0 : d1;
return true;
}
inline bool b3FindSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
const b3Float4& posA1,
const b3Quaternion& ornA,
const b3Float4& posB1,
const b3Quaternion& ornB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
const b3AlignedObjectArray<b3GpuFace>& facesA,
const b3AlignedObjectArray<int>& indicesA,
const b3AlignedObjectArray<b3Vector3>& verticesB,
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
const b3AlignedObjectArray<b3GpuFace>& facesB,
const b3AlignedObjectArray<int>& indicesB,
inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
const b3Float4& posA1,
const b3Quaternion& ornA,
const b3Float4& posB1,
const b3Quaternion& ornB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
const b3AlignedObjectArray<b3GpuFace>& facesA,
const b3AlignedObjectArray<int>& indicesA,
const b3AlignedObjectArray<b3Vector3>& verticesB,
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
const b3AlignedObjectArray<b3GpuFace>& facesB,
const b3AlignedObjectArray<int>& indicesB,
b3Vector3& sep)
b3Vector3& sep)
{
B3_PROFILE("findSeparatingAxis");
@@ -77,42 +74,41 @@ inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3C
posA.w = 0.f;
b3Float4 posB = posB1;
posB.w = 0.f;
//#ifdef TEST_INTERNAL_OBJECTS
//#ifdef TEST_INTERNAL_OBJECTS
b3Float4 c0local = (b3Float4&)hullA.m_localCenter;
b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
b3Float4 c1local = (b3Float4&)hullB.m_localCenter;
b3Float4 c1 = b3TransformPoint(c1local,posB,ornB);
b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
const b3Float4 deltaC2 = c0 - c1;
//#endif
//#endif
b3Scalar dmin = FLT_MAX;
int curPlaneTests=0;
int curPlaneTests = 0;
int numFacesA = hullA.m_numFaces;
// Test normals from hullA
for(int i=0;i<numFacesA;i++)
for (int i = 0; i < numFacesA; i++)
{
const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset+i].m_plane;
b3Float4 faceANormalWS = b3QuatRotate(ornA,normal);
const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset + i].m_plane;
b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
if (b3Dot3F4(deltaC2,faceANormalWS)<0)
faceANormalWS*=-1.f;
if (b3Dot3F4(deltaC2, faceANormalWS) < 0)
faceANormalWS *= -1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
b3Scalar d;
if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,faceANormalWS, verticesA, verticesB,d))
if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d))
return false;
if(d<dmin)
if (d < dmin)
{
dmin = d;
sep = (b3Vector3&)faceANormalWS;
@@ -121,86 +117,81 @@ inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3C
int numFacesB = hullB.m_numFaces;
// Test normals from hullB
for(int i=0;i<numFacesB;i++)
for (int i = 0; i < numFacesB; i++)
{
b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset+i].m_plane;
b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset + i].m_plane;
b3Float4 WorldNormal = b3QuatRotate(ornB, normal);
if (b3Dot3F4(deltaC2,WorldNormal)<0)
if (b3Dot3F4(deltaC2, WorldNormal) < 0)
{
WorldNormal*=-1.f;
WorldNormal *= -1.f;
}
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
b3Scalar d;
if(!b3TestSepAxis(hullA, hullB,posA,ornA,posB,ornB,WorldNormal,verticesA,verticesB,d))
if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d))
return false;
if(d<dmin)
if (d < dmin)
{
dmin = d;
sep = (b3Vector3&)WorldNormal;
}
}
// b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
// b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
int curEdgeEdge = 0;
// Test edges
for(int e0=0;e0<hullA.m_numUniqueEdges;e0++)
for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++)
{
const b3Float4& edge0 = (b3Float4&) uniqueEdgesA[hullA.m_uniqueEdgesOffset+e0];
b3Float4 edge0World = b3QuatRotate(ornA,(b3Float4&)edge0);
const b3Float4& edge0 = (b3Float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0];
b3Float4 edge0World = b3QuatRotate(ornA, (b3Float4&)edge0);
for(int e1=0;e1<hullB.m_numUniqueEdges;e1++)
for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++)
{
const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset+e1];
b3Float4 edge1World = b3QuatRotate(ornB,(b3Float4&)edge1);
const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1];
b3Float4 edge1World = b3QuatRotate(ornB, (b3Float4&)edge1);
b3Float4 crossje = b3Cross3(edge0World,edge1World);
b3Float4 crossje = b3Cross3(edge0World, edge1World);
curEdgeEdge++;
if(!b3IsAlmostZero((b3Vector3&)crossje))
if (!b3IsAlmostZero((b3Vector3&)crossje))
{
crossje = b3FastNormalized3(crossje);
if (b3Dot3F4(deltaC2,crossje)<0)
crossje*=-1.f;
if (b3Dot3F4(deltaC2, crossje) < 0)
crossje *= -1.f;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
b3Scalar dist;
if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,crossje, verticesA,verticesB,dist))
if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist))
return false;
if(dist<dmin)
if (dist < dmin)
{
dmin = dist;
sep = (b3Vector3&)crossje;
}
}
}
}
if((b3Dot3F4(-deltaC2,(b3Float4&)sep))>0.0f)
if ((b3Dot3F4(-deltaC2, (b3Float4&)sep)) > 0.0f)
sep = -sep;
return true;
}
#endif //B3_FIND_SEPARATING_AXIS_H
#endif //B3_FIND_SEPARATING_AXIS_H

1190
src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h
View File

File diff suppressed because it is too large Load Diff

285
src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h
View File

@@ -8,189 +8,168 @@
#define GET_NPOINTS(x) (x).m_worldNormalOnB.w
int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx)
{
if( nPoints == 0 )
return 0;
if (nPoints <=4)
return nPoints;
if (nPoints >64)
nPoints = 64;
b3Float4 center = b3MakeFloat4(0,0,0,0);
if (nPoints == 0)
return 0;
if (nPoints <= 4)
return nPoints;
if (nPoints > 64)
nPoints = 64;
b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
{
for (int i=0;i<nPoints;i++)
for (int i = 0; i < nPoints; i++)
center += p[i];
center /= (float)nPoints;
}
// sample 4 directions
b3Float4 aVector = p[0] - center;
b3Float4 u = b3Cross( nearNormal, aVector );
b3Float4 v = b3Cross( nearNormal, u );
u = b3Normalized( u );
v = b3Normalized( v );
//keep point with deepest penetration
float minW= FLT_MAX;
int minIndex=-1;
b3Float4 maxDots;
maxDots.x = FLT_MIN;
maxDots.y = FLT_MIN;
maxDots.z = FLT_MIN;
maxDots.w = FLT_MIN;
// idx, distance
for(int ie = 0; ie<nPoints; ie++ )
{
if (p[ie].w<minW)
{
minW = p[ie].w;
minIndex=ie;
}
float f;
b3Float4 r = p[ie]-center;
f = b3Dot( u, r );
if (f<maxDots.x)
{
maxDots.x = f;
contactIdx[0].x = ie;
}
f = b3Dot( -u, r );
if (f<maxDots.y)
{
maxDots.y = f;
contactIdx[0].y = ie;
}
f = b3Dot( v, r );
if (f<maxDots.z)
{
maxDots.z = f;
contactIdx[0].z = ie;
}
f = b3Dot( -v, r );
if (f<maxDots.w)
{
maxDots.w = f;
contactIdx[0].w = ie;
}
}
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
{
//replace the first contact with minimum (todo: replace contact with least penetration)
contactIdx[0].x = minIndex;
}
return 4;
b3Float4 aVector = p[0] - center;
b3Float4 u = b3Cross(nearNormal, aVector);
b3Float4 v = b3Cross(nearNormal, u);
u = b3Normalized(u);
v = b3Normalized(v);
//keep point with deepest penetration
float minW = FLT_MAX;
int minIndex = -1;
b3Float4 maxDots;
maxDots.x = FLT_MIN;
maxDots.y = FLT_MIN;
maxDots.z = FLT_MIN;
maxDots.w = FLT_MIN;
// idx, distance
for (int ie = 0; ie < nPoints; ie++)
{
if (p[ie].w < minW)
{
minW = p[ie].w;
minIndex = ie;
}
float f;
b3Float4 r = p[ie] - center;
f = b3Dot(u, r);
if (f < maxDots.x)
{
maxDots.x = f;
contactIdx[0].x = ie;
}
f = b3Dot(-u, r);
if (f < maxDots.y)
{
maxDots.y = f;
contactIdx[0].y = ie;
}
f = b3Dot(v, r);
if (f < maxDots.z)
{
maxDots.z = f;
contactIdx[0].z = ie;
}
f = b3Dot(-v, r);
if (f < maxDots.w)
{
maxDots.w = f;
contactIdx[0].w = ie;
}
}
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
{
//replace the first contact with minimum (todo: replace contact with least penetration)
contactIdx[0].x = minIndex;
}
return 4;
}
__kernel void b3NewContactReductionKernel( __global b3Int4* pairs,
__global const b3RigidBodyData_t* rigidBodies,
__global const b3Float4* separatingNormals,
__global const int* hasSeparatingAxis,
__global struct b3Contact4Data* globalContactsOut,
__global b3Int4* clippingFaces,
__global b3Float4* worldVertsB2,
volatile __global int* nGlobalContactsOut,
int vertexFaceCapacity,
int contactCapacity,
int numPairs,
int pairIndex
)
__kernel void b3NewContactReductionKernel(__global b3Int4* pairs,
__global const b3RigidBodyData_t* rigidBodies,
__global const b3Float4* separatingNormals,
__global const int* hasSeparatingAxis,
__global struct b3Contact4Data* globalContactsOut,
__global b3Int4* clippingFaces,
__global b3Float4* worldVertsB2,
volatile __global int* nGlobalContactsOut,
int vertexFaceCapacity,
int contactCapacity,
int numPairs,
int pairIndex)
{
// int i = get_global_id(0);
// int i = get_global_id(0);
//int pairIndex = i;
int i = pairIndex;
b3Int4 contactIdx;
contactIdx=b3MakeInt4(0,1,2,3);
if (i<numPairs)
b3Int4 contactIdx;
contactIdx = b3MakeInt4(0, 1, 2, 3);
if (i < numPairs)
{
if (hasSeparatingAxis[i])
{
int nPoints = clippingFaces[pairIndex].w;
if (nPoints>0)
{
__global b3Float4* pointsIn = &worldVertsB2[pairIndex*vertexFaceCapacity];
b3Float4 normal = -separatingNormals[i];
int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
int dstIdx;
dstIdx = b3AtomicInc( nGlobalContactsOut);
//#if 0
b3Assert(dstIdx < contactCapacity);
if (nPoints > 0)
{
__global b3Float4* pointsIn = &worldVertsB2[pairIndex * vertexFaceCapacity];
b3Float4 normal = -separatingNormals[i];
int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
int dstIdx;
dstIdx = b3AtomicInc(nGlobalContactsOut);
//#if 0
b3Assert(dstIdx < contactCapacity);
if (dstIdx < contactCapacity)
{
__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
c->m_worldNormalOnB = -normal;
c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
c->m_restituitionCoeffCmp = (0.f * 0xffff);
c->m_frictionCoeffCmp = (0.7f * 0xffff);
c->m_batchIdx = pairIndex;
int bodyA = pairs[pairIndex].x;
int bodyB = pairs[pairIndex].y;
pairs[pairIndex].w = dstIdx;
c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;
c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;
c->m_childIndexA =-1;
c->m_childIndexB =-1;
c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass == 0 ? -bodyA : bodyA;
c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass == 0 ? -bodyB : bodyB;
c->m_childIndexA = -1;
c->m_childIndexB = -1;
switch (nReducedContacts)
{
case 4:
c->m_worldPosB[3] = pointsIn[contactIdx.w];
case 3:
c->m_worldPosB[2] = pointsIn[contactIdx.z];
case 2:
c->m_worldPosB[1] = pointsIn[contactIdx.y];
case 1:
c->m_worldPosB[0] = pointsIn[contactIdx.x];
default:
{
}
};
switch (nReducedContacts)
{
case 4:
c->m_worldPosB[3] = pointsIn[contactIdx.w];
case 3:
c->m_worldPosB[2] = pointsIn[contactIdx.z];
case 2:
c->m_worldPosB[1] = pointsIn[contactIdx.y];
case 1:
c->m_worldPosB[0] = pointsIn[contactIdx.x];
default:
{
}
};
GET_NPOINTS(*c) = nReducedContacts;
}
//#endif
}// if (numContactsOut>0)
}// if (hasSeparatingAxis[i])
}// if (i<numPairs)
}
//#endif
} // if (numContactsOut>0)
} // if (hasSeparatingAxis[i])
} // if (i<numPairs)
}
#endif

54
src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h
View File

@@ -14,57 +14,56 @@ typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t;
struct b3QuantizedBvhNodeData
{
//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;
};
inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
{
unsigned int x=0;
unsigned int y = (~(x&0))<<(31-B3_MAX_NUM_PARTS_IN_BITS);
unsigned int x = 0;
unsigned int y = (~(x & 0)) << (31 - B3_MAX_NUM_PARTS_IN_BITS);
// Get only the lower bits where the triangle index is stored
return (rootNode->m_escapeIndexOrTriangleIndex&~(y));
return (rootNode->m_escapeIndexOrTriangleIndex & ~(y));
}
inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode)
{
//skipindex is negative (internal node), triangleindex >=0 (leafnode)
return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;
return (rootNode->m_escapeIndexOrTriangleIndex >= 0) ? 1 : 0;
}
inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode)
{
return -rootNode->m_escapeIndexOrTriangleIndex;
}
inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2,int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2, int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
{
b3Float4 clampedPoint = b3MaxFloat4(point2,bvhAabbMin);
clampedPoint = b3MinFloat4 (clampedPoint, bvhAabbMax);
b3Float4 clampedPoint = b3MaxFloat4(point2, bvhAabbMin);
clampedPoint = b3MinFloat4(clampedPoint, bvhAabbMax);
b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
if (isMax)
{
out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1));
out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1));
out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1));
} else
{
out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe));
out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe));
out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe));
out[0] = (unsigned short)(((unsigned short)(v.x + 1.f) | 1));
out[1] = (unsigned short)(((unsigned short)(v.y + 1.f) | 1));
out[2] = (unsigned short)(((unsigned short)(v.z + 1.f) | 1));
}
else
{
out[0] = (unsigned short)(((unsigned short)(v.x) & 0xfffe));
out[1] = (unsigned short)(((unsigned short)(v.y) & 0xfffe));
out[2] = (unsigned short)(((unsigned short)(v.z) & 0xfffe));
}
}
inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
const unsigned short int* aabbMin1,
const unsigned short int* aabbMax1,
const unsigned short int* aabbMin2,
const unsigned short int* aabbMax2)
const unsigned short int* aabbMin1,
const unsigned short int* aabbMax1,
const unsigned short int* aabbMin2,
const unsigned short int* aabbMax2)
{
//int overlap = 1;
if (aabbMin1[0] > aabbMax2[0])
@@ -86,5 +85,4 @@ inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
//return overlap;
}
#endif //B3_QUANTIZED_BVH_NODE_H
#endif //B3_QUANTIZED_BVH_NODE_H

162
src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h
View File

@@ -3,95 +3,87 @@
inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx)
{
if( nPoints == 0 )
return 0;
if (nPoints <=4)
return nPoints;
if (nPoints >64)
nPoints = 64;
b3Float4 center = b3MakeFloat4(0,0,0,0);
if (nPoints == 0)
return 0;
if (nPoints <= 4)
return nPoints;
if (nPoints > 64)
nPoints = 64;
b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
{
for (int i=0;i<nPoints;i++)
for (int i = 0; i < nPoints; i++)
center += p[i];
center /= (float)nPoints;
}
// sample 4 directions
b3Float4 aVector = p[0] - center;
b3Float4 u = b3Cross3( nearNormal, aVector );
b3Float4 v = b3Cross3( nearNormal, u );
u = b3FastNormalized3( u );
v = b3FastNormalized3( v );
//keep point with deepest penetration
float minW= FLT_MAX;
int minIndex=-1;
b3Float4 maxDots;
maxDots.x = FLT_MIN;
maxDots.y = FLT_MIN;
maxDots.z = FLT_MIN;
maxDots.w = FLT_MIN;
// idx, distance
for(int ie = 0; ie<nPoints; ie++ )
{
if (p[ie].w<minW)
{
minW = p[ie].w;
minIndex=ie;
}
float f;
b3Float4 r = p[ie]-center;
f = b3Dot3F4( u, r );
if (f<maxDots.x)
{
maxDots.x = f;
contactIdx[0].x = ie;
}
f = b3Dot3F4( -u, r );
if (f<maxDots.y)
{
maxDots.y = f;
contactIdx[0].y = ie;
}
f = b3Dot3F4( v, r );
if (f<maxDots.z)
{
maxDots.z = f;
contactIdx[0].z = ie;
}
f = b3Dot3F4( -v, r );
if (f<maxDots.w)
{
maxDots.w = f;
contactIdx[0].w = ie;
}
}
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
{
//replace the first contact with minimum (todo: replace contact with least penetration)
contactIdx[0].x = minIndex;
}
return 4;
b3Float4 aVector = p[0] - center;
b3Float4 u = b3Cross3(nearNormal, aVector);
b3Float4 v = b3Cross3(nearNormal, u);
u = b3FastNormalized3(u);
v = b3FastNormalized3(v);
//keep point with deepest penetration
float minW = FLT_MAX;
int minIndex = -1;
b3Float4 maxDots;
maxDots.x = FLT_MIN;
maxDots.y = FLT_MIN;
maxDots.z = FLT_MIN;
maxDots.w = FLT_MIN;
// idx, distance
for (int ie = 0; ie < nPoints; ie++)
{
if (p[ie].w < minW)
{
minW = p[ie].w;
minIndex = ie;
}
float f;
b3Float4 r = p[ie] - center;
f = b3Dot3F4(u, r);
if (f < maxDots.x)
{
maxDots.x = f;
contactIdx[0].x = ie;
}
f = b3Dot3F4(-u, r);
if (f < maxDots.y)
{
maxDots.y = f;
contactIdx[0].y = ie;
}
f = b3Dot3F4(v, r);
if (f < maxDots.z)
{
maxDots.z = f;
contactIdx[0].z = ie;
}
f = b3Dot3F4(-v, r);
if (f < maxDots.w)
{
maxDots.w = f;
contactIdx[0].w = ie;
}
}
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
{
//replace the first contact with minimum (todo: replace contact with least penetration)
contactIdx[0].x = minIndex;
}
return 4;
}
#endif //B3_REDUCE_CONTACTS_H
#endif //B3_REDUCE_CONTACTS_H

21
src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h
View File

@@ -7,18 +7,17 @@
typedef struct b3RigidBodyData b3RigidBodyData_t;
struct b3RigidBodyData
{
b3Float4 m_pos;
b3Quat m_quat;
b3Float4 m_linVel;
b3Float4 m_angVel;
b3Float4 m_pos;
b3Quat m_quat;
b3Float4 m_linVel;
b3Float4 m_angVel;
int m_collidableIdx;
float m_invMass;
float m_restituitionCoeff;
float m_frictionCoeff;
int m_collidableIdx;
float m_invMass;
float m_restituitionCoeff;
float m_frictionCoeff;
};
typedef struct b3InertiaData b3InertiaData_t;
@@ -29,6 +28,4 @@ struct b3InertiaData
b3Mat3x3 m_initInvInertia;
};
#endif //B3_RIGIDBODY_DATA_H
#endif //B3_RIGIDBODY_DATA_H

29
src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h
View File

@@ -1,40 +1,35 @@
#ifndef B3_UPDATE_AABBS_H
#define B3_UPDATE_AABBS_H
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
void b3ComputeWorldAabb( int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)
void b3ComputeWorldAabb(int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)
{
__global const b3RigidBodyData_t* body = &bodies[bodyId];
b3Float4 position = body->m_pos;
b3Quat orientation = body->m_quat;
b3Quat orientation = body->m_quat;
int collidableIndex = body->m_collidableIdx;
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
if (shapeIndex>=0)
if (shapeIndex >= 0)
{
b3Aabb_t localAabb = localShapeAABB[collidableIndex];
b3Aabb_t worldAabb;
b3Float4 aabbAMinOut,aabbAMaxOut;
b3Float4 aabbAMinOut, aabbAMaxOut;
float margin = 0.f;
b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&aabbAMinOut,&aabbAMaxOut);
worldAabb.m_minVec =aabbAMinOut;
b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &aabbAMinOut, &aabbAMaxOut);
worldAabb.m_minVec = aabbAMinOut;
worldAabb.m_minIndices[3] = bodyId;
worldAabb.m_maxVec = aabbAMaxOut;
worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass==0.f? 0 : 1;
worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass == 0.f ? 0 : 1;
worldAabbs[bodyId] = worldAabb;
}
}
#endif //B3_UPDATE_AABBS_H
#endif //B3_UPDATE_AABBS_H