godotengine
/
godot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Update Bullet to the latest commit 126b676

This commit is contained in:
Oussama 2019-01-03 14:26:51 +01:00 committed by Rémi Verschelde
parent a6722cf362
commit 22b7c9dfa8
612 changed files with 114715 additions and 103413 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
thirdparty/README.md vendored
View File

@ -19,7 +19,7 @@ comments.
## bullet
- Upstream: https://github.com/bulletphysics/bullet3
- Version: git (12409f1118a7c7a266f9071350c70789dfe73bb9, Commits on Sep 6, 2018 )
- Version: git (126b676, 2018-12-31)
- License: zlib
Files extracted from upstream source:

16
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h vendored
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
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp vendored
View File

File diff suppressed because it is too large Load Diff

1593
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h vendored
View File

File diff suppressed because it is too large Load Diff

818
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp vendored
View File

File diff suppressed because it is too large Load Diff

193
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h vendored
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
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h vendored
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
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp vendored
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
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h vendored
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
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Config.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h vendored
View File

File diff suppressed because it is too large Load Diff

147
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h vendored
View File

File diff suppressed because it is too large Load Diff

285
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h vendored
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
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h vendored
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

181
thirdparty/bullet/Bullet3Common/b3AlignedAllocator.cpp vendored
View File

@ -15,9 +15,11 @@ subject to the following restrictions:
#include "b3AlignedAllocator.h"
#ifdef B3_ALLOCATOR_STATISTICS
int b3g_numAlignedAllocs = 0;
int b3g_numAlignedFree = 0;
int b3g_totalBytesAlignedAllocs = 0;//detect memory leaks
int b3g_totalBytesAlignedAllocs = 0; //detect memory leaks
#endif
static void *b3AllocDefault(size_t size)
{
@ -29,12 +31,10 @@ static void b3FreeDefault(void *ptr)
free(ptr);
}
static b3AllocFunc* b3s_allocFunc = b3AllocDefault;
static b3FreeFunc* b3s_freeFunc = b3FreeDefault;
static b3AllocFunc *b3s_allocFunc = b3AllocDefault;
static b3FreeFunc *b3s_freeFunc = b3FreeDefault;
#if defined (B3_HAS_ALIGNED_ALLOCATOR)
#if defined(B3_HAS_ALIGNED_ALLOCATOR)
#include <malloc.h>
static void *b3AlignedAllocDefault(size_t size, int alignment)
{
@ -59,123 +59,128 @@ static inline void b3AlignedFreeDefault(void *ptr)
}
#else
static inline void *b3AlignedAllocDefault(size_t size, int alignment)
{
void *ret;
char *real;
real = (char *)b3s_allocFunc(size + sizeof(void *) + (alignment-1));
if (real) {
ret = b3AlignPointer(real + sizeof(void *),alignment);
*((void **)(ret)-1) = (void *)(real);
} else {
ret = (void *)(real);
}
return (ret);
void *ret;
char *real;
real = (char *)b3s_allocFunc(size + sizeof(void *) + (alignment - 1));
if (real)
{
ret = b3AlignPointer(real + sizeof(void *), alignment);
*((void **)(ret)-1) = (void *)(real);
}
else
{
ret = (void *)(real);
}
return (ret);
}
static inline void b3AlignedFreeDefault(void *ptr)
{
void* real;
void *real;
if (ptr) {
real = *((void **)(ptr)-1);
b3s_freeFunc(real);
}
if (ptr)
{
real = *((void **)(ptr)-1);
b3s_freeFunc(real);
}
}
#endif
static b3AlignedAllocFunc* b3s_alignedAllocFunc = b3AlignedAllocDefault;
static b3AlignedFreeFunc* b3s_alignedFreeFunc = b3AlignedFreeDefault;
static b3AlignedAllocFunc *b3s_alignedAllocFunc = b3AlignedAllocDefault;
static b3AlignedFreeFunc *b3s_alignedFreeFunc = b3AlignedFreeDefault;
void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc)
{
b3s_alignedAllocFunc = allocFunc ? allocFunc : b3AlignedAllocDefault;
b3s_alignedFreeFunc = freeFunc ? freeFunc : b3AlignedFreeDefault;
b3s_alignedAllocFunc = allocFunc ? allocFunc : b3AlignedAllocDefault;
b3s_alignedFreeFunc = freeFunc ? freeFunc : b3AlignedFreeDefault;
}
void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc)
{
b3s_allocFunc = allocFunc ? allocFunc : b3AllocDefault;
b3s_freeFunc = freeFunc ? freeFunc : b3FreeDefault;
b3s_allocFunc = allocFunc ? allocFunc : b3AllocDefault;
b3s_freeFunc = freeFunc ? freeFunc : b3FreeDefault;
}
#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
//this generic allocator provides the total allocated number of bytes
#include <stdio.h>
void* b3AlignedAllocInternal (size_t size, int alignment,int line,char* filename)
{
void *ret;
char *real;
b3g_totalBytesAlignedAllocs += size;
b3g_numAlignedAllocs++;
real = (char *)b3s_allocFunc(size + 2*sizeof(void *) + (alignment-1));
if (real) {
ret = (void*) b3AlignPointer(real + 2*sizeof(void *), alignment);
*((void **)(ret)-1) = (void *)(real);
*((int*)(ret)-2) = size;
} else {
ret = (void *)(real);//??
}
b3Printf("allocation#%d at address %x, from %s,line %d, size %d\n",b3g_numAlignedAllocs,real, filename,line,size);
int* ptr = (int*)ret;
*ptr = 12;
return (ret);
}
void b3AlignedFreeInternal (void* ptr,int line,char* filename)
{
void* real;
b3g_numAlignedFree++;
if (ptr) {
real = *((void **)(ptr)-1);
int size = *((int*)(ptr)-2);
b3g_totalBytesAlignedAllocs -= size;
b3Printf("free #%d at address %x, from %s,line %d, size %d\n",b3g_numAlignedFree,real, filename,line,size);
b3s_freeFunc(real);
} else
{
b3Printf("NULL ptr\n");
}
}
#else //B3_DEBUG_MEMORY_ALLOCATIONS
void* b3AlignedAllocInternal (size_t size, int alignment)
void *b3AlignedAllocInternal(size_t size, int alignment, int line, char *filename)
{
void *ret;
char *real;
#ifdef B3_ALLOCATOR_STATISTICS
b3g_totalBytesAlignedAllocs += size;
b3g_numAlignedAllocs++;
void* ptr;
#endif
real = (char *)b3s_allocFunc(size + 2 * sizeof(void *) + (alignment - 1));
if (real)
{
ret = (void *)b3AlignPointer(real + 2 * sizeof(void *), alignment);
*((void **)(ret)-1) = (void *)(real);
*((int *)(ret)-2) = size;
}
else
{
ret = (void *)(real); //??
}
b3Printf("allocation#%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedAllocs, real, filename, line, size);
int *ptr = (int *)ret;
*ptr = 12;
return (ret);
}
void b3AlignedFreeInternal(void *ptr, int line, char *filename)
{
void *real;
#ifdef B3_ALLOCATOR_STATISTICS
b3g_numAlignedFree++;
#endif
if (ptr)
{
real = *((void **)(ptr)-1);
int size = *((int *)(ptr)-2);
#ifdef B3_ALLOCATOR_STATISTICS
b3g_totalBytesAlignedAllocs -= size;
#endif
b3Printf("free #%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedFree, real, filename, line, size);
b3s_freeFunc(real);
}
else
{
b3Printf("NULL ptr\n");
}
}
#else //B3_DEBUG_MEMORY_ALLOCATIONS
void *b3AlignedAllocInternal(size_t size, int alignment)
{
#ifdef B3_ALLOCATOR_STATISTICS
b3g_numAlignedAllocs++;
#endif
void *ptr;
ptr = b3s_alignedAllocFunc(size, alignment);
// b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr);
// b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr);
return ptr;
}
void b3AlignedFreeInternal (void* ptr)
void b3AlignedFreeInternal(void *ptr)
{
if (!ptr)
{
return;
}
#ifdef B3_ALLOCATOR_STATISTICS
b3g_numAlignedFree++;
// b3Printf("b3AlignedFreeInternal %x\n",ptr);
#endif
// b3Printf("b3AlignedFreeInternal %x\n",ptr);
b3s_alignedFreeFunc(ptr);
}
#endif //B3_DEBUG_MEMORY_ALLOCATIONS
#endif //B3_DEBUG_MEMORY_ALLOCATIONS

99
thirdparty/bullet/Bullet3Common/b3AlignedAllocator.h vendored
View File

@ -24,84 +24,87 @@ subject to the following restrictions:
//#define B3_DEBUG_MEMORY_ALLOCATIONS 1
#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
#define b3AlignedAlloc(a,b) \
b3AlignedAllocInternal(a,b,__LINE__,__FILE__)
#define b3AlignedAlloc(a, b) \
b3AlignedAllocInternal(a, b, __LINE__, __FILE__)
#define b3AlignedFree(ptr) \
b3AlignedFreeInternal(ptr,__LINE__,__FILE__)
b3AlignedFreeInternal(ptr, __LINE__, __FILE__)
void* b3AlignedAllocInternal (size_t size, int alignment,int line,char* filename);
void* b3AlignedAllocInternal(size_t size, int alignment, int line, char* filename);
void b3AlignedFreeInternal (void* ptr,int line,char* filename);
void b3AlignedFreeInternal(void* ptr, int line, char* filename);
#else
void* b3AlignedAllocInternal (size_t size, int alignment);
void b3AlignedFreeInternal (void* ptr);
void* b3AlignedAllocInternal(size_t size, int alignment);
void b3AlignedFreeInternal(void* ptr);
#define b3AlignedAlloc(size,alignment) b3AlignedAllocInternal(size,alignment)
#define b3AlignedFree(ptr) b3AlignedFreeInternal(ptr)
#define b3AlignedAlloc(size, alignment) b3AlignedAllocInternal(size, alignment)
#define b3AlignedFree(ptr) b3AlignedFreeInternal(ptr)
#endif
typedef int btSizeType;
typedef int btSizeType;
typedef void *(b3AlignedAllocFunc)(size_t size, int alignment);
typedef void (b3AlignedFreeFunc)(void *memblock);
typedef void *(b3AllocFunc)(size_t size);
typedef void (b3FreeFunc)(void *memblock);
typedef void*(b3AlignedAllocFunc)(size_t size, int alignment);
typedef void(b3AlignedFreeFunc)(void* memblock);
typedef void*(b3AllocFunc)(size_t size);
typedef void(b3FreeFunc)(void* memblock);
///The developer can let all Bullet memory allocations go through a custom memory allocator, using b3AlignedAllocSetCustom
void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc);
void b3AlignedAllocSetCustom(b3AllocFunc* allocFunc, b3FreeFunc* freeFunc);
///If the developer has already an custom aligned allocator, then b3AlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc);
void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc* allocFunc, b3AlignedFreeFunc* freeFunc);
///The b3AlignedAllocator is a portable class for aligned memory allocations.
///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using b3AlignedAllocSetCustom and b3AlignedAllocSetCustomAligned.
template < typename T , unsigned Alignment >
class b3AlignedAllocator {
typedef b3AlignedAllocator< T , Alignment > self_type;
public:
template <typename T, unsigned Alignment>
class b3AlignedAllocator
{
typedef b3AlignedAllocator<T, Alignment> self_type;
public:
//just going down a list:
b3AlignedAllocator() {}
/*
b3AlignedAllocator( const self_type & ) {}
*/
template < typename Other >
b3AlignedAllocator( const b3AlignedAllocator< Other , Alignment > & ) {}
template <typename Other>
b3AlignedAllocator(const b3AlignedAllocator<Other, Alignment>&)
{
}
typedef const T* const_pointer;
typedef const T& const_reference;
typedef T* pointer;
typedef T& reference;
typedef T value_type;
typedef const T* const_pointer;
typedef const T& const_reference;
typedef T* pointer;
typedef T& reference;
typedef T value_type;
pointer address ( reference ref ) const { return &ref; }
const_pointer address ( const_reference ref ) const { return &ref; }
pointer allocate ( btSizeType n , const_pointer * hint = 0 ) {
pointer address(reference ref) const { return &ref; }
const_pointer address(const_reference ref) const { return &ref; }
pointer allocate(btSizeType n, const_pointer* hint = 0)
{
(void)hint;
return reinterpret_cast< pointer >(b3AlignedAlloc( sizeof(value_type) * n , Alignment ));
return reinterpret_cast<pointer>(b3AlignedAlloc(sizeof(value_type) * n, Alignment));
}
void construct ( pointer ptr , const value_type & value ) { new (ptr) value_type( value ); }
void deallocate( pointer ptr ) {
b3AlignedFree( reinterpret_cast< void * >( ptr ) );
void construct(pointer ptr, const value_type& value) { new (ptr) value_type(value); }
void deallocate(pointer ptr)
{
b3AlignedFree(reinterpret_cast<void*>(ptr));
}
void destroy ( pointer ptr ) { ptr->~value_type(); }
void destroy(pointer ptr) { ptr->~value_type(); }
template < typename O > struct rebind {
typedef b3AlignedAllocator< O , Alignment > other;
template <typename O>
struct rebind
{
typedef b3AlignedAllocator<O, Alignment> other;
};
template < typename O >
self_type & operator=( const b3AlignedAllocator< O , Alignment > & ) { return *this; }
template <typename O>
self_type& operator=(const b3AlignedAllocator<O, Alignment>&)
{
return *this;
}
friend bool operator==( const self_type & , const self_type & ) { return true; }
friend bool operator==(const self_type&, const self_type&) { return true; }
};
#endif //B3_ALIGNED_ALLOCATOR
#endif //B3_ALIGNED_ALLOCATOR

739
thirdparty/bullet/Bullet3Common/b3AlignedObjectArray.h vendored
View File

@ -13,11 +13,10 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_OBJECT_ARRAY__
#define B3_OBJECT_ARRAY__
#include "b3Scalar.h" // has definitions like B3_FORCE_INLINE
#include "b3Scalar.h" // has definitions like B3_FORCE_INLINE
#include "b3AlignedAllocator.h"
///If the platform doesn't support placement new, you can disable B3_USE_PLACEMENT_NEW
@ -28,402 +27,386 @@ subject to the following restrictions:
#define B3_USE_PLACEMENT_NEW 1
//#define B3_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
#define B3_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful
#define B3_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful
#ifdef B3_USE_MEMCPY
#include <memory.h>
#include <string.h>
#endif //B3_USE_MEMCPY
#endif //B3_USE_MEMCPY
#ifdef B3_USE_PLACEMENT_NEW
#include <new> //for placement new
#endif //B3_USE_PLACEMENT_NEW
#include <new> //for placement new
#endif //B3_USE_PLACEMENT_NEW
///The b3AlignedObjectArray template class uses a subset of the stl::vector interface for its methods
///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
template <typename T>
//template <class T>
template <typename T>
//template <class T>
class b3AlignedObjectArray
{
b3AlignedAllocator<T , 16> m_allocator;
b3AlignedAllocator<T, 16> m_allocator;
int m_size;
int m_capacity;
T* m_data;
int m_size;
int m_capacity;
T* m_data;
//PCK: added this line
bool m_ownsMemory;
bool m_ownsMemory;
#ifdef B3_ALLOW_ARRAY_COPY_OPERATOR
public:
B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T> &other)
B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T>& other)
{
copyFromArray(other);
return *this;
}
#else//B3_ALLOW_ARRAY_COPY_OPERATOR
#else //B3_ALLOW_ARRAY_COPY_OPERATOR
private:
B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T> &other);
#endif//B3_ALLOW_ARRAY_COPY_OPERATOR
B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T>& other);
#endif //B3_ALLOW_ARRAY_COPY_OPERATOR
protected:
B3_FORCE_INLINE int allocSize(int size)
{
return (size ? size*2 : 1);
}
B3_FORCE_INLINE void copy(int start,int end, T* dest) const
{
int i;
for (i=start;i<end;++i)
B3_FORCE_INLINE int allocSize(int size)
{
return (size ? size * 2 : 1);
}
B3_FORCE_INLINE void copy(int start, int end, T* dest) const
{
int i;
for (i = start; i < end; ++i)
#ifdef B3_USE_PLACEMENT_NEW
new (&dest[i]) T(m_data[i]);
new (&dest[i]) T(m_data[i]);
#else
dest[i] = m_data[i];
#endif //B3_USE_PLACEMENT_NEW
}
dest[i] = m_data[i];
#endif //B3_USE_PLACEMENT_NEW
}
B3_FORCE_INLINE void init()
B3_FORCE_INLINE void init()
{
//PCK: added this line
m_ownsMemory = true;
m_data = 0;
m_size = 0;
m_capacity = 0;
}
B3_FORCE_INLINE void destroy(int first, int last)
{
int i;
for (i = first; i < last; i++)
{
//PCK: added this line
m_ownsMemory = true;
m_data = 0;
m_size = 0;
m_capacity = 0;
m_data[i].~T();
}
B3_FORCE_INLINE void destroy(int first,int last)
}
B3_FORCE_INLINE void* allocate(int size)
{
if (size)
return m_allocator.allocate(size);
return 0;
}
B3_FORCE_INLINE void deallocate()
{
if (m_data)
{
int i;
for (i=first; i<last;i++)
//PCK: enclosed the deallocation in this block
if (m_ownsMemory)
{
m_allocator.deallocate(m_data);
}
m_data = 0;
}
}
public:
b3AlignedObjectArray()
{
init();
}
~b3AlignedObjectArray()
{
clear();
}
///Generally it is best to avoid using the copy constructor of an b3AlignedObjectArray, and use a (const) reference to the array instead.
b3AlignedObjectArray(const b3AlignedObjectArray& otherArray)
{
init();
int otherSize = otherArray.size();
resize(otherSize);
otherArray.copy(0, otherSize, m_data);
}
/// return the number of elements in the array
B3_FORCE_INLINE int size() const
{
return m_size;
}
B3_FORCE_INLINE const T& at(int n) const
{
b3Assert(n >= 0);
b3Assert(n < size());
return m_data[n];
}
B3_FORCE_INLINE T& at(int n)
{
b3Assert(n >= 0);
b3Assert(n < size());
return m_data[n];
}
B3_FORCE_INLINE const T& operator[](int n) const
{
b3Assert(n >= 0);
b3Assert(n < size());
return m_data[n];
}
B3_FORCE_INLINE T& operator[](int n)
{
b3Assert(n >= 0);
b3Assert(n < size());
return m_data[n];
}
///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
B3_FORCE_INLINE void clear()
{
destroy(0, size());
deallocate();
init();
}
B3_FORCE_INLINE void pop_back()
{
b3Assert(m_size > 0);
m_size--;
m_data[m_size].~T();
}
///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
B3_FORCE_INLINE void resizeNoInitialize(int newsize)
{
int curSize = size();
if (newsize < curSize)
{
}
else
{
if (newsize > size())
{
reserve(newsize);
}
//leave this uninitialized
}
m_size = newsize;
}
B3_FORCE_INLINE void resize(int newsize, const T& fillData = T())
{
int curSize = size();
if (newsize < curSize)
{
for (int i = newsize; i < curSize; i++)
{
m_data[i].~T();
}
}
B3_FORCE_INLINE void* allocate(int size)
else
{
if (size)
return m_allocator.allocate(size);
return 0;
}
B3_FORCE_INLINE void deallocate()
{
if(m_data) {
//PCK: enclosed the deallocation in this block
if (m_ownsMemory)
{
m_allocator.deallocate(m_data);
}
m_data = 0;
if (newsize > size())
{
reserve(newsize);
}
}
public:
b3AlignedObjectArray()
{
init();
}
~b3AlignedObjectArray()
{
clear();
}
///Generally it is best to avoid using the copy constructor of an b3AlignedObjectArray, and use a (const) reference to the array instead.
b3AlignedObjectArray(const b3AlignedObjectArray& otherArray)
{
init();
int otherSize = otherArray.size();
resize (otherSize);
otherArray.copy(0, otherSize, m_data);
}
/// return the number of elements in the array
B3_FORCE_INLINE int size() const
{
return m_size;
}
B3_FORCE_INLINE const T& at(int n) const
{
b3Assert(n>=0);
b3Assert(n<size());
return m_data[n];
}
B3_FORCE_INLINE T& at(int n)
{
b3Assert(n>=0);
b3Assert(n<size());
return m_data[n];
}
B3_FORCE_INLINE const T& operator[](int n) const
{
b3Assert(n>=0);
b3Assert(n<size());
return m_data[n];
}
B3_FORCE_INLINE T& operator[](int n)
{
b3Assert(n>=0);
b3Assert(n<size());
return m_data[n];
}
///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
B3_FORCE_INLINE void clear()
{
destroy(0,size());
deallocate();
init();
}
B3_FORCE_INLINE void pop_back()
{
b3Assert(m_size>0);
m_size--;
m_data[m_size].~T();
}
///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
B3_FORCE_INLINE void resizeNoInitialize(int newsize)
{
int curSize = size();
if (newsize < curSize)
{
} else
{
if (newsize > size())
{
reserve(newsize);
}
//leave this uninitialized
}
m_size = newsize;
}
B3_FORCE_INLINE void resize(int newsize, const T& fillData=T())
{
int curSize = size();
if (newsize < curSize)
{
for(int i = newsize; i < curSize; i++)
{
m_data[i].~T();
}
} else
{
if (newsize > size())
{
reserve(newsize);
}
#ifdef B3_USE_PLACEMENT_NEW
for (int i=curSize;i<newsize;i++)
{
new ( &m_data[i]) T(fillData);
}
#endif //B3_USE_PLACEMENT_NEW
}
m_size = newsize;
}
B3_FORCE_INLINE T& expandNonInitializing( )
{
int sz = size();
if( sz == capacity() )
for (int i = curSize; i < newsize; i++)
{
reserve( allocSize(size()) );
new (&m_data[i]) T(fillData);
}
m_size++;
return m_data[sz];
#endif //B3_USE_PLACEMENT_NEW
}
m_size = newsize;
}
B3_FORCE_INLINE T& expandNonInitializing()
{
int sz = size();
if (sz == capacity())
{
reserve(allocSize(size()));
}
m_size++;
B3_FORCE_INLINE T& expand( const T& fillValue=T())
{
int sz = size();
if( sz == capacity() )
{
reserve( allocSize(size()) );
}
m_size++;
return m_data[sz];
}
B3_FORCE_INLINE T& expand(const T& fillValue = T())
{
int sz = size();
if (sz == capacity())
{
reserve(allocSize(size()));
}
m_size++;
#ifdef B3_USE_PLACEMENT_NEW
new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
#endif
return m_data[sz];
return m_data[sz];
}
B3_FORCE_INLINE void push_back(const T& _Val)
{
int sz = size();
if (sz == capacity())
{
reserve(allocSize(size()));
}
B3_FORCE_INLINE void push_back(const T& _Val)
{
int sz = size();
if( sz == capacity() )
{
reserve( allocSize(size()) );
}
#ifdef B3_USE_PLACEMENT_NEW
new ( &m_data[m_size] ) T(_Val);
new (&m_data[m_size]) T(_Val);
#else
m_data[size()] = _Val;
#endif //B3_USE_PLACEMENT_NEW
m_data[size()] = _Val;
#endif //B3_USE_PLACEMENT_NEW
m_size++;
}
m_size++;
}
/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
B3_FORCE_INLINE int capacity() const
{
return m_capacity;
}
B3_FORCE_INLINE void reserve(int _Count)
{ // determine new minimum length of allocated storage
if (capacity() < _Count)
{ // not enough room, reallocate
T* s = (T*)allocate(_Count);
b3Assert(s);
if (s==0)
{
b3Error("b3AlignedObjectArray reserve out-of-memory\n");
_Count=0;
m_size=0;
}
copy(0, size(), s);
destroy(0,size());
deallocate();
//PCK: added this line
m_ownsMemory = true;
m_data = s;
m_capacity = _Count;
/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
B3_FORCE_INLINE int capacity() const
{
return m_capacity;
}
B3_FORCE_INLINE void reserve(int _Count)
{ // determine new minimum length of allocated storage
if (capacity() < _Count)
{ // not enough room, reallocate
T* s = (T*)allocate(_Count);
b3Assert(s);
if (s == 0)
{
b3Error("b3AlignedObjectArray reserve out-of-memory\n");
_Count = 0;
m_size = 0;
}
copy(0, size(), s);
destroy(0, size());
deallocate();
//PCK: added this line
m_ownsMemory = true;
m_data = s;
m_capacity = _Count;
}
}
class less
class less
{
public:
bool operator()(const T& a, const T& b)
{
public:
return (a < b);
}
};
bool operator() ( const T& a, const T& b )
{
return ( a < b );
}
};
template <typename L>
void quickSortInternal(const L& CompareFunc,int lo, int hi)
{
template <typename L>
void quickSortInternal(const L& CompareFunc, int lo, int hi)
{
// lo is the lower index, hi is the upper index
// of the region of array a that is to be sorted
int i=lo, j=hi;
T x=m_data[(lo+hi)/2];
int i = lo, j = hi;
T x = m_data[(lo + hi) / 2];
// partition
do
{
while (CompareFunc(m_data[i],x))
i++;
while (CompareFunc(x,m_data[j]))
j--;
if (i<=j)
{
swap(i,j);
i++; j--;
}
} while (i<=j);
// recursion
if (lo<j)
quickSortInternal( CompareFunc, lo, j);
if (i<hi)
quickSortInternal( CompareFunc, i, hi);
}
template <typename L>
void quickSort(const L& CompareFunc)
// partition
do
{
//don't sort 0 or 1 elements
if (size()>1)
while (CompareFunc(m_data[i], x))
i++;
while (CompareFunc(x, m_data[j]))
j--;
if (i <= j)
{
quickSortInternal(CompareFunc,0,size()-1);
swap(i, j);
i++;
j--;
}
} while (i <= j);
// recursion
if (lo < j)
quickSortInternal(CompareFunc, lo, j);
if (i < hi)
quickSortInternal(CompareFunc, i, hi);
}
template <typename L>
void quickSort(const L& CompareFunc)
{
//don't sort 0 or 1 elements
if (size() > 1)
{
quickSortInternal(CompareFunc, 0, size() - 1);
}
}
///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
template <typename L>
void downHeap(T* pArr, int k, int n, const L& CompareFunc)
{
/* PRE: a[k+1..N] is a heap */
/* POST: a[k..N] is a heap */
T temp = pArr[k - 1];
/* k has child(s) */
while (k <= n / 2)
{
int child = 2 * k;
if ((child < n) && CompareFunc(pArr[child - 1], pArr[child]))
{
child++;
}
/* pick larger child */
if (CompareFunc(temp, pArr[child - 1]))
{
/* move child up */
pArr[k - 1] = pArr[child - 1];
k = child;
}
else
{
break;
}
}
pArr[k - 1] = temp;
} /*downHeap*/
///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
template <typename L>
void downHeap(T *pArr, int k, int n, const L& CompareFunc)
{
/* PRE: a[k+1..N] is a heap */
/* POST: a[k..N] is a heap */
T temp = pArr[k - 1];
/* k has child(s) */
while (k <= n/2)
{
int child = 2*k;
if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child]))
{
child++;
}
/* pick larger child */
if (CompareFunc(temp , pArr[child - 1]))
{
/* move child up */
pArr[k - 1] = pArr[child - 1];
k = child;
}
else
{
break;
}
}
pArr[k - 1] = temp;
} /*downHeap*/
void swap(int index0,int index1)
{
void swap(int index0, int index1)
{
#ifdef B3_USE_MEMCPY
char temp[sizeof(T)];
memcpy(temp,&m_data[index0],sizeof(T));
memcpy(&m_data[index0],&m_data[index1],sizeof(T));
memcpy(&m_data[index1],temp,sizeof(T));
char temp[sizeof(T)];
memcpy(temp, &m_data[index0], sizeof(T));
memcpy(&m_data[index0], &m_data[index1], sizeof(T));
memcpy(&m_data[index1], temp, sizeof(T));
#else
T temp = m_data[index0];
m_data[index0] = m_data[index1];
m_data[index1] = temp;
#endif //B3_USE_PLACEMENT_NEW
}
T temp = m_data[index0];
m_data[index0] = m_data[index1];
m_data[index1] = temp;
#endif //B3_USE_PLACEMENT_NEW
}
template <typename L>
void heapSort(const L& CompareFunc)
@ -431,49 +414,48 @@ protected:
/* sort a[0..N-1], N.B. 0 to N-1 */
int k;
int n = m_size;
for (k = n/2; k > 0; k--)
for (k = n / 2; k > 0; k--)
{
downHeap(m_data, k, n, CompareFunc);
}
/* a[1..N] is now a heap */
while ( n>=1 )
while (n >= 1)
{
swap(0,n-1); /* largest of a[0..n-1] */
swap(0, n - 1); /* largest of a[0..n-1] */
n = n - 1;
/* restore a[1..i-1] heap */
downHeap(m_data, 1, n, CompareFunc);
}
}
}
///non-recursive binary search, assumes sorted array
int findBinarySearch(const T& key) const
int findBinarySearch(const T& key) const
{
int first = 0;
int last = size()-1;
int last = size() - 1;
//assume sorted array
while (first <= last) {
while (first <= last)
{
int mid = (first + last) / 2; // compute mid point.
if (key > m_data[mid])
if (key > m_data[mid])
first = mid + 1; // repeat search in top half.
else if (key < m_data[mid])
last = mid - 1; // repeat search in bottom half.
else if (key < m_data[mid])
last = mid - 1; // repeat search in bottom half.
else
return mid; // found it. return position /////
return mid; // found it. return position /////
}
return size(); // failed to find key
return size(); // failed to find key
}
int findLinearSearch(const T& key) const
int findLinearSearch(const T& key) const
{
int index=size();
int index = size();
int i;
for (i=0;i<size();i++)
for (i = 0; i < size(); i++)
{
if (m_data[i] == key)
{
@ -483,36 +465,35 @@ protected:
}
return index;
}
int findLinearSearch2(const T& key) const
{
int index=-1;
int i;
for (i=0;i<size();i++)
{
if (m_data[i] == key)
{
index = i;
break;
}
}
return index;
}
void remove(const T& key)
int findLinearSearch2(const T& key) const
{
int index = -1;
int i;
int findIndex = findLinearSearch(key);
if (findIndex<size())
for (i = 0; i < size(); i++)
{
swap( findIndex,size()-1);
if (m_data[i] == key)
{
index = i;
break;
}
}
return index;
}
void remove(const T& key)
{
int findIndex = findLinearSearch(key);
if (findIndex < size())
{
swap(findIndex, size() - 1);
pop_back();
}
}
//PCK: whole function
void initializeFromBuffer(void *buffer, int size, int capacity)
void initializeFromBuffer(void* buffer, int size, int capacity)
{
clear();
m_ownsMemory = false;
@ -524,18 +505,18 @@ protected:
void copyFromArray(const b3AlignedObjectArray& otherArray)
{
int otherSize = otherArray.size();
resize (otherSize);
resize(otherSize);
otherArray.copy(0, otherSize, m_data);
}
void removeAtIndex(int index)
{
if (index<size())
{
swap( index,size()-1);
pop_back();
}
}
{
if (index < size())
{
swap(index, size() - 1);
pop_back();
}
}
};
#endif //B3_OBJECT_ARRAY__
#endif //B3_OBJECT_ARRAY__

97
thirdparty/bullet/Bullet3Common/b3CommandLineArgs.h vendored
View File

@ -12,51 +12,54 @@
class b3CommandLineArgs
{
protected:
std::map<std::string, std::string> pairs;
public:
// Constructor
b3CommandLineArgs(int argc, char **argv)
{
addArgs(argc,argv);
addArgs(argc, argv);
}
void addArgs(int argc, char**argv)
void addArgs(int argc, char **argv)
{
for (int i = 1; i < argc; i++)
{
std::string arg = argv[i];
for (int i = 1; i < argc; i++)
{
std::string arg = argv[i];
if ((arg.length() < 2) || (arg[0] != '-') || (arg[1] != '-')) {
continue;
}
std::string::size_type pos;
std::string key, val;
if ((pos = arg.find( '=')) == std::string::npos) {
key = std::string(arg, 2, arg.length() - 2);
val = "";
} else {
key = std::string(arg, 2, pos - 2);
val = std::string(arg, pos + 1, arg.length() - 1);
}
//only add new keys, don't replace existing
if(pairs.find(key) == pairs.end())
if ((arg.length() < 2) || (arg[0] != '-') || (arg[1] != '-'))
{
pairs[key] = val;
continue;
}
}
std::string::size_type pos;
std::string key, val;
if ((pos = arg.find('=')) == std::string::npos)
{
key = std::string(arg, 2, arg.length() - 2);
val = "";
}
else
{
key = std::string(arg, 2, pos - 2);
val = std::string(arg, pos + 1, arg.length() - 1);
}
//only add new keys, don't replace existing
if (pairs.find(key) == pairs.end())
{
pairs[key] = val;
}
}
}
bool CheckCmdLineFlag(const char* arg_name)
bool CheckCmdLineFlag(const char *arg_name)
{
std::map<std::string, std::string>::iterator itr;
if ((itr = pairs.find(arg_name)) != pairs.end()) {
if ((itr = pairs.find(arg_name)) != pairs.end())
{
return true;
}
}
return false;
}
@ -73,29 +76,31 @@ template <typename T>
inline bool b3CommandLineArgs::GetCmdLineArgument(const char *arg_name, T &val)
{
std::map<std::string, std::string>::iterator itr;
if ((itr = pairs.find(arg_name)) != pairs.end()) {
if ((itr = pairs.find(arg_name)) != pairs.end())
{
std::istringstream strstream(itr->second);
strstream >> val;
return true;
}
return false;
}
template <>
inline bool b3CommandLineArgs::GetCmdLineArgument<char*>(const char* arg_name, char* &val)
{
std::map<std::string, std::string>::iterator itr;
if ((itr = pairs.find(arg_name)) != pairs.end()) {
std::string s = itr->second;
val = (char*) malloc(sizeof(char) * (s.length() + 1));
std::strcpy(val, s.c_str());
return true;
} else {
val = NULL;
}
return false;
}
template <>
inline bool b3CommandLineArgs::GetCmdLineArgument<char *>(const char *arg_name, char *&val)
{
std::map<std::string, std::string>::iterator itr;
if ((itr = pairs.find(arg_name)) != pairs.end())
{
std::string s = itr->second;
val = (char *)malloc(sizeof(char) * (s.length() + 1));
std::strcpy(val, s.c_str());
return true;
}
else
{
val = NULL;
}
return false;
}
#endif //COMMAND_LINE_ARGS_H
#endif //COMMAND_LINE_ARGS_H

85
thirdparty/bullet/Bullet3Common/b3FileUtils.h vendored
View File

@ -3,7 +3,7 @@
#include <stdio.h>
#include "b3Scalar.h"
#include <stddef.h>//ptrdiff_h
#include <stddef.h> //ptrdiff_h
#include <string.h>
struct b3FileUtils
@ -17,42 +17,42 @@ struct b3FileUtils
static bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
{
FILE* f=0;
f = fopen(orgFileName,"rb");
if (f)
{
FILE* f = 0;
f = fopen(orgFileName, "rb");
if (f)
{
//printf("original file found: [%s]\n", orgFileName);
sprintf(relativeFileName,"%s", orgFileName);
sprintf(relativeFileName, "%s", orgFileName);
fclose(f);
return true;
}
//printf("Trying various directories, relative to current working directory\n");
const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
int numPrefixes = sizeof(prefix)/sizeof(const char*);
f=0;
bool fileFound = false;
//printf("Trying various directories, relative to current working directory\n");
const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
int numPrefixes = sizeof(prefix) / sizeof(const char*);
for (int i=0;!f && i<numPrefixes;i++)
{
f = 0;
bool fileFound = false;
for (int i = 0; !f && i < numPrefixes; i++)
{
#ifdef _MSC_VER
sprintf_s(relativeFileName,maxRelativeFileNameMaxLen,"%s%s",prefix[i],orgFileName);
sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName);
#else
sprintf(relativeFileName,"%s%s",prefix[i],orgFileName);
sprintf(relativeFileName, "%s%s", prefix[i], orgFileName);
#endif
f = fopen(relativeFileName,"rb");
if (f)
{
fileFound = true;
break;
}
}
f = fopen(relativeFileName, "rb");
if (f)
{
fclose(f);
fileFound = true;
break;
}
}
if (f)
{
fclose(f);
}
return fileFound;
}
@ -60,8 +60,8 @@ struct b3FileUtils
{
size_t const patlen = strlen(pattern);
size_t patcnt = 0;
const char * oriptr;
const char * patloc;
const char* oriptr;
const char* patloc;
// find how many times the pattern occurs in the original string
for (oriptr = name; (patloc = strstr(oriptr, pattern)); oriptr = patloc + patlen)
{
@ -70,29 +70,27 @@ struct b3FileUtils
return oriptr;
}
static int extractPath(const char* fileName, char* path, int maxPathLength)
{
const char* stripped = strip2(fileName, "/");
stripped = strip2(stripped, "\\");
ptrdiff_t len = stripped-fileName;
b3Assert((len+1)<maxPathLength);
ptrdiff_t len = stripped - fileName;
b3Assert((len + 1) < maxPathLength);
if (len && ((len+1)<maxPathLength))
if (len && ((len + 1) < maxPathLength))
{
for (int i=0;i<len;i++)
for (int i = 0; i < len; i++)
{
path[i] = fileName[i];
}
path[len]=0;
} else
path[len] = 0;
}
else
{
len = 0;
b3Assert(maxPathLength>0);
if (maxPathLength>0)
b3Assert(maxPathLength > 0);
if (maxPathLength > 0)
{
path[len] = 0;
}
@ -102,23 +100,21 @@ struct b3FileUtils
static char toLowerChar(const char t)
{
if (t>=(char)'A' && t<=(char)'Z')
if (t >= (char)'A' && t <= (char)'Z')
return t + ((char)'a' - (char)'A');
else
return t;
}
static void toLower(char* str)
{
int len=strlen(str);
for (int i=0;i<len;i++)
int len = strlen(str);
for (int i = 0; i < len; i++)
{
str[i] = toLowerChar(str[i]);
}
}
/*static const char* strip2(const char* name, const char* pattern)
{
size_t const patlen = strlen(pattern);
@ -133,6 +129,5 @@ struct b3FileUtils
return oriptr;
}
*/
};
#endif //B3_FILE_UTILS_H
#endif //B3_FILE_UTILS_H

248
thirdparty/bullet/Bullet3Common/b3HashMap.h vendored
View File

@ -13,86 +13,80 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_HASH_MAP_H
#define B3_HASH_MAP_H
#include "b3AlignedObjectArray.h"
#include <string>
///very basic hashable string implementation, compatible with b3HashMap
struct b3HashString
{
std::string m_string;
unsigned int m_hash;
unsigned int m_hash;
B3_FORCE_INLINE unsigned int getHash()const
B3_FORCE_INLINE unsigned int getHash() const
{
return m_hash;
}
b3HashString(const char* name)
:m_string(name)
: m_string(name)
{
/* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
static const unsigned int InitialFNV = 2166136261u;
static const unsigned int InitialFNV = 2166136261u;
static const unsigned int FNVMultiple = 16777619u;
/* Fowler / Noll / Vo (FNV) Hash */
unsigned int hash = InitialFNV;
int len = m_string.length();
for(int i = 0; i<len; i++)
for (int i = 0; i < len; i++)
{
hash = hash ^ (m_string[i]); /* xor the low 8 bits */
hash = hash * FNVMultiple; /* multiply by the magic number */
hash = hash ^ (m_string[i]); /* xor the low 8 bits */
hash = hash * FNVMultiple; /* multiply by the magic number */
}
m_hash = hash;
}
int portableStringCompare(const char* src, const char* dst) const
int portableStringCompare(const char* src, const char* dst) const
{
int ret = 0 ;
int ret = 0;
while( ! (ret = *(unsigned char *)src - *(unsigned char *)dst) && *dst)
++src, ++dst;
while (!(ret = *(unsigned char*)src - *(unsigned char*)dst) && *dst)
++src, ++dst;
if ( ret < 0 )
ret = -1 ;
else if ( ret > 0 )
ret = 1 ;
if (ret < 0)
ret = -1;
else if (ret > 0)
ret = 1;
return( ret );
return (ret);
}
bool equals(const b3HashString& other) const
{
return (m_string == other.m_string);
}
};
const int B3_HASH_NULL=0xffffffff;
const int B3_HASH_NULL = 0xffffffff;
class b3HashInt
{
int m_uid;
int m_uid;
public:
b3HashInt(int uid) :m_uid(uid)
b3HashInt(int uid) : m_uid(uid)
{
}
int getUid1() const
int getUid1() const
{
return m_uid;
}
void setUid1(int uid)
void setUid1(int uid)
{
m_uid = uid;
}
@ -102,34 +96,34 @@ public:
return getUid1() == other.getUid1();
}
//to our success
B3_FORCE_INLINE unsigned int getHash()const
B3_FORCE_INLINE unsigned int getHash() const
{
int key = m_uid;
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
};
class b3HashPtr
{
union
{
const void* m_pointer;
int m_hashValues[2];
union {
const void* m_pointer;
int m_hashValues[2];
};
public:
b3HashPtr(const void* ptr)
:m_pointer(ptr)
: m_pointer(ptr)
{
}
const void* getPointer() const
const void* getPointer() const
{
return m_pointer;
}
@ -140,65 +134,69 @@ public:
}
//to our success
B3_FORCE_INLINE unsigned int getHash()const
B3_FORCE_INLINE unsigned int getHash() const
{
const bool VOID_IS_8 = ((sizeof(void*)==8));
int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0];
const bool VOID_IS_8 = ((sizeof(void*) == 8));
int key = VOID_IS_8 ? m_hashValues[0] + m_hashValues[1] : m_hashValues[0];
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
};
template <class Value>
class b3HashKeyPtr
{
int m_uid;
int m_uid;
public:
b3HashKeyPtr(int uid) : m_uid(uid)
{
}
b3HashKeyPtr(int uid) :m_uid(uid)
{
}
int getUid1() const
{
return m_uid;
}
int getUid1() const
{
return m_uid;
}
bool equals(const b3HashKeyPtr<Value>& other) const
{
return getUid1() == other.getUid1();
}
bool equals(const b3HashKeyPtr<Value>& other) const
{
return getUid1() == other.getUid1();
}
//to our success
B3_FORCE_INLINE unsigned int getHash()const
{
int key = m_uid;
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
return key;
}
//to our success
B3_FORCE_INLINE unsigned int getHash() const
{
int key = m_uid;
// Thomas Wang's hash
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
};
template <class Value>
class b3HashKey
{
int m_uid;
public:
int m_uid;
b3HashKey(int uid) :m_uid(uid)
public:
b3HashKey(int uid) : m_uid(uid)
{
}
int getUid1() const
int getUid1() const
{
return m_uid;
}
@ -208,30 +206,33 @@ public:
return getUid1() == other.getUid1();
}
//to our success
B3_FORCE_INLINE unsigned int getHash()const
B3_FORCE_INLINE unsigned int getHash() const
{
int key = m_uid;
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
};
///The b3HashMap template class implements a generic and lightweight hashmap.
///A basic sample of how to use b3HashMap is located in Demos\BasicDemo\main.cpp
template <class Key, class Value>
class b3HashMap
{
protected:
b3AlignedObjectArray<int> m_hashTable;
b3AlignedObjectArray<int> m_next;
b3AlignedObjectArray<Value> m_valueArray;
b3AlignedObjectArray<Key> m_keyArray;
b3AlignedObjectArray<int> m_hashTable;
b3AlignedObjectArray<int> m_next;
void growTables(const Key& /*key*/)
b3AlignedObjectArray<Value> m_valueArray;
b3AlignedObjectArray<Key> m_keyArray;
void growTables(const Key& /*key*/)
{
int newCapacity = m_valueArray.capacity();
@ -245,7 +246,7 @@ protected:
int i;
for (i= 0; i < newCapacity; ++i)
for (i = 0; i < newCapacity; ++i)
{
m_hashTable[i] = B3_HASH_NULL;
}
@ -254,30 +255,28 @@ protected:
m_next[i] = B3_HASH_NULL;
}
for(i=0;i<curHashtableSize;i++)
for (i = 0; i < curHashtableSize; i++)
{
//const Value& value = m_valueArray[i];
//const Key& key = m_keyArray[i];
int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity()-1); // New hash value with new mask
int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity() - 1); // New hash value with new mask
m_next[i] = m_hashTable[hashValue];
m_hashTable[hashValue] = i;
}
}
}
public:
void insert(const Key& key, const Value& value) {
int hash = key.getHash() & (m_valueArray.capacity()-1);
public:
void insert(const Key& key, const Value& value)
{
int hash = key.getHash() & (m_valueArray.capacity() - 1);
//replace value if the key is already there
int index = findIndex(key);
if (index != B3_HASH_NULL)
{
m_valueArray[index]=value;
m_valueArray[index] = value;
return;
}
@ -291,19 +290,19 @@ protected:
{
growTables(key);
//hash with new capacity
hash = key.getHash() & (m_valueArray.capacity()-1);
hash = key.getHash() & (m_valueArray.capacity() - 1);
}
m_next[count] = m_hashTable[hash];
m_hashTable[hash] = count;
}
void remove(const Key& key) {
int hash = key.getHash() & (m_valueArray.capacity()-1);
void remove(const Key& key)
{
int hash = key.getHash() & (m_valueArray.capacity() - 1);
int pairIndex = findIndex(key);
if (pairIndex ==B3_HASH_NULL)
if (pairIndex == B3_HASH_NULL)
{
return;
}
@ -344,7 +343,7 @@ protected:
}
// Remove the last pair from the hash table.
int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity()-1);
int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity() - 1);
index = m_hashTable[lastHash];
b3Assert(index != B3_HASH_NULL);
@ -376,10 +375,8 @@ protected:
m_valueArray.pop_back();
m_keyArray.pop_back();
}
int size() const
{
return m_valueArray.size();
@ -399,23 +396,24 @@ protected:
return &m_valueArray[index];
}
Key getKeyAtIndex(int index)
{
b3Assert(index < m_keyArray.size());
return m_keyArray[index];
}
const Key getKeyAtIndex(int index) const
{
b3Assert(index < m_keyArray.size());
return m_keyArray[index];
}
Key getKeyAtIndex(int index)
{
b3Assert(index < m_keyArray.size());
return m_keyArray[index];
}
Value* operator[](const Key& key) {
const Key getKeyAtIndex(int index) const
{
b3Assert(index < m_keyArray.size());
return m_keyArray[index];
}
Value* operator[](const Key& key)
{
return find(key);
}
const Value* find(const Key& key) const
const Value* find(const Key& key) const
{
int index = findIndex(key);
if (index == B3_HASH_NULL)
@ -425,7 +423,7 @@ protected:
return &m_valueArray[index];
}
Value* find(const Key& key)
Value* find(const Key& key)
{
int index = findIndex(key);
if (index == B3_HASH_NULL)
@ -435,10 +433,9 @@ protected:
return &m_valueArray[index];
}
int findIndex(const Key& key) const
int findIndex(const Key& key) const
{
unsigned int hash = key.getHash() & (m_valueArray.capacity()-1);
unsigned int hash = key.getHash() & (m_valueArray.capacity() - 1);
if (hash >= (unsigned int)m_hashTable.size())
{
@ -453,14 +450,13 @@ protected:
return index;
}
void clear()
void clear()
{
m_hashTable.clear();
m_next.clear();
m_valueArray.clear();
m_keyArray.clear();
}
};
#endif //B3_HASH_MAP_H
#endif //B3_HASH_MAP_H

87
thirdparty/bullet/Bullet3Common/b3Logging.cpp vendored
View File

@ -20,17 +20,16 @@ subject to the following restrictions:
#ifdef _WIN32
#include <windows.h>
#endif //_WIN32
#endif //_WIN32
void b3PrintfFuncDefault(const char* msg)
{
#ifdef _WIN32
OutputDebugStringA(msg);
#endif
printf("%s",msg);
//is this portable?
fflush(stdout);
printf("%s", msg);
//is this portable?
fflush(stdout);
}
void b3WarningMessageFuncDefault(const char* msg)
@ -38,32 +37,26 @@ void b3WarningMessageFuncDefault(const char* msg)
#ifdef _WIN32
OutputDebugStringA(msg);
#endif
printf("%s",msg);
//is this portable?
fflush(stdout);
printf("%s", msg);
//is this portable?
fflush(stdout);
}
void b3ErrorMessageFuncDefault(const char* msg)
{
#ifdef _WIN32
OutputDebugStringA(msg);
#endif
printf("%s",msg);
printf("%s", msg);
//is this portable?
fflush(stdout);
//is this portable?
fflush(stdout);
}
static b3PrintfFunc* b3s_printfFunc = b3PrintfFuncDefault;
static b3WarningMessageFunc* b3s_warningMessageFunc = b3WarningMessageFuncDefault;
static b3ErrorMessageFunc* b3s_errorMessageFunc = b3ErrorMessageFuncDefault;
///The developer can route b3Printf output using their own implementation
void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc)
{
@ -81,54 +74,50 @@ void b3SetCustomErrorMessageFunc(b3PrintfFunc* errorMessageFunc)
//#define B3_MAX_DEBUG_STRING_LENGTH 2048
#define B3_MAX_DEBUG_STRING_LENGTH 32768
void b3OutputPrintfVarArgsInternal(const char *str, ...)
void b3OutputPrintfVarArgsInternal(const char* str, ...)
{
char strDebug[B3_MAX_DEBUG_STRING_LENGTH]={0};
va_list argList;
va_start(argList, str);
char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
va_list argList;
va_start(argList, str);
#ifdef _MSC_VER
vsprintf_s(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
#else
vsnprintf(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
#endif
(b3s_printfFunc)(strDebug);
va_end(argList);
(b3s_printfFunc)(strDebug);
va_end(argList);
}
void b3OutputWarningMessageVarArgsInternal(const char *str, ...)
void b3OutputWarningMessageVarArgsInternal(const char* str, ...)
{
char strDebug[B3_MAX_DEBUG_STRING_LENGTH]={0};
va_list argList;
va_start(argList, str);
char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
va_list argList;
va_start(argList, str);
#ifdef _MSC_VER
vsprintf_s(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
#else
vsnprintf(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
#endif
(b3s_warningMessageFunc)(strDebug);
va_end(argList);
(b3s_warningMessageFunc)(strDebug);
va_end(argList);
}
void b3OutputErrorMessageVarArgsInternal(const char *str, ...)
void b3OutputErrorMessageVarArgsInternal(const char* str, ...)
{
char strDebug[B3_MAX_DEBUG_STRING_LENGTH]={0};
va_list argList;
va_start(argList, str);
char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
va_list argList;
va_start(argList, str);
#ifdef _MSC_VER
vsprintf_s(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
#else
vsnprintf(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
#endif
(b3s_errorMessageFunc)(strDebug);
va_end(argList);
(b3s_errorMessageFunc)(strDebug);
va_end(argList);
}
void b3EnterProfileZoneDefault(const char* name)
void b3EnterProfileZoneDefault(const char* name)
{
}
void b3LeaveProfileZoneDefault()
void b3LeaveProfileZoneDefault()
{
}
static b3EnterProfileZoneFunc* b3s_enterFunc = b3EnterProfileZoneDefault;
@ -151,10 +140,6 @@ void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc)
b3s_leaveFunc = leaveFunc;
}
#ifndef _MSC_VER
#undef vsprintf_s
#endif

101
thirdparty/bullet/Bullet3Common/b3Logging.h vendored
View File

@ -3,75 +3,84 @@
#define B3_LOGGING_H
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
///We add the do/while so that the statement "if (condition) b3Printf("test"); else {...}" would fail
///You can also customize the message by uncommenting out a different line below
#define b3Printf(...) b3OutputPrintfVarArgsInternal(__VA_ARGS__)
//#define b3Printf(...) do {b3OutputPrintfVarArgsInternal("b3Printf[%s,%d]:",__FILE__,__LINE__);b3OutputPrintfVarArgsInternal(__VA_ARGS__); } while(0)
//#define b3Printf b3OutputPrintfVarArgsInternal
//#define b3Printf(...) printf(__VA_ARGS__)
//#define b3Printf(...)
#define b3Warning(...) do {b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n",__FILE__,__LINE__);b3OutputWarningMessageVarArgsInternal(__VA_ARGS__); }while(0)
#define b3Error(...) do {b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n",__FILE__,__LINE__);b3OutputErrorMessageVarArgsInternal(__VA_ARGS__); } while(0)
//#define b3Printf(...) do {b3OutputPrintfVarArgsInternal("b3Printf[%s,%d]:",__FILE__,__LINE__);b3OutputPrintfVarArgsInternal(__VA_ARGS__); } while(0)
//#define b3Printf b3OutputPrintfVarArgsInternal
//#define b3Printf(...) printf(__VA_ARGS__)
//#define b3Printf(...)
#define b3Warning(...) \
do \
{ \
b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n", __FILE__, __LINE__); \
b3OutputWarningMessageVarArgsInternal(__VA_ARGS__); \
} while (0)
#define b3Error(...) \
do \
{ \
b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n", __FILE__, __LINE__); \
b3OutputErrorMessageVarArgsInternal(__VA_ARGS__); \
} while (0)
#ifndef B3_NO_PROFILE
void b3EnterProfileZone(const char* name);
void b3LeaveProfileZone();
void b3EnterProfileZone(const char* name);
void b3LeaveProfileZone();
#ifdef __cplusplus
class b3ProfileZone
{
public:
b3ProfileZone(const char* name)
{
b3EnterProfileZone( name );
}
class b3ProfileZone
{
public:
b3ProfileZone(const char* name)
{
b3EnterProfileZone(name);
}
~b3ProfileZone()
{
b3LeaveProfileZone();
}
};
~b3ProfileZone()
{
b3LeaveProfileZone();
}
};
#define B3_PROFILE( name ) b3ProfileZone __profile( name )
#define B3_PROFILE(name) b3ProfileZone __profile(name)
#endif
#else //B3_NO_PROFILE
#else //B3_NO_PROFILE
#define B3_PROFILE( name )
#define B3_PROFILE(name)
#define b3StartProfile(a)
#define b3StopProfile
#endif //#ifndef B3_NO_PROFILE
#endif //#ifndef B3_NO_PROFILE
typedef void(b3PrintfFunc)(const char* msg);
typedef void(b3WarningMessageFunc)(const char* msg);
typedef void(b3ErrorMessageFunc)(const char* msg);
typedef void(b3EnterProfileZoneFunc)(const char* msg);
typedef void(b3LeaveProfileZoneFunc)();
typedef void (b3PrintfFunc)(const char* msg);
typedef void (b3WarningMessageFunc)(const char* msg);
typedef void (b3ErrorMessageFunc)(const char* msg);
typedef void (b3EnterProfileZoneFunc)(const char* msg);
typedef void (b3LeaveProfileZoneFunc)();
///The developer can route b3Printf output using their own implementation
void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc);
void b3SetCustomWarningMessageFunc(b3WarningMessageFunc* warningMsgFunc);
void b3SetCustomErrorMessageFunc(b3ErrorMessageFunc* errorMsgFunc);
///The developer can route b3Printf output using their own implementation
void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc);
void b3SetCustomWarningMessageFunc(b3WarningMessageFunc* warningMsgFunc);
void b3SetCustomErrorMessageFunc(b3ErrorMessageFunc* errorMsgFunc);
///Set custom profile zone functions (zones can be nested)
void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc);
void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc);
///Set custom profile zone functions (zones can be nested)
void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc);
void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc);
///Don't use those internal functions directly, use the b3Printf or b3SetCustomPrintfFunc instead (or warning/error version)
void b3OutputPrintfVarArgsInternal(const char *str, ...);
void b3OutputWarningMessageVarArgsInternal(const char *str, ...);
void b3OutputErrorMessageVarArgsInternal(const char *str, ...);
///Don't use those internal functions directly, use the b3Printf or b3SetCustomPrintfFunc instead (or warning/error version)
void b3OutputPrintfVarArgsInternal(const char* str, ...);
void b3OutputWarningMessageVarArgsInternal(const char* str, ...);
void b3OutputErrorMessageVarArgsInternal(const char* str, ...);
#ifdef __cplusplus
}
}
#endif
#endif//B3_LOGGING_H
#endif //B3_LOGGING_H

1412
thirdparty/bullet/Bullet3Common/b3Matrix3x3.h vendored
View File

File diff suppressed because it is too large Load Diff

32
thirdparty/bullet/Bullet3Common/b3MinMax.h vendored
View File

@ -12,60 +12,58 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_GEN_MINMAX_H
#define B3_GEN_MINMAX_H
#include "b3Scalar.h"
template <class T>
B3_FORCE_INLINE const T& b3Min(const T& a, const T& b)
B3_FORCE_INLINE const T& b3Min(const T& a, const T& b)
{
return a < b ? a : b ;
return a < b ? a : b;
}
template <class T>
B3_FORCE_INLINE const T& b3Max(const T& a, const T& b)
B3_FORCE_INLINE const T& b3Max(const T& a, const T& b)
{
return a > b ? a : b;
return a > b ? a : b;
}
template <class T>
B3_FORCE_INLINE const T& b3Clamped(const T& a, const T& lb, const T& ub)
B3_FORCE_INLINE const T& b3Clamped(const T& a, const T& lb, const T& ub)
{
return a < lb ? lb : (ub < a ? ub : a);
return a < lb ? lb : (ub < a ? ub : a);
}
template <class T>
B3_FORCE_INLINE void b3SetMin(T& a, const T& b)
B3_FORCE_INLINE void b3SetMin(T& a, const T& b)
{
if (b < a)
if (b < a)
{
a = b;
}
}
template <class T>
B3_FORCE_INLINE void b3SetMax(T& a, const T& b)
B3_FORCE_INLINE void b3SetMax(T& a, const T& b)
{
if (a < b)
if (a < b)
{
a = b;
}
}
template <class T>
B3_FORCE_INLINE void b3Clamp(T& a, const T& lb, const T& ub)
B3_FORCE_INLINE void b3Clamp(T& a, const T& lb, const T& ub)
{
if (a < lb)
if (a < lb)
{
a = lb;
a = lb;
}
else if (ub < a)
else if (ub < a)
{
a = ub;
}
}
#endif //B3_GEN_MINMAX_H
#endif //B3_GEN_MINMAX_H

82
thirdparty/bullet/Bullet3Common/b3PoolAllocator.h vendored
View File

@ -12,7 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _BT_POOL_ALLOCATOR_H
#define _BT_POOL_ALLOCATOR_H
@ -22,37 +21,37 @@ subject to the following restrictions:
///The b3PoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
class b3PoolAllocator
{
int m_elemSize;
int m_maxElements;
int m_freeCount;
void* m_firstFree;
unsigned char* m_pool;
int m_elemSize;
int m_maxElements;
int m_freeCount;
void* m_firstFree;
unsigned char* m_pool;
public:
b3PoolAllocator(int elemSize, int maxElements)
:m_elemSize(elemSize),
m_maxElements(maxElements)
: m_elemSize(elemSize),
m_maxElements(maxElements)
{
m_pool = (unsigned char*) b3AlignedAlloc( static_cast<unsigned int>(m_elemSize*m_maxElements),16);
m_pool = (unsigned char*)b3AlignedAlloc(static_cast<unsigned int>(m_elemSize * m_maxElements), 16);
unsigned char* p = m_pool;
m_firstFree = p;
m_freeCount = m_maxElements;
int count = m_maxElements;
while (--count) {
*(void**)p = (p + m_elemSize);
p += m_elemSize;
}
*(void**)p = 0;
}
m_firstFree = p;
m_freeCount = m_maxElements;
int count = m_maxElements;
while (--count)
{
*(void**)p = (p + m_elemSize);
p += m_elemSize;
}
*(void**)p = 0;
}
~b3PoolAllocator()
{
b3AlignedFree( m_pool);
b3AlignedFree(m_pool);
}
int getFreeCount() const
int getFreeCount() const
{
return m_freeCount;
}
@ -67,21 +66,22 @@ public:
return m_maxElements;
}
void* allocate(int size)
void* allocate(int size)
{
// release mode fix
(void)size;
b3Assert(!size || size<=m_elemSize);
b3Assert(m_freeCount>0);
void* result = m_firstFree;
m_firstFree = *(void**)m_firstFree;
--m_freeCount;
return result;
b3Assert(!size || size <= m_elemSize);
b3Assert(m_freeCount > 0);
void* result = m_firstFree;
m_firstFree = *(void**)m_firstFree;
--m_freeCount;
return result;
}
bool validPtr(void* ptr)
{
if (ptr) {
if (ptr)
{
if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
{
return true;
@ -90,32 +90,32 @@ public:
return false;
}
void freeMemory(void* ptr)
void freeMemory(void* ptr)
{
if (ptr) {
b3Assert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
if (ptr)
{
b3Assert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
*(void**)ptr = m_firstFree;
m_firstFree = ptr;
++m_freeCount;
}
*(void**)ptr = m_firstFree;
m_firstFree = ptr;
++m_freeCount;
}
}
int getElementSize() const
int getElementSize() const
{
return m_elemSize;
}
unsigned char* getPoolAddress()
unsigned char* getPoolAddress()
{
return m_pool;
}
const unsigned char* getPoolAddress() const
const unsigned char* getPoolAddress() const
{
return m_pool;
}
};
#endif //_BT_POOL_ALLOCATOR_H
#endif //_BT_POOL_ALLOCATOR_H

241
thirdparty/bullet/Bullet3Common/b3QuadWord.h vendored
View File

@ -12,18 +12,13 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_SIMD_QUADWORD_H
#define B3_SIMD_QUADWORD_H
#include "b3Scalar.h"
#include "b3MinMax.h"
#if defined (__CELLOS_LV2) && defined (__SPU__)
#if defined(__CELLOS_LV2) && defined(__SPU__)
#include <altivec.h>
#endif
@ -31,58 +26,64 @@ subject to the following restrictions:
* Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
*/
#ifndef USE_LIBSPE2
B3_ATTRIBUTE_ALIGNED16(class) b3QuadWord
B3_ATTRIBUTE_ALIGNED16(class)
b3QuadWord
#else
class b3QuadWord
#endif
{
protected:
#if defined (__SPU__) && defined (__CELLOS_LV2__)
#if defined(__SPU__) && defined(__CELLOS_LV2__)
union {
vec_float4 mVec128;
b3Scalar m_floats[4];
b3Scalar m_floats[4];
};
public:
vec_float4 get128() const
vec_float4 get128() const
{
return mVec128;
}
#else //__CELLOS_LV2__ __SPU__
#else //__CELLOS_LV2__ __SPU__
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
public:
union {
b3SimdFloat4 mVec128;
b3Scalar m_floats[4];
struct {b3Scalar x,y,z,w;};
b3Scalar m_floats[4];
struct
{
b3Scalar x, y, z, w;
};
};
public:
B3_FORCE_INLINE b3SimdFloat4 get128() const
B3_FORCE_INLINE b3SimdFloat4 get128() const
{
return mVec128;
}
B3_FORCE_INLINE void set128(b3SimdFloat4 v128)
B3_FORCE_INLINE void set128(b3SimdFloat4 v128)
{
mVec128 = v128;
}
#else
public:
union
{
b3Scalar m_floats[4];
struct {b3Scalar x,y,z,w;};
union {
b3Scalar m_floats[4];
struct
{
b3Scalar x, y, z, w;
};
};
#endif // B3_USE_SSE
#endif // B3_USE_SSE
#endif //__CELLOS_LV2__ __SPU__
#endif //__CELLOS_LV2__ __SPU__
public:
public:
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
// Set Vector
// Set Vector
B3_FORCE_INLINE b3QuadWord(const b3SimdFloat4 vec)
{
mVec128 = vec;
@ -95,151 +96,147 @@ public:
}
// Assignment Operator
B3_FORCE_INLINE b3QuadWord&
operator=(const b3QuadWord& v)
B3_FORCE_INLINE b3QuadWord&
operator=(const b3QuadWord& v)
{
mVec128 = v.mVec128;
return *this;
}
#endif
/**@brief Return the x value */
B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
/**@brief Return the y value */
B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
/**@brief Return the z value */
B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
/**@brief Set the x value */
B3_FORCE_INLINE void setX(b3Scalar _x) { m_floats[0] = _x;};
/**@brief Set the y value */
B3_FORCE_INLINE void setY(b3Scalar _y) { m_floats[1] = _y;};
/**@brief Set the z value */
B3_FORCE_INLINE void setZ(b3Scalar _z) { m_floats[2] = _z;};
/**@brief Set the w value */
B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w;};
/**@brief Return the x value */
/**@brief Return the x value */
B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
/**@brief Return the y value */
B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
/**@brief Return the z value */
B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
/**@brief Set the x value */
B3_FORCE_INLINE void setX(b3Scalar _x) { m_floats[0] = _x; };
/**@brief Set the y value */
B3_FORCE_INLINE void setY(b3Scalar _y) { m_floats[1] = _y; };
/**@brief Set the z value */
B3_FORCE_INLINE void setZ(b3Scalar _z) { m_floats[2] = _z; };
/**@brief Set the w value */
B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w; };
/**@brief Return the x value */
//B3_FORCE_INLINE b3Scalar& operator[](int i) { return (&m_floats[0])[i]; }
//B3_FORCE_INLINE b3Scalar& operator[](int i) { return (&m_floats[0])[i]; }
//B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
B3_FORCE_INLINE operator b3Scalar *() { return &m_floats[0]; }
B3_FORCE_INLINE operator const b3Scalar *() const { return &m_floats[0]; }
B3_FORCE_INLINE operator b3Scalar*() { return &m_floats[0]; }
B3_FORCE_INLINE operator const b3Scalar*() const { return &m_floats[0]; }
B3_FORCE_INLINE bool operator==(const b3QuadWord& other) const
B3_FORCE_INLINE bool operator==(const b3QuadWord& other) const
{
#ifdef B3_USE_SSE
return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
#else
return ((m_floats[3]==other.m_floats[3]) &&
(m_floats[2]==other.m_floats[2]) &&
(m_floats[1]==other.m_floats[1]) &&
(m_floats[0]==other.m_floats[0]));
return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
#else
return ((m_floats[3] == other.m_floats[3]) &&
(m_floats[2] == other.m_floats[2]) &&
(m_floats[1] == other.m_floats[1]) &&
(m_floats[0] == other.m_floats[0]));
#endif
}
B3_FORCE_INLINE bool operator!=(const b3QuadWord& other) const
B3_FORCE_INLINE bool operator!=(const b3QuadWord& other) const
{
return !(*this == other);
}
/**@brief Set x,y,z and zero w
/**@brief Set x,y,z and zero w
* @param x Value of x
* @param y Value of y
* @param z Value of z
*/
B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
{
m_floats[0]=_x;
m_floats[1]=_y;
m_floats[2]=_z;
m_floats[3] = 0.f;
}
B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
{
m_floats[0] = _x;
m_floats[1] = _y;
m_floats[2] = _z;
m_floats[3] = 0.f;
}
/* void getValue(b3Scalar *m) const
/* void getValue(b3Scalar *m) const
{
m[0] = m_floats[0];
m[1] = m_floats[1];
m[2] = m_floats[2];
}
*/
/**@brief Set the values
/**@brief Set the values
* @param x Value of x
* @param y Value of y
* @param z Value of z
* @param w Value of w
*/
B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w)
{
m_floats[0]=_x;
m_floats[1]=_y;
m_floats[2]=_z;
m_floats[3]=_w;
}
/**@brief No initialization constructor */
B3_FORCE_INLINE b3QuadWord()
// :m_floats[0](b3Scalar(0.)),m_floats[1](b3Scalar(0.)),m_floats[2](b3Scalar(0.)),m_floats[3](b3Scalar(0.))
{
}
/**@brief Three argument constructor (zeros w)
B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
{
m_floats[0] = _x;
m_floats[1] = _y;
m_floats[2] = _z;
m_floats[3] = _w;
}
/**@brief No initialization constructor */
B3_FORCE_INLINE b3QuadWord()
// :m_floats[0](b3Scalar(0.)),m_floats[1](b3Scalar(0.)),m_floats[2](b3Scalar(0.)),m_floats[3](b3Scalar(0.))
{
}
/**@brief Three argument constructor (zeros w)
* @param x Value of x
* @param y Value of y
* @param z Value of z
*/
B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
{
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
}
B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
{
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
}
/**@brief Initializing constructor
/**@brief Initializing constructor
* @param x Value of x
* @param y Value of y
* @param z Value of z
* @param w Value of w
*/
B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w)
{
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
}
B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
{
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
}
/**@brief Set each element to the max of the current values and the values of another b3QuadWord
/**@brief Set each element to the max of the current values and the values of another b3QuadWord
* @param other The other b3QuadWord to compare with
*/
B3_FORCE_INLINE void setMax(const b3QuadWord& other)
{
#ifdef B3_USE_SSE
mVec128 = _mm_max_ps(mVec128, other.mVec128);
#elif defined(B3_USE_NEON)
mVec128 = vmaxq_f32(mVec128, other.mVec128);
#else
b3SetMax(m_floats[0], other.m_floats[0]);
b3SetMax(m_floats[1], other.m_floats[1]);
b3SetMax(m_floats[2], other.m_floats[2]);
b3SetMax(m_floats[3], other.m_floats[3]);
#endif
}
/**@brief Set each element to the min of the current values and the values of another b3QuadWord
B3_FORCE_INLINE void setMax(const b3QuadWord& other)
{
#ifdef B3_USE_SSE
mVec128 = _mm_max_ps(mVec128, other.mVec128);
#elif defined(B3_USE_NEON)
mVec128 = vmaxq_f32(mVec128, other.mVec128);
#else
b3SetMax(m_floats[0], other.m_floats[0]);
b3SetMax(m_floats[1], other.m_floats[1]);
b3SetMax(m_floats[2], other.m_floats[2]);
b3SetMax(m_floats[3], other.m_floats[3]);
#endif
}
/**@brief Set each element to the min of the current values and the values of another b3QuadWord
* @param other The other b3QuadWord to compare with
*/
B3_FORCE_INLINE void setMin(const b3QuadWord& other)
{
#ifdef B3_USE_SSE
mVec128 = _mm_min_ps(mVec128, other.mVec128);
#elif defined(B3_USE_NEON)
mVec128 = vminq_f32(mVec128, other.mVec128);
#else
b3SetMin(m_floats[0], other.m_floats[0]);
b3SetMin(m_floats[1], other.m_floats[1]);
b3SetMin(m_floats[2], other.m_floats[2]);
b3SetMin(m_floats[3], other.m_floats[3]);
#endif
}
B3_FORCE_INLINE void setMin(const b3QuadWord& other)
{
#ifdef B3_USE_SSE
mVec128 = _mm_min_ps(mVec128, other.mVec128);
#elif defined(B3_USE_NEON)
mVec128 = vminq_f32(mVec128, other.mVec128);
#else
b3SetMin(m_floats[0], other.m_floats[0]);
b3SetMin(m_floats[1], other.m_floats[1]);
b3SetMin(m_floats[2], other.m_floats[2]);
b3SetMin(m_floats[3], other.m_floats[3]);
#endif
}
};
#endif //B3_SIMD_QUADWORD_H
#endif //B3_SIMD_QUADWORD_H

811
thirdparty/bullet/Bullet3Common/b3Quaternion.h vendored
View File

File diff suppressed because it is too large Load Diff

14
thirdparty/bullet/Bullet3Common/b3Random.h vendored
View File

@ -12,8 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_GEN_RANDOM_H
#define B3_GEN_RANDOM_H
@ -26,8 +24,8 @@ subject to the following restrictions:
#define B3_RAND_MAX UINT_MAX
B3_FORCE_INLINE void b3Srand(unsigned int seed) { init_genrand(seed); }
B3_FORCE_INLINE unsigned int b3rand() { return genrand_int32(); }
B3_FORCE_INLINE void b3Srand(unsigned int seed) { init_genrand(seed); }
B3_FORCE_INLINE unsigned int b3rand() { return genrand_int32(); }
#else
@ -35,8 +33,8 @@ B3_FORCE_INLINE unsigned int b3rand() { return genrand_int32()
#define B3_RAND_MAX RAND_MAX
B3_FORCE_INLINE void b3Srand(unsigned int seed) { srand(seed); }
B3_FORCE_INLINE unsigned int b3rand() { return rand(); }
B3_FORCE_INLINE void b3Srand(unsigned int seed) { srand(seed); }
B3_FORCE_INLINE unsigned int b3rand() { return rand(); }
#endif
@ -45,6 +43,4 @@ inline b3Scalar b3RandRange(b3Scalar minRange, b3Scalar maxRange)
return (b3rand() / (b3Scalar(B3_RAND_MAX) + b3Scalar(1.0))) * (maxRange - minRange) + minRange;
}
#endif //B3_GEN_RANDOM_H
#endif //B3_GEN_RANDOM_H

65
thirdparty/bullet/Bullet3Common/b3ResizablePool.h vendored
View File

@ -4,10 +4,10 @@
#include "Bullet3Common/b3AlignedObjectArray.h"
enum
enum
{
B3_POOL_HANDLE_TERMINAL_FREE=-1,
B3_POOL_HANDLE_TERMINAL_USED =-2
B3_POOL_HANDLE_TERMINAL_FREE = -1,
B3_POOL_HANDLE_TERMINAL_USED = -2
};
template <typename U>
@ -20,25 +20,23 @@ struct b3PoolBodyHandle : public U
{
m_nextFreeHandle = next;
}
int getNextFree() const
int getNextFree() const
{
return m_nextFreeHandle;
}
};
template <typename T>
template <typename T>
class b3ResizablePool
{
protected:
b3AlignedObjectArray<T> m_bodyHandles;
int m_numUsedHandles; // number of active handles
int m_firstFreeHandle; // free handles list
b3AlignedObjectArray<T> m_bodyHandles;
int m_numUsedHandles; // number of active handles
int m_firstFreeHandle; // free handles list
T* getHandleInternal(int handle)
{
return &m_bodyHandles[handle];
}
const T* getHandleInternal(int handle) const
{
@ -46,17 +44,16 @@ protected:
}
public:
b3ResizablePool()
{
initHandles();
}
virtual ~b3ResizablePool()
{
exitHandles();
}
///handle management
///handle management
int getNumHandles() const
{
@ -65,44 +62,40 @@ public:
void getUsedHandles(b3AlignedObjectArray<int>& usedHandles) const
{
for (int i=0;i<m_bodyHandles.size();i++)
for (int i = 0; i < m_bodyHandles.size(); i++)
{
if (m_bodyHandles[i].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED)
if (m_bodyHandles[i].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
{
usedHandles.push_back(i);
}
}
}
T* getHandle(int handle)
{
b3Assert(handle>=0);
b3Assert(handle<m_bodyHandles.size());
if ((handle<0) || (handle>=m_bodyHandles.size()))
b3Assert(handle >= 0);
b3Assert(handle < m_bodyHandles.size());
if ((handle < 0) || (handle >= m_bodyHandles.size()))
{
return 0;
}
if (m_bodyHandles[handle].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED)
if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
{
return &m_bodyHandles[handle];
}
return 0;
}
const T* getHandle(int handle) const
{
b3Assert(handle>=0);
b3Assert(handle<m_bodyHandles.size());
if ((handle<0) || (handle>=m_bodyHandles.size()))
b3Assert(handle >= 0);
b3Assert(handle < m_bodyHandles.size());
if ((handle < 0) || (handle >= m_bodyHandles.size()))
{
return 0;
}
if (m_bodyHandles[handle].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED)
if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
{
return &m_bodyHandles[handle];
}
@ -120,7 +113,6 @@ public:
for (int i = curCapacity; i < newCapacity; i++)
m_bodyHandles[i].setNextFree(i + 1);
m_bodyHandles[newCapacity - 1].setNextFree(-1);
}
m_firstFreeHandle = curCapacity;
@ -142,19 +134,18 @@ public:
int allocHandle()
{
b3Assert(m_firstFreeHandle>=0);
b3Assert(m_firstFreeHandle >= 0);
int handle = m_firstFreeHandle;
m_firstFreeHandle = getHandleInternal(handle)->getNextFree();
m_numUsedHandles++;
if (m_firstFreeHandle<0)
if (m_firstFreeHandle < 0)
{
//int curCapacity = m_bodyHandles.size();
int additionalCapacity= m_bodyHandles.size();
int additionalCapacity = m_bodyHandles.size();
increaseHandleCapacity(additionalCapacity);
getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
}
getHandleInternal(handle)->setNextFree(B3_POOL_HANDLE_TERMINAL_USED);
@ -162,12 +153,11 @@ public:
return handle;
}
void freeHandle(int handle)
{
b3Assert(handle >= 0);
if (m_bodyHandles[handle].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED)
if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
{
getHandleInternal(handle)->clear();
getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
@ -176,7 +166,6 @@ public:
}
}
};
///end handle management
#endif //B3_RESIZABLE_POOL_H
///end handle management
#endif //B3_RESIZABLE_POOL_H

714
thirdparty/bullet/Bullet3Common/b3Scalar.h vendored
View File

@ -12,8 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_SCALAR_H
#define B3_SCALAR_H
@ -22,238 +20,252 @@ subject to the following restrictions:
#pragma unmanaged
#endif
#include <math.h>
#include <stdlib.h>//size_t for MSVC 6.0
#include <stdlib.h> //size_t for MSVC 6.0
#include <float.h>
//Original repository is at http://github.com/erwincoumans/bullet3
#define B3_BULLET_VERSION 300
inline int b3GetVersion()
inline int b3GetVersion()
{
return B3_BULLET_VERSION;
}
#if defined(DEBUG) || defined (_DEBUG)
#if defined(DEBUG) || defined(_DEBUG)
#define B3_DEBUG
#endif
#include "b3Logging.h"//for b3Error
#include "b3Logging.h" //for b3Error
#ifdef _WIN32
#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined(_MSC_VER) && _MSC_VER < 1300)
#define B3_FORCE_INLINE inline
#define B3_ATTRIBUTE_ALIGNED16(a) a
#define B3_ATTRIBUTE_ALIGNED64(a) a
#define B3_ATTRIBUTE_ALIGNED128(a) a
#else
//#define B3_HAS_ALIGNED_ALLOCATOR
#pragma warning(disable : 4324) // disable padding warning
#define B3_FORCE_INLINE inline
#define B3_ATTRIBUTE_ALIGNED16(a) a
#define B3_ATTRIBUTE_ALIGNED64(a) a
#define B3_ATTRIBUTE_ALIGNED128(a) a
#else
//#define B3_HAS_ALIGNED_ALLOCATOR
#pragma warning(disable : 4324) // disable padding warning
// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
#pragma warning(disable : 4996) //Turn off warnings about deprecated C routines
// #pragma warning(disable:4786) // Disable the "debug name too long" warning
#define B3_FORCE_INLINE __forceinline
#define B3_ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
#define B3_ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
#define B3_ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
#ifdef _XBOX
#define B3_USE_VMX128
#define B3_FORCE_INLINE __forceinline
#define B3_ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
#define B3_ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
#define B3_ATTRIBUTE_ALIGNED128(a) __declspec(align(128)) a
#ifdef _XBOX
#define B3_USE_VMX128
#include <ppcintrinsics.h>
#define B3_HAVE_NATIVE_FSEL
#define b3Fsel(a,b,c) __fsel((a),(b),(c))
#else
#include <ppcintrinsics.h>
#define B3_HAVE_NATIVE_FSEL
#define b3Fsel(a, b, c) __fsel((a), (b), (c))
#else
#if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (B3_USE_DOUBLE_PRECISION))
#if (defined (_M_IX86) || defined (_M_X64))
#define B3_USE_SSE
#ifdef B3_USE_SSE
//B3_USE_SSE_IN_API is disabled under Windows by default, because
//it makes it harder to integrate Bullet into your application under Windows
//(structured embedding Bullet structs/classes need to be 16-byte aligned)
//with relatively little performance gain
//If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries
//you can manually enable this line or set it in the build system for a bit of performance gain (a few percent, dependent on usage)
//#define B3_USE_SSE_IN_API
#endif //B3_USE_SSE
#include <emmintrin.h>
#endif
#if (defined(_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined(B3_USE_DOUBLE_PRECISION))
#if (defined(_M_IX86) || defined(_M_X64))
#define B3_USE_SSE
#ifdef B3_USE_SSE
//B3_USE_SSE_IN_API is disabled under Windows by default, because
//it makes it harder to integrate Bullet into your application under Windows
//(structured embedding Bullet structs/classes need to be 16-byte aligned)
//with relatively little performance gain
//If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries
//you can manually enable this line or set it in the build system for a bit of performance gain (a few percent, dependent on usage)
//#define B3_USE_SSE_IN_API
#endif //B3_USE_SSE
#include <emmintrin.h>
#endif
#endif
#endif//_XBOX
#endif //_XBOX
#endif //__MINGW32__
#endif //__MINGW32__
#ifdef B3_DEBUG
#ifdef _MSC_VER
#include <stdio.h>
#define b3Assert(x) { if(!(x)){b3Error("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak(); }}
#else//_MSC_VER
#include <assert.h>
#define b3Assert assert
#endif//_MSC_VER
#ifdef _MSC_VER
#include <stdio.h>
#define b3Assert(x) \
{ \
if (!(x)) \
{ \
b3Error( \
"Assert "__FILE__ \
":%u (" #x ")\n", \
__LINE__); \
__debugbreak(); \
} \
}
#else //_MSC_VER
#include <assert.h>
#define b3Assert assert
#endif //_MSC_VER
#else
#define b3Assert(x)
#define b3Assert(x)
#endif
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define b3Likely(_c) _c
#define b3Unlikely(_c) _c
#define b3Likely(_c) _c
#define b3Unlikely(_c) _c
#else
#if defined (__CELLOS_LV2__)
#define B3_FORCE_INLINE inline __attribute__((always_inline))
#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
#ifndef assert
#include <assert.h>
#endif
#if defined(__CELLOS_LV2__)
#define B3_FORCE_INLINE inline __attribute__((always_inline))
#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
#ifndef assert
#include <assert.h>
#endif
#ifdef B3_DEBUG
#ifdef __SPU__
#include <spu_printf.h>
#define printf spu_printf
#define b3Assert(x) {if(!(x)){b3Error("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
#define b3Assert(x) \
{ \
if (!(x)) \
{ \
b3Error( \
"Assert "__FILE__ \
":%u (" #x ")\n", \
__LINE__); \
spu_hcmpeq(0, 0); \
} \
}
#else
#define b3Assert assert
#define b3Assert assert
#endif
#else
#define b3Assert(x)
#endif
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define b3Likely(_c) _c
#define b3Unlikely(_c) _c
#else
#define b3Assert(x)
#endif
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define b3Likely(_c) _c
#define b3Unlikely(_c) _c
#else
#ifdef USE_LIBSPE2
#define B3_FORCE_INLINE __inline
#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
#ifndef assert
#include <assert.h>
#endif
#define B3_FORCE_INLINE __inline
#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
#ifndef assert
#include <assert.h>
#endif
#ifdef B3_DEBUG
#define b3Assert assert
#define b3Assert assert
#else
#define b3Assert(x)
#define b3Assert(x)
#endif
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define b3Likely(_c) __builtin_expect((_c), 1)
#define b3Unlikely(_c) __builtin_expect((_c), 0)
#define b3Likely(_c) __builtin_expect((_c), 1)
#define b3Unlikely(_c) __builtin_expect((_c), 0)
#else
//non-windows systems
//non-windows systems
#if (defined (__APPLE__) && (!defined (B3_USE_DOUBLE_PRECISION)))
#if defined (__i386__) || defined (__x86_64__)
#define B3_USE_SSE
//B3_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
//if apps run into issues, we will disable the next line
#define B3_USE_SSE_IN_API
#ifdef B3_USE_SSE
// include appropriate SSE level
#if defined (__SSE4_1__)
#include <smmintrin.h>
#elif defined (__SSSE3__)
#include <tmmintrin.h>
#elif defined (__SSE3__)
#include <pmmintrin.h>
#else
#include <emmintrin.h>
#endif
#endif //B3_USE_SSE
#elif defined( __armv7__ )
#ifdef __clang__
#define B3_USE_NEON 1
#if (defined(__APPLE__) && (!defined(B3_USE_DOUBLE_PRECISION)))
#if defined(__i386__) || defined(__x86_64__)
#define B3_USE_SSE
//B3_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
//if apps run into issues, we will disable the next line
#define B3_USE_SSE_IN_API
#ifdef B3_USE_SSE
// include appropriate SSE level
#if defined(__SSE4_1__)
#include <smmintrin.h>
#elif defined(__SSSE3__)
#include <tmmintrin.h>
#elif defined(__SSE3__)
#include <pmmintrin.h>
#else
#include <emmintrin.h>
#endif
#endif //B3_USE_SSE
#elif defined(__armv7__)
#ifdef __clang__
#define B3_USE_NEON 1
#if defined B3_USE_NEON && defined (__clang__)
#include <arm_neon.h>
#endif//B3_USE_NEON
#endif //__clang__
#endif//__arm__
#if defined B3_USE_NEON && defined(__clang__)
#include <arm_neon.h>
#endif //B3_USE_NEON
#endif //__clang__
#endif //__arm__
#define B3_FORCE_INLINE inline __attribute__ ((always_inline))
#define B3_FORCE_INLINE inline __attribute__((always_inline))
///@todo: check out alignment methods for other platforms/compilers
#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
#ifndef assert
#include <assert.h>
#endif
#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
#ifndef assert
#include <assert.h>
#endif
#if defined(DEBUG) || defined (_DEBUG)
#if defined (__i386__) || defined (__x86_64__)
#include <stdio.h>
#define b3Assert(x)\
{\
if(!(x))\
{\
b3Error("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\
asm volatile ("int3");\
}\
#if defined(DEBUG) || defined(_DEBUG)
#if defined(__i386__) || defined(__x86_64__)
#include <stdio.h>
#define b3Assert(x) \
{ \
if (!(x)) \
{ \
b3Error("Assert %s in line %d, file %s\n", #x, __LINE__, __FILE__); \
asm volatile("int3"); \
} \
}
#else//defined (__i386__) || defined (__x86_64__)
#define b3Assert assert
#endif//defined (__i386__) || defined (__x86_64__)
#else//defined(DEBUG) || defined (_DEBUG)
#define b3Assert(x)
#endif//defined(DEBUG) || defined (_DEBUG)
#else //defined (__i386__) || defined (__x86_64__)
#define b3Assert assert
#endif //defined (__i386__) || defined (__x86_64__)
#else //defined(DEBUG) || defined (_DEBUG)
#define b3Assert(x)
#endif //defined(DEBUG) || defined (_DEBUG)
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define b3Likely(_c) _c
#define b3Unlikely(_c) _c
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define b3Likely(_c) _c
#define b3Unlikely(_c) _c
#else
#define B3_FORCE_INLINE inline
///@todo: check out alignment methods for other platforms/compilers
#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
///#define B3_ATTRIBUTE_ALIGNED16(a) a
///#define B3_ATTRIBUTE_ALIGNED64(a) a
///#define B3_ATTRIBUTE_ALIGNED128(a) a
#ifndef assert
#include <assert.h>
#endif
#define B3_FORCE_INLINE inline
///@todo: check out alignment methods for other platforms/compilers
#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
///#define B3_ATTRIBUTE_ALIGNED16(a) a
///#define B3_ATTRIBUTE_ALIGNED64(a) a
///#define B3_ATTRIBUTE_ALIGNED128(a) a
#ifndef assert
#include <assert.h>
#endif
#if defined(DEBUG) || defined (_DEBUG)
#define b3Assert assert
#if defined(DEBUG) || defined(_DEBUG)
#define b3Assert assert
#else
#define b3Assert(x)
#define b3Assert(x)
#endif
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define b3Likely(_c) _c
#define b3Unlikely(_c) _c
#endif //__APPLE__
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define b3Likely(_c) _c
#define b3Unlikely(_c) _c
#endif //__APPLE__
#endif // LIBSPE2
#endif // LIBSPE2
#endif //__CELLOS_LV2__
#endif //__CELLOS_LV2__
#endif
///The b3Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
#if defined(B3_USE_DOUBLE_PRECISION)
typedef double b3Scalar;
@ -267,34 +279,34 @@ typedef float b3Scalar;
#ifdef B3_USE_SSE
typedef __m128 b3SimdFloat4;
#endif//B3_USE_SSE
#endif //B3_USE_SSE
#if defined B3_USE_SSE_IN_API && defined (B3_USE_SSE)
#if defined B3_USE_SSE_IN_API && defined(B3_USE_SSE)
#ifdef _WIN32
#ifndef B3_NAN
static int b3NanMask = 0x7F800001;
#define B3_NAN (*(float*)&b3NanMask)
#define B3_NAN (*(float *)&b3NanMask)
#endif
#ifndef B3_INFINITY_MASK
static int b3InfinityMask = 0x7F800000;
#define B3_INFINITY_MASK (*(float*)&b3InfinityMask)
static int b3InfinityMask = 0x7F800000;
#define B3_INFINITY_MASK (*(float *)&b3InfinityMask)
#endif
inline __m128 operator + (const __m128 A, const __m128 B)
inline __m128 operator+(const __m128 A, const __m128 B)
{
return _mm_add_ps(A, B);
return _mm_add_ps(A, B);
}
inline __m128 operator - (const __m128 A, const __m128 B)
inline __m128 operator-(const __m128 A, const __m128 B)
{
return _mm_sub_ps(A, B);
return _mm_sub_ps(A, B);
}
inline __m128 operator * (const __m128 A, const __m128 B)
inline __m128 operator*(const __m128 A, const __m128 B)
{
return _mm_mul_ps(A, B);
return _mm_mul_ps(A, B);
}
#define b3CastfTo128i(a) (_mm_castps_si128(a))
@ -302,18 +314,19 @@ inline __m128 operator * (const __m128 A, const __m128 B)
#define b3CastiTo128f(a) (_mm_castsi128_ps(a))
#define b3CastdTo128f(a) (_mm_castpd_ps(a))
#define b3CastdTo128i(a) (_mm_castpd_si128(a))
#define b3Assign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3)
#define b3Assign128(r0, r1, r2, r3) _mm_setr_ps(r0, r1, r2, r3)
#else//_WIN32
#else //_WIN32
#define b3CastfTo128i(a) ((__m128i)(a))
#define b3CastfTo128d(a) ((__m128d)(a))
#define b3CastiTo128f(a) ((__m128) (a))
#define b3CastdTo128f(a) ((__m128) (a))
#define b3CastiTo128f(a) ((__m128)(a))
#define b3CastdTo128f(a) ((__m128)(a))
#define b3CastdTo128i(a) ((__m128i)(a))
#define b3Assign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3}
#endif//_WIN32
#endif //B3_USE_SSE_IN_API
#define b3Assign128(r0, r1, r2, r3) \
(__m128) { r0, r1, r2, r3 }
#endif //_WIN32
#endif //B3_USE_SSE_IN_API
#ifdef B3_USE_NEON
#include <arm_neon.h>
@ -321,142 +334,160 @@ inline __m128 operator * (const __m128 A, const __m128 B)
typedef float32x4_t b3SimdFloat4;
#define B3_INFINITY INFINITY
#define B3_NAN NAN
#define b3Assign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
#define b3Assign128(r0, r1, r2, r3) \
(float32x4_t) { r0, r1, r2, r3 }
#endif
#define B3_DECLARE_ALIGNED_ALLOCATOR() \
B3_FORCE_INLINE void* operator new(size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes,16); } \
B3_FORCE_INLINE void operator delete(void* ptr) { b3AlignedFree(ptr); } \
B3_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \
B3_FORCE_INLINE void operator delete(void*, void*) { } \
B3_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes,16); } \
B3_FORCE_INLINE void operator delete[](void* ptr) { b3AlignedFree(ptr); } \
B3_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \
B3_FORCE_INLINE void operator delete[](void*, void*) { } \
#define B3_DECLARE_ALIGNED_ALLOCATOR() \
B3_FORCE_INLINE void *operator new(size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes, 16); } \
B3_FORCE_INLINE void operator delete(void *ptr) { b3AlignedFree(ptr); } \
B3_FORCE_INLINE void *operator new(size_t, void *ptr) { return ptr; } \
B3_FORCE_INLINE void operator delete(void *, void *) {} \
B3_FORCE_INLINE void *operator new[](size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes, 16); } \
B3_FORCE_INLINE void operator delete[](void *ptr) { b3AlignedFree(ptr); } \
B3_FORCE_INLINE void *operator new[](size_t, void *ptr) { return ptr; } \
B3_FORCE_INLINE void operator delete[](void *, void *) {}
#if defined(B3_USE_DOUBLE_PRECISION) || defined(B3_FORCE_DOUBLE_FUNCTIONS)
B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar x) { return sqrt(x); }
B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar x)
{
return sqrt(x);
}
B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabs(x); }
B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cos(x); }
B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sin(x); }
B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tan(x); }
B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x) { if (x<b3Scalar(-1)) x=b3Scalar(-1); if (x>b3Scalar(1)) x=b3Scalar(1); return acos(x); }
B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x) { if (x<b3Scalar(-1)) x=b3Scalar(-1); if (x>b3Scalar(1)) x=b3Scalar(1); return asin(x); }
B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x)
{
if (x < b3Scalar(-1)) x = b3Scalar(-1);
if (x > b3Scalar(1)) x = b3Scalar(1);
return acos(x);
}
B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x)
{
if (x < b3Scalar(-1)) x = b3Scalar(-1);
if (x > b3Scalar(1)) x = b3Scalar(1);
return asin(x);
}
B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atan(x); }
B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2(x, y); }
B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return exp(x); }
B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return log(x); }
B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x,b3Scalar y) { return pow(x,y); }
B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x,b3Scalar y) { return fmod(x,y); }
B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x, b3Scalar y) { return pow(x, y); }
B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x, b3Scalar y) { return fmod(x, y); }
#else
B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar y)
{
B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar y)
{
#ifdef USE_APPROXIMATION
double x, z, tempf;
unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
double x, z, tempf;
unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
tempf = y;
*tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
x = tempf;
z = y*b3Scalar(0.5);
x = (b3Scalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */
x = (b3Scalar(1.5)*x)-(x*x)*(x*z);
x = (b3Scalar(1.5)*x)-(x*x)*(x*z);
x = (b3Scalar(1.5)*x)-(x*x)*(x*z);
x = (b3Scalar(1.5)*x)-(x*x)*(x*z);
return x*y;
*tfptr = (0xbfcdd90a - *tfptr) >> 1; /* estimate of 1/sqrt(y) */
x = tempf;
z = y * b3Scalar(0.5);
x = (b3Scalar(1.5) * x) - (x * x) * (x * z); /* iteration formula */
x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
return x * y;
#else
return sqrtf(y);
return sqrtf(y);
#endif
}
B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabsf(x); }
B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cosf(x); }
B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sinf(x); }
B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tanf(x); }
B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x) {
if (x<b3Scalar(-1))
x=b3Scalar(-1);
if (x>b3Scalar(1))
x=b3Scalar(1);
return acosf(x);
B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x)
{
if (x < b3Scalar(-1))
x = b3Scalar(-1);
if (x > b3Scalar(1))
x = b3Scalar(1);
return acosf(x);
}
B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x) {
if (x<b3Scalar(-1))
x=b3Scalar(-1);
if (x>b3Scalar(1))
x=b3Scalar(1);
return asinf(x);
B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x)
{
if (x < b3Scalar(-1))
x = b3Scalar(-1);
if (x > b3Scalar(1))
x = b3Scalar(1);
return asinf(x);
}
B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atanf(x); }
B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2f(x, y); }
B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return expf(x); }
B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return logf(x); }
B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x,b3Scalar y) { return powf(x,y); }
B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x,b3Scalar y) { return fmodf(x,y); }
B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x, b3Scalar y) { return powf(x, y); }
B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x, b3Scalar y) { return fmodf(x, y); }
#endif
#define B3_2_PI b3Scalar(6.283185307179586232)
#define B3_PI (B3_2_PI * b3Scalar(0.5))
#define B3_HALF_PI (B3_2_PI * b3Scalar(0.25))
#define B3_2_PI b3Scalar(6.283185307179586232)
#define B3_PI (B3_2_PI * b3Scalar(0.5))
#define B3_HALF_PI (B3_2_PI * b3Scalar(0.25))
#define B3_RADS_PER_DEG (B3_2_PI / b3Scalar(360.0))
#define B3_DEGS_PER_RAD (b3Scalar(360.0) / B3_2_PI)
#define B3_DEGS_PER_RAD (b3Scalar(360.0) / B3_2_PI)
#define B3_SQRT12 b3Scalar(0.7071067811865475244008443621048490)
#define b3RecipSqrt(x) ((b3Scalar)(b3Scalar(1.0)/b3Sqrt(b3Scalar(x)))) /* reciprocal square root */
#define b3RecipSqrt(x) ((b3Scalar)(b3Scalar(1.0) / b3Sqrt(b3Scalar(x)))) /* reciprocal square root */
#ifdef B3_USE_DOUBLE_PRECISION
#define B3_EPSILON DBL_EPSILON
#define B3_INFINITY DBL_MAX
#define B3_EPSILON DBL_EPSILON
#define B3_INFINITY DBL_MAX
#else
#define B3_EPSILON FLT_EPSILON
#define B3_INFINITY FLT_MAX
#define B3_EPSILON FLT_EPSILON
#define B3_INFINITY FLT_MAX
#endif
B3_FORCE_INLINE b3Scalar b3Atan2Fast(b3Scalar y, b3Scalar x)
B3_FORCE_INLINE b3Scalar b3Atan2Fast(b3Scalar y, b3Scalar x)
{
b3Scalar coeff_1 = B3_PI / 4.0f;
b3Scalar coeff_2 = 3.0f * coeff_1;
b3Scalar abs_y = b3Fabs(y);
b3Scalar angle;
if (x >= 0.0f) {
if (x >= 0.0f)
{
b3Scalar r = (x - abs_y) / (x + abs_y);
angle = coeff_1 - coeff_1 * r;
} else {
}
else
{
b3Scalar r = (x + abs_y) / (abs_y - x);
angle = coeff_2 - coeff_1 * r;
}
return (y < 0.0f) ? -angle : angle;
}
B3_FORCE_INLINE bool b3FuzzyZero(b3Scalar x) { return b3Fabs(x) < B3_EPSILON; }
B3_FORCE_INLINE bool b3FuzzyZero(b3Scalar x) { return b3Fabs(x) < B3_EPSILON; }
B3_FORCE_INLINE bool b3Equal(b3Scalar a, b3Scalar eps) {
B3_FORCE_INLINE bool b3Equal(b3Scalar a, b3Scalar eps)
{
return (((a) <= eps) && !((a) < -eps));
}
B3_FORCE_INLINE bool b3GreaterEqual (b3Scalar a, b3Scalar eps) {
B3_FORCE_INLINE bool b3GreaterEqual(b3Scalar a, b3Scalar eps)
{
return (!((a) <= eps));
}
B3_FORCE_INLINE int b3IsNegative(b3Scalar x) {
return x < b3Scalar(0.0) ? 1 : 0;
B3_FORCE_INLINE int b3IsNegative(b3Scalar x)
{
return x < b3Scalar(0.0) ? 1 : 0;
}
B3_FORCE_INLINE b3Scalar b3Radians(b3Scalar x) { return x * B3_RADS_PER_DEG; }
B3_FORCE_INLINE b3Scalar b3Degrees(b3Scalar x) { return x * B3_DEGS_PER_RAD; }
#define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
#define B3_DECLARE_HANDLE(name) \
typedef struct name##__ \
{ \
int unused; \
} * name
#ifndef b3Fsel
B3_FORCE_INLINE b3Scalar b3Fsel(b3Scalar a, b3Scalar b, b3Scalar c)
@ -464,60 +495,57 @@ B3_FORCE_INLINE b3Scalar b3Fsel(b3Scalar a, b3Scalar b, b3Scalar c)
return a >= 0 ? b : c;
}
#endif
#define b3Fsels(a,b,c) (b3Scalar)b3Fsel(a,b,c)
#define b3Fsels(a, b, c) (b3Scalar) b3Fsel(a, b, c)
B3_FORCE_INLINE bool b3MachineIsLittleEndian()
{
long int i = 1;
const char *p = (const char *) &i;
if (p[0] == 1) // Lowest address contains the least significant byte
return true;
else
return false;
long int i = 1;
const char *p = (const char *)&i;
if (p[0] == 1) // Lowest address contains the least significant byte
return true;
else
return false;
}
///b3Select avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
B3_FORCE_INLINE unsigned b3Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
B3_FORCE_INLINE unsigned b3Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
{
// Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
// Rely on positive value or'ed with its negative having sign bit on
// and zero value or'ed with its negative (which is still zero) having sign bit off
// Use arithmetic shift right, shifting the sign bit through all 32 bits
unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
unsigned testEqz = ~testNz;
return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
// Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
// Rely on positive value or'ed with its negative having sign bit on
// and zero value or'ed with its negative (which is still zero) having sign bit off
// Use arithmetic shift right, shifting the sign bit through all 32 bits
unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
unsigned testEqz = ~testNz;
return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
}
B3_FORCE_INLINE int b3Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
{
unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
unsigned testEqz = ~testNz;
return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
unsigned testEqz = ~testNz;
return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
}
B3_FORCE_INLINE float b3Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
{
#ifdef B3_HAVE_NATIVE_FSEL
return (float)b3Fsel((b3Scalar)condition - b3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
return (float)b3Fsel((b3Scalar)condition - b3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
#else
return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
#endif
}
template<typename T> B3_FORCE_INLINE void b3Swap(T& a, T& b)
template <typename T>
B3_FORCE_INLINE void b3Swap(T &a, T &b)
{
T tmp = a;
a = b;
b = tmp;
}
//PCK: endian swapping functions
B3_FORCE_INLINE unsigned b3SwapEndian(unsigned val)
{
return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
}
B3_FORCE_INLINE unsigned short b3SwapEndian(unsigned short val)
@ -532,87 +560,85 @@ B3_FORCE_INLINE unsigned b3SwapEndian(int val)
B3_FORCE_INLINE unsigned short b3SwapEndian(short val)
{
return b3SwapEndian((unsigned short) val);
return b3SwapEndian((unsigned short)val);
}
///b3SwapFloat uses using char pointers to swap the endianness
////b3SwapFloat/b3SwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754.
///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you.
///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754.
///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you.
///so instead of returning a float/double, we return integer/long long integer
B3_FORCE_INLINE unsigned int b3SwapEndianFloat(float d)
B3_FORCE_INLINE unsigned int b3SwapEndianFloat(float d)
{
unsigned int a = 0;
unsigned char *dst = (unsigned char *)&a;
unsigned char *src = (unsigned char *)&d;
unsigned int a = 0;
unsigned char *dst = (unsigned char *)&a;
unsigned char *src = (unsigned char *)&d;
dst[0] = src[3];
dst[1] = src[2];
dst[2] = src[1];
dst[3] = src[0];
return a;
dst[0] = src[3];
dst[1] = src[2];
dst[2] = src[1];
dst[3] = src[0];
return a;
}
// unswap using char pointers
B3_FORCE_INLINE float b3UnswapEndianFloat(unsigned int a)
B3_FORCE_INLINE float b3UnswapEndianFloat(unsigned int a)
{
float d = 0.0f;
unsigned char *src = (unsigned char *)&a;
unsigned char *dst = (unsigned char *)&d;
float d = 0.0f;
unsigned char *src = (unsigned char *)&a;
unsigned char *dst = (unsigned char *)&d;
dst[0] = src[3];
dst[1] = src[2];
dst[2] = src[1];
dst[3] = src[0];
dst[0] = src[3];
dst[1] = src[2];
dst[2] = src[1];
dst[3] = src[0];
return d;
return d;
}
// swap using char pointers
B3_FORCE_INLINE void b3SwapEndianDouble(double d, unsigned char* dst)
B3_FORCE_INLINE void b3SwapEndianDouble(double d, unsigned char *dst)
{
unsigned char *src = (unsigned char *)&d;
dst[0] = src[7];
dst[1] = src[6];
dst[2] = src[5];
dst[3] = src[4];
dst[4] = src[3];
dst[5] = src[2];
dst[6] = src[1];
dst[7] = src[0];
unsigned char *src = (unsigned char *)&d;
dst[0] = src[7];
dst[1] = src[6];
dst[2] = src[5];
dst[3] = src[4];
dst[4] = src[3];
dst[5] = src[2];
dst[6] = src[1];
dst[7] = src[0];
}
// unswap using char pointers
B3_FORCE_INLINE double b3UnswapEndianDouble(const unsigned char *src)
B3_FORCE_INLINE double b3UnswapEndianDouble(const unsigned char *src)
{
double d = 0.0;
unsigned char *dst = (unsigned char *)&d;
double d = 0.0;
unsigned char *dst = (unsigned char *)&d;
dst[0] = src[7];
dst[1] = src[6];
dst[2] = src[5];
dst[3] = src[4];
dst[4] = src[3];
dst[5] = src[2];
dst[6] = src[1];
dst[7] = src[0];
dst[0] = src[7];
dst[1] = src[6];
dst[2] = src[5];
dst[3] = src[4];
dst[4] = src[3];
dst[5] = src[2];
dst[6] = src[1];
dst[7] = src[0];
return d;
}
// returns normalized value in range [-B3_PI, B3_PI]
B3_FORCE_INLINE b3Scalar b3NormalizeAngle(b3Scalar angleInRadians)
B3_FORCE_INLINE b3Scalar b3NormalizeAngle(b3Scalar angleInRadians)
{
angleInRadians = b3Fmod(angleInRadians, B3_2_PI);
if(angleInRadians < -B3_PI)
if (angleInRadians < -B3_PI)
{
return angleInRadians + B3_2_PI;
}
else if(angleInRadians > B3_PI)
else if (angleInRadians > B3_PI)
{
return angleInRadians - B3_2_PI;
}
@ -626,38 +652,34 @@ B3_FORCE_INLINE b3Scalar b3NormalizeAngle(b3Scalar angleInRadians)
struct b3TypedObject
{
b3TypedObject(int objectType)
:m_objectType(objectType)
: m_objectType(objectType)
{
}
int m_objectType;
int m_objectType;
inline int getObjectType() const
{
return m_objectType;
}
};
///align a pointer to the provided alignment, upwards
template <typename T>T* b3AlignPointer(T* unalignedPtr, size_t alignment)
template <typename T>
T *b3AlignPointer(T *unalignedPtr, size_t alignment)
{
struct b3ConvertPointerSizeT
{
union
{
T* ptr;
size_t integer;
union {
T *ptr;
size_t integer;
};
};
b3ConvertPointerSizeT converter;
b3ConvertPointerSizeT converter;
const size_t bit_mask = ~(alignment - 1);
converter.ptr = unalignedPtr;
converter.integer += alignment-1;
converter.ptr = unalignedPtr;
converter.integer += alignment - 1;
converter.integer &= bit_mask;
return converter.ptr;
}
#endif //B3_SCALAR_H
#endif //B3_SCALAR_H

96
thirdparty/bullet/Bullet3Common/b3StackAlloc.h vendored
View File

@ -20,97 +20,99 @@ Nov.2006
#ifndef B3_STACK_ALLOC
#define B3_STACK_ALLOC
#include "b3Scalar.h" //for b3Assert
#include "b3Scalar.h" //for b3Assert
#include "b3AlignedAllocator.h"
///The b3Block class is an internal structure for the b3StackAlloc memory allocator.
struct b3Block
{
b3Block* previous;
unsigned char* address;
b3Block* previous;
unsigned char* address;
};
///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out)
class b3StackAlloc
{
public:
b3StackAlloc(unsigned int size)
{
ctor();
create(size);
}
~b3StackAlloc() { destroy(); }
b3StackAlloc(unsigned int size) { ctor();create(size); }
~b3StackAlloc() { destroy(); }
inline void create(unsigned int size)
inline void create(unsigned int size)
{
destroy();
data = (unsigned char*) b3AlignedAlloc(size,16);
totalsize = size;
data = (unsigned char*)b3AlignedAlloc(size, 16);
totalsize = size;
}
inline void destroy()
inline void destroy()
{
b3Assert(usedsize==0);
b3Assert(usedsize == 0);
//Raise(L"StackAlloc is still in use");
if(usedsize==0)
if (usedsize == 0)
{
if(!ischild && data)
if (!ischild && data)
b3AlignedFree(data);
data = 0;
usedsize = 0;
data = 0;
usedsize = 0;
}
}
int getAvailableMemory() const
int getAvailableMemory() const
{
return static_cast<int>(totalsize - usedsize);
}
unsigned char* allocate(unsigned int size)
unsigned char* allocate(unsigned int size)
{
const unsigned int nus(usedsize+size);
if(nus<totalsize)
const unsigned int nus(usedsize + size);
if (nus < totalsize)
{
usedsize=nus;
return(data+(usedsize-size));
usedsize = nus;
return (data + (usedsize - size));
}
b3Assert(0);
//&& (L"Not enough memory"));
return(0);
return (0);
}
B3_FORCE_INLINE b3Block* beginBlock()
B3_FORCE_INLINE b3Block* beginBlock()
{
b3Block* pb = (b3Block*)allocate(sizeof(b3Block));
pb->previous = current;
pb->address = data+usedsize;
current = pb;
return(pb);
b3Block* pb = (b3Block*)allocate(sizeof(b3Block));
pb->previous = current;
pb->address = data + usedsize;
current = pb;
return (pb);
}
B3_FORCE_INLINE void endBlock(b3Block* block)
B3_FORCE_INLINE void endBlock(b3Block* block)
{
b3Assert(block==current);
b3Assert(block == current);
//Raise(L"Unmatched blocks");
if(block==current)
if (block == current)
{
current = block->previous;
usedsize = (unsigned int)((block->address-data)-sizeof(b3Block));
current = block->previous;
usedsize = (unsigned int)((block->address - data) - sizeof(b3Block));
}
}
private:
void ctor()
void ctor()
{
data = 0;
totalsize = 0;
usedsize = 0;
current = 0;
ischild = false;
data = 0;
totalsize = 0;
usedsize = 0;
current = 0;
ischild = false;
}
unsigned char* data;
unsigned int totalsize;
unsigned int usedsize;
b3Block* current;
bool ischild;
unsigned char* data;
unsigned int totalsize;
unsigned int usedsize;
b3Block* current;
bool ischild;
};
#endif //B3_STACK_ALLOC
#endif //B3_STACK_ALLOC

216
thirdparty/bullet/Bullet3Common/b3Transform.h vendored
View File

@ -12,11 +12,9 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_TRANSFORM_H
#define B3_TRANSFORM_H
#include "b3Matrix3x3.h"
#ifdef B3_USE_DOUBLE_PRECISION
@ -25,46 +23,45 @@ subject to the following restrictions:
#define b3TransformData b3TransformFloatData
#endif
/**@brief The b3Transform class supports rigid transforms with only translation and rotation and no scaling/shear.
*It can be used in combination with b3Vector3, b3Quaternion and b3Matrix3x3 linear algebra classes. */
B3_ATTRIBUTE_ALIGNED16(class) b3Transform {
///Storage for the rotation
B3_ATTRIBUTE_ALIGNED16(class)
b3Transform
{
///Storage for the rotation
b3Matrix3x3 m_basis;
///Storage for the translation
b3Vector3 m_origin;
///Storage for the translation
b3Vector3 m_origin;
public:
/**@brief No initialization constructor */
/**@brief No initialization constructor */
b3Transform() {}
/**@brief Constructor from b3Quaternion (optional b3Vector3 )
/**@brief Constructor from b3Quaternion (optional b3Vector3 )
* @param q Rotation from quaternion
* @param c Translation from Vector (default 0,0,0) */
explicit B3_FORCE_INLINE b3Transform(const b3Quaternion& q,
const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
explicit B3_FORCE_INLINE b3Transform(const b3Quaternion& q,
const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
: m_basis(q),
m_origin(c)
{}
/**@brief Constructor from b3Matrix3x3 (optional b3Vector3)
* @param b Rotation from Matrix
* @param c Translation from Vector default (0,0,0)*/
explicit B3_FORCE_INLINE b3Transform(const b3Matrix3x3& b,
const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
: m_basis(b),
m_origin(c)
{}
/**@brief Copy constructor */
B3_FORCE_INLINE b3Transform (const b3Transform& other)
: m_basis(other.m_basis),
m_origin(other.m_origin)
m_origin(c)
{
}
/**@brief Assignment Operator */
/**@brief Constructor from b3Matrix3x3 (optional b3Vector3)
* @param b Rotation from Matrix
* @param c Translation from Vector default (0,0,0)*/
explicit B3_FORCE_INLINE b3Transform(const b3Matrix3x3& b,
const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
: m_basis(b),
m_origin(c)
{
}
/**@brief Copy constructor */
B3_FORCE_INLINE b3Transform(const b3Transform& other)
: m_basis(other.m_basis),
m_origin(other.m_origin)
{
}
/**@brief Assignment Operator */
B3_FORCE_INLINE b3Transform& operator=(const b3Transform& other)
{
m_basis = other.m_basis;
@ -72,70 +69,70 @@ public:
return *this;
}
/**@brief Set the current transform as the value of the product of two transforms
/**@brief Set the current transform as the value of the product of two transforms
* @param t1 Transform 1
* @param t2 Transform 2
* This = Transform1 * Transform2 */
B3_FORCE_INLINE void mult(const b3Transform& t1, const b3Transform& t2) {
m_basis = t1.m_basis * t2.m_basis;
m_origin = t1(t2.m_origin);
}
B3_FORCE_INLINE void mult(const b3Transform& t1, const b3Transform& t2)
{
m_basis = t1.m_basis * t2.m_basis;
m_origin = t1(t2.m_origin);
}
/* void multInverseLeft(const b3Transform& t1, const b3Transform& t2) {
/* void multInverseLeft(const b3Transform& t1, const b3Transform& t2) {
b3Vector3 v = t2.m_origin - t1.m_origin;
m_basis = b3MultTransposeLeft(t1.m_basis, t2.m_basis);
m_origin = v * t1.m_basis;
}
*/
/**@brief Return the transform of the vector */
/**@brief Return the transform of the vector */
B3_FORCE_INLINE b3Vector3 operator()(const b3Vector3& x) const
{
return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin;
return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin;
}
/**@brief Return the transform of the vector */
/**@brief Return the transform of the vector */
B3_FORCE_INLINE b3Vector3 operator*(const b3Vector3& x) const
{
return (*this)(x);
}
/**@brief Return the transform of the b3Quaternion */
/**@brief Return the transform of the b3Quaternion */
B3_FORCE_INLINE b3Quaternion operator*(const b3Quaternion& q) const
{
return getRotation() * q;
}
/**@brief Return the basis matrix for the rotation */
B3_FORCE_INLINE b3Matrix3x3& getBasis() { return m_basis; }
/**@brief Return the basis matrix for the rotation */
B3_FORCE_INLINE const b3Matrix3x3& getBasis() const { return m_basis; }
/**@brief Return the basis matrix for the rotation */
B3_FORCE_INLINE b3Matrix3x3& getBasis() { return m_basis; }
/**@brief Return the basis matrix for the rotation */
B3_FORCE_INLINE const b3Matrix3x3& getBasis() const { return m_basis; }
/**@brief Return the origin vector translation */
B3_FORCE_INLINE b3Vector3& getOrigin() { return m_origin; }
/**@brief Return the origin vector translation */
B3_FORCE_INLINE const b3Vector3& getOrigin() const { return m_origin; }
/**@brief Return the origin vector translation */
B3_FORCE_INLINE b3Vector3& getOrigin() { return m_origin; }
/**@brief Return the origin vector translation */
B3_FORCE_INLINE const b3Vector3& getOrigin() const { return m_origin; }
/**@brief Return a quaternion representing the rotation */
b3Quaternion getRotation() const {
/**@brief Return a quaternion representing the rotation */
b3Quaternion getRotation() const
{
b3Quaternion q;
m_basis.getRotation(q);
return q;
}
/**@brief Set from an array
/**@brief Set from an array
* @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
void setFromOpenGLMatrix(const b3Scalar *m)
void setFromOpenGLMatrix(const b3Scalar* m)
{
m_basis.setFromOpenGLSubMatrix(m);
m_origin.setValue(m[12],m[13],m[14]);
m_origin.setValue(m[12], m[13], m[14]);
}
/**@brief Fill an array representation
/**@brief Fill an array representation
* @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
void getOpenGLMatrix(b3Scalar *m) const
void getOpenGLMatrix(b3Scalar * m) const
{
m_basis.getOpenGLSubMatrix(m);
m[12] = m_origin.getX();
@ -144,80 +141,76 @@ public:
m[15] = b3Scalar(1.0);
}
/**@brief Set the translational element
/**@brief Set the translational element
* @param origin The vector to set the translation to */
B3_FORCE_INLINE void setOrigin(const b3Vector3& origin)
{
B3_FORCE_INLINE void setOrigin(const b3Vector3& origin)
{
m_origin = origin;
}
B3_FORCE_INLINE b3Vector3 invXform(const b3Vector3& inVec) const;
/**@brief Set the rotational element by b3Matrix3x3 */
/**@brief Set the rotational element by b3Matrix3x3 */
B3_FORCE_INLINE void setBasis(const b3Matrix3x3& basis)
{
{
m_basis = basis;
}
/**@brief Set the rotational element by b3Quaternion */
/**@brief Set the rotational element by b3Quaternion */
B3_FORCE_INLINE void setRotation(const b3Quaternion& q)
{
m_basis.setRotation(q);
}
/**@brief Set this transformation to the identity */
/**@brief Set this transformation to the identity */
void setIdentity()
{
m_basis.setIdentity();
m_origin.setValue(b3Scalar(0.0), b3Scalar(0.0), b3Scalar(0.0));
}
/**@brief Multiply this Transform by another(this = this * another)
/**@brief Multiply this Transform by another(this = this * another)
* @param t The other transform */
b3Transform& operator*=(const b3Transform& t)
b3Transform& operator*=(const b3Transform& t)
{
m_origin += m_basis * t.m_origin;
m_basis *= t.m_basis;
return *this;
}
/**@brief Return the inverse of this transform */
/**@brief Return the inverse of this transform */
b3Transform inverse() const
{
{
b3Matrix3x3 inv = m_basis.transpose();
return b3Transform(inv, inv * -m_origin);
}
/**@brief Return the inverse of this transform times the other transform
/**@brief Return the inverse of this transform times the other transform
* @param t The other transform
* return this.inverse() * the other */
b3Transform inverseTimes(const b3Transform& t) const;
b3Transform inverseTimes(const b3Transform& t) const;
/**@brief Return the product of this transform and the other */
/**@brief Return the product of this transform and the other */
b3Transform operator*(const b3Transform& t) const;
/**@brief Return an identity transform */
static const b3Transform& getIdentity()
/**@brief Return an identity transform */
static const b3Transform& getIdentity()
{
static const b3Transform identityTransform(b3Matrix3x3::getIdentity());
return identityTransform;
}
void serialize(struct b3TransformData& dataOut) const;
void serialize(struct b3TransformData & dataOut) const;
void serializeFloat(struct b3TransformFloatData& dataOut) const;
void serializeFloat(struct b3TransformFloatData & dataOut) const;
void deSerialize(const struct b3TransformData& dataIn);
void deSerialize(const struct b3TransformData& dataIn);
void deSerializeDouble(const struct b3TransformDoubleData& dataIn);
void deSerializeFloat(const struct b3TransformFloatData& dataIn);
void deSerializeDouble(const struct b3TransformDoubleData& dataIn);
void deSerializeFloat(const struct b3TransformFloatData& dataIn);
};
B3_FORCE_INLINE b3Vector3
b3Transform::invXform(const b3Vector3& inVec) const
{
@ -225,80 +218,69 @@ b3Transform::invXform(const b3Vector3& inVec) const
return (m_basis.transpose() * v);
}
B3_FORCE_INLINE b3Transform
b3Transform::inverseTimes(const b3Transform& t) const
B3_FORCE_INLINE b3Transform
b3Transform::inverseTimes(const b3Transform& t) const
{
b3Vector3 v = t.getOrigin() - m_origin;
return b3Transform(m_basis.transposeTimes(t.m_basis),
v * m_basis);
return b3Transform(m_basis.transposeTimes(t.m_basis),
v * m_basis);
}
B3_FORCE_INLINE b3Transform
b3Transform::operator*(const b3Transform& t) const
B3_FORCE_INLINE b3Transform
b3Transform::operator*(const b3Transform& t) const
{
return b3Transform(m_basis * t.m_basis,
(*this)(t.m_origin));
return b3Transform(m_basis * t.m_basis,
(*this)(t.m_origin));
}
/**@brief Test if two transforms have all elements equal */
B3_FORCE_INLINE bool operator==(const b3Transform& t1, const b3Transform& t2)
{
return ( t1.getBasis() == t2.getBasis() &&
t1.getOrigin() == t2.getOrigin() );
return (t1.getBasis() == t2.getBasis() &&
t1.getOrigin() == t2.getOrigin());
}
///for serialization
struct b3TransformFloatData
struct b3TransformFloatData
{
b3Matrix3x3FloatData m_basis;
b3Vector3FloatData m_origin;
b3Matrix3x3FloatData m_basis;
b3Vector3FloatData m_origin;
};
struct b3TransformDoubleData
struct b3TransformDoubleData
{
b3Matrix3x3DoubleData m_basis;
b3Vector3DoubleData m_origin;
b3Matrix3x3DoubleData m_basis;
b3Vector3DoubleData m_origin;
};
B3_FORCE_INLINE void b3Transform::serialize(b3TransformData& dataOut) const
B3_FORCE_INLINE void b3Transform::serialize(b3TransformData& dataOut) const
{
m_basis.serialize(dataOut.m_basis);
m_origin.serialize(dataOut.m_origin);
}
B3_FORCE_INLINE void b3Transform::serializeFloat(b3TransformFloatData& dataOut) const
B3_FORCE_INLINE void b3Transform::serializeFloat(b3TransformFloatData& dataOut) const
{
m_basis.serializeFloat(dataOut.m_basis);
m_origin.serializeFloat(dataOut.m_origin);
}
B3_FORCE_INLINE void b3Transform::deSerialize(const b3TransformData& dataIn)
B3_FORCE_INLINE void b3Transform::deSerialize(const b3TransformData& dataIn)
{
m_basis.deSerialize(dataIn.m_basis);
m_origin.deSerialize(dataIn.m_origin);
}
B3_FORCE_INLINE void b3Transform::deSerializeFloat(const b3TransformFloatData& dataIn)
B3_FORCE_INLINE void b3Transform::deSerializeFloat(const b3TransformFloatData& dataIn)
{
m_basis.deSerializeFloat(dataIn.m_basis);
m_origin.deSerializeFloat(dataIn.m_origin);
}
B3_FORCE_INLINE void b3Transform::deSerializeDouble(const b3TransformDoubleData& dataIn)
B3_FORCE_INLINE void b3Transform::deSerializeDouble(const b3TransformDoubleData& dataIn)
{
m_basis.deSerializeDouble(dataIn.m_basis);
m_origin.deSerializeDouble(dataIn.m_origin);
}
#endif //B3_TRANSFORM_H
#endif //B3_TRANSFORM_H

146
thirdparty/bullet/Bullet3Common/b3TransformUtil.h vendored
View File

@ -12,204 +12,189 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_TRANSFORM_UTIL_H
#define B3_TRANSFORM_UTIL_H
#include "b3Transform.h"
#define B3_ANGULAR_MOTION_THRESHOLD b3Scalar(0.5)*B3_HALF_PI
#define B3_ANGULAR_MOTION_THRESHOLD b3Scalar(0.5) * B3_HALF_PI
B3_FORCE_INLINE b3Vector3 b3AabbSupport(const b3Vector3& halfExtents,const b3Vector3& supportDir)
B3_FORCE_INLINE b3Vector3 b3AabbSupport(const b3Vector3& halfExtents, const b3Vector3& supportDir)
{
return b3MakeVector3(supportDir.getX() < b3Scalar(0.0) ? -halfExtents.getX() : halfExtents.getX(),
supportDir.getY() < b3Scalar(0.0) ? -halfExtents.getY() : halfExtents.getY(),
supportDir.getZ() < b3Scalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ());
supportDir.getY() < b3Scalar(0.0) ? -halfExtents.getY() : halfExtents.getY(),
supportDir.getZ() < b3Scalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ());
}
/// Utils related to temporal transforms
class b3TransformUtil
{
public:
static void integrateTransform(const b3Transform& curTrans,const b3Vector3& linvel,const b3Vector3& angvel,b3Scalar timeStep,b3Transform& predictedTransform)
static void integrateTransform(const b3Transform& curTrans, const b3Vector3& linvel, const b3Vector3& angvel, b3Scalar timeStep, b3Transform& predictedTransform)
{
predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
// #define QUATERNION_DERIVATIVE
#ifdef QUATERNION_DERIVATIVE
// #define QUATERNION_DERIVATIVE
#ifdef QUATERNION_DERIVATIVE
b3Quaternion predictedOrn = curTrans.getRotation();
predictedOrn += (angvel * predictedOrn) * (timeStep * b3Scalar(0.5));
predictedOrn.normalize();
#else
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
b3Vector3 axis;
b3Scalar fAngle = angvel.length();
b3Scalar fAngle = angvel.length();
//limit the angular motion
if (fAngle*timeStep > B3_ANGULAR_MOTION_THRESHOLD)
if (fAngle * timeStep > B3_ANGULAR_MOTION_THRESHOLD)
{
fAngle = B3_ANGULAR_MOTION_THRESHOLD / timeStep;
}
if ( fAngle < b3Scalar(0.001) )
if (fAngle < b3Scalar(0.001))
{
// use Taylor's expansions of sync function
axis = angvel*( b3Scalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(b3Scalar(0.020833333333))*fAngle*fAngle );
axis = angvel * (b3Scalar(0.5) * timeStep - (timeStep * timeStep * timeStep) * (b3Scalar(0.020833333333)) * fAngle * fAngle);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel*( b3Sin(b3Scalar(0.5)*fAngle*timeStep)/fAngle );
axis = angvel * (b3Sin(b3Scalar(0.5) * fAngle * timeStep) / fAngle);
}
b3Quaternion dorn (axis.getX(),axis.getY(),axis.getZ(),b3Cos( fAngle*timeStep*b3Scalar(0.5) ));
b3Quaternion dorn(axis.getX(), axis.getY(), axis.getZ(), b3Cos(fAngle * timeStep * b3Scalar(0.5)));
b3Quaternion orn0 = curTrans.getRotation();
b3Quaternion predictedOrn = dorn * orn0;
predictedOrn.normalize();
#endif
#endif
predictedTransform.setRotation(predictedOrn);
}
static void calculateVelocityQuaternion(const b3Vector3& pos0,const b3Vector3& pos1,const b3Quaternion& orn0,const b3Quaternion& orn1,b3Scalar timeStep,b3Vector3& linVel,b3Vector3& angVel)
static void calculateVelocityQuaternion(const b3Vector3& pos0, const b3Vector3& pos1, const b3Quaternion& orn0, const b3Quaternion& orn1, b3Scalar timeStep, b3Vector3& linVel, b3Vector3& angVel)
{
linVel = (pos1 - pos0) / timeStep;
b3Vector3 axis;
b3Scalar angle;
b3Scalar angle;
if (orn0 != orn1)
{
calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
calculateDiffAxisAngleQuaternion(orn0, orn1, axis, angle);
angVel = axis * angle / timeStep;
} else
}
else
{
angVel.setValue(0,0,0);
angVel.setValue(0, 0, 0);
}
}
static void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0,const b3Quaternion& orn1a,b3Vector3& axis,b3Scalar& angle)
static void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0, const b3Quaternion& orn1a, b3Vector3& axis, b3Scalar& angle)
{
b3Quaternion orn1 = orn0.nearest(orn1a);
b3Quaternion dorn = orn1 * orn0.inverse();
angle = dorn.getAngle();
axis = b3MakeVector3(dorn.getX(),dorn.getY(),dorn.getZ());
axis = b3MakeVector3(dorn.getX(), dorn.getY(), dorn.getZ());
axis[3] = b3Scalar(0.);
//check for axis length
b3Scalar len = axis.length2();
if (len < B3_EPSILON*B3_EPSILON)
axis = b3MakeVector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));
if (len < B3_EPSILON * B3_EPSILON)
axis = b3MakeVector3(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.));
else
axis /= b3Sqrt(len);
}
static void calculateVelocity(const b3Transform& transform0,const b3Transform& transform1,b3Scalar timeStep,b3Vector3& linVel,b3Vector3& angVel)
static void calculateVelocity(const b3Transform& transform0, const b3Transform& transform1, b3Scalar timeStep, b3Vector3& linVel, b3Vector3& angVel)
{
linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
b3Vector3 axis;
b3Scalar angle;
calculateDiffAxisAngle(transform0,transform1,axis,angle);
b3Scalar angle;
calculateDiffAxisAngle(transform0, transform1, axis, angle);
angVel = axis * angle / timeStep;
}
static void calculateDiffAxisAngle(const b3Transform& transform0,const b3Transform& transform1,b3Vector3& axis,b3Scalar& angle)
static void calculateDiffAxisAngle(const b3Transform& transform0, const b3Transform& transform1, b3Vector3& axis, b3Scalar& angle)
{
b3Matrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
b3Quaternion dorn;
dmat.getRotation(dorn);
///floating point inaccuracy can lead to w component > 1..., which breaks
///floating point inaccuracy can lead to w component > 1..., which breaks
dorn.normalize();
angle = dorn.getAngle();
axis = b3MakeVector3(dorn.getX(),dorn.getY(),dorn.getZ());
axis = b3MakeVector3(dorn.getX(), dorn.getY(), dorn.getZ());
axis[3] = b3Scalar(0.);
//check for axis length
b3Scalar len = axis.length2();
if (len < B3_EPSILON*B3_EPSILON)
axis = b3MakeVector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));
if (len < B3_EPSILON * B3_EPSILON)
axis = b3MakeVector3(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.));
else
axis /= b3Sqrt(len);
}
};
///The b3ConvexSeparatingDistanceUtil can help speed up convex collision detection
///The b3ConvexSeparatingDistanceUtil can help speed up convex collision detection
///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
class b3ConvexSeparatingDistanceUtil
class b3ConvexSeparatingDistanceUtil
{
b3Quaternion m_ornA;
b3Quaternion m_ornB;
b3Vector3 m_posA;
b3Vector3 m_posB;
b3Vector3 m_separatingNormal;
b3Quaternion m_ornA;
b3Quaternion m_ornB;
b3Vector3 m_posA;
b3Vector3 m_posB;
b3Scalar m_boundingRadiusA;
b3Scalar m_boundingRadiusB;
b3Scalar m_separatingDistance;
b3Vector3 m_separatingNormal;
b3Scalar m_boundingRadiusA;
b3Scalar m_boundingRadiusB;
b3Scalar m_separatingDistance;
public:
b3ConvexSeparatingDistanceUtil(b3Scalar boundingRadiusA,b3Scalar boundingRadiusB)
:m_boundingRadiusA(boundingRadiusA),
m_boundingRadiusB(boundingRadiusB),
m_separatingDistance(0.f)
b3ConvexSeparatingDistanceUtil(b3Scalar boundingRadiusA, b3Scalar boundingRadiusB)
: m_boundingRadiusA(boundingRadiusA),
m_boundingRadiusB(boundingRadiusB),
m_separatingDistance(0.f)
{
}
b3Scalar getConservativeSeparatingDistance()
b3Scalar getConservativeSeparatingDistance()
{
return m_separatingDistance;
}
void updateSeparatingDistance(const b3Transform& transA,const b3Transform& transB)
void updateSeparatingDistance(const b3Transform& transA, const b3Transform& transB)
{
const b3Vector3& toPosA = transA.getOrigin();
const b3Vector3& toPosB = transB.getOrigin();
b3Quaternion toOrnA = transA.getRotation();
b3Quaternion toOrnB = transB.getRotation();
if (m_separatingDistance>0.f)
if (m_separatingDistance > 0.f)
{
b3Vector3 linVelA,angVelA,linVelB,angVelB;
b3TransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,b3Scalar(1.),linVelA,angVelA);
b3TransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,b3Scalar(1.),linVelB,angVelB);
b3Vector3 linVelA, angVelA, linVelB, angVelB;
b3TransformUtil::calculateVelocityQuaternion(m_posA, toPosA, m_ornA, toOrnA, b3Scalar(1.), linVelA, angVelA);
b3TransformUtil::calculateVelocityQuaternion(m_posB, toPosB, m_ornB, toOrnB, b3Scalar(1.), linVelB, angVelB);
b3Scalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
b3Vector3 relLinVel = (linVelB-linVelA);
b3Vector3 relLinVel = (linVelB - linVelA);
b3Scalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
if (relLinVelocLength<0.f)
if (relLinVelocLength < 0.f)
{
relLinVelocLength = 0.f;
}
b3Scalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
b3Scalar projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
m_separatingDistance -= projectedMotion;
}
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
void initSeparatingDistance(const b3Vector3& separatingVector,b3Scalar separatingDistance,const b3Transform& transA,const b3Transform& transB)
void initSeparatingDistance(const b3Vector3& separatingVector, b3Scalar separatingDistance, const b3Transform& transA, const b3Transform& transB)
{
m_separatingDistance = separatingDistance;
if (m_separatingDistance>0.f)
if (m_separatingDistance > 0.f)
{
m_separatingNormal = separatingVector;
const b3Vector3& toPosA = transA.getOrigin();
const b3Vector3& toPosB = transB.getOrigin();
b3Quaternion toOrnA = transA.getRotation();
@ -220,9 +205,6 @@ public:
m_ornB = toOrnB;
}
}
};
#endif //B3_TRANSFORM_UTIL_H
#endif //B3_TRANSFORM_UTIL_H

2642
thirdparty/bullet/Bullet3Common/b3Vector3.cpp vendored
View File

File diff suppressed because it is too large Load Diff

839
thirdparty/bullet/Bullet3Common/b3Vector3.h vendored
View File

File diff suppressed because it is too large Load Diff

137
thirdparty/bullet/Bullet3Common/shared/b3Float4.h vendored
View File

@ -4,94 +4,87 @@
#include "Bullet3Common/shared/b3PlatformDefinitions.h"
#ifdef __cplusplus
#include "Bullet3Common/b3Vector3.h"
#define b3Float4 b3Vector3
#define b3Float4ConstArg const b3Vector3&
#define b3Dot3F4 b3Dot
#define b3Cross3 b3Cross
#define b3MakeFloat4 b3MakeVector3
inline b3Vector3 b3Normalized(const b3Vector3& vec)
{
return vec.normalized();
}
inline b3Float4 b3FastNormalized3(b3Float4ConstArg v)
{
return v.normalized();
}
inline b3Float4 b3MaxFloat4 (const b3Float4& a, const b3Float4& b)
{
b3Float4 tmp = a;
tmp.setMax(b);
return tmp;
}
inline b3Float4 b3MinFloat4 (const b3Float4& a, const b3Float4& b)
{
b3Float4 tmp = a;
tmp.setMin(b);
return tmp;
}
#include "Bullet3Common/b3Vector3.h"
#define b3Float4 b3Vector3
#define b3Float4ConstArg const b3Vector3&
#define b3Dot3F4 b3Dot
#define b3Cross3 b3Cross
#define b3MakeFloat4 b3MakeVector3
inline b3Vector3 b3Normalized(const b3Vector3& vec)
{
return vec.normalized();
}
inline b3Float4 b3FastNormalized3(b3Float4ConstArg v)
{
return v.normalized();
}
inline b3Float4 b3MaxFloat4(const b3Float4& a, const b3Float4& b)
{
b3Float4 tmp = a;
tmp.setMax(b);
return tmp;
}
inline b3Float4 b3MinFloat4(const b3Float4& a, const b3Float4& b)
{
b3Float4 tmp = a;
tmp.setMin(b);
return tmp;
}
#else
typedef float4 b3Float4;
#define b3Float4ConstArg const b3Float4
#define b3MakeFloat4 (float4)
float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)
{
float4 a1 = b3MakeFloat4(v0.xyz,0.f);
float4 b1 = b3MakeFloat4(v1.xyz,0.f);
return dot(a1, b1);
}
b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)
{
float4 a1 = b3MakeFloat4(v0.xyz,0.f);
float4 b1 = b3MakeFloat4(v1.xyz,0.f);
return cross(a1, b1);
}
#define b3MinFloat4 min
#define b3MaxFloat4 max
typedef float4 b3Float4;
#define b3Float4ConstArg const b3Float4
#define b3MakeFloat4 (float4)
float b3Dot3F4(b3Float4ConstArg v0, b3Float4ConstArg v1)
{
float4 a1 = b3MakeFloat4(v0.xyz, 0.f);
float4 b1 = b3MakeFloat4(v1.xyz, 0.f);
return dot(a1, b1);
}
b3Float4 b3Cross3(b3Float4ConstArg v0, b3Float4ConstArg v1)
{
float4 a1 = b3MakeFloat4(v0.xyz, 0.f);
float4 b1 = b3MakeFloat4(v1.xyz, 0.f);
return cross(a1, b1);
}
#define b3MinFloat4 min
#define b3MaxFloat4 max
#define b3Normalized(a) normalize(a)
#define b3Normalized(a) normalize(a)
#endif
#endif
inline bool b3IsAlmostZero(b3Float4ConstArg v)
{
if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6)
if (b3Fabs(v.x) > 1e-6 || b3Fabs(v.y) > 1e-6 || b3Fabs(v.z) > 1e-6)
return false;
return true;
}
inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )
inline int b3MaxDot(b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut)
{
float maxDot = -B3_INFINITY;
int i = 0;
int ptIndex = -1;
for( i = 0; i < vecLen; i++ )
{
float dot = b3Dot3F4(vecArray[i],vec);
if( dot > maxDot )
{
maxDot = dot;
ptIndex = i;
}
}
b3Assert(ptIndex>=0);
if (ptIndex<0)
float maxDot = -B3_INFINITY;
int i = 0;
int ptIndex = -1;
for (i = 0; i < vecLen; i++)
{
float dot = b3Dot3F4(vecArray[i], vec);
if (dot > maxDot)
{
maxDot = dot;
ptIndex = i;
}
}
b3Assert(ptIndex >= 0);
if (ptIndex < 0)
{
ptIndex = 0;
}
*dotOut = maxDot;
return ptIndex;
*dotOut = maxDot;
return ptIndex;
}
#endif //B3_FLOAT4_H
#endif //B3_FLOAT4_H

15
thirdparty/bullet/Bullet3Common/shared/b3Int2.h vendored
View File

@ -20,11 +20,10 @@ subject to the following restrictions:
struct b3UnsignedInt2
{
union
{
union {
struct
{
unsigned int x,y;
unsigned int x, y;
};
struct
{
@ -35,11 +34,10 @@ struct b3UnsignedInt2
struct b3Int2
{
union
{
union {
struct
{
int x,y;
int x, y;
};
struct
{
@ -51,7 +49,8 @@ struct b3Int2
inline b3Int2 b3MakeInt2(int x, int y)
{
b3Int2 v;
v.s[0] = x; v.s[1] = y;
v.s[0] = x;
v.s[1] = y;
return v;
}
#else
@ -60,5 +59,5 @@ inline b3Int2 b3MakeInt2(int x, int y)
#define b3Int2 int2
#define b3MakeInt2 (int2)
#endif //__cplusplus
#endif //__cplusplus
#endif

33
thirdparty/bullet/Bullet3Common/shared/b3Int4.h vendored
View File

@ -5,16 +5,15 @@
#include "Bullet3Common/b3Scalar.h"
B3_ATTRIBUTE_ALIGNED16(struct) b3UnsignedInt4
B3_ATTRIBUTE_ALIGNED16(struct)
b3UnsignedInt4
{
B3_DECLARE_ALIGNED_ALLOCATOR();
union
{
union {
struct
{
unsigned int x,y,z,w;
unsigned int x, y, z, w;
};
struct
{
@ -23,15 +22,15 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3UnsignedInt4
};
};
B3_ATTRIBUTE_ALIGNED16(struct) b3Int4
B3_ATTRIBUTE_ALIGNED16(struct)
b3Int4
{
B3_DECLARE_ALIGNED_ALLOCATOR();
union
{
union {
struct
{
int x,y,z,w;
int x, y, z, w;
};
struct
{
@ -43,26 +42,30 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3Int4
B3_FORCE_INLINE b3Int4 b3MakeInt4(int x, int y, int z, int w = 0)
{
b3Int4 v;
v.s[0] = x; v.s[1] = y; v.s[2] = z; v.s[3] = w;
v.s[0] = x;
v.s[1] = y;
v.s[2] = z;
v.s[3] = w;
return v;
}
B3_FORCE_INLINE b3UnsignedInt4 b3MakeUnsignedInt4(unsigned int x, unsigned int y, unsigned int z, unsigned int w = 0)
{
b3UnsignedInt4 v;
v.s[0] = x; v.s[1] = y; v.s[2] = z; v.s[3] = w;
v.s[0] = x;
v.s[1] = y;
v.s[2] = z;
v.s[3] = w;
return v;
}
#else
#define b3UnsignedInt4 uint4
#define b3Int4 int4
#define b3MakeInt4 (int4)
#define b3MakeUnsignedInt4 (uint4)
#endif //__cplusplus
#endif //__cplusplus
#endif //B3_INT4_H
#endif //B3_INT4_H

108
thirdparty/bullet/Bullet3Common/shared/b3Mat3x3.h vendored
View File

@ -4,7 +4,6 @@
#include "Bullet3Common/shared/b3Quat.h"
#ifdef __cplusplus
#include "Bullet3Common/b3Matrix3x3.h"
@ -22,43 +21,41 @@ inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg mat)
return mat.absolute();
}
#define b3GetRow(m,row) m.getRow(row)
#define b3GetRow(m, row) m.getRow(row)
__inline
b3Float4 mtMul3(b3Float4ConstArg a, b3Mat3x3ConstArg b)
__inline b3Float4 mtMul3(b3Float4ConstArg a, b3Mat3x3ConstArg b)
{
return b*a;
return b * a;
}
#else
typedef struct
{
b3Float4 m_row[3];
}b3Mat3x3;
} b3Mat3x3;
#define b3Mat3x3ConstArg const b3Mat3x3
#define b3GetRow(m,row) (m.m_row[row])
#define b3GetRow(m, row) (m.m_row[row])
inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)
{
b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);
b3Float4 quat2 = (b3Float4)(quat.x * quat.x, quat.y * quat.y, quat.z * quat.z, 0.f);
b3Mat3x3 out;
out.m_row[0].x=1-2*quat2.y-2*quat2.z;
out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;
out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;
out.m_row[0].x = 1 - 2 * quat2.y - 2 * quat2.z;
out.m_row[0].y = 2 * quat.x * quat.y - 2 * quat.w * quat.z;
out.m_row[0].z = 2 * quat.x * quat.z + 2 * quat.w * quat.y;
out.m_row[0].w = 0.f;
out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;
out.m_row[1].y=1-2*quat2.x-2*quat2.z;
out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;
out.m_row[1].x = 2 * quat.x * quat.y + 2 * quat.w * quat.z;
out.m_row[1].y = 1 - 2 * quat2.x - 2 * quat2.z;
out.m_row[1].z = 2 * quat.y * quat.z - 2 * quat.w * quat.x;
out.m_row[1].w = 0.f;
out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;
out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;
out.m_row[2].z=1-2*quat2.x-2*quat2.y;
out.m_row[2].x = 2 * quat.x * quat.z - 2 * quat.w * quat.y;
out.m_row[2].y = 2 * quat.y * quat.z + 2 * quat.w * quat.x;
out.m_row[2].z = 1 - 2 * quat2.x - 2 * quat2.y;
out.m_row[2].w = 0.f;
return out;
@ -73,27 +70,19 @@ inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)
return out;
}
__inline b3Mat3x3 mtZero();
__inline
b3Mat3x3 mtZero();
__inline b3Mat3x3 mtIdentity();
__inline
b3Mat3x3 mtIdentity();
__inline b3Mat3x3 mtTranspose(b3Mat3x3 m);
__inline
b3Mat3x3 mtTranspose(b3Mat3x3 m);
__inline b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);
__inline
b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);
__inline b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);
__inline
b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);
__inline b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);
__inline
b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);
__inline
b3Mat3x3 mtZero()
__inline b3Mat3x3 mtZero()
{
b3Mat3x3 m;
m.m_row[0] = (b3Float4)(0.f);
@ -102,18 +91,16 @@ b3Mat3x3 mtZero()
return m;
}
__inline
b3Mat3x3 mtIdentity()
__inline b3Mat3x3 mtIdentity()
{
b3Mat3x3 m;
m.m_row[0] = (b3Float4)(1,0,0,0);
m.m_row[1] = (b3Float4)(0,1,0,0);
m.m_row[2] = (b3Float4)(0,0,1,0);
m.m_row[0] = (b3Float4)(1, 0, 0, 0);
m.m_row[1] = (b3Float4)(0, 1, 0, 0);
m.m_row[2] = (b3Float4)(0, 0, 1, 0);
return m;
}
__inline
b3Mat3x3 mtTranspose(b3Mat3x3 m)
__inline b3Mat3x3 mtTranspose(b3Mat3x3 m)
{
b3Mat3x3 out;
out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);
@ -122,58 +109,49 @@ b3Mat3x3 mtTranspose(b3Mat3x3 m)
return out;
}
__inline
b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)
__inline b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)
{
b3Mat3x3 transB;
transB = mtTranspose( b );
transB = mtTranspose(b);
b3Mat3x3 ans;
// why this doesn't run when 0ing in the for{}
a.m_row[0].w = 0.f;
a.m_row[1].w = 0.f;
a.m_row[2].w = 0.f;
for(int i=0; i<3; i++)
for (int i = 0; i < 3; i++)
{
// a.m_row[i].w = 0.f;
ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);
ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);
ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);
// a.m_row[i].w = 0.f;
ans.m_row[i].x = b3Dot3F4(a.m_row[i], transB.m_row[0]);
ans.m_row[i].y = b3Dot3F4(a.m_row[i], transB.m_row[1]);
ans.m_row[i].z = b3Dot3F4(a.m_row[i], transB.m_row[2]);
ans.m_row[i].w = 0.f;
}
return ans;
}
__inline
b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)
__inline b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)
{
b3Float4 ans;
ans.x = b3Dot3F4( a.m_row[0], b );
ans.y = b3Dot3F4( a.m_row[1], b );
ans.z = b3Dot3F4( a.m_row[2], b );
ans.x = b3Dot3F4(a.m_row[0], b);
ans.y = b3Dot3F4(a.m_row[1], b);
ans.z = b3Dot3F4(a.m_row[2], b);
ans.w = 0.f;
return ans;
}
__inline
b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)
__inline b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)
{
b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);
b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);
b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);
b3Float4 ans;
ans.x = b3Dot3F4( a, colx );
ans.y = b3Dot3F4( a, coly );
ans.z = b3Dot3F4( a, colz );
ans.x = b3Dot3F4(a, colx);
ans.y = b3Dot3F4(a, coly);
ans.z = b3Dot3F4(a, colz);
return ans;
}
#endif
#endif //B3_MAT3x3_H
#endif //B3_MAT3x3_H

10
thirdparty/bullet/Bullet3Common/shared/b3PlatformDefinitions.h vendored
View File

@ -8,18 +8,18 @@ struct MyTest
#ifdef __cplusplus
//#define b3ConstArray(a) const b3AlignedObjectArray<a>&
#define b3ConstArray(a) const a*
#define b3ConstArray(a) const a *
#define b3AtomicInc(a) ((*a)++)
inline int b3AtomicAdd (volatile int *p, int val)
inline int b3AtomicAdd(volatile int *p, int val)
{
int oldValue = *p;
int newValue = oldValue+val;
int newValue = oldValue + val;
*p = newValue;
return oldValue;
}
#define __global
#define __global
#define B3_STATIC static
#else
@ -27,7 +27,7 @@ inline int b3AtomicAdd (volatile int *p, int val)
#define B3_LARGE_FLOAT 1e18f
#define B3_INFINITY 1e18f
#define b3Assert(a)
#define b3ConstArray(a) __global const a*
#define b3ConstArray(a) __global const a *
#define b3AtomicInc atomic_inc
#define b3AtomicAdd atomic_add
#define b3Fabs fabs

71
thirdparty/bullet/Bullet3Common/shared/b3Quat.h vendored
View File

@ -5,35 +5,34 @@
#include "Bullet3Common/shared/b3Float4.h"
#ifdef __cplusplus
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Transform.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Transform.h"
#define b3Quat b3Quaternion
#define b3QuatConstArg const b3Quaternion&
inline b3Quat b3QuatInverse(b3QuatConstArg orn)
{
return orn.inverse();
}
#define b3Quat b3Quaternion
#define b3QuatConstArg const b3Quaternion&
inline b3Quat b3QuatInverse(b3QuatConstArg orn)
{
return orn.inverse();
}
inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
{
b3Transform tr;
tr.setOrigin(translation);
tr.setRotation(orientation);
return tr(point);
}
inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
{
b3Transform tr;
tr.setOrigin(translation);
tr.setRotation(orientation);
return tr(point);
}
#else
typedef float4 b3Quat;
#define b3QuatConstArg const b3Quat
typedef float4 b3Quat;
#define b3QuatConstArg const b3Quat
inline float4 b3FastNormalize4(float4 v)
{
v = (float4)(v.xyz,0.f);
v = (float4)(v.xyz, 0.f);
return fast_normalize(v);
}
inline b3Quat b3QuatMul(b3Quat a, b3Quat b);
inline b3Quat b3QuatNormalized(b3QuatConstArg in);
inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);
@ -43,20 +42,20 @@ inline b3Quat b3QuatInverse(b3QuatConstArg q);
inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)
{
b3Quat ans;
ans = b3Cross3( a, b );
ans += a.w*b+b.w*a;
// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
ans.w = a.w*b.w - b3Dot3F4(a, b);
ans = b3Cross3(a, b);
ans += a.w * b + b.w * a;
// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
ans.w = a.w * b.w - b3Dot3F4(a, b);
return ans;
}
inline b3Quat b3QuatNormalized(b3QuatConstArg in)
{
b3Quat q;
q=in;
q = in;
//return b3FastNormalize4(in);
float len = native_sqrt(dot(q, q));
if(len > 0.f)
if (len > 0.f)
{
q *= 1.f / len;
}
@ -69,15 +68,13 @@ inline b3Quat b3QuatNormalized(b3QuatConstArg in)
}
inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)
{
b3Quat qInv = b3QuatInvert( q );
b3Quat qInv = b3QuatInvert(q);
float4 vcpy = vec;
vcpy.w = 0.f;
float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);
float4 out = b3QuatMul(b3QuatMul(q, vcpy), qInv);
return out;
}
inline b3Quat b3QuatInverse(b3QuatConstArg q)
{
return (b3Quat)(-q.xyz, q.w);
@ -90,14 +87,14 @@ inline b3Quat b3QuatInvert(b3QuatConstArg q)
inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)
{
return b3QuatRotate( b3QuatInvert( q ), vec );
return b3QuatRotate(b3QuatInvert(q), vec);
}
inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
{
return b3QuatRotate( orientation, point ) + (translation);
return b3QuatRotate(orientation, point) + (translation);
}
#endif
#endif //B3_QUAT_H
#endif
#endif //B3_QUAT_H

158
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h vendored
View File

@ -18,7 +18,7 @@ subject to the following restrictions:
#include "Bullet3Common/b3Scalar.h"
enum b3SolverMode
enum b3SolverMode
{
B3_SOLVER_RANDMIZE_ORDER = 1,
B3_SOLVER_FRICTION_SEPARATE = 2,
@ -34,45 +34,38 @@ enum b3SolverMode
struct b3ContactSolverInfoData
{
b3Scalar m_tau;
b3Scalar m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
b3Scalar m_friction;
b3Scalar m_timeStep;
b3Scalar m_restitution;
int m_numIterations;
b3Scalar m_maxErrorReduction;
b3Scalar m_sor;
b3Scalar m_erp;//used as Baumgarte factor
b3Scalar m_erp2;//used in Split Impulse
b3Scalar m_globalCfm;//constraint force mixing
int m_splitImpulse;
b3Scalar m_splitImpulsePenetrationThreshold;
b3Scalar m_splitImpulseTurnErp;
b3Scalar m_linearSlop;
b3Scalar m_warmstartingFactor;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
b3Scalar m_maxGyroscopicForce;
b3Scalar m_singleAxisRollingFrictionThreshold;
b3Scalar m_tau;
b3Scalar m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
b3Scalar m_friction;
b3Scalar m_timeStep;
b3Scalar m_restitution;
int m_numIterations;
b3Scalar m_maxErrorReduction;
b3Scalar m_sor;
b3Scalar m_erp; //used as Baumgarte factor
b3Scalar m_erp2; //used in Split Impulse
b3Scalar m_globalCfm; //constraint force mixing
int m_splitImpulse;
b3Scalar m_splitImpulsePenetrationThreshold;
b3Scalar m_splitImpulseTurnErp;
b3Scalar m_linearSlop;
b3Scalar m_warmstartingFactor;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
b3Scalar m_maxGyroscopicForce;
b3Scalar m_singleAxisRollingFrictionThreshold;
};
struct b3ContactSolverInfo : public b3ContactSolverInfoData
{
inline b3ContactSolverInfo()
{
m_tau = b3Scalar(0.6);
m_damping = b3Scalar(1.0);
m_friction = b3Scalar(0.3);
m_timeStep = b3Scalar(1.f/60.f);
m_timeStep = b3Scalar(1.f / 60.f);
m_restitution = b3Scalar(0.);
m_maxErrorReduction = b3Scalar(20.);
m_numIterations = 10;
@ -84,76 +77,73 @@ struct b3ContactSolverInfo : public b3ContactSolverInfoData
m_splitImpulsePenetrationThreshold = -.04f;
m_splitImpulseTurnErp = 0.1f;
m_linearSlop = b3Scalar(0.0);
m_warmstartingFactor=b3Scalar(0.85);
m_warmstartingFactor = b3Scalar(0.85);
//m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD | B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | B3_SOLVER_RANDMIZE_ORDER;
m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD;// | B3_SOLVER_RANDMIZE_ORDER;
m_restingContactRestitutionThreshold = 2;//unused as of 2.81
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)
m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD; // | B3_SOLVER_RANDMIZE_ORDER;
m_restingContactRestitutionThreshold = 2; //unused as of 2.81
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)
m_singleAxisRollingFrictionThreshold = 1e30f; ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
}
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct b3ContactSolverInfoDoubleData
{
double m_tau;
double m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
double m_friction;
double m_timeStep;
double m_restitution;
double m_maxErrorReduction;
double m_sor;
double m_erp;//used as Baumgarte factor
double m_erp2;//used in Split Impulse
double m_globalCfm;//constraint force mixing
double m_splitImpulsePenetrationThreshold;
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_maxGyroscopicForce;
double m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
double m_tau;
double m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
double m_friction;
double m_timeStep;
double m_restitution;
double m_maxErrorReduction;
double m_sor;
double m_erp; //used as Baumgarte factor
double m_erp2; //used in Split Impulse
double m_globalCfm; //constraint force mixing
double m_splitImpulsePenetrationThreshold;
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_maxGyroscopicForce;
double m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct b3ContactSolverInfoFloatData
{
float m_tau;
float m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
float m_friction;
float m_timeStep;
float m_tau;
float m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
float m_friction;
float m_timeStep;
float m_restitution;
float m_maxErrorReduction;
float m_sor;
float m_erp;//used as Baumgarte factor
float m_restitution;
float m_maxErrorReduction;
float m_sor;
float m_erp; //used as Baumgarte factor
float m_erp2;//used in Split Impulse
float m_globalCfm;//constraint force mixing
float m_splitImpulsePenetrationThreshold;
float m_splitImpulseTurnErp;
float m_erp2; //used in Split Impulse
float m_globalCfm; //constraint force mixing
float m_splitImpulsePenetrationThreshold;
float m_splitImpulseTurnErp;
float m_linearSlop;
float m_warmstartingFactor;
float m_maxGyroscopicForce;
float m_singleAxisRollingFrictionThreshold;
float m_linearSlop;
float m_warmstartingFactor;
float m_maxGyroscopicForce;
float m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
int m_splitImpulse;
char m_padding[4];
};
#endif //B3_CONTACT_SOLVER_INFO
#endif //B3_CONTACT_SOLVER_INFO

103
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp vendored
View File

@ -4,105 +4,100 @@
#include "Bullet3Common/b3TransformUtil.h"
#include <new>
b3FixedConstraint::b3FixedConstraint(int rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB)
:b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE,rbA,rbB)
b3FixedConstraint::b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB)
: b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE, rbA, rbB)
{
m_pivotInA = frameInA.getOrigin();
m_pivotInB = frameInB.getOrigin();
m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
m_relTargetAB = frameInA.getRotation() * frameInB.getRotation().inverse();
}
b3FixedConstraint::~b3FixedConstraint ()
b3FixedConstraint::~b3FixedConstraint()
{
}
void b3FixedConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
void b3FixedConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
{
info->m_numConstraintRows = 6;
info->nub = 6;
}
void b3FixedConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
void b3FixedConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
{
//fix the 3 linear degrees of freedom
const b3Vector3& worldPosA = bodies[m_rbA].m_pos;
const b3Quaternion& worldOrnA = bodies[m_rbA].m_quat;
const b3Vector3& worldPosB= bodies[m_rbB].m_pos;
const b3Vector3& worldPosB = bodies[m_rbB].m_pos;
const b3Quaternion& worldOrnB = bodies[m_rbB].m_quat;
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
info->m_J1linearAxis[info->rowskip + 1] = 1;
info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
b3Vector3 a1 = b3QuatRotate(worldOrnA,m_pivotInA);
b3Vector3 a1 = b3QuatRotate(worldOrnA, m_pivotInA);
{
b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip);
b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
b3Vector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
}
if (info->m_J2linearAxis)
{
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[info->rowskip+1] = -1;
info->m_J2linearAxis[2*info->rowskip+2] = -1;
info->m_J2linearAxis[info->rowskip + 1] = -1;
info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
}
b3Vector3 a2 = b3QuatRotate(worldOrnB,m_pivotInB);
b3Vector3 a2 = b3QuatRotate(worldOrnB, m_pivotInB);
{
// b3Vector3 a2n = -a2;
// b3Vector3 a2n = -a2;
b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
}
// set right hand side for the linear dofs
// set right hand side for the linear dofs
b3Scalar k = info->fps * info->erp;
b3Vector3 linearError = k*(a2+worldPosB-a1-worldPosA);
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = linearError[j];
b3Vector3 linearError = k * (a2 + worldPosB - a1 - worldPosA);
int j;
for (j = 0; j < 3; j++)
{
info->m_constraintError[j * info->rowskip] = linearError[j];
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
}
//fix the 3 angular degrees of freedom
//fix the 3 angular degrees of freedom
int start_row = 3;
int s = info->rowskip;
int start_index = start_row * s;
int start_index = start_row * s;
// 3 rows to make body rotations equal
// 3 rows to make body rotations equal
info->m_J1angularAxis[start_index] = 1;
info->m_J1angularAxis[start_index + s + 1] = 1;
info->m_J1angularAxis[start_index + s*2+2] = 1;
if ( info->m_J2angularAxis)
{
info->m_J2angularAxis[start_index] = -1;
info->m_J2angularAxis[start_index + s+1] = -1;
info->m_J2angularAxis[start_index + s*2+2] = -1;
}
info->m_J1angularAxis[start_index + s + 1] = 1;
info->m_J1angularAxis[start_index + s * 2 + 2] = 1;
if (info->m_J2angularAxis)
{
info->m_J2angularAxis[start_index] = -1;
info->m_J2angularAxis[start_index + s + 1] = -1;
info->m_J2angularAxis[start_index + s * 2 + 2] = -1;
}
// set right hand side for the angular dofs
// set right hand side for the angular dofs
b3Vector3 diff;
b3Scalar angle;
b3Quaternion qrelCur = worldOrnA *worldOrnB.inverse();
b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
diff*=-angle;
for (j=0; j<3; j++)
{
info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
}
b3Quaternion qrelCur = worldOrnA * worldOrnB.inverse();
b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB, qrelCur, diff, angle);
diff *= -angle;
for (j = 0; j < 3; j++)
{
info->m_constraintError[(3 + j) * info->rowskip] = k * diff[j];
}
}

19
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h vendored
View File

@ -4,32 +4,31 @@
#include "b3TypedConstraint.h"
B3_ATTRIBUTE_ALIGNED16(class) b3FixedConstraint : public b3TypedConstraint
B3_ATTRIBUTE_ALIGNED16(class)
b3FixedConstraint : public b3TypedConstraint
{
b3Vector3 m_pivotInA;
b3Vector3 m_pivotInB;
b3Quaternion m_relTargetAB;
public:
b3FixedConstraint(int rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB);
b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB);
virtual ~b3FixedConstraint();
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies);
virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
virtual void setParam(int num, b3Scalar value, int axis = -1)
virtual void setParam(int num, b3Scalar value, int axis = -1)
{
b3Assert(0);
}
virtual b3Scalar getParam(int num, int axis = -1) const
virtual b3Scalar getParam(int num, int axis = -1) const
{
b3Assert(0);
return 0.f;
}
};
#endif //B3_FIXED_CONSTRAINT_H
#endif //B3_FIXED_CONSTRAINT_H

576
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp vendored
View File

@ -26,69 +26,48 @@ http://gimpact.sf.net
#include "Bullet3Common/b3TransformUtil.h"
#include <new>
#define D6_USE_OBSOLETE_METHOD false
#define D6_USE_FRAME_OFFSET true
b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
m_flags(0)
b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0)
{
calculateTransforms(bodies);
}
#define GENERIC_D6_DISABLE_WARMSTARTING 1
b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index);
b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index)
{
int i = index%3;
int j = index/3;
int i = index % 3;
int j = index / 3;
return mat[i][j];
}
///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz);
bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz);
bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz)
{
// // rot = cy*cz -cy*sz sy
// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
//
b3Scalar fi = btGetMatrixElem(mat,2);
b3Scalar fi = btGetMatrixElem(mat, 2);
if (fi < b3Scalar(1.0f))
{
if (fi > b3Scalar(-1.0f))
{
xyz[0] = b3Atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
xyz[1] = b3Asin(btGetMatrixElem(mat,2));
xyz[2] = b3Atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
xyz[0] = b3Atan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
xyz[1] = b3Asin(btGetMatrixElem(mat, 2));
xyz[2] = b3Atan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
return true;
}
else
{
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
xyz[0] = -b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
xyz[0] = -b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
xyz[1] = -B3_HALF_PI;
xyz[2] = b3Scalar(0.0);
return false;
@ -97,7 +76,7 @@ bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
else
{
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
xyz[0] = b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
xyz[0] = b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
xyz[1] = B3_HALF_PI;
xyz[2] = 0.0;
}
@ -108,85 +87,75 @@ bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
int b3RotationalLimitMotor::testLimitValue(b3Scalar test_value)
{
if(m_loLimit>m_hiLimit)
if (m_loLimit > m_hiLimit)
{
m_currentLimit = 0;//Free from violation
m_currentLimit = 0; //Free from violation
return 0;
}
if (test_value < m_loLimit)
{
m_currentLimit = 1;//low limit violation
m_currentLimitError = test_value - m_loLimit;
if(m_currentLimitError>B3_PI)
m_currentLimitError-=B3_2_PI;
else if(m_currentLimitError<-B3_PI)
m_currentLimitError+=B3_2_PI;
m_currentLimit = 1; //low limit violation
m_currentLimitError = test_value - m_loLimit;
if (m_currentLimitError > B3_PI)
m_currentLimitError -= B3_2_PI;
else if (m_currentLimitError < -B3_PI)
m_currentLimitError += B3_2_PI;
return 1;
}
else if (test_value> m_hiLimit)
else if (test_value > m_hiLimit)
{
m_currentLimit = 2;//High limit violation
m_currentLimit = 2; //High limit violation
m_currentLimitError = test_value - m_hiLimit;
if(m_currentLimitError>B3_PI)
m_currentLimitError-=B3_2_PI;
else if(m_currentLimitError<-B3_PI)
m_currentLimitError+=B3_2_PI;
if (m_currentLimitError > B3_PI)
m_currentLimitError -= B3_2_PI;
else if (m_currentLimitError < -B3_PI)
m_currentLimitError += B3_2_PI;
return 2;
};
m_currentLimit = 0;//Free from violation
m_currentLimit = 0; //Free from violation
return 0;
}
//////////////////////////// End b3RotationalLimitMotor ////////////////////////////////////
//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_value)
{
b3Scalar loLimit = m_lowerLimit[limitIndex];
b3Scalar hiLimit = m_upperLimit[limitIndex];
if(loLimit > hiLimit)
if (loLimit > hiLimit)
{
m_currentLimit[limitIndex] = 0;//Free from violation
m_currentLimit[limitIndex] = 0; //Free from violation
m_currentLimitError[limitIndex] = b3Scalar(0.f);
return 0;
}
if (test_value < loLimit)
{
m_currentLimit[limitIndex] = 2;//low limit violation
m_currentLimitError[limitIndex] = test_value - loLimit;
m_currentLimit[limitIndex] = 2; //low limit violation
m_currentLimitError[limitIndex] = test_value - loLimit;
return 2;
}
else if (test_value> hiLimit)
else if (test_value > hiLimit)
{
m_currentLimit[limitIndex] = 1;//High limit violation
m_currentLimit[limitIndex] = 1; //High limit violation
m_currentLimitError[limitIndex] = test_value - hiLimit;
return 1;
};
m_currentLimit[limitIndex] = 0;//Free from violation
m_currentLimit[limitIndex] = 0; //Free from violation
m_currentLimitError[limitIndex] = b3Scalar(0.f);
return 0;
}
//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
void b3Generic6DofConstraint::calculateAngleInfo()
{
b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
// in euler angle mode we do not actually constrain the angular velocity
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
//
@ -211,12 +180,11 @@ void b3Generic6DofConstraint::calculateAngleInfo()
m_calculatedAxis[0].normalize();
m_calculatedAxis[1].normalize();
m_calculatedAxis[2].normalize();
}
static b3Transform getCenterOfMassTransform(const b3RigidBodyData& body)
{
b3Transform tr(body.m_quat,body.m_pos);
b3Transform tr(body.m_quat, body.m_pos);
return tr;
}
@ -226,26 +194,26 @@ void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies)
b3Transform transB;
transA = getCenterOfMassTransform(bodies[m_rbA]);
transB = getCenterOfMassTransform(bodies[m_rbB]);
calculateTransforms(transA,transB,bodies);
calculateTransforms(transA, transB, bodies);
}
void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies)
void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies)
{
m_calculatedTransformA = transA * m_frameInA;
m_calculatedTransformB = transB * m_frameInB;
calculateLinearInfo();
calculateAngleInfo();
if(m_useOffsetForConstraintFrame)
{ // get weight factors depending on masses
if (m_useOffsetForConstraintFrame)
{ // get weight factors depending on masses
b3Scalar miA = bodies[m_rbA].m_invMass;
b3Scalar miB = bodies[m_rbB].m_invMass;
m_hasStaticBody = (miA < B3_EPSILON) || (miB < B3_EPSILON);
b3Scalar miS = miA + miB;
if(miS > b3Scalar(0.f))
if (miS > b3Scalar(0.f))
{
m_factA = miB / miS;
}
else
else
{
m_factA = b3Scalar(0.5f);
}
@ -253,12 +221,6 @@ void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,cons
}
}
bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
{
b3Scalar angle = m_calculatedAxisAngleDiff[axis_index];
@ -269,48 +231,43 @@ bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
return m_angularLimits[axis_index].needApplyTorques();
}
void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
void b3Generic6DofConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
{
//prepare constraint
calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies);
calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]), getCenterOfMassTransform(bodies[m_rbB]), bodies);
info->m_numConstraintRows = 0;
info->nub = 6;
int i;
//test linear limits
for(i = 0; i < 3; i++)
for (i = 0; i < 3; i++)
{
if(m_linearLimits.needApplyForce(i))
if (m_linearLimits.needApplyForce(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
//test angular limits
for (i=0;i<3 ;i++ )
for (i = 0; i < 3; i++)
{
if(testAngularLimitMotor(i))
if (testAngularLimitMotor(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
// printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
// printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
}
void b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
void b3Generic6DofConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
{
//pre-allocate all 6
info->m_numConstraintRows = 6;
info->nub = 0;
}
void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies)
void b3Generic6DofConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
{
b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
const b3Vector3& linVelA = bodies[m_rbA].m_linVel;
@ -318,136 +275,124 @@ void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBod
const b3Vector3& angVelA = bodies[m_rbA].m_angVel;
const b3Vector3& angVelB = bodies[m_rbB].m_angVel;
if(m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
if (m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
else
{ // leave old version for compatibility
int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
{ // leave old version for compatibility
int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
}
void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies)
void b3Generic6DofConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies)
{
//prepare constraint
calculateTransforms(transA,transB,bodies);
calculateTransforms(transA, transB, bodies);
int i;
for (i=0;i<3 ;i++ )
for (i = 0; i < 3; i++)
{
testAngularLimitMotor(i);
}
if(m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
if (m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
else
{ // leave old version for compatibility
int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
{ // leave old version for compatibility
int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
}
int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
{
// int row = 0;
// int row = 0;
//solve linear limits
b3RotationalLimitMotor limot;
for (int i=0;i<3 ;i++ )
for (int i = 0; i < 3; i++)
{
if(m_linearLimits.needApplyForce(i))
{ // re-use rotational motor code
if (m_linearLimits.needApplyForce(i))
{ // re-use rotational motor code
limot.m_bounce = b3Scalar(0.f);
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_damping = m_linearLimits.m_damping;
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
limot.m_maxLimitForce = b3Scalar(0.f);
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_damping = m_linearLimits.m_damping;
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
limot.m_maxLimitForce = b3Scalar(0.f);
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
b3Vector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
int flags = m_flags >> (i * B3_6DOF_FLAGS_AXIS_SHIFT);
limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
if(m_useOffsetForConstraintFrame)
limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
if (m_useOffsetForConstraintFrame)
{
int indx1 = (i + 1) % 3;
int indx2 = (i + 2) % 3;
int rotAllowed = 1; // rotations around orthos to current axis
if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
int rotAllowed = 1; // rotations around orthos to current axis
if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
{
rotAllowed = 0;
}
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
}
else
{
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0);
}
}
}
return row;
}
int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2 *info, int row_offset, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2* info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
{
b3Generic6DofConstraint * d6constraint = this;
b3Generic6DofConstraint* d6constraint = this;
int row = row_offset;
//solve angular limits
for (int i=0;i<3 ;i++ )
for (int i = 0; i < 3; i++)
{
if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
{
b3Vector3 axis = d6constraint->getAxis(i);
int flags = m_flags >> ((i + 3) * B3_6DOF_FLAGS_AXIS_SHIFT);
if(!(flags & B3_6DOF_FLAGS_CFM_NORM))
if (!(flags & B3_6DOF_FLAGS_CFM_NORM))
{
m_angularLimits[i].m_normalCFM = info->cfm[0];
}
if(!(flags & B3_6DOF_FLAGS_CFM_STOP))
if (!(flags & B3_6DOF_FLAGS_CFM_STOP))
{
m_angularLimits[i].m_stopCFM = info->cfm[0];
}
if(!(flags & B3_6DOF_FLAGS_ERP_STOP))
if (!(flags & B3_6DOF_FLAGS_ERP_STOP))
{
m_angularLimits[i].m_stopERP = info->erp;
}
row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
}
}
return row;
}
void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep)
void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep)
{
(void)timeStep;
}
void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB,const b3RigidBodyData* bodies)
void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies)
{
m_frameInA = frameA;
m_frameInB = frameB;
@ -455,33 +400,27 @@ void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Trans
calculateTransforms(bodies);
}
b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const
{
return m_calculatedAxis[axis_index];
}
b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
{
return m_calculatedLinearDiff[axisIndex];
}
b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const
{
return m_calculatedAxisAngleDiff[axisIndex];
}
void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
{
b3Scalar imA = bodies[m_rbA].m_invMass;
b3Scalar imB = bodies[m_rbB].m_invMass;
b3Scalar weight;
if(imB == b3Scalar(0.0))
if (imB == b3Scalar(0.0))
{
weight = b3Scalar(1.0);
}
@ -495,47 +434,43 @@ void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
return;
}
void b3Generic6DofConstraint::calculateLinearInfo()
{
m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
for(int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
}
}
int b3Generic6DofConstraint::get_limit_motor_info2(
b3RotationalLimitMotor * limot,
const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,
b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational,int rotAllowed)
b3RotationalLimitMotor* limot,
const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed)
{
int srow = row * info->rowskip;
bool powered = limot->m_enableMotor;
int limit = limot->m_currentLimit;
if (powered || limit)
{ // if the joint is powered, or has joint limits, add in the extra row
b3Scalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
b3Scalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
int srow = row * info->rowskip;
bool powered = limot->m_enableMotor;
int limit = limot->m_currentLimit;
if (powered || limit)
{ // if the joint is powered, or has joint limits, add in the extra row
b3Scalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
b3Scalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
if (J1)
{
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
J1[srow + 0] = ax1[0];
J1[srow + 1] = ax1[1];
J1[srow + 2] = ax1[2];
}
if (J2)
{
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
J2[srow + 0] = -ax1[0];
J2[srow + 1] = -ax1[1];
J2[srow + 2] = -ax1[2];
}
if((!rotational))
{
if ((!rotational))
{
if (m_useOffsetForConstraintFrame)
{
b3Vector3 tmpA, tmpB, relA, relB;
@ -558,55 +493,56 @@ int b3Generic6DofConstraint::get_limit_motor_info2(
relB = orthoB - totalDist * m_factB;
tmpA = relA.cross(ax1);
tmpB = relB.cross(ax1);
if(m_hasStaticBody && (!rotAllowed))
if (m_hasStaticBody && (!rotAllowed))
{
tmpA *= m_factA;
tmpB *= m_factB;
}
int i;
for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
} else
for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
}
else
{
b3Vector3 ltd; // Linear Torque Decoupling vector
b3Vector3 ltd; // Linear Torque Decoupling vector
b3Vector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
ltd = c.cross(ax1);
info->m_J1angularAxis[srow+0] = ltd[0];
info->m_J1angularAxis[srow+1] = ltd[1];
info->m_J1angularAxis[srow+2] = ltd[2];
info->m_J1angularAxis[srow + 0] = ltd[0];
info->m_J1angularAxis[srow + 1] = ltd[1];
info->m_J1angularAxis[srow + 2] = ltd[2];
c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
ltd = -c.cross(ax1);
info->m_J2angularAxis[srow+0] = ltd[0];
info->m_J2angularAxis[srow+1] = ltd[1];
info->m_J2angularAxis[srow+2] = ltd[2];
info->m_J2angularAxis[srow + 0] = ltd[0];
info->m_J2angularAxis[srow + 1] = ltd[1];
info->m_J2angularAxis[srow + 2] = ltd[2];
}
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
info->m_constraintError[srow] = b3Scalar(0.f);
if (powered)
{
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
info->m_constraintError[srow] = b3Scalar(0.f);
if (powered)
{
info->cfm[srow] = limot->m_normalCFM;
if(!limit)
{
if (!limit)
{
b3Scalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
b3Scalar mot_fact = getMotorFactor( limot->m_currentPosition,
limot->m_loLimit,
limot->m_hiLimit,
tag_vel,
info->fps * limot->m_stopERP);
b3Scalar mot_fact = getMotorFactor(limot->m_currentPosition,
limot->m_loLimit,
limot->m_hiLimit,
tag_vel,
info->fps * limot->m_stopERP);
info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
}
}
if(limit)
{
b3Scalar k = info->fps * limot->m_stopERP;
if(!rotational)
info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
}
}
if (limit)
{
b3Scalar k = info->fps * limot->m_stopERP;
if (!rotational)
{
info->m_constraintError[srow] += k * limot->m_currentLimitError;
}
@ -615,116 +551,112 @@ int b3Generic6DofConstraint::get_limit_motor_info2(
info->m_constraintError[srow] += -k * limot->m_currentLimitError;
}
info->cfm[srow] = limot->m_stopCFM;
if (limot->m_loLimit == limot->m_hiLimit)
{ // limited low and high simultaneously
info->m_lowerLimit[srow] = -B3_INFINITY;
info->m_upperLimit[srow] = B3_INFINITY;
}
else
{
if (limit == 1)
{
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = B3_INFINITY;
}
else
{
info->m_lowerLimit[srow] = -B3_INFINITY;
info->m_upperLimit[srow] = 0;
}
// deal with bounce
if (limot->m_bounce > 0)
{
// calculate joint velocity
b3Scalar vel;
if (rotational)
{
vel = angVelA.dot(ax1);
//make sure that if no body -> angVelB == zero vec
// if (body1)
vel -= angVelB.dot(ax1);
}
else
{
vel = linVelA.dot(ax1);
//make sure that if no body -> angVelB == zero vec
// if (body1)
vel -= linVelB.dot(ax1);
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{
if (vel < 0)
{
b3Scalar newc = -limot->m_bounce* vel;
if (newc > info->m_constraintError[srow])
if (limot->m_loLimit == limot->m_hiLimit)
{ // limited low and high simultaneously
info->m_lowerLimit[srow] = -B3_INFINITY;
info->m_upperLimit[srow] = B3_INFINITY;
}
else
{
if (limit == 1)
{
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = B3_INFINITY;
}
else
{
info->m_lowerLimit[srow] = -B3_INFINITY;
info->m_upperLimit[srow] = 0;
}
// deal with bounce
if (limot->m_bounce > 0)
{
// calculate joint velocity
b3Scalar vel;
if (rotational)
{
vel = angVelA.dot(ax1);
//make sure that if no body -> angVelB == zero vec
// if (body1)
vel -= angVelB.dot(ax1);
}
else
{
vel = linVelA.dot(ax1);
//make sure that if no body -> angVelB == zero vec
// if (body1)
vel -= linVelB.dot(ax1);
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{
if (vel < 0)
{
b3Scalar newc = -limot->m_bounce * vel;
if (newc > info->m_constraintError[srow])
info->m_constraintError[srow] = newc;
}
}
else
{
if (vel > 0)
{
b3Scalar newc = -limot->m_bounce * vel;
if (newc < info->m_constraintError[srow])
}
}
else
{
if (vel > 0)
{
b3Scalar newc = -limot->m_bounce * vel;
if (newc < info->m_constraintError[srow])
info->m_constraintError[srow] = newc;
}
}
}
}
}
return 1;
}
else return 0;
}
}
}
}
}
return 1;
}
else
return 0;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
{
if((axis >= 0) && (axis < 3))
if ((axis >= 0) && (axis < 3))
{
switch(num)
switch (num)
{
case B3_CONSTRAINT_STOP_ERP :
case B3_CONSTRAINT_STOP_ERP:
m_linearLimits.m_stopERP[axis] = value;
m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
case B3_CONSTRAINT_STOP_CFM :
case B3_CONSTRAINT_STOP_CFM:
m_linearLimits.m_stopCFM[axis] = value;
m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
case B3_CONSTRAINT_CFM :
case B3_CONSTRAINT_CFM:
m_linearLimits.m_normalCFM[axis] = value;
m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
default :
default:
b3AssertConstrParams(0);
}
}
else if((axis >=3) && (axis < 6))
else if ((axis >= 3) && (axis < 6))
{
switch(num)
switch (num)
{
case B3_CONSTRAINT_STOP_ERP :
case B3_CONSTRAINT_STOP_ERP:
m_angularLimits[axis - 3].m_stopERP = value;
m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
case B3_CONSTRAINT_STOP_CFM :
case B3_CONSTRAINT_STOP_CFM:
m_angularLimits[axis - 3].m_stopCFM = value;
m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
case B3_CONSTRAINT_CFM :
case B3_CONSTRAINT_CFM:
m_angularLimits[axis - 3].m_normalCFM = value;
m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
default :
default:
b3AssertConstrParams(0);
}
}
@ -734,47 +666,47 @@ void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
}
}
///return the local value of parameter
b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
///return the local value of parameter
b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
{
b3Scalar retVal = 0;
if((axis >= 0) && (axis < 3))
if ((axis >= 0) && (axis < 3))
{
switch(num)
switch (num)
{
case B3_CONSTRAINT_STOP_ERP :
case B3_CONSTRAINT_STOP_ERP:
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_stopERP[axis];
break;
case B3_CONSTRAINT_STOP_CFM :
case B3_CONSTRAINT_STOP_CFM:
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_stopCFM[axis];
break;
case B3_CONSTRAINT_CFM :
case B3_CONSTRAINT_CFM:
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_normalCFM[axis];
break;
default :
default:
b3AssertConstrParams(0);
}
}
else if((axis >=3) && (axis < 6))
else if ((axis >= 3) && (axis < 6))
{
switch(num)
switch (num)
{
case B3_CONSTRAINT_STOP_ERP :
case B3_CONSTRAINT_STOP_ERP:
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_stopERP;
break;
case B3_CONSTRAINT_STOP_CFM :
case B3_CONSTRAINT_STOP_CFM:
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_stopCFM;
break;
case B3_CONSTRAINT_CFM :
case B3_CONSTRAINT_CFM:
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_normalCFM;
break;
default :
default:
b3AssertConstrParams(0);
}
}
@ -785,23 +717,21 @@ b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
return retVal;
}
void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1,const b3Vector3& axis2, const b3RigidBodyData* bodies)
void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies)
{
b3Vector3 zAxis = axis1.normalized();
b3Vector3 yAxis = axis2.normalized();
b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
b3Transform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
// now get constraint frame in local coordinate systems
m_frameInA = getCenterOfMassTransform(bodies[m_rbA]).inverse() * frameInW;
m_frameInB = getCenterOfMassTransform(bodies[m_rbB]).inverse() * frameInW;
calculateTransforms(bodies);
}

567
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h vendored
View File

@ -23,7 +23,6 @@ email: projectileman@yahoo.com
http://gimpact.sf.net
*/
#ifndef B3_GENERIC_6DOF_CONSTRAINT_H
#define B3_GENERIC_6DOF_CONSTRAINT_H
@ -33,88 +32,83 @@ http://gimpact.sf.net
struct b3RigidBodyData;
//! Rotation Limit structure for generic joints
class b3RotationalLimitMotor
{
public:
//! limit_parameters
//!@{
b3Scalar m_loLimit;//!< joint limit
b3Scalar m_hiLimit;//!< joint limit
b3Scalar m_targetVelocity;//!< target motor velocity
b3Scalar m_maxMotorForce;//!< max force on motor
b3Scalar m_maxLimitForce;//!< max force on limit
b3Scalar m_damping;//!< Damping.
b3Scalar m_limitSoftness;//! Relaxation factor
b3Scalar m_normalCFM;//!< Constraint force mixing factor
b3Scalar m_stopERP;//!< Error tolerance factor when joint is at limit
b3Scalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
b3Scalar m_bounce;//!< restitution factor
bool m_enableMotor;
//! limit_parameters
//!@{
b3Scalar m_loLimit; //!< joint limit
b3Scalar m_hiLimit; //!< joint limit
b3Scalar m_targetVelocity; //!< target motor velocity
b3Scalar m_maxMotorForce; //!< max force on motor
b3Scalar m_maxLimitForce; //!< max force on limit
b3Scalar m_damping; //!< Damping.
b3Scalar m_limitSoftness; //! Relaxation factor
b3Scalar m_normalCFM; //!< Constraint force mixing factor
b3Scalar m_stopERP; //!< Error tolerance factor when joint is at limit
b3Scalar m_stopCFM; //!< Constraint force mixing factor when joint is at limit
b3Scalar m_bounce; //!< restitution factor
bool m_enableMotor;
//!@}
//!@}
//! temp_variables
//!@{
b3Scalar m_currentLimitError;//! How much is violated this limit
b3Scalar m_currentPosition; //! current value of angle
int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
b3Scalar m_accumulatedImpulse;
//!@}
//! temp_variables
//!@{
b3Scalar m_currentLimitError; //! How much is violated this limit
b3Scalar m_currentPosition; //! current value of angle
int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit
b3Scalar m_accumulatedImpulse;
//!@}
b3RotationalLimitMotor()
{
m_accumulatedImpulse = 0.f;
m_targetVelocity = 0;
m_maxMotorForce = 6.0f;
m_maxLimitForce = 300.0f;
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
b3RotationalLimitMotor()
{
m_accumulatedImpulse = 0.f;
m_targetVelocity = 0;
m_maxMotorForce = 6.0f;
m_maxLimitForce = 300.0f;
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
m_normalCFM = 0.f;
m_stopERP = 0.2f;
m_stopCFM = 0.f;
m_bounce = 0.0f;
m_damping = 1.0f;
m_limitSoftness = 0.5f;
m_currentLimit = 0;
m_currentLimitError = 0;
m_enableMotor = false;
}
m_bounce = 0.0f;
m_damping = 1.0f;
m_limitSoftness = 0.5f;
m_currentLimit = 0;
m_currentLimitError = 0;
m_enableMotor = false;
}
b3RotationalLimitMotor(const b3RotationalLimitMotor & limot)
{
m_targetVelocity = limot.m_targetVelocity;
m_maxMotorForce = limot.m_maxMotorForce;
m_limitSoftness = limot.m_limitSoftness;
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
b3RotationalLimitMotor(const b3RotationalLimitMotor& limot)
{
m_targetVelocity = limot.m_targetVelocity;
m_maxMotorForce = limot.m_maxMotorForce;
m_limitSoftness = limot.m_limitSoftness;
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
m_normalCFM = limot.m_normalCFM;
m_stopERP = limot.m_stopERP;
m_stopCFM = limot.m_stopCFM;
m_bounce = limot.m_bounce;
m_currentLimit = limot.m_currentLimit;
m_currentLimitError = limot.m_currentLimitError;
m_enableMotor = limot.m_enableMotor;
}
m_stopCFM = limot.m_stopCFM;
m_bounce = limot.m_bounce;
m_currentLimit = limot.m_currentLimit;
m_currentLimitError = limot.m_currentLimitError;
m_enableMotor = limot.m_enableMotor;
}
//! Is limited
bool isLimited()
{
if(m_loLimit > m_hiLimit) return false;
return true;
}
bool isLimited()
{
if (m_loLimit > m_hiLimit) return false;
return true;
}
//! Need apply correction
bool needApplyTorques()
{
if(m_currentLimit == 0 && m_enableMotor == false) return false;
return true;
}
bool needApplyTorques()
{
if (m_currentLimit == 0 && m_enableMotor == false) return false;
return true;
}
//! calculates error
/*!
@ -123,104 +117,98 @@ public:
int testLimitValue(b3Scalar test_value);
//! apply the correction impulses for two bodies
b3Scalar solveAngularLimits(b3Scalar timeStep,b3Vector3& axis, b3Scalar jacDiagABInv,b3RigidBodyData * body0, b3RigidBodyData * body1);
b3Scalar solveAngularLimits(b3Scalar timeStep, b3Vector3& axis, b3Scalar jacDiagABInv, b3RigidBodyData* body0, b3RigidBodyData* body1);
};
class b3TranslationalLimitMotor
{
public:
b3Vector3 m_lowerLimit;//!< the constraint lower limits
b3Vector3 m_upperLimit;//!< the constraint upper limits
b3Vector3 m_accumulatedImpulse;
//! Linear_Limit_parameters
//!@{
b3Vector3 m_normalCFM;//!< Constraint force mixing factor
b3Vector3 m_stopERP;//!< Error tolerance factor when joint is at limit
b3Vector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit
b3Vector3 m_targetVelocity;//!< target motor velocity
b3Vector3 m_maxMotorForce;//!< max force on motor
b3Vector3 m_currentLimitError;//! How much is violated this limit
b3Vector3 m_currentLinearDiff;//! Current relative offset of constraint frames
b3Scalar m_limitSoftness;//!< Softness for linear limit
b3Scalar m_damping;//!< Damping for linear limit
b3Scalar m_restitution;//! Bounce parameter for linear limit
b3Vector3 m_lowerLimit; //!< the constraint lower limits
b3Vector3 m_upperLimit; //!< the constraint upper limits
b3Vector3 m_accumulatedImpulse;
//! Linear_Limit_parameters
//!@{
b3Vector3 m_normalCFM; //!< Constraint force mixing factor
b3Vector3 m_stopERP; //!< Error tolerance factor when joint is at limit
b3Vector3 m_stopCFM; //!< Constraint force mixing factor when joint is at limit
b3Vector3 m_targetVelocity; //!< target motor velocity
b3Vector3 m_maxMotorForce; //!< max force on motor
b3Vector3 m_currentLimitError; //! How much is violated this limit
b3Vector3 m_currentLinearDiff; //! Current relative offset of constraint frames
b3Scalar m_limitSoftness; //!< Softness for linear limit
b3Scalar m_damping; //!< Damping for linear limit
b3Scalar m_restitution; //! Bounce parameter for linear limit
//!@}
bool m_enableMotor[3];
int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
bool m_enableMotor[3];
int m_currentLimit[3]; //!< 0=free, 1=at lower limit, 2=at upper limit
b3TranslationalLimitMotor()
{
m_lowerLimit.setValue(0.f,0.f,0.f);
m_upperLimit.setValue(0.f,0.f,0.f);
m_accumulatedImpulse.setValue(0.f,0.f,0.f);
b3TranslationalLimitMotor()
{
m_lowerLimit.setValue(0.f, 0.f, 0.f);
m_upperLimit.setValue(0.f, 0.f, 0.f);
m_accumulatedImpulse.setValue(0.f, 0.f, 0.f);
m_normalCFM.setValue(0.f, 0.f, 0.f);
m_stopERP.setValue(0.2f, 0.2f, 0.2f);
m_stopCFM.setValue(0.f, 0.f, 0.f);
m_limitSoftness = 0.7f;
m_damping = b3Scalar(1.0f);
m_restitution = b3Scalar(0.5f);
for(int i=0; i < 3; i++)
m_limitSoftness = 0.7f;
m_damping = b3Scalar(1.0f);
m_restitution = b3Scalar(0.5f);
for (int i = 0; i < 3; i++)
{
m_enableMotor[i] = false;
m_targetVelocity[i] = b3Scalar(0.f);
m_maxMotorForce[i] = b3Scalar(0.f);
}
}
}
b3TranslationalLimitMotor(const b3TranslationalLimitMotor & other )
{
m_lowerLimit = other.m_lowerLimit;
m_upperLimit = other.m_upperLimit;
m_accumulatedImpulse = other.m_accumulatedImpulse;
b3TranslationalLimitMotor(const b3TranslationalLimitMotor& other)
{
m_lowerLimit = other.m_lowerLimit;
m_upperLimit = other.m_upperLimit;
m_accumulatedImpulse = other.m_accumulatedImpulse;
m_limitSoftness = other.m_limitSoftness ;
m_damping = other.m_damping;
m_restitution = other.m_restitution;
m_limitSoftness = other.m_limitSoftness;
m_damping = other.m_damping;
m_restitution = other.m_restitution;
m_normalCFM = other.m_normalCFM;
m_stopERP = other.m_stopERP;
m_stopCFM = other.m_stopCFM;
for(int i=0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
m_enableMotor[i] = other.m_enableMotor[i];
m_targetVelocity[i] = other.m_targetVelocity[i];
m_maxMotorForce[i] = other.m_maxMotorForce[i];
}
}
}
//! Test limit
//! Test limit
/*!
- free means upper < lower,
- locked means upper == lower
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
inline bool isLimited(int limitIndex)
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
inline bool needApplyForce(int limitIndex)
{
if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
return true;
}
inline bool isLimited(int limitIndex)
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
inline bool needApplyForce(int limitIndex)
{
if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
return true;
}
int testLimitValue(int limitIndex, b3Scalar test_value);
b3Scalar solveLinearAxis(
b3Scalar timeStep,
b3Scalar jacDiagABInv,
b3RigidBodyData& body1,const b3Vector3 &pointInA,
b3RigidBodyData& body2,const b3Vector3 &pointInB,
int limit_index,
const b3Vector3 & axis_normal_on_a,
const b3Vector3 & anchorPos);
b3Scalar solveLinearAxis(
b3Scalar timeStep,
b3Scalar jacDiagABInv,
b3RigidBodyData& body1, const b3Vector3& pointInA,
b3RigidBodyData& body2, const b3Vector3& pointInB,
int limit_index,
const b3Vector3& axis_normal_on_a,
const b3Vector3& anchorPos);
};
enum b36DofFlags
@ -229,8 +217,7 @@ enum b36DofFlags
B3_6DOF_FLAGS_CFM_STOP = 2,
B3_6DOF_FLAGS_ERP_STOP = 4
};
#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
/// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/*!
@ -268,240 +255,229 @@ This brings support for limit parameters and motors. </li>
</ul>
*/
B3_ATTRIBUTE_ALIGNED16(class) b3Generic6DofConstraint : public b3TypedConstraint
B3_ATTRIBUTE_ALIGNED16(class)
b3Generic6DofConstraint : public b3TypedConstraint
{
protected:
//! relative_frames
//!@{
b3Transform m_frameInA;//!< the constraint space w.r.t body A
b3Transform m_frameInB;//!< the constraint space w.r.t body B
//!@}
//! Jacobians
//!@{
// b3JacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints
// b3JacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints
//!@}
//! Linear_Limit_parameters
//!@{
b3TranslationalLimitMotor m_linearLimits;
//!@}
//! hinge_parameters
//!@{
b3RotationalLimitMotor m_angularLimits[3];
//!@{
b3Transform m_frameInA; //!< the constraint space w.r.t body A
b3Transform m_frameInB; //!< the constraint space w.r.t body B
//!@}
//! Jacobians
//!@{
// b3JacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints
// b3JacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints
//!@}
//! Linear_Limit_parameters
//!@{
b3TranslationalLimitMotor m_linearLimits;
//!@}
//! hinge_parameters
//!@{
b3RotationalLimitMotor m_angularLimits[3];
//!@}
protected:
//! temporal variables
//!@{
b3Transform m_calculatedTransformA;
b3Transform m_calculatedTransformB;
b3Vector3 m_calculatedAxisAngleDiff;
b3Vector3 m_calculatedAxis[3];
b3Vector3 m_calculatedLinearDiff;
b3Scalar m_timeStep;
b3Scalar m_factA;
b3Scalar m_factB;
bool m_hasStaticBody;
b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
//! temporal variables
//!@{
b3Transform m_calculatedTransformA;
b3Transform m_calculatedTransformB;
b3Vector3 m_calculatedAxisAngleDiff;
b3Vector3 m_calculatedAxis[3];
b3Vector3 m_calculatedLinearDiff;
b3Scalar m_timeStep;
b3Scalar m_factA;
b3Scalar m_factB;
bool m_hasStaticBody;
bool m_useLinearReferenceFrameA;
bool m_useOffsetForConstraintFrame;
int m_flags;
b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
//!@}
bool m_useLinearReferenceFrameA;
bool m_useOffsetForConstraintFrame;
b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other)
{
b3Assert(0);
(void) other;
return *this;
}
int m_flags;
//!@}
int setAngularLimits(b3ConstraintInfo2 *info, int row_offset,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB);
b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other)
{
b3Assert(0);
(void)other;
return *this;
}
int setLinearLimits(b3ConstraintInfo2 *info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB);
int setAngularLimits(b3ConstraintInfo2 * info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
int setLinearLimits(b3ConstraintInfo2 * info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
// tests linear limits
void calculateLinearInfo();
//! calcs the euler angles between the two bodies.
void calculateAngleInfo();
void calculateAngleInfo();
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB ,bool useLinearReferenceFrameA,const b3RigidBodyData* bodies);
b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies);
//! Calcs global transform of the offsets
/*!
Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
\sa b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo
*/
void calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies);
void calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies);
void calculateTransforms(const b3RigidBodyData* bodies);
//! Gets the global transform of the offset for body A
/*!
/*!
\sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
*/
const b3Transform & getCalculatedTransformA() const
{
return m_calculatedTransformA;
}
const b3Transform& getCalculatedTransformA() const
{
return m_calculatedTransformA;
}
//! Gets the global transform of the offset for body B
/*!
//! Gets the global transform of the offset for body B
/*!
\sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
*/
const b3Transform & getCalculatedTransformB() const
{
return m_calculatedTransformB;
}
const b3Transform& getCalculatedTransformB() const
{
return m_calculatedTransformB;
}
const b3Transform & getFrameOffsetA() const
{
return m_frameInA;
}
const b3Transform& getFrameOffsetA() const
{
return m_frameInA;
}
const b3Transform & getFrameOffsetB() const
{
return m_frameInB;
}
const b3Transform& getFrameOffsetB() const
{
return m_frameInB;
}
b3Transform& getFrameOffsetA()
{
return m_frameInA;
}
b3Transform & getFrameOffsetA()
{
return m_frameInA;
}
b3Transform& getFrameOffsetB()
{
return m_frameInB;
}
b3Transform & getFrameOffsetB()
{
return m_frameInB;
}
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies);
void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
virtual void getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies);
void getInfo2NonVirtual (b3ConstraintInfo2* info,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies);
void updateRHS(b3Scalar timeStep);
void updateRHS(b3Scalar timeStep);
//! Get the rotation axis in global coordinates
b3Vector3 getAxis(int axis_index) const;
b3Vector3 getAxis(int axis_index) const;
//! Get the relative Euler angle
/*!
//! Get the relative Euler angle
/*!
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
*/
b3Scalar getAngle(int axis_index) const;
b3Scalar getAngle(int axis_index) const;
//! Get the relative position of the constraint pivot
/*!
/*!
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
*/
b3Scalar getRelativePivotPosition(int axis_index) const;
void setFrames(const b3Transform & frameA, const b3Transform & frameB, const b3RigidBodyData* bodies);
void setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies);
//! Test angular limit.
/*!
Calculates angular correction and returns true if limit needs to be corrected.
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
*/
bool testAngularLimitMotor(int axis_index);
bool testAngularLimitMotor(int axis_index);
void setLinearLowerLimit(const b3Vector3& linearLower)
{
m_linearLimits.m_lowerLimit = linearLower;
}
void setLinearLowerLimit(const b3Vector3& linearLower)
{
m_linearLimits.m_lowerLimit = linearLower;
}
void getLinearLowerLimit(b3Vector3& linearLower)
void getLinearLowerLimit(b3Vector3 & linearLower)
{
linearLower = m_linearLimits.m_lowerLimit;
}
void setLinearUpperLimit(const b3Vector3& linearUpper)
void setLinearUpperLimit(const b3Vector3& linearUpper)
{
m_linearLimits.m_upperLimit = linearUpper;
}
void getLinearUpperLimit(b3Vector3& linearUpper)
void getLinearUpperLimit(b3Vector3 & linearUpper)
{
linearUpper = m_linearLimits.m_upperLimit;
}
void setAngularLowerLimit(const b3Vector3& angularLower)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]);
}
void getAngularLowerLimit(b3Vector3& angularLower)
void setAngularLowerLimit(const b3Vector3& angularLower)
{
for(int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]);
}
void getAngularLowerLimit(b3Vector3 & angularLower)
{
for (int i = 0; i < 3; i++)
angularLower[i] = m_angularLimits[i].m_loLimit;
}
void setAngularUpperLimit(const b3Vector3& angularUpper)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]);
}
void getAngularUpperLimit(b3Vector3& angularUpper)
void setAngularUpperLimit(const b3Vector3& angularUpper)
{
for(int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]);
}
void getAngularUpperLimit(b3Vector3 & angularUpper)
{
for (int i = 0; i < 3; i++)
angularUpper[i] = m_angularLimits[i].m_hiLimit;
}
//! Retrieves the angular limit informacion
b3RotationalLimitMotor * getRotationalLimitMotor(int index)
{
return &m_angularLimits[index];
}
b3RotationalLimitMotor* getRotationalLimitMotor(int index)
{
return &m_angularLimits[index];
}
//! Retrieves the limit informacion
b3TranslationalLimitMotor * getTranslationalLimitMotor()
{
return &m_linearLimits;
}
//! Retrieves the limit informacion
b3TranslationalLimitMotor* getTranslationalLimitMotor()
{
return &m_linearLimits;
}
//first 3 are linear, next 3 are angular
void setLimit(int axis, b3Scalar lo, b3Scalar hi)
{
if(axis<3)
{
m_linearLimits.m_lowerLimit[axis] = lo;
m_linearLimits.m_upperLimit[axis] = hi;
}
else
{
//first 3 are linear, next 3 are angular
void setLimit(int axis, b3Scalar lo, b3Scalar hi)
{
if (axis < 3)
{
m_linearLimits.m_lowerLimit[axis] = lo;
m_linearLimits.m_upperLimit[axis] = hi;
}
else
{
lo = b3NormalizeAngle(lo);
hi = b3NormalizeAngle(hi);
m_angularLimits[axis-3].m_loLimit = lo;
m_angularLimits[axis-3].m_hiLimit = hi;
}
}
m_angularLimits[axis - 3].m_loLimit = lo;
m_angularLimits[axis - 3].m_hiLimit = hi;
}
}
//! Test limit
/*!
@ -510,41 +486,32 @@ public:
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
bool isLimited(int limitIndex)
{
if(limitIndex<3)
{
bool isLimited(int limitIndex)
{
if (limitIndex < 3)
{
return m_linearLimits.isLimited(limitIndex);
}
return m_angularLimits[limitIndex - 3].isLimited();
}
}
return m_angularLimits[limitIndex-3].isLimited();
}
virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable
virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable
int get_limit_motor_info2( b3RotationalLimitMotor * limot,
const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,
b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false);
int get_limit_motor_info2(b3RotationalLimitMotor * limot,
const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false);
// access for UseFrameOffset
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, b3Scalar value, int axis = -1);
virtual void setParam(int num, b3Scalar value, int axis = -1);
///return the local value of parameter
virtual b3Scalar getParam(int num, int axis = -1) const;
virtual b3Scalar getParam(int num, int axis = -1) const;
void setAxis( const b3Vector3& axis1, const b3Vector3& axis2,const b3RigidBodyData* bodies);
void setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies);
};
#endif //B3_GENERIC_6DOF_CONSTRAINT_H
#endif //B3_GENERIC_6DOF_CONSTRAINT_H

103
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h vendored
View File

@ -18,7 +18,6 @@ subject to the following restrictions:
#include "Bullet3Common/b3Matrix3x3.h"
//notes:
// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
// which makes the b3JacobianEntry memory layout 16 bytes
@ -27,25 +26,26 @@ subject to the following restrictions:
/// Jacobian entry is an abstraction that allows to describe constraints
/// it can be used in combination with a constraint solver
/// Can be used to relate the effect of an impulse to the constraint error
B3_ATTRIBUTE_ALIGNED16(class) b3JacobianEntry
B3_ATTRIBUTE_ALIGNED16(class)
b3JacobianEntry
{
public:
b3JacobianEntry() {};
b3JacobianEntry(){};
//constraint between two different rigidbodies
b3JacobianEntry(
const b3Matrix3x3& world2A,
const b3Matrix3x3& world2B,
const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
const b3Vector3& jointAxis,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvA,
const b3Scalar massInvA,
const b3Vector3& inertiaInvB,
const b3Scalar massInvB)
:m_linearJointAxis(jointAxis)
: m_linearJointAxis(jointAxis)
{
m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_aJ = world2A * (rel_pos1.cross(m_linearJointAxis));
m_bJ = world2B * (rel_pos2.cross(-m_linearJointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
@ -54,33 +54,31 @@ public:
//angular constraint between two different rigidbodies
b3JacobianEntry(const b3Vector3& jointAxis,
const b3Matrix3x3& world2A,
const b3Matrix3x3& world2B,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvB)
:m_linearJointAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)))
const b3Matrix3x3& world2A,
const b3Matrix3x3& world2B,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvB)
: m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.)))
{
m_aJ= world2A*jointAxis;
m_bJ = world2B*-jointAxis;
m_0MinvJt = inertiaInvA * m_aJ;
m_aJ = world2A * jointAxis;
m_bJ = world2B * -jointAxis;
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
b3Assert(m_Adiag > b3Scalar(0.0));
}
//angular constraint between two different rigidbodies
b3JacobianEntry(const b3Vector3& axisInA,
const b3Vector3& axisInB,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvB)
: m_linearJointAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)))
, m_aJ(axisInA)
, m_bJ(-axisInB)
const b3Vector3& axisInB,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvB)
: m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.))), m_aJ(axisInA), m_bJ(-axisInB)
{
m_0MinvJt = inertiaInvA * m_aJ;
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
b3Assert(m_Adiag > b3Scalar(0.0));
}
@ -88,25 +86,25 @@ public:
//constraint on one rigidbody
b3JacobianEntry(
const b3Matrix3x3& world2A,
const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
const b3Vector3& jointAxis,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvA,
const b3Scalar massInvA)
:m_linearJointAxis(jointAxis)
: m_linearJointAxis(jointAxis)
{
m_aJ= world2A*(rel_pos1.cross(jointAxis));
m_bJ = world2A*(rel_pos2.cross(-jointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
m_aJ = world2A * (rel_pos1.cross(jointAxis));
m_bJ = world2A * (rel_pos2.cross(-jointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
b3Assert(m_Adiag > b3Scalar(0.0));
}
b3Scalar getDiagonal() const { return m_Adiag; }
b3Scalar getDiagonal() const { return m_Adiag; }
// for two constraints on the same rigidbody (for example vehicle friction)
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const
{
const b3JacobianEntry& jacA = *this;
b3Scalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
@ -114,42 +112,39 @@ public:
return lin + ang;
}
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB,const b3Scalar massInvA,const b3Scalar massInvB) const
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA, const b3Scalar massInvB) const
{
const b3JacobianEntry& jacA = *this;
b3Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
b3Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
b3Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
b3Vector3 lin0 = massInvA * lin ;
b3Vector3 lin0 = massInvA * lin;
b3Vector3 lin1 = massInvB * lin;
b3Vector3 sum = ang0+ang1+lin0+lin1;
return sum[0]+sum[1]+sum[2];
b3Vector3 sum = ang0 + ang1 + lin0 + lin1;
return sum[0] + sum[1] + sum[2];
}
b3Scalar getRelativeVelocity(const b3Vector3& linvelA,const b3Vector3& angvelA,const b3Vector3& linvelB,const b3Vector3& angvelB)
b3Scalar getRelativeVelocity(const b3Vector3& linvelA, const b3Vector3& angvelA, const b3Vector3& linvelB, const b3Vector3& angvelB)
{
b3Vector3 linrel = linvelA - linvelB;
b3Vector3 angvela = angvelA * m_aJ;
b3Vector3 angvelb = angvelB * m_bJ;
b3Vector3 angvela = angvelA * m_aJ;
b3Vector3 angvelb = angvelB * m_bJ;
linrel *= m_linearJointAxis;
angvela += angvelb;
angvela += linrel;
b3Scalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
b3Scalar rel_vel2 = angvela[0] + angvela[1] + angvela[2];
return rel_vel2 + B3_EPSILON;
}
//private:
//private:
b3Vector3 m_linearJointAxis;
b3Vector3 m_aJ;
b3Vector3 m_bJ;
b3Vector3 m_0MinvJt;
b3Vector3 m_1MinvJt;
b3Vector3 m_linearJointAxis;
b3Vector3 m_aJ;
b3Vector3 m_bJ;
b3Vector3 m_0MinvJt;
b3Vector3 m_1MinvJt;
//Optimization: can be stored in the w/last component of one of the vectors
b3Scalar m_Adiag;
b3Scalar m_Adiag;
};
#endif //B3_JACOBIAN_ENTRY_H
#endif //B3_JACOBIAN_ENTRY_H

1429
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp vendored
View File

File diff suppressed because it is too large Load Diff

146
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h vendored
View File

@ -1,11 +1,9 @@
#ifndef B3_PGS_JACOBI_SOLVER
#define B3_PGS_JACOBI_SOLVER
struct b3Contact4;
struct b3ContactPoint;
class b3Dispatcher;
#include "b3TypedConstraint.h"
@ -18,132 +16,118 @@ struct b3InertiaData;
class b3PgsJacobiSolver
{
protected:
b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
b3ConstraintArray m_tmpSolverContactConstraintPool;
b3ConstraintArray m_tmpSolverNonContactConstraintPool;
b3ConstraintArray m_tmpSolverContactFrictionConstraintPool;
b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
b3ConstraintArray m_tmpSolverContactConstraintPool;
b3ConstraintArray m_tmpSolverNonContactConstraintPool;
b3ConstraintArray m_tmpSolverContactFrictionConstraintPool;
b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
b3AlignedObjectArray<int> m_orderTmpConstraintPool;
b3AlignedObjectArray<int> m_orderNonContactConstraintPool;
b3AlignedObjectArray<int> m_orderFrictionConstraintPool;
b3AlignedObjectArray<int> m_orderTmpConstraintPool;
b3AlignedObjectArray<int> m_orderNonContactConstraintPool;
b3AlignedObjectArray<int> m_orderFrictionConstraintPool;
b3AlignedObjectArray<b3TypedConstraint::b3ConstraintInfo1> m_tmpConstraintSizesPool;
b3AlignedObjectArray<int> m_bodyCount;
b3AlignedObjectArray<int> m_bodyCountCheck;
b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities;
b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities;
bool m_usePgs;
void averageVelocities();
b3AlignedObjectArray<int> m_bodyCount;
b3AlignedObjectArray<int> m_bodyCountCheck;
int m_maxOverrideNumSolverIterations;
b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities;
b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities;
int m_numSplitImpulseRecoveries;
bool m_usePgs;
void averageVelocities();
b3Scalar getContactProcessingThreshold(b3Contact4* contact)
int m_maxOverrideNumSolverIterations;
int m_numSplitImpulseRecoveries;
b3Scalar getContactProcessingThreshold(b3Contact4* contact)
{
return 0.02f;
}
void setupFrictionConstraint( b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
void setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
void setupRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f);
void setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0, b3Scalar cfmSlip = 0.f);
void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,
b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp,
const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp,
const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
b3Vector3& rel_pos1, b3Vector3& rel_pos2);
void setFrictionConstraintImpulse( b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
void setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB,
b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
unsigned long m_btSeed2;
unsigned long m_btSeed2;
b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal);
void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal);
void resolveSplitPenetrationSIMD(
b3SolverBody& bodyA, b3SolverBody& bodyB,
const b3SolverConstraint& contactConstraint);
void resolveSplitPenetrationSIMD(
b3SolverBody& bodyA,b3SolverBody& bodyB,
const b3SolverConstraint& contactConstraint);
void resolveSplitPenetrationImpulseCacheFriendly(
b3SolverBody& bodyA,b3SolverBody& bodyB,
const b3SolverConstraint& contactConstraint);
void resolveSplitPenetrationImpulseCacheFriendly(
b3SolverBody& bodyA, b3SolverBody& bodyB,
const b3SolverConstraint& contactConstraint);
//internal method
int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias);
void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject);
int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias);
void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject);
void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
protected:
virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal);
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
b3PgsJacobiSolver(bool usePgs);
virtual ~b3PgsJacobiSolver();
// void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts);
void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
// void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts);
void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
b3Scalar solveGroup(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
b3Scalar solveGroup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
///clear internal cached data and reset random seed
virtual void reset();
virtual void reset();
unsigned long b3Rand2();
int b3RandInt2 (int n);
int b3RandInt2(int n);
void setRandSeed(unsigned long seed)
void setRandSeed(unsigned long seed)
{
m_btSeed2 = seed;
}
unsigned long getRandSeed() const
unsigned long getRandSeed() const
{
return m_btSeed2;
}
};
#endif //B3_PGS_JACOBI_SOLVER
#endif //B3_PGS_JACOBI_SOLVER

153
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp vendored
View File

@ -13,21 +13,14 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "b3Point2PointConstraint.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
#include <new>
b3Point2PointConstraint::b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB)
:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
m_flags(0)
b3Point2PointConstraint::b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB)
: b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0)
{
}
/*
@ -40,22 +33,18 @@ m_useSolveConstraintObsolete(false)
}
*/
void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
void b3Point2PointConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
{
getInfo1NonVirtual(info,bodies);
getInfo1NonVirtual(info, bodies);
}
void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
void b3Point2PointConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
{
info->m_numConstraintRows = 3;
info->nub = 3;
info->m_numConstraintRows = 3;
info->nub = 3;
}
void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
void b3Point2PointConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
{
b3Transform trA;
trA.setIdentity();
@ -67,143 +56,135 @@ void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBo
trB.setOrigin(bodies[m_rbB].m_pos);
trB.setRotation(bodies[m_rbB].m_quat);
getInfo2NonVirtual(info, trA,trB);
getInfo2NonVirtual(info, trA, trB);
}
void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
void b3Point2PointConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
{
//retrieve matrices
//retrieve matrices
// anchor points in global coordinates with respect to body PORs.
// set jacobian
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
b3Vector3 a1 = body0_trans.getBasis()*getPivotInA();
// set jacobian
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip + 1] = 1;
info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
b3Vector3 a1 = body0_trans.getBasis() * getPivotInA();
//b3Vector3 a1a = b3QuatRotate(body0_trans.getRotation(),getPivotInA());
{
b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip);
b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
b3Vector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
}
if (info->m_J2linearAxis)
{
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[info->rowskip+1] = -1;
info->m_J2linearAxis[2*info->rowskip+2] = -1;
info->m_J2linearAxis[info->rowskip + 1] = -1;
info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
}
b3Vector3 a2 = body1_trans.getBasis()*getPivotInB();
b3Vector3 a2 = body1_trans.getBasis() * getPivotInB();
{
// b3Vector3 a2n = -a2;
// b3Vector3 a2n = -a2;
b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
}
// set right hand side
// set right hand side
b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;
b3Scalar k = info->fps * currERP;
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
if(m_flags & B3_P2P_FLAGS_CFM)
b3Scalar k = info->fps * currERP;
int j;
for (j = 0; j < 3; j++)
{
for (j=0; j<3; j++)
info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
if (m_flags & B3_P2P_FLAGS_CFM)
{
for (j = 0; j < 3; j++)
{
info->cfm[j*info->rowskip] = m_cfm;
info->cfm[j * info->rowskip] = m_cfm;
}
}
b3Scalar impulseClamp = m_setting.m_impulseClamp;//
for (j=0; j<3; j++)
{
b3Scalar impulseClamp = m_setting.m_impulseClamp; //
for (j = 0; j < 3; j++)
{
if (m_setting.m_impulseClamp > 0)
{
info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
info->m_upperLimit[j*info->rowskip] = impulseClamp;
info->m_lowerLimit[j * info->rowskip] = -impulseClamp;
info->m_upperLimit[j * info->rowskip] = impulseClamp;
}
}
info->m_damping = m_setting.m_damping;
}
void b3Point2PointConstraint::updateRHS(b3Scalar timeStep)
void b3Point2PointConstraint::updateRHS(b3Scalar timeStep)
{
(void)timeStep;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis)
{
if(axis != -1)
if (axis != -1)
{
b3AssertConstrParams(0);
}
else
{
switch(num)
switch (num)
{
case B3_CONSTRAINT_ERP :
case B3_CONSTRAINT_STOP_ERP :
m_erp = value;
case B3_CONSTRAINT_ERP:
case B3_CONSTRAINT_STOP_ERP:
m_erp = value;
m_flags |= B3_P2P_FLAGS_ERP;
break;
case B3_CONSTRAINT_CFM :
case B3_CONSTRAINT_STOP_CFM :
m_cfm = value;
case B3_CONSTRAINT_CFM:
case B3_CONSTRAINT_STOP_CFM:
m_cfm = value;
m_flags |= B3_P2P_FLAGS_CFM;
break;
default:
default:
b3AssertConstrParams(0);
}
}
}
///return the local value of parameter
b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
{
b3Scalar retVal(B3_INFINITY);
if(axis != -1)
if (axis != -1)
{
b3AssertConstrParams(0);
}
else
{
switch(num)
switch (num)
{
case B3_CONSTRAINT_ERP :
case B3_CONSTRAINT_STOP_ERP :
case B3_CONSTRAINT_ERP:
case B3_CONSTRAINT_STOP_ERP:
b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP);
retVal = m_erp;
retVal = m_erp;
break;
case B3_CONSTRAINT_CFM :
case B3_CONSTRAINT_STOP_CFM :
case B3_CONSTRAINT_CFM:
case B3_CONSTRAINT_STOP_CFM:
b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM);
retVal = m_cfm;
retVal = m_cfm;
break;
default:
default:
b3AssertConstrParams(0);
}
}
return retVal;
}

96
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h vendored
View File

@ -22,26 +22,24 @@ subject to the following restrictions:
class b3RigidBody;
#ifdef B3_USE_DOUBLE_PRECISION
#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData
#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData"
#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData
#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData"
#else
#define b3Point2PointConstraintData b3Point2PointConstraintFloatData
#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData"
#endif //B3_USE_DOUBLE_PRECISION
#define b3Point2PointConstraintData b3Point2PointConstraintFloatData
#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData"
#endif //B3_USE_DOUBLE_PRECISION
struct b3ConstraintSetting
struct b3ConstraintSetting
{
b3ConstraintSetting() :
m_tau(b3Scalar(0.3)),
m_damping(b3Scalar(1.)),
m_impulseClamp(b3Scalar(0.))
b3ConstraintSetting() : m_tau(b3Scalar(0.3)),
m_damping(b3Scalar(1.)),
m_impulseClamp(b3Scalar(0.))
{
}
b3Scalar m_tau;
b3Scalar m_damping;
b3Scalar m_impulseClamp;
b3Scalar m_tau;
b3Scalar m_damping;
b3Scalar m_impulseClamp;
};
enum b3Point2PointFlags
@ -51,47 +49,45 @@ enum b3Point2PointFlags
};
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
B3_ATTRIBUTE_ALIGNED16(class) b3Point2PointConstraint : public b3TypedConstraint
B3_ATTRIBUTE_ALIGNED16(class)
b3Point2PointConstraint : public b3TypedConstraint
{
#ifdef IN_PARALLELL_SOLVER
public:
#endif
b3Vector3 m_pivotInA;
b3Vector3 m_pivotInB;
int m_flags;
b3Scalar m_erp;
b3Scalar m_cfm;
public:
b3Vector3 m_pivotInA;
b3Vector3 m_pivotInB;
int m_flags;
b3Scalar m_erp;
b3Scalar m_cfm;
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
b3ConstraintSetting m_setting;
b3ConstraintSetting m_setting;
b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB);
b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB);
//b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA);
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& body0_trans, const b3Transform& body1_trans);
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies);
void updateRHS(b3Scalar timeStep);
void getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans);
void updateRHS(b3Scalar timeStep);
void setPivotA(const b3Vector3& pivotA)
void setPivotA(const b3Vector3& pivotA)
{
m_pivotInA = pivotA;
}
void setPivotB(const b3Vector3& pivotB)
void setPivotB(const b3Vector3& pivotB)
{
m_pivotInB = pivotB;
}
@ -106,34 +102,32 @@ public:
return m_pivotInB;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, b3Scalar value, int axis = -1);
virtual void setParam(int num, b3Scalar value, int axis = -1);
///return the local value of parameter
virtual b3Scalar getParam(int num, int axis = -1) const;
virtual b3Scalar getParam(int num, int axis = -1) const;
// virtual int calculateSerializeBufferSize() const;
// virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
// virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
// virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct b3Point2PointConstraintFloatData
struct b3Point2PointConstraintFloatData
{
b3TypedConstraintData m_typeConstraintData;
b3Vector3FloatData m_pivotInA;
b3Vector3FloatData m_pivotInB;
b3TypedConstraintData m_typeConstraintData;
b3Vector3FloatData m_pivotInA;
b3Vector3FloatData m_pivotInB;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct b3Point2PointConstraintDoubleData
struct b3Point2PointConstraintDoubleData
{
b3TypedConstraintData m_typeConstraintData;
b3Vector3DoubleData m_pivotInA;
b3Vector3DoubleData m_pivotInB;
b3TypedConstraintData m_typeConstraintData;
b3Vector3DoubleData m_pivotInA;
b3Vector3DoubleData m_pivotInB;
};
/*
@ -156,4 +150,4 @@ B3_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer,
}
*/
#endif //B3_POINT2POINTCONSTRAINT_H
#endif //B3_POINT2POINTCONSTRAINT_H

189
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h vendored
View File

@ -16,7 +16,6 @@ subject to the following restrictions:
#ifndef B3_SOLVER_BODY_H
#define B3_SOLVER_BODY_H
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
@ -26,110 +25,104 @@ subject to the following restrictions:
///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
#ifdef B3_USE_SSE
#define USE_SIMD 1
#endif //
#endif //
#ifdef USE_SIMD
struct b3SimdScalar
struct b3SimdScalar
{
B3_FORCE_INLINE b3SimdScalar()
{
}
B3_FORCE_INLINE b3SimdScalar(float fl)
:m_vec128 (_mm_set1_ps(fl))
B3_FORCE_INLINE b3SimdScalar()
{
}
B3_FORCE_INLINE b3SimdScalar(__m128 v128)
:m_vec128(v128)
B3_FORCE_INLINE b3SimdScalar(float fl)
: m_vec128(_mm_set1_ps(fl))
{
}
union
B3_FORCE_INLINE b3SimdScalar(__m128 v128)
: m_vec128(v128)
{
__m128 m_vec128;
float m_floats[4];
float x,y,z,w;
int m_ints[4];
b3Scalar m_unusedPadding;
}
union {
__m128 m_vec128;
float m_floats[4];
float x, y, z, w;
int m_ints[4];
b3Scalar m_unusedPadding;
};
B3_FORCE_INLINE __m128 get128()
B3_FORCE_INLINE __m128 get128()
{
return m_vec128;
}
B3_FORCE_INLINE const __m128 get128() const
B3_FORCE_INLINE const __m128 get128() const
{
return m_vec128;
}
B3_FORCE_INLINE void set128(__m128 v128)
B3_FORCE_INLINE void set128(__m128 v128)
{
m_vec128 = v128;
}
B3_FORCE_INLINE operator __m128()
{
return m_vec128;
B3_FORCE_INLINE operator __m128()
{
return m_vec128;
}
B3_FORCE_INLINE operator const __m128() const
{
return m_vec128;
}
B3_FORCE_INLINE operator float() const
{
return m_floats[0];
B3_FORCE_INLINE operator const __m128() const
{
return m_vec128;
}
B3_FORCE_INLINE operator float() const
{
return m_floats[0];
}
};
///@brief Return the elementwise product of two b3SimdScalar
B3_FORCE_INLINE b3SimdScalar
operator*(const b3SimdScalar& v1, const b3SimdScalar& v2)
B3_FORCE_INLINE b3SimdScalar
operator*(const b3SimdScalar& v1, const b3SimdScalar& v2)
{
return b3SimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
return b3SimdScalar(_mm_mul_ps(v1.get128(), v2.get128()));
}
///@brief Return the elementwise product of two b3SimdScalar
B3_FORCE_INLINE b3SimdScalar
operator+(const b3SimdScalar& v1, const b3SimdScalar& v2)
B3_FORCE_INLINE b3SimdScalar
operator+(const b3SimdScalar& v1, const b3SimdScalar& v2)
{
return b3SimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
return b3SimdScalar(_mm_add_ps(v1.get128(), v2.get128()));
}
#else
#define b3SimdScalar b3Scalar
#endif
///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
B3_ATTRIBUTE_ALIGNED16(struct)
b3SolverBody
{
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Transform m_worldTransform;
b3Vector3 m_deltaLinearVelocity;
b3Vector3 m_deltaAngularVelocity;
b3Vector3 m_angularFactor;
b3Vector3 m_linearFactor;
b3Vector3 m_invMass;
b3Vector3 m_pushVelocity;
b3Vector3 m_turnVelocity;
b3Vector3 m_linearVelocity;
b3Vector3 m_angularVelocity;
b3Transform m_worldTransform;
b3Vector3 m_deltaLinearVelocity;
b3Vector3 m_deltaAngularVelocity;
b3Vector3 m_angularFactor;
b3Vector3 m_linearFactor;
b3Vector3 m_invMass;
b3Vector3 m_pushVelocity;
b3Vector3 m_turnVelocity;
b3Vector3 m_linearVelocity;
b3Vector3 m_angularVelocity;
union
{
void* m_originalBody;
int m_originalBodyIndex;
union {
void* m_originalBody;
int m_originalBodyIndex;
};
int padding[3];
void setWorldTransform(const b3Transform& worldTransform)
void setWorldTransform(const b3Transform& worldTransform)
{
m_worldTransform = worldTransform;
}
@ -138,45 +131,42 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
{
return m_worldTransform;
}
B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
{
if (m_originalBody)
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
else
velocity.setValue(0,0,0);
velocity.setValue(0, 0, 0);
}
B3_FORCE_INLINE void getAngularVelocity(b3Vector3& angVel) const
B3_FORCE_INLINE void getAngularVelocity(b3Vector3 & angVel) const
{
if (m_originalBody)
angVel =m_angularVelocity+m_deltaAngularVelocity;
angVel = m_angularVelocity + m_deltaAngularVelocity;
else
angVel.setValue(0,0,0);
angVel.setValue(0, 0, 0);
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
{
if (m_originalBody)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
}
}
B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,b3Scalar impulseMagnitude)
B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, b3Scalar impulseMagnitude)
{
if (m_originalBody)
{
m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor;
m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
}
}
const b3Vector3& getDeltaLinearVelocity() const
{
return m_deltaLinearVelocity;
@ -187,20 +177,19 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
return m_deltaAngularVelocity;
}
const b3Vector3& getPushVelocity() const
const b3Vector3& getPushVelocity() const
{
return m_pushVelocity;
}
const b3Vector3& getTurnVelocity() const
const b3Vector3& getTurnVelocity() const
{
return m_turnVelocity;
}
////////////////////////////////////////////////
///some internal methods, don't use them
b3Vector3& internalGetDeltaLinearVelocity()
{
return m_deltaLinearVelocity;
@ -225,7 +214,7 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
{
m_invMass = invMass;
}
b3Vector3& internalGetPushVelocity()
{
return m_pushVelocity;
@ -236,67 +225,57 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
return m_turnVelocity;
}
B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
{
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
}
B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3& angVel) const
B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3 & angVel) const
{
angVel = m_angularVelocity+m_deltaAngularVelocity;
angVel = m_angularVelocity + m_deltaAngularVelocity;
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
{
//if (m_originalBody)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
}
}
void writebackVelocity()
void writebackVelocity()
{
//if (m_originalBody>=0)
{
m_linearVelocity +=m_deltaLinearVelocity;
m_linearVelocity += m_deltaLinearVelocity;
m_angularVelocity += m_deltaAngularVelocity;
//m_originalBody->setCompanionId(-1);
}
}
void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
{
(void) timeStep;
(void)timeStep;
if (m_originalBody)
{
m_linearVelocity += m_deltaLinearVelocity;
m_angularVelocity += m_deltaAngularVelocity;
//correct the position/orientation based on push/turn recovery
b3Transform newTransform;
if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
if (m_pushVelocity[0] != 0.f || m_pushVelocity[1] != 0 || m_pushVelocity[2] != 0 || m_turnVelocity[0] != 0.f || m_turnVelocity[1] != 0 || m_turnVelocity[2] != 0)
{
// b3Quaternion orn = m_worldTransform.getRotation();
b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
// b3Quaternion orn = m_worldTransform.getRotation();
b3TransformUtil::integrateTransform(m_worldTransform, m_pushVelocity, m_turnVelocity * splitImpulseTurnErp, timeStep, newTransform);
m_worldTransform = newTransform;
}
//m_worldTransform.setRotation(orn);
//m_originalBody->setCompanionId(-1);
}
}
};
#endif //B3_SOLVER_BODY_H
#endif //B3_SOLVER_BODY_H

59
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h vendored
View File

@ -16,7 +16,6 @@ subject to the following restrictions:
#ifndef B3_SOLVER_CONSTRAINT_H
#define B3_SOLVER_CONSTRAINT_H
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
//#include "b3JacobianEntry.h"
@ -25,56 +24,50 @@ subject to the following restrictions:
//#define NO_FRICTION_TANGENTIALS 1
#include "b3SolverBody.h"
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
B3_ATTRIBUTE_ALIGNED16(struct)
b3SolverConstraint
{
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Vector3 m_relpos1CrossNormal;
b3Vector3 m_contactNormal;
b3Vector3 m_relpos1CrossNormal;
b3Vector3 m_contactNormal;
b3Vector3 m_relpos2CrossNormal;
b3Vector3 m_relpos2CrossNormal;
//b3Vector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
b3Vector3 m_angularComponentA;
b3Vector3 m_angularComponentB;
mutable b3SimdScalar m_appliedPushImpulse;
mutable b3SimdScalar m_appliedImpulse;
b3Vector3 m_angularComponentA;
b3Vector3 m_angularComponentB;
mutable b3SimdScalar m_appliedPushImpulse;
mutable b3SimdScalar m_appliedImpulse;
int m_padding1;
int m_padding2;
b3Scalar m_friction;
b3Scalar m_jacDiagABInv;
b3Scalar m_rhs;
b3Scalar m_cfm;
b3Scalar m_lowerLimit;
b3Scalar m_upperLimit;
b3Scalar m_rhsPenetration;
union
{
void* m_originalContactPoint;
b3Scalar m_unusedPadding4;
b3Scalar m_friction;
b3Scalar m_jacDiagABInv;
b3Scalar m_rhs;
b3Scalar m_cfm;
b3Scalar m_lowerLimit;
b3Scalar m_upperLimit;
b3Scalar m_rhsPenetration;
union {
void* m_originalContactPoint;
b3Scalar m_unusedPadding4;
};
int m_overrideNumSolverIterations;
int m_frictionIndex;
int m_overrideNumSolverIterations;
int m_frictionIndex;
int m_solverBodyIdA;
int m_solverBodyIdB;
enum b3SolverConstraintType
enum b3SolverConstraintType
{
B3_SOLVER_CONTACT_1D = 0,
B3_SOLVER_FRICTION_1D
};
};
typedef b3AlignedObjectArray<b3SolverConstraint> b3ConstraintArray;
#endif //B3_SOLVER_CONSTRAINT_H
typedef b3AlignedObjectArray<b3SolverConstraint> b3ConstraintArray;
#endif //B3_SOLVER_CONSTRAINT_H

58
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp vendored
View File

@ -13,53 +13,46 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "b3TypedConstraint.h"
//#include "Bullet3Common/b3Serializer.h"
#define B3_DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f)
b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA,int rbB)
:b3TypedObject(type),
m_userConstraintType(-1),
m_userConstraintPtr((void*)-1),
m_breakingImpulseThreshold(B3_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),
m_overrideNumSolverIterations(-1),
m_rbA(rbA),
m_rbB(rbB),
m_appliedImpulse(b3Scalar(0.)),
m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE),
m_jointFeedback(0)
b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA, int rbB)
: b3TypedObject(type),
m_userConstraintType(-1),
m_userConstraintPtr((void*)-1),
m_breakingImpulseThreshold(B3_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),
m_overrideNumSolverIterations(-1),
m_rbA(rbA),
m_rbB(rbB),
m_appliedImpulse(b3Scalar(0.)),
m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE),
m_jointFeedback(0)
{
}
b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact)
{
if(lowLim > uppLim)
if (lowLim > uppLim)
{
return b3Scalar(1.0f);
}
else if(lowLim == uppLim)
else if (lowLim == uppLim)
{
return b3Scalar(0.0f);
}
b3Scalar lim_fact = b3Scalar(1.0f);
b3Scalar delta_max = vel / timeFact;
if(delta_max < b3Scalar(0.0f))
if (delta_max < b3Scalar(0.0f))
{
if((pos >= lowLim) && (pos < (lowLim - delta_max)))
if ((pos >= lowLim) && (pos < (lowLim - delta_max)))
{
lim_fact = (lowLim - pos) / delta_max;
}
else if(pos < lowLim)
else if (pos < lowLim)
{
lim_fact = b3Scalar(0.0f);
}
@ -68,13 +61,13 @@ b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scal
lim_fact = b3Scalar(1.0f);
}
}
else if(delta_max > b3Scalar(0.0f))
else if (delta_max > b3Scalar(0.0f))
{
if((pos <= uppLim) && (pos > (uppLim - delta_max)))
if ((pos <= uppLim) && (pos > (uppLim - delta_max)))
{
lim_fact = (uppLim - pos) / delta_max;
}
else if(pos > uppLim)
else if (pos > uppLim)
{
lim_fact = b3Scalar(0.0f);
}
@ -85,18 +78,16 @@ b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scal
}
else
{
lim_fact = b3Scalar(0.0f);
lim_fact = b3Scalar(0.0f);
}
return lim_fact;
}
void b3AngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor)
{
m_halfRange = (high - low) / 2.0f;
m_center = b3NormalizeAngle(low + m_halfRange);
m_softness = _softness;
m_softness = _softness;
m_biasFactor = _biasFactor;
m_relaxationFactor = _relaxationFactor;
}
@ -113,7 +104,7 @@ void b3AngularLimit::test(const b3Scalar angle)
if (deviation < -m_halfRange)
{
m_solveLimit = true;
m_correction = - (deviation + m_halfRange);
m_correction = -(deviation + m_halfRange);
m_sign = +1.0f;
}
else if (deviation > m_halfRange)
@ -125,7 +116,6 @@ void b3AngularLimit::test(const b3Scalar angle)
}
}
b3Scalar b3AngularLimit::getError() const
{
return m_correction * m_sign;

212
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h vendored
View File

@ -16,7 +16,6 @@ subject to the following restrictions:
#ifndef B3_TYPED_CONSTRAINT_H
#define B3_TYPED_CONSTRAINT_H
#include "Bullet3Common/b3Scalar.h"
#include "b3SolverConstraint.h"
@ -25,7 +24,7 @@ class b3Serializer;
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
enum b3TypedConstraintType
{
B3_POINT2POINT_CONSTRAINT_TYPE=3,
B3_POINT2POINT_CONSTRAINT_TYPE = 3,
B3_HINGE_CONSTRAINT_TYPE,
B3_CONETWIST_CONSTRAINT_TYPE,
B3_D6_CONSTRAINT_TYPE,
@ -37,92 +36,86 @@ enum b3TypedConstraintType
B3_MAX_CONSTRAINT_TYPE
};
enum b3ConstraintParams
{
B3_CONSTRAINT_ERP=1,
B3_CONSTRAINT_ERP = 1,
B3_CONSTRAINT_STOP_ERP,
B3_CONSTRAINT_CFM,
B3_CONSTRAINT_STOP_CFM
};
#if 1
#define b3AssertConstrParams(_par) b3Assert(_par)
#define b3AssertConstrParams(_par) b3Assert(_par)
#else
#define b3AssertConstrParams(_par)
#define b3AssertConstrParams(_par)
#endif
B3_ATTRIBUTE_ALIGNED16(struct) b3JointFeedback
B3_ATTRIBUTE_ALIGNED16(struct)
b3JointFeedback
{
b3Vector3 m_appliedForceBodyA;
b3Vector3 m_appliedTorqueBodyA;
b3Vector3 m_appliedForceBodyB;
b3Vector3 m_appliedTorqueBodyB;
b3Vector3 m_appliedForceBodyA;
b3Vector3 m_appliedTorqueBodyA;
b3Vector3 m_appliedForceBodyB;
b3Vector3 m_appliedTorqueBodyB;
};
struct b3RigidBodyData;
///TypedConstraint is the baseclass for Bullet constraints and vehicles
B3_ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public b3TypedObject
B3_ATTRIBUTE_ALIGNED16(class)
b3TypedConstraint : public b3TypedObject
{
int m_userConstraintType;
int m_userConstraintType;
union
{
int m_userConstraintId;
union {
int m_userConstraintId;
void* m_userConstraintPtr;
};
b3Scalar m_breakingImpulseThreshold;
bool m_isEnabled;
bool m_needsFeedback;
int m_overrideNumSolverIterations;
b3Scalar m_breakingImpulseThreshold;
bool m_isEnabled;
bool m_needsFeedback;
int m_overrideNumSolverIterations;
b3TypedConstraint& operator=(b3TypedConstraint& other)
b3TypedConstraint& operator=(b3TypedConstraint& other)
{
b3Assert(0);
(void) other;
(void)other;
return *this;
}
protected:
int m_rbA;
int m_rbB;
b3Scalar m_appliedImpulse;
b3Scalar m_dbgDrawSize;
b3JointFeedback* m_jointFeedback;
int m_rbA;
int m_rbB;
b3Scalar m_appliedImpulse;
b3Scalar m_dbgDrawSize;
b3JointFeedback* m_jointFeedback;
///internal method used by the constraint solver, don't use them directly
b3Scalar getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact);
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
virtual ~b3TypedConstraint() {};
b3TypedConstraint(b3TypedConstraintType type, int bodyA,int bodyB);
virtual ~b3TypedConstraint(){};
b3TypedConstraint(b3TypedConstraintType type, int bodyA, int bodyB);
struct b3ConstraintInfo1 {
int m_numConstraintRows,nub;
struct b3ConstraintInfo1
{
int m_numConstraintRows, nub;
};
struct b3ConstraintInfo2 {
struct b3ConstraintInfo2
{
// integrator parameters: frames per second (1/stepsize), default error
// reduction parameter (0..1).
b3Scalar fps,erp;
b3Scalar fps, erp;
// for the first and second body, pointers to two (linear and angular)
// n*3 jacobian sub matrices, stored by rows. these matrices will have
// been initialized to 0 on entry. if the second body is zero then the
// J2xx pointers may be 0.
b3Scalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
b3Scalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis;
// elements to jump from one row to the next in J's
int rowskip;
@ -130,24 +123,24 @@ public:
// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
// "constraint force mixing" vector. c is set to zero on entry, cfm is
// set to a constant value (typically very small or zero) value on entry.
b3Scalar *m_constraintError,*cfm;
b3Scalar *m_constraintError, *cfm;
// lo and hi limits for variables (set to -/+ infinity on entry).
b3Scalar *m_lowerLimit,*m_upperLimit;
b3Scalar *m_lowerLimit, *m_upperLimit;
// findex vector for variables. see the LCP solver interface for a
// description of what this does. this is set to -1 on entry.
// note that the returned indexes are relative to the first index of
// the constraint.
int *findex;
int* findex;
// number of solver iterations
int m_numIterations;
//damping of the velocity
b3Scalar m_damping;
b3Scalar m_damping;
};
int getOverrideNumSolverIterations() const
int getOverrideNumSolverIterations() const
{
return m_overrideNumSolverIterations;
}
@ -159,59 +152,55 @@ public:
m_overrideNumSolverIterations = overideNumIterations;
}
///internal method used by the constraint solver, don't use them directly
virtual void setupSolverConstraint(b3ConstraintArray& ca, int solverBodyA,int solverBodyB, b3Scalar timeStep)
virtual void setupSolverConstraint(b3ConstraintArray & ca, int solverBodyA, int solverBodyB, b3Scalar timeStep)
{
(void)ca;
(void)solverBodyA;
(void)solverBodyB;
(void)timeStep;
(void)ca;
(void)solverBodyA;
(void)solverBodyB;
(void)timeStep;
}
///internal method used by the constraint solver, don't use them directly
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)=0;
///internal method used by the constraint solver, don't use them directly
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)=0;
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies) = 0;
///internal method used by the constraint solver, don't use them directly
void internalSetAppliedImpulse(b3Scalar appliedImpulse)
virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies) = 0;
///internal method used by the constraint solver, don't use them directly
void internalSetAppliedImpulse(b3Scalar appliedImpulse)
{
m_appliedImpulse = appliedImpulse;
}
///internal method used by the constraint solver, don't use them directly
b3Scalar internalGetAppliedImpulse()
b3Scalar internalGetAppliedImpulse()
{
return m_appliedImpulse;
}
b3Scalar getBreakingImpulseThreshold() const
b3Scalar getBreakingImpulseThreshold() const
{
return m_breakingImpulseThreshold;
return m_breakingImpulseThreshold;
}
void setBreakingImpulseThreshold(b3Scalar threshold)
void setBreakingImpulseThreshold(b3Scalar threshold)
{
m_breakingImpulseThreshold = threshold;
}
bool isEnabled() const
bool isEnabled() const
{
return m_isEnabled;
}
void setEnabled(bool enabled)
void setEnabled(bool enabled)
{
m_isEnabled=enabled;
m_isEnabled = enabled;
}
///internal method used by the constraint solver, don't use them directly
virtual void solveConstraintObsolete(b3SolverBody& /*bodyA*/,b3SolverBody& /*bodyB*/,b3Scalar /*timeStep*/) {};
virtual void solveConstraintObsolete(b3SolverBody& /*bodyA*/, b3SolverBody& /*bodyB*/, b3Scalar /*timeStep*/){};
int getRigidBodyA() const
{
return m_rbA;
@ -221,8 +210,7 @@ public:
return m_rbB;
}
int getRigidBodyA()
int getRigidBodyA()
{
return m_rbA;
}
@ -233,15 +221,15 @@ public:
int getUserConstraintType() const
{
return m_userConstraintType ;
return m_userConstraintType;
}
void setUserConstraintType(int userConstraintType)
void setUserConstraintType(int userConstraintType)
{
m_userConstraintType = userConstraintType;
};
void setUserConstraintId(int uid)
void setUserConstraintId(int uid)
{
m_userConstraintId = uid;
}
@ -251,17 +239,17 @@ public:
return m_userConstraintId;
}
void setUserConstraintPtr(void* ptr)
void setUserConstraintPtr(void* ptr)
{
m_userConstraintPtr = ptr;
}
void* getUserConstraintPtr()
void* getUserConstraintPtr()
{
return m_userConstraintPtr;
}
void setJointFeedback(b3JointFeedback* jointFeedback)
void setJointFeedback(b3JointFeedback * jointFeedback)
{
m_jointFeedback = jointFeedback;
}
@ -276,37 +264,36 @@ public:
return m_jointFeedback;
}
int getUid() const
{
return m_userConstraintId;
}
return m_userConstraintId;
}
bool needsFeedback() const
bool needsFeedback() const
{
return m_needsFeedback;
}
///enableFeedback will allow to read the applied linear and angular impulse
///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
void enableFeedback(bool needsFeedback)
void enableFeedback(bool needsFeedback)
{
m_needsFeedback = needsFeedback;
}
///getAppliedImpulse is an estimated total applied impulse.
///getAppliedImpulse is an estimated total applied impulse.
///This feedback could be used to determine breaking constraints or playing sounds.
b3Scalar getAppliedImpulse() const
b3Scalar getAppliedImpulse() const
{
b3Assert(m_needsFeedback);
return m_appliedImpulse;
}
b3TypedConstraintType getConstraintType () const
b3TypedConstraintType getConstraintType() const
{
return b3TypedConstraintType(m_objectType);
}
void setDbgDrawSize(b3Scalar dbgDrawSize)
{
m_dbgDrawSize = dbgDrawSize;
@ -316,35 +303,34 @@ public:
return m_dbgDrawSize;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, b3Scalar value, int axis = -1) = 0;
virtual void setParam(int num, b3Scalar value, int axis = -1) = 0;
///return the local value of parameter
virtual b3Scalar getParam(int num, int axis = -1) const = 0;
// virtual int calculateSerializeBufferSize() const;
virtual b3Scalar getParam(int num, int axis = -1) const = 0;
// virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
//virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
};
// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits
// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits
// all arguments should be normalized angles (i.e. in range [-B3_PI, B3_PI])
B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians)
{
if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
if (angleLowerLimitInRadians >= angleUpperLimitInRadians)
{
return angleInRadians;
}
else if(angleInRadians < angleLowerLimitInRadians)
else if (angleInRadians < angleLowerLimitInRadians)
{
b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleLowerLimitInRadians - angleInRadians));
b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleUpperLimitInRadians - angleInRadians));
return (diffLo < diffHi) ? angleInRadians : (angleInRadians + B3_2_PI);
}
else if(angleInRadians > angleUpperLimitInRadians)
else if (angleInRadians > angleUpperLimitInRadians)
{
b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleInRadians - angleUpperLimitInRadians));
b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleInRadians - angleLowerLimitInRadians));
@ -356,6 +342,7 @@ B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar
}
}
// clang-format off
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct b3TypedConstraintData
{
@ -379,17 +366,18 @@ struct b3TypedConstraintData
};
// clang-format on
/*B3_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const
{
return sizeof(b3TypedConstraintData);
}
*/
class b3AngularLimit
{
private:
b3Scalar
b3Scalar
m_center,
m_halfRange,
m_softness,
@ -404,15 +392,16 @@ private:
public:
/// Default constructor initializes limit as inactive, allowing free constraint movement
b3AngularLimit()
:m_center(0.0f),
m_halfRange(-1.0f),
m_softness(0.9f),
m_biasFactor(0.3f),
m_relaxationFactor(1.0f),
m_correction(0.0f),
m_sign(0.0f),
m_solveLimit(false)
{}
: m_center(0.0f),
m_halfRange(-1.0f),
m_softness(0.9f),
m_biasFactor(0.3f),
m_relaxationFactor(1.0f),
m_correction(0.0f),
m_sign(0.0f),
m_solveLimit(false)
{
}
/// Sets all limit's parameters.
/// When low > high limit becomes inactive.
@ -441,13 +430,13 @@ public:
return m_relaxationFactor;
}
/// Returns correction value evaluated when test() was invoked
/// Returns correction value evaluated when test() was invoked
inline b3Scalar getCorrection() const
{
return m_correction;
}
/// Returns sign value evaluated when test() was invoked
/// Returns sign value evaluated when test() was invoked
inline b3Scalar getSign() const
{
return m_sign;
@ -475,9 +464,6 @@ public:
b3Scalar getLow() const;
b3Scalar getHigh() const;
};
#endif //B3_TYPED_CONSTRAINT_H
#endif //B3_TYPED_CONSTRAINT_H

313
thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp vendored
View File

@ -11,7 +11,6 @@
#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
#include "Bullet3Dynamics/shared/b3Inertia.h"
struct b3CpuRigidBodyPipelineInternalData
{
b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies;
@ -22,7 +21,6 @@ struct b3CpuRigidBodyPipelineInternalData
b3CpuNarrowPhase* m_np;
b3Config m_config;
};
b3CpuRigidBodyPipeline::b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config)
{
@ -39,49 +37,43 @@ b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline()
void b3CpuRigidBodyPipeline::updateAabbWorldSpace()
{
for (int i=0;i<this->getNumBodies();i++)
for (int i = 0; i < this->getNumBodies(); i++)
{
b3RigidBodyData* body = &m_data->m_rigidBodies[i];
b3Float4 position = body->m_pos;
b3Quat orientation = body->m_quat;
b3Quat orientation = body->m_quat;
int collidableIndex = body->m_collidableIdx;
b3Collidable& collidable = m_data->m_np->getCollidableCpu(collidableIndex);
int shapeIndex = collidable.m_shapeIndex;
if (shapeIndex>=0)
{
if (shapeIndex >= 0)
{
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex);
b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i];
float margin=0.f;
b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&worldAabb.m_minVec,&worldAabb.m_maxVec);
m_data->m_bp->setAabb(i,worldAabb.m_minVec,worldAabb.m_maxVec,0);
float margin = 0.f;
b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &worldAabb.m_minVec, &worldAabb.m_maxVec);
m_data->m_bp->setAabb(i, worldAabb.m_minVec, worldAabb.m_maxVec, 0);
}
}
}
void b3CpuRigidBodyPipeline::computeOverlappingPairs()
void b3CpuRigidBodyPipeline::computeOverlappingPairs()
{
int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
m_data->m_bp->calculateOverlappingPairs();
numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
printf("numPairs=%d\n",numPairs);
printf("numPairs=%d\n", numPairs);
}
void b3CpuRigidBodyPipeline::computeContactPoints()
{
b3AlignedObjectArray<b3Int4>& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray();
m_data->m_np->computeContacts(pairs,m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
m_data->m_np->computeContacts(pairs, m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
}
void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
{
//update world space aabb's
updateAabbWorldSpace();
@ -92,73 +84,71 @@ void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
computeContactPoints();
//solve contacts
//update transforms
integrate(deltaTime);
}
static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1,
const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1,
const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
{
return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1);
}
static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
{
linear = -n;
angular0 = -b3Cross(r0, n);
angular1 = b3Cross(r1, n);
}
static inline void b3SolveContact(b3ContactConstraint4& cs,
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
static inline void b3SolveContact(b3ContactConstraint4& cs,
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
{
b3Vector3 dLinVelA;
dLinVelA.setZero();
b3Vector3 dAngVelA;
dAngVelA.setZero();
b3Vector3 dLinVelB;
dLinVelB.setZero();
b3Vector3 dAngVelB;
dAngVelB.setZero();
b3Vector3 dLinVelA; dLinVelA.setZero();
b3Vector3 dAngVelA; dAngVelA.setZero();
b3Vector3 dLinVelB; dLinVelB.setZero();
b3Vector3 dAngVelB; dAngVelB.setZero();
for(int ic=0; ic<4; ic++)
for (int ic = 0; ic < 4; ic++)
{
// dont necessary because this makes change to 0
if( cs.m_jacCoeffInv[ic] == 0.f ) continue;
if (cs.m_jacCoeffInv[ic] == 0.f) continue;
{
b3Vector3 angular0, angular1, linear;
b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA;
b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB;
b3SetLinearAndAngular( (const b3Vector3 &)-cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, linear, angular0, angular1 );
b3SetLinearAndAngular((const b3Vector3&)-cs.m_linear, (const b3Vector3&)r0, (const b3Vector3&)r1, linear, angular0, angular1);
float rambdaDt = b3CalcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic];
float rambdaDt = b3CalcRelVel((const b3Vector3&)cs.m_linear, (const b3Vector3&)-cs.m_linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB) +
cs.m_b[ic];
rambdaDt *= cs.m_jacCoeffInv[ic];
{
float prevSum = cs.m_appliedRambdaDt[ic];
float updated = prevSum;
updated += rambdaDt;
updated = b3Max( updated, minRambdaDt[ic] );
updated = b3Min( updated, maxRambdaDt[ic] );
updated = b3Max(updated, minRambdaDt[ic]);
updated = b3Min(updated, maxRambdaDt[ic]);
rambdaDt = updated - prevSum;
cs.m_appliedRambdaDt[ic] = updated;
}
b3Vector3 linImp0 = invMassA*linear*rambdaDt;
b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
b3Vector3 linImp0 = invMassA * linear * rambdaDt;
b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
#ifdef _WIN32
b3Assert(_finite(linImp0.getX()));
b3Assert(_finite(linImp0.getX()));
b3Assert(_finite(linImp1.getX()));
#endif
{
@ -169,53 +159,46 @@ static inline void b3SolveContact(b3ContactConstraint4& cs,
}
}
}
}
static inline void b3SolveFriction(b3ContactConstraint4& cs,
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
static inline void b3SolveFriction(b3ContactConstraint4& cs,
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
{
if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;
if (cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0) return;
const b3Vector3& center = (const b3Vector3&)cs.m_center;
b3Vector3 n = -(const b3Vector3&)cs.m_linear;
b3Vector3 tangent[2];
b3PlaneSpace1 (n, tangent[0],tangent[1]);
b3PlaneSpace1(n, tangent[0], tangent[1]);
b3Vector3 angular0, angular1, linear;
b3Vector3 r0 = center - posA;
b3Vector3 r1 = center - posB;
for(int i=0; i<2; i++)
for (int i = 0; i < 2; i++)
{
b3SetLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 );
b3SetLinearAndAngular(tangent[i], r0, r1, linear, angular0, angular1);
float rambdaDt = b3CalcRelVel(linear, -linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB );
linVelA, angVelA, linVelB, angVelB);
rambdaDt *= cs.m_fJacCoeffInv[i];
{
float prevSum = cs.m_fAppliedRambdaDt[i];
float updated = prevSum;
updated += rambdaDt;
updated = b3Max( updated, minRambdaDt[i] );
updated = b3Min( updated, maxRambdaDt[i] );
rambdaDt = updated - prevSum;
cs.m_fAppliedRambdaDt[i] = updated;
}
{
float prevSum = cs.m_fAppliedRambdaDt[i];
float updated = prevSum;
updated += rambdaDt;
updated = b3Max(updated, minRambdaDt[i]);
updated = b3Min(updated, maxRambdaDt[i]);
rambdaDt = updated - prevSum;
cs.m_fAppliedRambdaDt[i] = updated;
}
b3Vector3 linImp0 = invMassA*linear*rambdaDt;
b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
b3Vector3 linImp0 = invMassA * linear * rambdaDt;
b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
#ifdef _WIN32
b3Assert(_finite(linImp0.getX()));
b3Assert(_finite(linImp1.getX()));
@ -226,57 +209,45 @@ static inline void b3SolveFriction(b3ContactConstraint4& cs,
angVelB += angImp1;
}
{ // angular damping for point constraint
b3Vector3 ab = ( posB - posA ).normalized();
b3Vector3 ac = ( center - posA ).normalized();
if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
{ // angular damping for point constraint
b3Vector3 ab = (posB - posA).normalized();
b3Vector3 ac = (center - posA).normalized();
if (b3Dot(ab, ac) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
{
float angNA = b3Dot( n, angVelA );
float angNB = b3Dot( n, angVelB );
float angNA = b3Dot(n, angVelA);
float angNB = b3Dot(n, angVelB);
angVelA -= (angNA*0.1f)*n;
angVelB -= (angNB*0.1f)*n;
angVelA -= (angNA * 0.1f) * n;
angVelB -= (angNB * 0.1f) * n;
}
}
}
struct b3SolveTask// : public ThreadPool::Task
struct b3SolveTask // : public ThreadPool::Task
{
b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,
b3AlignedObjectArray<b3Inertia>& shapes,
b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,
b3AlignedObjectArray<b3Inertia>& shapes,
b3AlignedObjectArray<b3ContactConstraint4>& constraints,
int start, int nConstraints,
int maxNumBatches,
b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx
)
: m_bodies( bodies ), m_shapes( shapes ), m_constraints( constraints ),
m_wgUsedBodies(wgUsedBodies),m_curWgidx(curWgidx),
m_start( start ),
m_nConstraints( nConstraints ),
m_solveFriction( true ),
m_maxNumBatches(maxNumBatches)
{}
b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx)
: m_bodies(bodies), m_shapes(shapes), m_constraints(constraints), m_wgUsedBodies(wgUsedBodies), m_curWgidx(curWgidx), m_start(start), m_nConstraints(nConstraints), m_solveFriction(true), m_maxNumBatches(maxNumBatches)
{
}
unsigned short int getType(){ return 0; }
unsigned short int getType() { return 0; }
void run(int tIdx)
{
b3AlignedObjectArray<int> usedBodies;
//printf("run..............\n");
for (int bb=0;bb<m_maxNumBatches;bb++)
for (int bb = 0; bb < m_maxNumBatches; bb++)
{
usedBodies.resize(0);
for(int ic=m_nConstraints-1; ic>=0; ic--)
for (int ic = m_nConstraints - 1; ic >= 0; ic--)
//for(int ic=0; ic<m_nConstraints; ic++)
{
int i = m_start + ic;
if (m_constraints[i].m_batchIdx != bb)
continue;
@ -298,87 +269,80 @@ m_start( start ),
//printf("ic(b)=%d, localBatch=%d\n",ic,localBatch);
}
#endif
if (aIdx==10)
if (aIdx == 10)
{
//printf("ic(a)=%d, localBatch=%d\n",ic,localBatch);
}
if (usedBodies.size()<(aIdx+1))
if (usedBodies.size() < (aIdx + 1))
{
usedBodies.resize(aIdx+1,0);
usedBodies.resize(aIdx + 1, 0);
}
if (usedBodies.size()<(bIdx+1))
if (usedBodies.size() < (bIdx + 1))
{
usedBodies.resize(bIdx+1,0);
usedBodies.resize(bIdx + 1, 0);
}
if (bodyA.m_invMass)
{
b3Assert(usedBodies[aIdx]==0);
b3Assert(usedBodies[aIdx] == 0);
usedBodies[aIdx]++;
}
if (bodyB.m_invMass)
{
b3Assert(usedBodies[bIdx]==0);
b3Assert(usedBodies[bIdx] == 0);
usedBodies[bIdx]++;
}
if( !m_solveFriction )
if (!m_solveFriction)
{
float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
b3SolveContact( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld,
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld,
maxRambdaDt, minRambdaDt );
float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
b3SolveContact(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
maxRambdaDt, minRambdaDt);
}
else
{
float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
float sum = 0;
for(int j=0; j<4; j++)
for (int j = 0; j < 4; j++)
{
sum +=m_constraints[i].m_appliedRambdaDt[j];
sum += m_constraints[i].m_appliedRambdaDt[j];
}
frictionCoeff = 0.7f;
for(int j=0; j<4; j++)
for (int j = 0; j < 4; j++)
{
maxRambdaDt[j] = frictionCoeff*sum;
maxRambdaDt[j] = frictionCoeff * sum;
minRambdaDt[j] = -maxRambdaDt[j];
}
b3SolveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld,
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld,
maxRambdaDt, minRambdaDt );
b3SolveFriction(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
maxRambdaDt, minRambdaDt);
}
}
if (m_wgUsedBodies)
{
if (m_wgUsedBodies[m_curWgidx].size()<usedBodies.size())
if (m_wgUsedBodies[m_curWgidx].size() < usedBodies.size())
{
m_wgUsedBodies[m_curWgidx].resize(usedBodies.size());
}
for (int i=0;i<usedBodies.size();i++)
for (int i = 0; i < usedBodies.size(); i++)
{
if (usedBodies[i])
{
//printf("cell %d uses body %d\n", m_curWgidx,i);
m_wgUsedBodies[m_curWgidx][i]=1;
m_wgUsedBodies[m_curWgidx][i] = 1;
}
}
}
}
}
b3AlignedObjectArray<b3RigidBodyData>& m_bodies;
@ -397,24 +361,22 @@ void b3CpuRigidBodyPipeline::solveContactConstraints()
int m_nIterations = 4;
b3AlignedObjectArray<b3ContactConstraint4> contactConstraints;
// const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
// const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
int n = contactConstraints.size();
//convert contacts...
int maxNumBatches = 250;
for(int iter=0; iter<m_nIterations; iter++)
for (int iter = 0; iter < m_nIterations; iter++)
{
b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0);
b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
task.m_solveFriction = false;
task.run(0);
}
for(int iter=0; iter<m_nIterations; iter++)
for (int iter = 0; iter < m_nIterations; iter++)
{
b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0);
b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
task.m_solveFriction = true;
task.run(0);
}
@ -422,53 +384,51 @@ void b3CpuRigidBodyPipeline::solveContactConstraints()
void b3CpuRigidBodyPipeline::integrate(float deltaTime)
{
float angDamping=0.f;
b3Vector3 gravityAcceleration=b3MakeVector3(0,-9,0);
float angDamping = 0.f;
b3Vector3 gravityAcceleration = b3MakeVector3(0, -9, 0);
//integrate transforms (external forces/gravity should be moved into constraint solver)
for (int i=0;i<m_data->m_rigidBodies.size();i++)
for (int i = 0; i < m_data->m_rigidBodies.size(); i++)
{
b3IntegrateTransform(&m_data->m_rigidBodies[i],deltaTime,angDamping,gravityAcceleration);
b3IntegrateTransform(&m_data->m_rigidBodies[i], deltaTime, angDamping, gravityAcceleration);
}
}
int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
{
b3RigidBodyData body;
int bodyIndex = m_data->m_rigidBodies.size();
body.m_invMass = mass ? 1.f/mass : 0.f;
body.m_angVel.setValue(0,0,0);
body.m_invMass = mass ? 1.f / mass : 0.f;
body.m_angVel.setValue(0, 0, 0);
body.m_collidableIdx = collidableIndex;
body.m_frictionCoeff = 0.3f;
body.m_linVel.setValue(0,0,0);
body.m_pos.setValue(position[0],position[1],position[2]);
body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
body.m_linVel.setValue(0, 0, 0);
body.m_pos.setValue(position[0], position[1], position[2]);
body.m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]);
body.m_restituitionCoeff = 0.f;
m_data->m_rigidBodies.push_back(body);
if (collidableIndex>=0)
if (collidableIndex >= 0)
{
b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex);
b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
b3Vector3 localAabbMin = b3MakeVector3(localAabb.m_min[0], localAabb.m_min[1], localAabb.m_min[2]);
b3Vector3 localAabbMax = b3MakeVector3(localAabb.m_max[0], localAabb.m_max[1], localAabb.m_max[2]);
b3Scalar margin = 0.01f;
b3Transform t;
t.setIdentity();
t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec);
t.setOrigin(b3MakeVector3(position[0], position[1], position[2]));
t.setRotation(b3Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]));
b3TransformAabb(localAabbMin, localAabbMax, margin, t, worldAabb.m_minVec, worldAabb.m_maxVec);
m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1);
// b3Vector3 aabbMin,aabbMax;
// m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
} else
m_data->m_bp->createProxy(worldAabb.m_minVec, worldAabb.m_maxVec, bodyIndex, 0, 1, 1);
// b3Vector3 aabbMin,aabbMax;
// m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
}
else
{
b3Error("registerPhysicsInstance using invalid collidableIndex\n");
}
@ -476,13 +436,12 @@ int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
return bodyIndex;
}
const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const
{
return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0;
}
int b3CpuRigidBodyPipeline::getNumBodies() const
int b3CpuRigidBodyPipeline::getNumBodies() const
{
return m_data->m_rigidBodies.size();
}

45
thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.h vendored
View File

@ -16,52 +16,47 @@ subject to the following restrictions:
#ifndef B3_CPU_RIGIDBODY_PIPELINE_H
#define B3_CPU_RIGIDBODY_PIPELINE_H
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h"
class b3CpuRigidBodyPipeline
{
protected:
struct b3CpuRigidBodyPipelineInternalData* m_data;
struct b3CpuRigidBodyPipelineInternalData* m_data;
int allocateCollidable();
public:
b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const struct b3Config& config);
virtual ~b3CpuRigidBodyPipeline();
virtual void stepSimulation(float deltaTime);
virtual void integrate(float timeStep);
virtual void updateAabbWorldSpace();
virtual void computeOverlappingPairs();
virtual void computeContactPoints();
virtual void solveContactConstraints();
virtual void stepSimulation(float deltaTime);
virtual void integrate(float timeStep);
virtual void updateAabbWorldSpace();
virtual void computeOverlappingPairs();
virtual void computeContactPoints();
virtual void solveContactConstraints();
int registerConvexPolyhedron(class b3ConvexUtility* convex);
int registerConvexPolyhedron(class b3ConvexUtility* convex);
int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData);
void writeAllInstancesToGpu();
void copyConstraintsToHost();
void setGravity(const float* grav);
void reset();
int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold);
int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData);
void writeAllInstancesToGpu();
void copyConstraintsToHost();
void setGravity(const float* grav);
void reset();
int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, float breakingThreshold);
int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold);
void removeConstraintByUid(int uid);
void addConstraint(class b3TypedConstraint* constraint);
void removeConstraint(b3TypedConstraint* constraint);
void addConstraint(class b3TypedConstraint* constraint);
void removeConstraint(b3TypedConstraint* constraint);
void castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults);
void castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults);
const struct b3RigidBodyData* getBodyBuffer() const;
int getNumBodies() const;
int getNumBodies() const;
};
#endif //B3_CPU_RIGIDBODY_PIPELINE_H
#endif //B3_CPU_RIGIDBODY_PIPELINE_H

19
thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h vendored
View File

@ -5,30 +5,27 @@
typedef struct b3ContactConstraint4 b3ContactConstraint4_t;
struct b3ContactConstraint4
{
b3Float4 m_linear;//normal?
b3Float4 m_linear; //normal?
b3Float4 m_worldPos[4];
b3Float4 m_center; // friction
b3Float4 m_center; // friction
float m_jacCoeffInv[4];
float m_b[4];
float m_appliedRambdaDt[4];
float m_fJacCoeffInv[2]; // friction
float m_fAppliedRambdaDt[2]; // friction
float m_fJacCoeffInv[2]; // friction
float m_fAppliedRambdaDt[2]; // friction
unsigned int m_bodyA;
unsigned int m_bodyB;
int m_batchIdx;
int m_batchIdx;
unsigned int m_paddings;
};
//inline void setFrictionCoeff(float value) { m_linear[3] = value; }
inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint)
inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint)
{
return constraint->m_linear.w;
return constraint->m_linear.w;
}
#endif //B3_CONTACT_CONSTRAINT5_H
#endif //B3_CONTACT_CONSTRAINT5_H

133
thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h vendored
View File

@ -4,89 +4,84 @@
#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q);
void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q)
void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q);
void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q)
{
if (b3Fabs(n.z) > 0.70710678f) {
// choose p in y-z plane
float a = n.y*n.y + n.z*n.z;
float k = 1.f/sqrt(a);
p[0].x = 0;
p[0].y = -n.z*k;
p[0].z = n.y*k;
// set q = n x p
q[0].x = a*k;
q[0].y = -n.x*p[0].z;
q[0].z = n.x*p[0].y;
}
else {
// choose p in x-y plane
float a = n.x*n.x + n.y*n.y;
float k = 1.f/sqrt(a);
p[0].x = -n.y*k;
p[0].y = n.x*k;
p[0].z = 0;
// set q = n x p
q[0].x = -n.z*p[0].y;
q[0].y = n.z*p[0].x;
q[0].z = a*k;
}
if (b3Fabs(n.z) > 0.70710678f)
{
// choose p in y-z plane
float a = n.y * n.y + n.z * n.z;
float k = 1.f / sqrt(a);
p[0].x = 0;
p[0].y = -n.z * k;
p[0].z = n.y * k;
// set q = n x p
q[0].x = a * k;
q[0].y = -n.x * p[0].z;
q[0].z = n.x * p[0].y;
}
else
{
// choose p in x-y plane
float a = n.x * n.x + n.y * n.y;
float k = 1.f / sqrt(a);
p[0].x = -n.y * k;
p[0].y = n.x * k;
p[0].z = 0;
// set q = n x p
q[0].x = -n.z * p[0].y;
q[0].y = n.z * p[0].x;
q[0].z = a * k;
}
}
void setLinearAndAngular( b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
void setLinearAndAngular(b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
{
*linear = b3MakeFloat4(n.x,n.y,n.z,0.f);
*linear = b3MakeFloat4(n.x, n.y, n.z, 0.f);
*angular0 = b3Cross3(r0, n);
*angular1 = -b3Cross3(r1, n);
}
float calcRelVel( b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1 )
float calcRelVel(b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1)
{
return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1);
}
float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1,
float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
{
// linear0,1 are normlized
float jmj0 = invMass0;//b3Dot3F4(linear0, linear0)*invMass0;
float jmj1 = b3Dot3F4(mtMul3(angular0,*invInertia0), angular0);
float jmj2 = invMass1;//b3Dot3F4(linear1, linear1)*invMass1;
float jmj3 = b3Dot3F4(mtMul3(angular1,*invInertia1), angular1);
return -1.f/(jmj0+jmj1+jmj2+jmj3);
float jmj0 = invMass0; //b3Dot3F4(linear0, linear0)*invMass0;
float jmj1 = b3Dot3F4(mtMul3(angular0, *invInertia0), angular0);
float jmj2 = invMass1; //b3Dot3F4(linear1, linear1)*invMass1;
float jmj3 = b3Dot3F4(mtMul3(angular1, *invInertia1), angular1);
return -1.f / (jmj0 + jmj1 + jmj2 + jmj3);
}
void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB,
__global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
b3ContactConstraint4_t* dstC )
void setConstraint4(b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB,
__global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
b3ContactConstraint4_t* dstC)
{
dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);
dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);
float dtInv = 1.f/dt;
for(int ic=0; ic<4; ic++)
float dtInv = 1.f / dt;
for (int ic = 0; ic < 4; ic++)
{
dstC->m_appliedRambdaDt[ic] = 0.f;
}
dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
dstC->m_linear = src->m_worldNormalOnB;
dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );
for(int ic=0; ic<4; ic++)
dstC->m_linear.w = 0.7f; //src->getFrictionCoeff() );
for (int ic = 0; ic < 4; ic++)
{
b3Float4 r0 = src->m_worldPosB[ic] - posA;
b3Float4 r1 = src->m_worldPosB[ic] - posB;
if( ic >= src->m_worldNormalOnB.w )//npoints
if (ic >= src->m_worldNormalOnB.w) //npoints
{
dstC->m_jacCoeffInv[ic] = 0.f;
continue;
@ -98,56 +93,56 @@ void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4Co
setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);
dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,
invMassA, &invInertiaA, invMassB, &invInertiaB );
invMassA, &invInertiaA, invMassB, &invInertiaB);
relVelN = calcRelVel(linear, -linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB);
linVelA, angVelA, linVelB, angVelB);
float e = 0.f;//src->getRestituitionCoeff();
if( relVelN*relVelN < 0.004f ) e = 0.f;
float e = 0.f; //src->getRestituitionCoeff();
if (relVelN * relVelN < 0.004f) e = 0.f;
dstC->m_b[ic] = e*relVelN;
dstC->m_b[ic] = e * relVelN;
//float penetration = src->m_worldPosB[ic].w;
dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;
dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift) * positionConstraintCoeff * dtInv;
dstC->m_appliedRambdaDt[ic] = 0.f;
}
}
if( src->m_worldNormalOnB.w > 0 )//npoints
{ // prepare friction
b3Float4 center = b3MakeFloat4(0.f,0.f,0.f,0.f);
for(int i=0; i<src->m_worldNormalOnB.w; i++)
if (src->m_worldNormalOnB.w > 0) //npoints
{ // prepare friction
b3Float4 center = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
for (int i = 0; i < src->m_worldNormalOnB.w; i++)
center += src->m_worldPosB[i];
center /= (float)src->m_worldNormalOnB.w;
b3Float4 tangent[2];
b3PlaneSpace1(src->m_worldNormalOnB,&tangent[0],&tangent[1]);
b3PlaneSpace1(src->m_worldNormalOnB, &tangent[0], &tangent[1]);
b3Float4 r[2];
r[0] = center - posA;
r[1] = center - posB;
for(int i=0; i<2; i++)
for (int i = 0; i < 2; i++)
{
b3Float4 linear, angular0, angular1;
setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);
dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,
invMassA, &invInertiaA, invMassB, &invInertiaB );
invMassA, &invInertiaA, invMassB, &invInertiaB);
dstC->m_fAppliedRambdaDt[i] = 0.f;
}
dstC->m_center = center;
}
for(int i=0; i<4; i++)
for (int i = 0; i < 4; i++)
{
if( i<src->m_worldNormalOnB.w )
if (i < src->m_worldNormalOnB.w)
{
dstC->m_worldPos[i] = src->m_worldPosB[i];
}
else
{
dstC->m_worldPos[i] = b3MakeFloat4(0.f,0.f,0.f,0.f);
dstC->m_worldPos[i] = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
}
}
}

3
thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h vendored
View File

@ -11,5 +11,4 @@ struct b3Inertia
b3Mat3x3 m_initInvInertia;
};
#endif //B3_INERTIA_H
#endif //B3_INERTIA_H

53
thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h vendored
View File

@ -2,11 +2,8 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
inline void integrateSingleTransform(__global b3RigidBodyData_t* bodies, int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
{
if (bodies[nodeID].m_invMass != 0.f)
{
float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
@ -18,27 +15,27 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod
bodies[nodeID].m_angVel.x *= angularDamping;
bodies[nodeID].m_angVel.y *= angularDamping;
bodies[nodeID].m_angVel.z *= angularDamping;
b3Float4 angvel = bodies[nodeID].m_angVel;
float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
//limit the angular motion
if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
{
fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
}
if(fAngle < 0.001f)
if (fAngle < 0.001f)
{
// use Taylor's expansions of sync function
axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);
axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);
axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
}
b3Quat dorn;
dorn.x = axis.x;
dorn.y = axis.y;
@ -47,23 +44,21 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod
b3Quat orn0 = bodies[nodeID].m_quat;
b3Quat predictedOrn = b3QuatMul(dorn, orn0);
predictedOrn = b3QuatNormalized(predictedOrn);
bodies[nodeID].m_quat=predictedOrn;
bodies[nodeID].m_quat = predictedOrn;
}
//linear velocity
bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;
//linear velocity
bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;
//apply gravity
bodies[nodeID].m_linVel += gravityAcceleration * timeStep;
}
}
inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
inline void b3IntegrateTransform(__global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
{
float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
if( (body->m_invMass != 0.f))
if ((body->m_invMass != 0.f))
{
//angular velocity
{
@ -72,23 +67,23 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt
body->m_angVel.x *= angularDamping;
body->m_angVel.y *= angularDamping;
body->m_angVel.z *= angularDamping;
b3Float4 angvel = body->m_angVel;
float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
//limit the angular motion
if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
{
fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
}
if(fAngle < 0.001f)
if (fAngle < 0.001f)
{
// use Taylor's expansions of sync function
axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);
axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);
axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
}
b3Quat dorn;
dorn.x = axis.x;
@ -99,15 +94,13 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt
b3Quat predictedOrn = b3QuatMul(dorn, orn0);
predictedOrn = b3QuatNormalized(predictedOrn);
body->m_quat=predictedOrn;
body->m_quat = predictedOrn;
}
//apply gravity
body->m_linVel += gravityAcceleration * timeStep;
//linear velocity
body->m_pos += body->m_linVel * timeStep;
//linear velocity
body->m_pos += body->m_linVel * timeStep;
}
}

187
thirdparty/bullet/Bullet3Geometry/b3AabbUtil.h vendored
View File

@ -12,8 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_AABB_UTIL2
#define B3_AABB_UTIL2
@ -21,20 +19,18 @@ subject to the following restrictions:
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3MinMax.h"
B3_FORCE_INLINE void b3AabbExpand (b3Vector3& aabbMin,
b3Vector3& aabbMax,
const b3Vector3& expansionMin,
const b3Vector3& expansionMax)
B3_FORCE_INLINE void b3AabbExpand(b3Vector3& aabbMin,
b3Vector3& aabbMax,
const b3Vector3& expansionMin,
const b3Vector3& expansionMax)
{
aabbMin = aabbMin + expansionMin;
aabbMax = aabbMax + expansionMax;
}
/// conservative test for overlap between two aabbs
B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aabbMax1,
const b3Vector3 &point)
B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aabbMax1,
const b3Vector3& point)
{
bool overlap = true;
overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
@ -43,10 +39,9 @@ B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3 &aabbMin1, const b3
return overlap;
}
/// conservative test for overlap between two aabbs
B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aabbMax1,
const b3Vector3 &aabbMin2, const b3Vector3 &aabbMax2)
B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aabbMax1,
const b3Vector3& aabbMin2, const b3Vector3& aabbMax2)
{
bool overlap = true;
overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
@ -56,52 +51,49 @@ B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(const b3Vector3 &aabbMin1, const b3V
}
/// conservative test for overlap between triangle and aabb
B3_FORCE_INLINE bool b3TestTriangleAgainstAabb2(const b3Vector3 *vertices,
const b3Vector3 &aabbMin, const b3Vector3 &aabbMax)
B3_FORCE_INLINE bool b3TestTriangleAgainstAabb2(const b3Vector3* vertices,
const b3Vector3& aabbMin, const b3Vector3& aabbMax)
{
const b3Vector3 &p1 = vertices[0];
const b3Vector3 &p2 = vertices[1];
const b3Vector3 &p3 = vertices[2];
const b3Vector3& p1 = vertices[0];
const b3Vector3& p2 = vertices[1];
const b3Vector3& p3 = vertices[2];
if (b3Min(b3Min(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
if (b3Max(b3Max(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
if (b3Min(b3Min(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
if (b3Max(b3Max(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
if (b3Min(b3Min(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
if (b3Max(b3Max(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
return true;
}
B3_FORCE_INLINE int b3Outcode(const b3Vector3& p,const b3Vector3& halfExtent)
B3_FORCE_INLINE int b3Outcode(const b3Vector3& p, const b3Vector3& halfExtent)
{
return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
(p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
(p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
(p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
(p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
(p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
(p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
(p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
(p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
(p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
(p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
}
B3_FORCE_INLINE bool b3RayAabb2(const b3Vector3& rayFrom,
const b3Vector3& rayInvDirection,
const unsigned int raySign[3],
const b3Vector3 bounds[2],
b3Scalar& tmin,
b3Scalar lambda_min,
b3Scalar lambda_max)
const b3Vector3& rayInvDirection,
const unsigned int raySign[3],
const b3Vector3 bounds[2],
b3Scalar& tmin,
b3Scalar lambda_min,
b3Scalar lambda_max)
{
b3Scalar tmax, tymin, tymax, tzmin, tzmax;
tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
tmax = (bounds[1 - raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
tymax = (bounds[1 - raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
if ( (tmin > tymax) || (tymin > tmax) )
if ((tmin > tymax) || (tymin > tmax))
return false;
if (tymin > tmin)
@ -111,59 +103,59 @@ B3_FORCE_INLINE bool b3RayAabb2(const b3Vector3& rayFrom,
tmax = tymax;
tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
tzmax = (bounds[1 - raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
if ( (tmin > tzmax) || (tzmin > tmax) )
if ((tmin > tzmax) || (tzmin > tmax))
return false;
if (tzmin > tmin)
tmin = tzmin;
if (tzmax < tmax)
tmax = tzmax;
return ( (tmin < lambda_max) && (tmax > lambda_min) );
return ((tmin < lambda_max) && (tmax > lambda_min));
}
B3_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom,
const b3Vector3& rayTo,
const b3Vector3& aabbMin,
const b3Vector3& aabbMax,
b3Scalar& param, b3Vector3& normal)
B3_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom,
const b3Vector3& rayTo,
const b3Vector3& aabbMin,
const b3Vector3& aabbMax,
b3Scalar& param, b3Vector3& normal)
{
b3Vector3 aabbHalfExtent = (aabbMax-aabbMin)* b3Scalar(0.5);
b3Vector3 aabbCenter = (aabbMax+aabbMin)* b3Scalar(0.5);
b3Vector3 source = rayFrom - aabbCenter;
b3Vector3 target = rayTo - aabbCenter;
int sourceOutcode = b3Outcode(source,aabbHalfExtent);
int targetOutcode = b3Outcode(target,aabbHalfExtent);
b3Vector3 aabbHalfExtent = (aabbMax - aabbMin) * b3Scalar(0.5);
b3Vector3 aabbCenter = (aabbMax + aabbMin) * b3Scalar(0.5);
b3Vector3 source = rayFrom - aabbCenter;
b3Vector3 target = rayTo - aabbCenter;
int sourceOutcode = b3Outcode(source, aabbHalfExtent);
int targetOutcode = b3Outcode(target, aabbHalfExtent);
if ((sourceOutcode & targetOutcode) == 0x0)
{
b3Scalar lambda_enter = b3Scalar(0.0);
b3Scalar lambda_exit = param;
b3Scalar lambda_exit = param;
b3Vector3 r = target - source;
int i;
b3Scalar normSign = 1;
b3Vector3 hitNormal = b3MakeVector3(0,0,0);
int bit=1;
b3Scalar normSign = 1;
b3Vector3 hitNormal = b3MakeVector3(0, 0, 0);
int bit = 1;
for (int j=0;j<2;j++)
for (int j = 0; j < 2; j++)
{
for (i = 0; i != 3; ++i)
{
if (sourceOutcode & bit)
{
b3Scalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
b3Scalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
if (lambda_enter <= lambda)
{
lambda_enter = lambda;
hitNormal.setValue(0,0,0);
hitNormal.setValue(0, 0, 0);
hitNormal[i] = normSign;
}
}
else if (targetOutcode & bit)
else if (targetOutcode & bit)
{
b3Scalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
b3Scalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
b3SetMin(lambda_exit, lambda);
}
bit<<=1;
bit <<= 1;
}
normSign = b3Scalar(-1.);
}
@ -177,56 +169,49 @@ B3_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom,
return false;
}
B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& halfExtents, b3Scalar margin,const b3Transform& t,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& halfExtents, b3Scalar margin, const b3Transform& t, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
{
b3Vector3 halfExtentsWithMargin = halfExtents+b3MakeVector3(margin,margin,margin);
b3Matrix3x3 abs_b = t.getBasis().absolute();
b3Vector3 halfExtentsWithMargin = halfExtents + b3MakeVector3(margin, margin, margin);
b3Matrix3x3 abs_b = t.getBasis().absolute();
b3Vector3 center = t.getOrigin();
b3Vector3 extent = halfExtentsWithMargin.dot3( abs_b[0], abs_b[1], abs_b[2] );
b3Vector3 extent = halfExtentsWithMargin.dot3(abs_b[0], abs_b[1], abs_b[2]);
aabbMinOut = center - extent;
aabbMaxOut = center + extent;
}
B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& localAabbMin,const b3Vector3& localAabbMax, b3Scalar margin,const b3Transform& trans,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& localAabbMin, const b3Vector3& localAabbMax, b3Scalar margin, const b3Transform& trans, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
{
//b3Assert(localAabbMin.getX() <= localAabbMax.getX());
//b3Assert(localAabbMin.getY() <= localAabbMax.getY());
//b3Assert(localAabbMin.getZ() <= localAabbMax.getZ());
b3Vector3 localHalfExtents = b3Scalar(0.5)*(localAabbMax-localAabbMin);
localHalfExtents+=b3MakeVector3(margin,margin,margin);
//b3Assert(localAabbMin.getX() <= localAabbMax.getX());
//b3Assert(localAabbMin.getY() <= localAabbMax.getY());
//b3Assert(localAabbMin.getZ() <= localAabbMax.getZ());
b3Vector3 localHalfExtents = b3Scalar(0.5) * (localAabbMax - localAabbMin);
localHalfExtents += b3MakeVector3(margin, margin, margin);
b3Vector3 localCenter = b3Scalar(0.5)*(localAabbMax+localAabbMin);
b3Matrix3x3 abs_b = trans.getBasis().absolute();
b3Vector3 center = trans(localCenter);
b3Vector3 extent = localHalfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
aabbMinOut = center-extent;
aabbMaxOut = center+extent;
b3Vector3 localCenter = b3Scalar(0.5) * (localAabbMax + localAabbMin);
b3Matrix3x3 abs_b = trans.getBasis().absolute();
b3Vector3 center = trans(localCenter);
b3Vector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
aabbMinOut = center - extent;
aabbMaxOut = center + extent;
}
#define B3_USE_BANCHLESS 1
#ifdef B3_USE_BANCHLESS
//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
B3_FORCE_INLINE unsigned b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
{
return static_cast<unsigned int>(b3Select((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
& (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
1, 0));
}
//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
B3_FORCE_INLINE unsigned b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2)
{
return static_cast<unsigned int>(b3Select((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
1, 0));
}
#else
B3_FORCE_INLINE bool b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
{
bool overlap = true;
overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
return overlap;
}
#endif //B3_USE_BANCHLESS
#endif //B3_AABB_UTIL2
B3_FORCE_INLINE bool b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2)
{
bool overlap = true;
overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
return overlap;
}
#endif //B3_USE_BANCHLESS
#endif //B3_AABB_UTIL2

1768
thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.cpp vendored
View File

File diff suppressed because it is too large Load Diff

100
thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.h vendored
View File

@ -23,58 +23,56 @@ subject to the following restrictions:
/// Ole Kniemeyer, MAXON Computer GmbH
class b3ConvexHullComputer
{
private:
b3Scalar compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp);
public:
class Edge
{
private:
b3Scalar compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp);
int next;
int reverse;
int targetVertex;
friend class b3ConvexHullComputer;
public:
class Edge
int getSourceVertex() const
{
private:
int next;
int reverse;
int targetVertex;
return (this + reverse)->targetVertex;
}
friend class b3ConvexHullComputer;
int getTargetVertex() const
{
return targetVertex;
}
public:
int getSourceVertex() const
{
return (this + reverse)->targetVertex;
}
const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
{
return this + next;
}
int getTargetVertex() const
{
return targetVertex;
}
const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
{
return (this + reverse)->getNextEdgeOfVertex();
}
const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
{
return this + next;
}
const Edge* getReverseEdge() const
{
return this + reverse;
}
};
const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
{
return (this + reverse)->getNextEdgeOfVertex();
}
// Vertices of the output hull
b3AlignedObjectArray<b3Vector3> vertices;
const Edge* getReverseEdge() const
{
return this + reverse;
}
};
// Edges of the output hull
b3AlignedObjectArray<Edge> edges;
// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
b3AlignedObjectArray<int> faces;
// Vertices of the output hull
b3AlignedObjectArray<b3Vector3> vertices;
// Edges of the output hull
b3AlignedObjectArray<Edge> edges;
// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
b3AlignedObjectArray<int> faces;
/*
/*
Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes
between the addresses of consecutive vertices. If "shrink" is positive, the convex hull is shrunken
by that amount (each face is moved by "shrink" length units towards the center along its normal).
@ -86,18 +84,16 @@ class b3ConvexHullComputer
The output convex hull can be found in the member variables "vertices", "edges", "faces".
*/
b3Scalar compute(const float* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
{
return compute(coords, false, stride, count, shrink, shrinkClamp);
}
b3Scalar compute(const float* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
{
return compute(coords, false, stride, count, shrink, shrinkClamp);
}
// same as above, but double precision
b3Scalar compute(const double* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
{
return compute(coords, true, stride, count, shrink, shrinkClamp);
}
// same as above, but double precision
b3Scalar compute(const double* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
{
return compute(coords, true, stride, count, shrink, shrinkClamp);
}
};
#endif //B3_CONVEX_HULL_COMPUTER_H
#endif //B3_CONVEX_HULL_COMPUTER_H

113
thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.cpp vendored
View File

@ -12,49 +12,43 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "b3GeometryUtil.h"
/*
Make sure this dummy function never changes so that it
can be used by probes that are checking whether the
library is actually installed.
*/
extern "C"
{
void b3BulletMathProbe ();
{
void b3BulletMathProbe();
void b3BulletMathProbe () {}
void b3BulletMathProbe() {}
}
bool b3GeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin)
bool b3GeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin)
{
int numbrushes = planeEquations.size();
for (int i=0;i<numbrushes;i++)
for (int i = 0; i < numbrushes; i++)
{
const b3Vector3& N1 = planeEquations[i];
b3Scalar dist = b3Scalar(N1.dot(point))+b3Scalar(N1[3])-margin;
if (dist>b3Scalar(0.))
b3Scalar dist = b3Scalar(N1.dot(point)) + b3Scalar(N1[3]) - margin;
if (dist > b3Scalar(0.))
{
return false;
}
}
return true;
}
bool b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin)
bool b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin)
{
int numvertices = vertices.size();
for (int i=0;i<numvertices;i++)
for (int i = 0; i < numvertices; i++)
{
const b3Vector3& N1 = vertices[i];
b3Scalar dist = b3Scalar(planeNormal.dot(N1))+b3Scalar(planeNormal[3])-margin;
if (dist>b3Scalar(0.))
b3Scalar dist = b3Scalar(planeNormal.dot(N1)) + b3Scalar(planeNormal[3]) - margin;
if (dist > b3Scalar(0.))
{
return false;
}
@ -62,102 +56,98 @@ bool b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const
return true;
}
bool notExist(const b3Vector3& planeEquation,const b3AlignedObjectArray<b3Vector3>& planeEquations);
bool notExist(const b3Vector3& planeEquation, const b3AlignedObjectArray<b3Vector3>& planeEquations);
bool notExist(const b3Vector3& planeEquation,const b3AlignedObjectArray<b3Vector3>& planeEquations)
bool notExist(const b3Vector3& planeEquation, const b3AlignedObjectArray<b3Vector3>& planeEquations)
{
int numbrushes = planeEquations.size();
for (int i=0;i<numbrushes;i++)
for (int i = 0; i < numbrushes; i++)
{
const b3Vector3& N1 = planeEquations[i];
if (planeEquation.dot(N1) > b3Scalar(0.999))
{
return false;
}
}
}
return true;
}
void b3GeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut )
void b3GeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut)
{
const int numvertices = vertices.size();
const int numvertices = vertices.size();
// brute force:
for (int i=0;i<numvertices;i++)
for (int i = 0; i < numvertices; i++)
{
const b3Vector3& N1 = vertices[i];
for (int j=i+1;j<numvertices;j++)
for (int j = i + 1; j < numvertices; j++)
{
const b3Vector3& N2 = vertices[j];
for (int k=j+1;k<numvertices;k++)
{
for (int k = j + 1; k < numvertices; k++)
{
const b3Vector3& N3 = vertices[k];
b3Vector3 planeEquation,edge0,edge1;
edge0 = N2-N1;
edge1 = N3-N1;
b3Vector3 planeEquation, edge0, edge1;
edge0 = N2 - N1;
edge1 = N3 - N1;
b3Scalar normalSign = b3Scalar(1.);
for (int ww=0;ww<2;ww++)
for (int ww = 0; ww < 2; ww++)
{
planeEquation = normalSign * edge0.cross(edge1);
if (planeEquation.length2() > b3Scalar(0.0001))
{
planeEquation.normalize();
if (notExist(planeEquation,planeEquationsOut))
if (notExist(planeEquation, planeEquationsOut))
{
planeEquation[3] = -planeEquation.dot(N1);
//check if inside, and replace supportingVertexOut if needed
if (areVerticesBehindPlane(planeEquation,vertices,b3Scalar(0.01)))
{
planeEquationsOut.push_back(planeEquation);
}
//check if inside, and replace supportingVertexOut if needed
if (areVerticesBehindPlane(planeEquation, vertices, b3Scalar(0.01)))
{
planeEquationsOut.push_back(planeEquation);
}
}
}
normalSign = b3Scalar(-1.);
}
}
}
}
}
void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations , b3AlignedObjectArray<b3Vector3>& verticesOut )
void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations, b3AlignedObjectArray<b3Vector3>& verticesOut)
{
const int numbrushes = planeEquations.size();
// brute force:
for (int i=0;i<numbrushes;i++)
for (int i = 0; i < numbrushes; i++)
{
const b3Vector3& N1 = planeEquations[i];
for (int j=i+1;j<numbrushes;j++)
for (int j = i + 1; j < numbrushes; j++)
{
const b3Vector3& N2 = planeEquations[j];
for (int k=j+1;k<numbrushes;k++)
{
for (int k = j + 1; k < numbrushes; k++)
{
const b3Vector3& N3 = planeEquations[k];
b3Vector3 n2n3; n2n3 = N2.cross(N3);
b3Vector3 n3n1; n3n1 = N3.cross(N1);
b3Vector3 n1n2; n1n2 = N1.cross(N2);
if ( ( n2n3.length2() > b3Scalar(0.0001) ) &&
( n3n1.length2() > b3Scalar(0.0001) ) &&
( n1n2.length2() > b3Scalar(0.0001) ) )
b3Vector3 n2n3;
n2n3 = N2.cross(N3);
b3Vector3 n3n1;
n3n1 = N3.cross(N1);
b3Vector3 n1n2;
n1n2 = N1.cross(N2);
if ((n2n3.length2() > b3Scalar(0.0001)) &&
(n3n1.length2() > b3Scalar(0.0001)) &&
(n1n2.length2() > b3Scalar(0.0001)))
{
//point P out of 3 plane equations:
// d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
//P = -------------------------------------------------------------------------
// N1 . ( N2 * N3 )
// d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
//P = -------------------------------------------------------------------------
// N1 . ( N2 * N3 )
b3Scalar quotient = (N1.dot(n2n3));
if (b3Fabs(quotient) > b3Scalar(0.000001))
@ -172,7 +162,7 @@ void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3
potentialVertex *= quotient;
//check if inside, and replace supportingVertexOut if needed
if (isPointInsidePlanes(planeEquations,potentialVertex,b3Scalar(0.01)))
if (isPointInsidePlanes(planeEquations, potentialVertex, b3Scalar(0.01)))
{
verticesOut.push_back(potentialVertex);
}
@ -182,4 +172,3 @@ void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3
}
}
}

22
thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.h vendored
View File

@ -12,7 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_GEOMETRY_UTIL_H
#define B3_GEOMETRY_UTIL_H
@ -22,21 +21,16 @@ subject to the following restrictions:
///The b3GeometryUtil helper class provides a few methods to convert between plane equations and vertices.
class b3GeometryUtil
{
public:
static void getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut );
public:
static void getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut);
static void getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations , b3AlignedObjectArray<b3Vector3>& verticesOut );
static bool isInside(const b3AlignedObjectArray<b3Vector3>& vertices, const b3Vector3& planeNormal, b3Scalar margin);
static bool isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin);
static void getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations, b3AlignedObjectArray<b3Vector3>& verticesOut);
static bool areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin);
static bool isInside(const b3AlignedObjectArray<b3Vector3>& vertices, const b3Vector3& planeNormal, b3Scalar margin);
static bool isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin);
static bool areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin);
};
#endif //B3_GEOMETRY_UTIL_H
#endif //B3_GEOMETRY_UTIL_H

63
thirdparty/bullet/Bullet3Geometry/b3GrahamScan2dConvexHull.h vendored
View File

@ -13,41 +13,40 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
#define B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
struct b3GrahamVector3 : public b3Vector3
{
b3GrahamVector3(const b3Vector3& org, int orgIndex)
:b3Vector3(org),
m_orgIndex(orgIndex)
: b3Vector3(org),
m_orgIndex(orgIndex)
{
}
b3Scalar m_angle;
b3Scalar m_angle;
int m_orgIndex;
};
struct b3AngleCompareFunc {
struct b3AngleCompareFunc
{
b3Vector3 m_anchor;
b3AngleCompareFunc(const b3Vector3& anchor)
: m_anchor(anchor)
: m_anchor(anchor)
{
}
bool operator()(const b3GrahamVector3& a, const b3GrahamVector3& b) const {
bool operator()(const b3GrahamVector3& a, const b3GrahamVector3& b) const
{
if (a.m_angle != b.m_angle)
return a.m_angle < b.m_angle;
else
{
b3Scalar al = (a-m_anchor).length2();
b3Scalar bl = (b-m_anchor).length2();
b3Scalar al = (a - m_anchor).length2();
b3Scalar bl = (b - m_anchor).length2();
if (al != bl)
return al < bl;
return al < bl;
else
{
return a.m_orgIndex < b.m_orgIndex;
@ -58,60 +57,60 @@ struct b3AngleCompareFunc {
inline void b3GrahamScanConvexHull2D(b3AlignedObjectArray<b3GrahamVector3>& originalPoints, b3AlignedObjectArray<b3GrahamVector3>& hull, const b3Vector3& normalAxis)
{
b3Vector3 axis0,axis1;
b3PlaneSpace1(normalAxis,axis0,axis1);
b3Vector3 axis0, axis1;
b3PlaneSpace1(normalAxis, axis0, axis1);
if (originalPoints.size()<=1)
if (originalPoints.size() <= 1)
{
for (int i=0;i<originalPoints.size();i++)
for (int i = 0; i < originalPoints.size(); i++)
hull.push_back(originalPoints[0]);
return;
}
//step1 : find anchor point with smallest projection on axis0 and move it to first location
for (int i=0;i<originalPoints.size();i++)
for (int i = 0; i < originalPoints.size(); i++)
{
// const b3Vector3& left = originalPoints[i];
// const b3Vector3& right = originalPoints[0];
// const b3Vector3& left = originalPoints[i];
// const b3Vector3& right = originalPoints[0];
b3Scalar projL = originalPoints[i].dot(axis0);
b3Scalar projR = originalPoints[0].dot(axis0);
if (projL < projR)
{
originalPoints.swap(0,i);
originalPoints.swap(0, i);
}
}
//also precompute angles
originalPoints[0].m_angle = -1e30f;
for (int i=1;i<originalPoints.size();i++)
for (int i = 1; i < originalPoints.size(); i++)
{
b3Vector3 xvec = axis0;
b3Vector3 ar = originalPoints[i]-originalPoints[0];
b3Vector3 ar = originalPoints[i] - originalPoints[0];
originalPoints[i].m_angle = b3Cross(xvec, ar).dot(normalAxis) / ar.length();
}
//step 2: sort all points, based on 'angle' with this anchor
b3AngleCompareFunc comp(originalPoints[0]);
originalPoints.quickSortInternal(comp,1,originalPoints.size()-1);
originalPoints.quickSortInternal(comp, 1, originalPoints.size() - 1);
int i;
for (i = 0; i<2; i++)
for (i = 0; i < 2; i++)
hull.push_back(originalPoints[i]);
//step 3: keep all 'convex' points and discard concave points (using back tracking)
for (; i != originalPoints.size(); i++)
for (; i != originalPoints.size(); i++)
{
bool isConvex = false;
while (!isConvex&& hull.size()>1) {
b3Vector3& a = hull[hull.size()-2];
b3Vector3& b = hull[hull.size()-1];
isConvex = b3Cross(a-b,a-originalPoints[i]).dot(normalAxis)> 0;
while (!isConvex && hull.size() > 1)
{
b3Vector3& a = hull[hull.size() - 2];
b3Vector3& b = hull[hull.size() - 1];
isConvex = b3Cross(a - b, a - originalPoints[i]).dot(normalAxis) > 0;
if (!isConvex)
hull.pop_back();
else
else
hull.push_back(originalPoints[i]);
}
}
}
#endif //B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
#endif //B3_GRAHAM_SCAN_2D_CONVEX_HULL_H

28
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h vendored
View File

@ -12,33 +12,31 @@
class b3GpuBroadphaseInterface
{
public:
typedef class b3GpuBroadphaseInterface* (CreateFunc)(cl_context ctx,cl_device_id device, cl_command_queue q);
typedef class b3GpuBroadphaseInterface*(CreateFunc)(cl_context ctx, cl_device_id device, cl_command_queue q);
virtual ~b3GpuBroadphaseInterface()
{
}
virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask)=0;
virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask)=0;
virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask) = 0;
virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask) = 0;
virtual void calculateOverlappingPairs(int maxPairs)=0;
virtual void calculateOverlappingPairsHost(int maxPairs)=0;
virtual void calculateOverlappingPairs(int maxPairs) = 0;
virtual void calculateOverlappingPairsHost(int maxPairs) = 0;
//call writeAabbsToGpu after done making all changes (createProxy etc)
virtual void writeAabbsToGpu()=0;
virtual void writeAabbsToGpu() = 0;
virtual cl_mem getAabbBufferWS()=0;
virtual int getNumOverlap()=0;
virtual cl_mem getOverlappingPairBuffer()=0;
virtual cl_mem getAabbBufferWS() = 0;
virtual int getNumOverlap() = 0;
virtual cl_mem getOverlappingPairBuffer() = 0;
virtual b3OpenCLArray<b3SapAabb>& getAllAabbsGPU() = 0;
virtual b3AlignedObjectArray<b3SapAabb>& getAllAabbsCPU() = 0;
virtual b3OpenCLArray<b3SapAabb>& getAllAabbsGPU()=0;
virtual b3AlignedObjectArray<b3SapAabb>& getAllAabbsCPU()=0;
virtual b3OpenCLArray<b3Int4>& getOverlappingPairsGPU() = 0;
virtual b3OpenCLArray<int>& getSmallAabbIndicesGPU() = 0;
virtual b3OpenCLArray<int>& getLargeAabbIndicesGPU() = 0;
};
#endif //B3_GPU_BROADPHASE_INTERFACE_H
#endif //B3_GPU_BROADPHASE_INTERFACE_H

245
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp vendored
View File

@ -5,12 +5,9 @@
#include "kernels/sapKernels.h"
//#include "kernels/gridBroadphase.cl"
#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
#define B3_BROADPHASE_SAP_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl"
#define B3_GRID_BROADPHASE_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphase.cl"
@ -21,31 +18,25 @@ cl_kernel kFindOverlappingPairs;
cl_kernel m_copyAabbsKernel;
cl_kernel m_sap2Kernel;
//int maxPairsPerBody = 64;
int maxBodiesPerCell = 256;//??
int maxBodiesPerCell = 256; //??
b3GpuGridBroadphase::b3GpuGridBroadphase(cl_context ctx,cl_device_id device, cl_command_queue q )
:m_context(ctx),
m_device(device),
m_queue(q),
m_allAabbsGPU1(ctx,q),
m_smallAabbsMappingGPU(ctx,q),
m_largeAabbsMappingGPU(ctx,q),
m_gpuPairs(ctx,q),
b3GpuGridBroadphase::b3GpuGridBroadphase(cl_context ctx, cl_device_id device, cl_command_queue q)
: m_context(ctx),
m_device(device),
m_queue(q),
m_allAabbsGPU1(ctx, q),
m_smallAabbsMappingGPU(ctx, q),
m_largeAabbsMappingGPU(ctx, q),
m_gpuPairs(ctx, q),
m_hashGpu(ctx,q),
m_hashGpu(ctx, q),
m_cellStartGpu(ctx,q),
m_paramsGPU(ctx,q)
m_cellStartGpu(ctx, q),
m_paramsGPU(ctx, q)
{
b3Vector3 gridSize = b3MakeVector3(3,3,3);
b3Vector3 invGridSize = b3MakeVector3(1.f/gridSize[0],1.f/gridSize[1],1.f/gridSize[2]);
b3Vector3 gridSize = b3MakeVector3(3, 3, 3);
b3Vector3 invGridSize = b3MakeVector3(1.f / gridSize[0], 1.f / gridSize[1], 1.f / gridSize[2]);
m_paramsCPU.m_gridSize[0] = 128;
m_paramsCPU.m_gridSize[1] = 128;
@ -58,92 +49,79 @@ m_paramsGPU(ctx,q)
m_paramsCPU.m_invCellSize[3] = 0.f;
m_paramsGPU.push_back(m_paramsCPU);
cl_int errNum=0;
cl_int errNum = 0;
{
const char* sapSrc = sapCL;
cl_program sapProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,sapSrc,&errNum,"",B3_BROADPHASE_SAP_PATH);
b3Assert(errNum==CL_SUCCESS);
m_copyAabbsKernel= b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "copyAabbsKernel",&errNum,sapProg );
m_sap2Kernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "computePairsKernelTwoArrays",&errNum,sapProg );
b3Assert(errNum==CL_SUCCESS);
cl_program sapProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, sapSrc, &errNum, "", B3_BROADPHASE_SAP_PATH);
b3Assert(errNum == CL_SUCCESS);
m_copyAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "copyAabbsKernel", &errNum, sapProg);
m_sap2Kernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "computePairsKernelTwoArrays", &errNum, sapProg);
b3Assert(errNum == CL_SUCCESS);
}
{
cl_program gridProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,gridBroadphaseCL,&errNum,"",B3_GRID_BROADPHASE_PATH);
b3Assert(errNum==CL_SUCCESS);
cl_program gridProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, gridBroadphaseCL, &errNum, "", B3_GRID_BROADPHASE_PATH);
b3Assert(errNum == CL_SUCCESS);
kCalcHashAABB = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,gridBroadphaseCL, "kCalcHashAABB",&errNum,gridProg);
b3Assert(errNum==CL_SUCCESS);
kClearCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,gridBroadphaseCL, "kClearCellStart",&errNum,gridProg);
b3Assert(errNum==CL_SUCCESS);
kCalcHashAABB = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kCalcHashAABB", &errNum, gridProg);
b3Assert(errNum == CL_SUCCESS);
kFindCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,gridBroadphaseCL, "kFindCellStart",&errNum,gridProg);
b3Assert(errNum==CL_SUCCESS);
kClearCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kClearCellStart", &errNum, gridProg);
b3Assert(errNum == CL_SUCCESS);
kFindOverlappingPairs = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,gridBroadphaseCL, "kFindOverlappingPairs",&errNum,gridProg);
b3Assert(errNum==CL_SUCCESS);
kFindCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kFindCellStart", &errNum, gridProg);
b3Assert(errNum == CL_SUCCESS);
kFindOverlappingPairs = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kFindOverlappingPairs", &errNum, gridProg);
b3Assert(errNum == CL_SUCCESS);
}
m_sorter = new b3RadixSort32CL(m_context,m_device,m_queue);
m_sorter = new b3RadixSort32CL(m_context, m_device, m_queue);
}
b3GpuGridBroadphase::~b3GpuGridBroadphase()
{
clReleaseKernel( kCalcHashAABB);
clReleaseKernel( kClearCellStart);
clReleaseKernel( kFindCellStart);
clReleaseKernel( kFindOverlappingPairs);
clReleaseKernel( m_sap2Kernel);
clReleaseKernel( m_copyAabbsKernel);
clReleaseKernel(kCalcHashAABB);
clReleaseKernel(kClearCellStart);
clReleaseKernel(kFindCellStart);
clReleaseKernel(kFindOverlappingPairs);
clReleaseKernel(m_sap2Kernel);
clReleaseKernel(m_copyAabbsKernel);
delete m_sorter;
}
void b3GpuGridBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask)
void b3GpuGridBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
{
b3SapAabb aabb;
aabb.m_minVec = aabbMin;
aabb.m_maxVec = aabbMax;
aabb.m_minIndices[3] = userPtr;
aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size();//NOT userPtr;
aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size(); //NOT userPtr;
m_smallAabbsMappingCPU.push_back(m_allAabbsCPU1.size());
m_allAabbsCPU1.push_back(aabb);
}
void b3GpuGridBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask)
void b3GpuGridBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
{
b3SapAabb aabb;
aabb.m_minVec = aabbMin;
aabb.m_maxVec = aabbMax;
aabb.m_minIndices[3] = userPtr;
aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size();//NOT userPtr;
aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size(); //NOT userPtr;
m_largeAabbsMappingCPU.push_back(m_allAabbsCPU1.size());
m_allAabbsCPU1.push_back(aabb);
}
void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
{
B3_PROFILE("b3GpuGridBroadphase::calculateOverlappingPairs");
if (0)
{
calculateOverlappingPairsHost(maxPairs);
/*
/*
b3AlignedObjectArray<b3Int4> cpuPairs;
m_gpuPairs.copyToHost(cpuPairs);
printf("host m_gpuPairs.size()=%d\n",m_gpuPairs.size());
@ -154,57 +132,50 @@ void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
*/
return;
}
int numSmallAabbs = m_smallAabbsMappingGPU.size();
b3OpenCLArray<int> pairCount(m_context,m_queue);
b3OpenCLArray<int> pairCount(m_context, m_queue);
pairCount.push_back(0);
m_gpuPairs.resize(maxPairs);//numSmallAabbs*maxPairsPerBody);
m_gpuPairs.resize(maxPairs); //numSmallAabbs*maxPairsPerBody);
{
int numLargeAabbs = m_largeAabbsMappingGPU.size();
if (numLargeAabbs && numSmallAabbs)
{
B3_PROFILE("sap2Kernel");
b3BufferInfoCL bInfo[] = {
b3BufferInfoCL( m_allAabbsGPU1.getBufferCL() ),
b3BufferInfoCL( m_largeAabbsMappingGPU.getBufferCL() ),
b3BufferInfoCL( m_smallAabbsMappingGPU.getBufferCL() ),
b3BufferInfoCL( m_gpuPairs.getBufferCL() ),
b3BufferInfoCL bInfo[] = {
b3BufferInfoCL(m_allAabbsGPU1.getBufferCL()),
b3BufferInfoCL(m_largeAabbsMappingGPU.getBufferCL()),
b3BufferInfoCL(m_smallAabbsMappingGPU.getBufferCL()),
b3BufferInfoCL(m_gpuPairs.getBufferCL()),
b3BufferInfoCL(pairCount.getBufferCL())};
b3LauncherCL launcher(m_queue, m_sap2Kernel,"m_sap2Kernel");
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
launcher.setConst( numLargeAabbs );
launcher.setConst( numSmallAabbs);
launcher.setConst( 0 );//axis is not used
launcher.setConst( maxPairs );
//@todo: use actual maximum work item sizes of the device instead of hardcoded values
launcher.launch2D( numLargeAabbs, numSmallAabbs,4,64);
b3LauncherCL launcher(m_queue, m_sap2Kernel, "m_sap2Kernel");
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(numLargeAabbs);
launcher.setConst(numSmallAabbs);
launcher.setConst(0); //axis is not used
launcher.setConst(maxPairs);
//@todo: use actual maximum work item sizes of the device instead of hardcoded values
launcher.launch2D(numLargeAabbs, numSmallAabbs, 4, 64);
int numPairs = pairCount.at(0);
if (numPairs >maxPairs)
if (numPairs > maxPairs)
{
b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
numPairs =maxPairs;
numPairs = maxPairs;
}
}
}
if (numSmallAabbs)
{
B3_PROFILE("gridKernel");
m_hashGpu.resize(numSmallAabbs);
{
B3_PROFILE("kCalcHashAABB");
b3LauncherCL launch(m_queue,kCalcHashAABB,"kCalcHashAABB");
b3LauncherCL launch(m_queue, kCalcHashAABB, "kCalcHashAABB");
launch.setConst(numSmallAabbs);
launch.setBuffer(m_allAabbsGPU1.getBufferCL());
launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL());
@ -214,117 +185,104 @@ void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
}
m_sorter->execute(m_hashGpu);
int numCells = this->m_paramsCPU.m_gridSize[0]*this->m_paramsCPU.m_gridSize[1]*this->m_paramsCPU.m_gridSize[2];
int numCells = this->m_paramsCPU.m_gridSize[0] * this->m_paramsCPU.m_gridSize[1] * this->m_paramsCPU.m_gridSize[2];
m_cellStartGpu.resize(numCells);
//b3AlignedObjectArray<int > cellStartCpu;
{
B3_PROFILE("kClearCellStart");
b3LauncherCL launch(m_queue,kClearCellStart,"kClearCellStart");
b3LauncherCL launch(m_queue, kClearCellStart, "kClearCellStart");
launch.setConst(numCells);
launch.setBuffer(m_cellStartGpu.getBufferCL());
launch.launch1D(numCells);
//m_cellStartGpu.copyToHost(cellStartCpu);
//printf("??\n");
}
{
B3_PROFILE("kFindCellStart");
b3LauncherCL launch(m_queue,kFindCellStart,"kFindCellStart");
b3LauncherCL launch(m_queue, kFindCellStart, "kFindCellStart");
launch.setConst(numSmallAabbs);
launch.setBuffer(m_hashGpu.getBufferCL());
launch.setBuffer(m_cellStartGpu.getBufferCL());
launch.launch1D(numSmallAabbs);
//m_cellStartGpu.copyToHost(cellStartCpu);
//printf("??\n");
}
{
B3_PROFILE("kFindOverlappingPairs");
b3LauncherCL launch(m_queue,kFindOverlappingPairs,"kFindOverlappingPairs");
b3LauncherCL launch(m_queue, kFindOverlappingPairs, "kFindOverlappingPairs");
launch.setConst(numSmallAabbs);
launch.setBuffer(m_allAabbsGPU1.getBufferCL());
launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL());
launch.setBuffer(m_hashGpu.getBufferCL());
launch.setBuffer(m_cellStartGpu.getBufferCL());
launch.setBuffer(m_paramsGPU.getBufferCL());
//launch.setBuffer(0);
launch.setBuffer(pairCount.getBufferCL());
launch.setBuffer(m_gpuPairs.getBufferCL());
launch.setConst(maxPairs);
launch.launch1D(numSmallAabbs);
int numPairs = pairCount.at(0);
if (numPairs >maxPairs)
if (numPairs > maxPairs)
{
b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
numPairs =maxPairs;
numPairs = maxPairs;
}
m_gpuPairs.resize(numPairs);
if (0)
{
b3AlignedObjectArray<b3Int4> pairsCpu;
m_gpuPairs.copyToHost(pairsCpu);
int sz = m_gpuPairs.size();
printf("m_gpuPairs.size()=%d\n",sz);
for (int i=0;i<m_gpuPairs.size();i++)
printf("m_gpuPairs.size()=%d\n", sz);
for (int i = 0; i < m_gpuPairs.size(); i++)
{
printf("pair %d = %d,%d\n",i,pairsCpu[i].x,pairsCpu[i].y);
printf("pair %d = %d,%d\n", i, pairsCpu[i].x, pairsCpu[i].y);
}
printf("?!?\n");
}
}
}
//calculateOverlappingPairsHost(maxPairs);
}
void b3GpuGridBroadphase::calculateOverlappingPairsHost(int maxPairs)
void b3GpuGridBroadphase::calculateOverlappingPairsHost(int maxPairs)
{
m_hostPairs.resize(0);
m_allAabbsGPU1.copyToHost(m_allAabbsCPU1);
for (int i=0;i<m_allAabbsCPU1.size();i++)
for (int i = 0; i < m_allAabbsCPU1.size(); i++)
{
for (int j=i+1;j<m_allAabbsCPU1.size();j++)
for (int j = i + 1; j < m_allAabbsCPU1.size(); j++)
{
if (b3TestAabbAgainstAabb2(m_allAabbsCPU1[i].m_minVec, m_allAabbsCPU1[i].m_maxVec,
m_allAabbsCPU1[j].m_minVec,m_allAabbsCPU1[j].m_maxVec))
m_allAabbsCPU1[j].m_minVec, m_allAabbsCPU1[j].m_maxVec))
{
b3Int4 pair;
int a = m_allAabbsCPU1[j].m_minIndices[3];
int b = m_allAabbsCPU1[i].m_minIndices[3];
if (a<=b)
if (a <= b)
{
pair.x = a;
pair.y = b;//store the original index in the unsorted aabb array
} else
pair.x = a;
pair.y = b; //store the original index in the unsorted aabb array
}
else
{
pair.x = b;
pair.y = a;//store the original index in the unsorted aabb array
pair.y = a; //store the original index in the unsorted aabb array
}
if (m_hostPairs.size()<maxPairs)
if (m_hostPairs.size() < maxPairs)
{
m_hostPairs.push_back(pair);
}
@ -332,40 +290,36 @@ void b3GpuGridBroadphase::calculateOverlappingPairsHost(int maxPairs)
}
}
m_gpuPairs.copyFromHost(m_hostPairs);
}
//call writeAabbsToGpu after done making all changes (createProxy etc)
//call writeAabbsToGpu after done making all changes (createProxy etc)
void b3GpuGridBroadphase::writeAabbsToGpu()
{
m_allAabbsGPU1.copyFromHost(m_allAabbsCPU1);
m_smallAabbsMappingGPU.copyFromHost(m_smallAabbsMappingCPU);
m_largeAabbsMappingGPU.copyFromHost(m_largeAabbsMappingCPU);
}
cl_mem b3GpuGridBroadphase::getAabbBufferWS()
cl_mem b3GpuGridBroadphase::getAabbBufferWS()
{
return this->m_allAabbsGPU1.getBufferCL();
}
int b3GpuGridBroadphase::getNumOverlap()
int b3GpuGridBroadphase::getNumOverlap()
{
return m_gpuPairs.size();
}
cl_mem b3GpuGridBroadphase::getOverlappingPairBuffer()
cl_mem b3GpuGridBroadphase::getOverlappingPairBuffer()
{
return m_gpuPairs.getBufferCL();
}
b3OpenCLArray<b3SapAabb>& b3GpuGridBroadphase::getAllAabbsGPU()
b3OpenCLArray<b3SapAabb>& b3GpuGridBroadphase::getAllAabbsGPU()
{
return m_allAabbsGPU1;
}
b3AlignedObjectArray<b3SapAabb>& b3GpuGridBroadphase::getAllAabbsCPU()
b3AlignedObjectArray<b3SapAabb>& b3GpuGridBroadphase::getAllAabbsCPU()
{
return m_allAabbsCPU1;
}
@ -382,4 +336,3 @@ b3OpenCLArray<int>& b3GpuGridBroadphase::getLargeAabbIndicesGPU()
{
return m_largeAabbsMappingGPU;
}

68
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.h vendored
View File

@ -6,83 +6,75 @@
struct b3ParamsGridBroadphaseCL
{
float m_invCellSize[4];
int m_gridSize[4];
int m_gridSize[4];
int getMaxBodiesPerCell() const
int getMaxBodiesPerCell() const
{
return m_gridSize[3];
}
void setMaxBodiesPerCell(int maxOverlap)
void setMaxBodiesPerCell(int maxOverlap)
{
m_gridSize[3] = maxOverlap;
}
};
class b3GpuGridBroadphase : public b3GpuBroadphaseInterface
{
protected:
cl_context m_context;
cl_device_id m_device;
cl_command_queue m_queue;
cl_context m_context;
cl_device_id m_device;
cl_command_queue m_queue;
b3OpenCLArray<b3SapAabb> m_allAabbsGPU1;
b3AlignedObjectArray<b3SapAabb> m_allAabbsCPU1;
b3OpenCLArray<b3SapAabb> m_allAabbsGPU1;
b3AlignedObjectArray<b3SapAabb> m_allAabbsCPU1;
b3OpenCLArray<int> m_smallAabbsMappingGPU;
b3OpenCLArray<int> m_smallAabbsMappingGPU;
b3AlignedObjectArray<int> m_smallAabbsMappingCPU;
b3OpenCLArray<int> m_largeAabbsMappingGPU;
b3OpenCLArray<int> m_largeAabbsMappingGPU;
b3AlignedObjectArray<int> m_largeAabbsMappingCPU;
b3AlignedObjectArray<b3Int4> m_hostPairs;
b3OpenCLArray<b3Int4> m_gpuPairs;
b3OpenCLArray<b3Int4> m_gpuPairs;
b3OpenCLArray<b3SortData> m_hashGpu;
b3OpenCLArray<int> m_cellStartGpu;
b3OpenCLArray<b3SortData> m_hashGpu;
b3OpenCLArray<int> m_cellStartGpu;
b3ParamsGridBroadphaseCL m_paramsCPU;
b3OpenCLArray<b3ParamsGridBroadphaseCL> m_paramsGPU;
b3ParamsGridBroadphaseCL m_paramsCPU;
b3OpenCLArray<b3ParamsGridBroadphaseCL> m_paramsGPU;
class b3RadixSort32CL* m_sorter;
class b3RadixSort32CL* m_sorter;
public:
b3GpuGridBroadphase(cl_context ctx,cl_device_id device, cl_command_queue q );
b3GpuGridBroadphase(cl_context ctx, cl_device_id device, cl_command_queue q);
virtual ~b3GpuGridBroadphase();
static b3GpuBroadphaseInterface* CreateFunc(cl_context ctx,cl_device_id device, cl_command_queue q)
static b3GpuBroadphaseInterface* CreateFunc(cl_context ctx, cl_device_id device, cl_command_queue q)
{
return new b3GpuGridBroadphase(ctx,device,q);
return new b3GpuGridBroadphase(ctx, device, q);
}
virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask);
virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask);
virtual void calculateOverlappingPairs(int maxPairs);
virtual void calculateOverlappingPairsHost(int maxPairs);
virtual void calculateOverlappingPairs(int maxPairs);
virtual void calculateOverlappingPairsHost(int maxPairs);
//call writeAabbsToGpu after done making all changes (createProxy etc)
virtual void writeAabbsToGpu();
virtual cl_mem getAabbBufferWS();
virtual int getNumOverlap();
virtual cl_mem getOverlappingPairBuffer();
virtual cl_mem getAabbBufferWS();
virtual int getNumOverlap();
virtual cl_mem getOverlappingPairBuffer();
virtual b3OpenCLArray<b3SapAabb>& getAllAabbsGPU();
virtual b3AlignedObjectArray<b3SapAabb>& getAllAabbsCPU();
virtual b3OpenCLArray<b3SapAabb>& getAllAabbsGPU();
virtual b3AlignedObjectArray<b3SapAabb>& getAllAabbsCPU();
virtual b3OpenCLArray<b3Int4>& getOverlappingPairsGPU();
virtual b3OpenCLArray<int>& getSmallAabbIndicesGPU();
virtual b3OpenCLArray<int>& getLargeAabbIndicesGPU();
};
#endif //B3_GPU_GRID_BROADPHASE_H
#endif //B3_GPU_GRID_BROADPHASE_H

540
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.cpp vendored
View File

@ -16,177 +16,174 @@ subject to the following restrictions:
#include "b3GpuParallelLinearBvh.h"
b3GpuParallelLinearBvh::b3GpuParallelLinearBvh(cl_context context, cl_device_id device, cl_command_queue queue) :
m_queue(queue),
m_radixSorter(context, device, queue),
m_rootNodeIndex(context, queue),
m_maxDistanceFromRoot(context, queue),
m_temp(context, queue),
m_internalNodeAabbs(context, queue),
m_internalNodeLeafIndexRanges(context, queue),
m_internalNodeChildNodes(context, queue),
m_internalNodeParentNodes(context, queue),
m_commonPrefixes(context, queue),
m_commonPrefixLengths(context, queue),
m_distanceFromRoot(context, queue),
m_leafNodeParentNodes(context, queue),
m_mortonCodesAndAabbIndicies(context, queue),
m_mergedAabb(context, queue),
m_leafNodeAabbs(context, queue),
m_largeAabbs(context, queue)
b3GpuParallelLinearBvh::b3GpuParallelLinearBvh(cl_context context, cl_device_id device, cl_command_queue queue) : m_queue(queue),
m_radixSorter(context, device, queue),
m_rootNodeIndex(context, queue),
m_maxDistanceFromRoot(context, queue),
m_temp(context, queue),
m_internalNodeAabbs(context, queue),
m_internalNodeLeafIndexRanges(context, queue),
m_internalNodeChildNodes(context, queue),
m_internalNodeParentNodes(context, queue),
m_commonPrefixes(context, queue),
m_commonPrefixLengths(context, queue),
m_distanceFromRoot(context, queue),
m_leafNodeParentNodes(context, queue),
m_mortonCodesAndAabbIndicies(context, queue),
m_mergedAabb(context, queue),
m_leafNodeAabbs(context, queue),
m_largeAabbs(context, queue)
{
m_rootNodeIndex.resize(1);
m_maxDistanceFromRoot.resize(1);
m_temp.resize(1);
//
const char CL_PROGRAM_PATH[] = "src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvh.cl";
const char* kernelSource = parallelLinearBvhCL; //parallelLinearBvhCL.h
const char* kernelSource = parallelLinearBvhCL; //parallelLinearBvhCL.h
cl_int error;
char* additionalMacros = 0;
m_parallelLinearBvhProgram = b3OpenCLUtils::compileCLProgramFromString(context, device, kernelSource, &error, additionalMacros, CL_PROGRAM_PATH);
b3Assert(m_parallelLinearBvhProgram);
m_separateAabbsKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "separateAabbs", &error, m_parallelLinearBvhProgram, additionalMacros );
m_separateAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "separateAabbs", &error, m_parallelLinearBvhProgram, additionalMacros);
b3Assert(m_separateAabbsKernel);
m_findAllNodesMergedAabbKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "findAllNodesMergedAabb", &error, m_parallelLinearBvhProgram, additionalMacros );
m_findAllNodesMergedAabbKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "findAllNodesMergedAabb", &error, m_parallelLinearBvhProgram, additionalMacros);
b3Assert(m_findAllNodesMergedAabbKernel);
m_assignMortonCodesAndAabbIndiciesKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "assignMortonCodesAndAabbIndicies", &error, m_parallelLinearBvhProgram, additionalMacros );
m_assignMortonCodesAndAabbIndiciesKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "assignMortonCodesAndAabbIndicies", &error, m_parallelLinearBvhProgram, additionalMacros);
b3Assert(m_assignMortonCodesAndAabbIndiciesKernel);
m_computeAdjacentPairCommonPrefixKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "computeAdjacentPairCommonPrefix", &error, m_parallelLinearBvhProgram, additionalMacros );
m_computeAdjacentPairCommonPrefixKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "computeAdjacentPairCommonPrefix", &error, m_parallelLinearBvhProgram, additionalMacros);
b3Assert(m_computeAdjacentPairCommonPrefixKernel);
m_buildBinaryRadixTreeLeafNodesKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "buildBinaryRadixTreeLeafNodes", &error, m_parallelLinearBvhProgram, additionalMacros );
m_buildBinaryRadixTreeLeafNodesKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "buildBinaryRadixTreeLeafNodes", &error, m_parallelLinearBvhProgram, additionalMacros);
b3Assert(m_buildBinaryRadixTreeLeafNodesKernel);
m_buildBinaryRadixTreeInternalNodesKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "buildBinaryRadixTreeInternalNodes", &error, m_parallelLinearBvhProgram, additionalMacros );
m_buildBinaryRadixTreeInternalNodesKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "buildBinaryRadixTreeInternalNodes", &error, m_parallelLinearBvhProgram, additionalMacros);
b3Assert(m_buildBinaryRadixTreeInternalNodesKernel);
m_findDistanceFromRootKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "findDistanceFromRoot", &error, m_parallelLinearBvhProgram, additionalMacros );
m_findDistanceFromRootKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "findDistanceFromRoot", &error, m_parallelLinearBvhProgram, additionalMacros);
b3Assert(m_findDistanceFromRootKernel);
m_buildBinaryRadixTreeAabbsRecursiveKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "buildBinaryRadixTreeAabbsRecursive", &error, m_parallelLinearBvhProgram, additionalMacros );
m_buildBinaryRadixTreeAabbsRecursiveKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "buildBinaryRadixTreeAabbsRecursive", &error, m_parallelLinearBvhProgram, additionalMacros);
b3Assert(m_buildBinaryRadixTreeAabbsRecursiveKernel);
m_findLeafIndexRangesKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "findLeafIndexRanges", &error, m_parallelLinearBvhProgram, additionalMacros );
m_findLeafIndexRangesKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "findLeafIndexRanges", &error, m_parallelLinearBvhProgram, additionalMacros);
b3Assert(m_findLeafIndexRangesKernel);
m_plbvhCalculateOverlappingPairsKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "plbvhCalculateOverlappingPairs", &error, m_parallelLinearBvhProgram, additionalMacros );
m_plbvhCalculateOverlappingPairsKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "plbvhCalculateOverlappingPairs", &error, m_parallelLinearBvhProgram, additionalMacros);
b3Assert(m_plbvhCalculateOverlappingPairsKernel);
m_plbvhRayTraverseKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "plbvhRayTraverse", &error, m_parallelLinearBvhProgram, additionalMacros );
m_plbvhRayTraverseKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "plbvhRayTraverse", &error, m_parallelLinearBvhProgram, additionalMacros);
b3Assert(m_plbvhRayTraverseKernel);
m_plbvhLargeAabbAabbTestKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "plbvhLargeAabbAabbTest", &error, m_parallelLinearBvhProgram, additionalMacros );
m_plbvhLargeAabbAabbTestKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "plbvhLargeAabbAabbTest", &error, m_parallelLinearBvhProgram, additionalMacros);
b3Assert(m_plbvhLargeAabbAabbTestKernel);
m_plbvhLargeAabbRayTestKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "plbvhLargeAabbRayTest", &error, m_parallelLinearBvhProgram, additionalMacros );
m_plbvhLargeAabbRayTestKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "plbvhLargeAabbRayTest", &error, m_parallelLinearBvhProgram, additionalMacros);
b3Assert(m_plbvhLargeAabbRayTestKernel);
}
b3GpuParallelLinearBvh::~b3GpuParallelLinearBvh()
b3GpuParallelLinearBvh::~b3GpuParallelLinearBvh()
{
clReleaseKernel(m_separateAabbsKernel);
clReleaseKernel(m_findAllNodesMergedAabbKernel);
clReleaseKernel(m_assignMortonCodesAndAabbIndiciesKernel);
clReleaseKernel(m_computeAdjacentPairCommonPrefixKernel);
clReleaseKernel(m_buildBinaryRadixTreeLeafNodesKernel);
clReleaseKernel(m_buildBinaryRadixTreeInternalNodesKernel);
clReleaseKernel(m_findDistanceFromRootKernel);
clReleaseKernel(m_buildBinaryRadixTreeAabbsRecursiveKernel);
clReleaseKernel(m_findLeafIndexRangesKernel);
clReleaseKernel(m_plbvhCalculateOverlappingPairsKernel);
clReleaseKernel(m_plbvhRayTraverseKernel);
clReleaseKernel(m_plbvhLargeAabbAabbTestKernel);
clReleaseKernel(m_plbvhLargeAabbRayTestKernel);
clReleaseProgram(m_parallelLinearBvhProgram);
}
void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAabbs, const b3OpenCLArray<int>& smallAabbIndices,
const b3OpenCLArray<int>& largeAabbIndices)
void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAabbs, const b3OpenCLArray<int>& smallAabbIndices,
const b3OpenCLArray<int>& largeAabbIndices)
{
B3_PROFILE("b3ParallelLinearBvh::build()");
int numLargeAabbs = largeAabbIndices.size();
int numSmallAabbs = smallAabbIndices.size();
//Since all AABBs(both large and small) are input as a contiguous array,
//Since all AABBs(both large and small) are input as a contiguous array,
//with 2 additional arrays used to indicate the indices of large and small AABBs,
//it is necessary to separate the AABBs so that the large AABBs will not degrade the quality of the BVH.
{
B3_PROFILE("Separate large and small AABBs");
m_largeAabbs.resize(numLargeAabbs);
m_leafNodeAabbs.resize(numSmallAabbs);
//Write large AABBs into m_largeAabbs
{
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( worldSpaceAabbs.getBufferCL() ),
b3BufferInfoCL( largeAabbIndices.getBufferCL() ),
b3BufferInfoCL( m_largeAabbs.getBufferCL() )
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(worldSpaceAabbs.getBufferCL()),
b3BufferInfoCL(largeAabbIndices.getBufferCL()),
b3BufferInfoCL(m_largeAabbs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_separateAabbsKernel, "m_separateAabbsKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(numLargeAabbs);
launcher.launch1D(numLargeAabbs);
}
//Write small AABBs into m_leafNodeAabbs
{
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( worldSpaceAabbs.getBufferCL() ),
b3BufferInfoCL( smallAabbIndices.getBufferCL() ),
b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() )
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(worldSpaceAabbs.getBufferCL()),
b3BufferInfoCL(smallAabbIndices.getBufferCL()),
b3BufferInfoCL(m_leafNodeAabbs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_separateAabbsKernel, "m_separateAabbsKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(numSmallAabbs);
launcher.launch1D(numSmallAabbs);
}
clFinish(m_queue);
}
//
int numLeaves = numSmallAabbs; //Number of leaves in the BVH == Number of rigid bodies with small AABBs
int numLeaves = numSmallAabbs; //Number of leaves in the BVH == Number of rigid bodies with small AABBs
int numInternalNodes = numLeaves - 1;
if(numLeaves < 2)
if (numLeaves < 2)
{
//Number of leaf nodes is checked in calculateOverlappingPairs() and testRaysAgainstBvhAabbs(),
//so it does not matter if numLeaves == 0 and rootNodeIndex == -1
int rootNodeIndex = numLeaves - 1;
m_rootNodeIndex.copyFromHostPointer(&rootNodeIndex, 1);
//Since the AABBs need to be rearranged(sorted) for the BVH construction algorithm,
//m_mortonCodesAndAabbIndicies.m_value is used to map a sorted AABB index to the unsorted AABB index
//instead of directly moving the AABBs. It needs to be set for the ray cast traversal kernel to work.
//( m_mortonCodesAndAabbIndicies[].m_value == unsorted index == index of m_leafNodeAabbs )
if(numLeaves == 1)
if (numLeaves == 1)
{
b3SortData leaf;
leaf.m_value = 0; //1 leaf so index is always 0; leaf.m_key does not need to be set
leaf.m_value = 0; //1 leaf so index is always 0; leaf.m_key does not need to be set
m_mortonCodesAndAabbIndicies.resize(1);
m_mortonCodesAndAabbIndicies.copyFromHostPointer(&leaf, 1);
}
return;
}
//
{
m_internalNodeAabbs.resize(numInternalNodes);
@ -197,37 +194,37 @@ void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAab
m_commonPrefixes.resize(numInternalNodes);
m_commonPrefixLengths.resize(numInternalNodes);
m_distanceFromRoot.resize(numInternalNodes);
m_leafNodeParentNodes.resize(numLeaves);
m_mortonCodesAndAabbIndicies.resize(numLeaves);
m_mergedAabb.resize(numLeaves);
}
//Find the merged AABB of all small AABBs; this is used to define the size of
//Find the merged AABB of all small AABBs; this is used to define the size of
//each cell in the virtual grid for the next kernel(2^10 cells in each dimension).
{
B3_PROFILE("Find AABB of merged nodes");
m_mergedAabb.copyFromOpenCLArray(m_leafNodeAabbs); //Need to make a copy since the kernel modifies the array
for(int numAabbsNeedingMerge = numLeaves; numAabbsNeedingMerge >= 2;
numAabbsNeedingMerge = numAabbsNeedingMerge / 2 + numAabbsNeedingMerge % 2)
m_mergedAabb.copyFromOpenCLArray(m_leafNodeAabbs); //Need to make a copy since the kernel modifies the array
for (int numAabbsNeedingMerge = numLeaves; numAabbsNeedingMerge >= 2;
numAabbsNeedingMerge = numAabbsNeedingMerge / 2 + numAabbsNeedingMerge % 2)
{
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( m_mergedAabb.getBufferCL() ) //Resulting AABB is stored in m_mergedAabb[0]
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(m_mergedAabb.getBufferCL()) //Resulting AABB is stored in m_mergedAabb[0]
};
b3LauncherCL launcher(m_queue, m_findAllNodesMergedAabbKernel, "m_findAllNodesMergedAabbKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(numAabbsNeedingMerge);
launcher.launch1D(numAabbsNeedingMerge);
}
clFinish(m_queue);
}
//Insert the center of the AABBs into a virtual grid,
//then convert the discrete grid coordinates into a morton code
//For each element in m_mortonCodesAndAabbIndicies, set
@ -235,34 +232,32 @@ void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAab
// m_value == small AABB index
{
B3_PROFILE("Assign morton codes");
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ),
b3BufferInfoCL( m_mergedAabb.getBufferCL() ),
b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() )
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(m_leafNodeAabbs.getBufferCL()),
b3BufferInfoCL(m_mergedAabb.getBufferCL()),
b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL())};
b3LauncherCL launcher(m_queue, m_assignMortonCodesAndAabbIndiciesKernel, "m_assignMortonCodesAndAabbIndiciesKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(numLeaves);
launcher.launch1D(numLeaves);
clFinish(m_queue);
}
//
{
B3_PROFILE("Sort leaves by morton codes");
m_radixSorter.execute(m_mortonCodesAndAabbIndicies);
clFinish(m_queue);
}
//
constructBinaryRadixTree();
//Since it is a sorted binary radix tree, each internal node contains a contiguous subset of leaf node indices.
//The root node contains leaf node indices in the range [0, numLeafNodes - 1].
//The child nodes of each node split their parent's index range into 2 contiguous halves.
@ -273,17 +268,16 @@ void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAab
//This property can be used for optimizing calculateOverlappingPairs(), to avoid testing each AABB pair twice
{
B3_PROFILE("m_findLeafIndexRangesKernel");
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ),
b3BufferInfoCL( m_internalNodeLeafIndexRanges.getBufferCL() )
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL()),
b3BufferInfoCL(m_internalNodeLeafIndexRanges.getBufferCL())};
b3LauncherCL launcher(m_queue, m_findLeafIndexRangesKernel, "m_findLeafIndexRangesKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(numInternalNodes);
launcher.launch1D(numInternalNodes);
clFinish(m_queue);
}
@ -293,285 +287,271 @@ void b3GpuParallelLinearBvh::calculateOverlappingPairs(b3OpenCLArray<b3Int4>& ou
{
int maxPairs = out_overlappingPairs.size();
b3OpenCLArray<int>& numPairsGpu = m_temp;
int reset = 0;
numPairsGpu.copyFromHostPointer(&reset, 1);
//
if( m_leafNodeAabbs.size() > 1 )
if (m_leafNodeAabbs.size() > 1)
{
B3_PROFILE("PLBVH small-small AABB test");
int numQueryAabbs = m_leafNodeAabbs.size();
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ),
b3BufferInfoCL( m_rootNodeIndex.getBufferCL() ),
b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ),
b3BufferInfoCL( m_internalNodeAabbs.getBufferCL() ),
b3BufferInfoCL( m_internalNodeLeafIndexRanges.getBufferCL() ),
b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() ),
b3BufferInfoCL( numPairsGpu.getBufferCL() ),
b3BufferInfoCL( out_overlappingPairs.getBufferCL() )
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(m_leafNodeAabbs.getBufferCL()),
b3BufferInfoCL(m_rootNodeIndex.getBufferCL()),
b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL()),
b3BufferInfoCL(m_internalNodeAabbs.getBufferCL()),
b3BufferInfoCL(m_internalNodeLeafIndexRanges.getBufferCL()),
b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL()),
b3BufferInfoCL(numPairsGpu.getBufferCL()),
b3BufferInfoCL(out_overlappingPairs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_plbvhCalculateOverlappingPairsKernel, "m_plbvhCalculateOverlappingPairsKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(maxPairs);
launcher.setConst(numQueryAabbs);
launcher.launch1D(numQueryAabbs);
clFinish(m_queue);
}
int numLargeAabbRigids = m_largeAabbs.size();
if( numLargeAabbRigids > 0 && m_leafNodeAabbs.size() > 0 )
if (numLargeAabbRigids > 0 && m_leafNodeAabbs.size() > 0)
{
B3_PROFILE("PLBVH large-small AABB test");
int numQueryAabbs = m_leafNodeAabbs.size();
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ),
b3BufferInfoCL( m_largeAabbs.getBufferCL() ),
b3BufferInfoCL( numPairsGpu.getBufferCL() ),
b3BufferInfoCL( out_overlappingPairs.getBufferCL() )
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(m_leafNodeAabbs.getBufferCL()),
b3BufferInfoCL(m_largeAabbs.getBufferCL()),
b3BufferInfoCL(numPairsGpu.getBufferCL()),
b3BufferInfoCL(out_overlappingPairs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_plbvhLargeAabbAabbTestKernel, "m_plbvhLargeAabbAabbTestKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(maxPairs);
launcher.setConst(numLargeAabbRigids);
launcher.setConst(numQueryAabbs);
launcher.launch1D(numQueryAabbs);
clFinish(m_queue);
}
//
int numPairs = -1;
numPairsGpu.copyToHostPointer(&numPairs, 1);
if(numPairs > maxPairs)
if (numPairs > maxPairs)
{
b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
numPairs = maxPairs;
numPairsGpu.copyFromHostPointer(&maxPairs, 1);
}
out_overlappingPairs.resize(numPairs);
}
void b3GpuParallelLinearBvh::testRaysAgainstBvhAabbs(const b3OpenCLArray<b3RayInfo>& rays,
b3OpenCLArray<int>& out_numRayRigidPairs, b3OpenCLArray<b3Int2>& out_rayRigidPairs)
void b3GpuParallelLinearBvh::testRaysAgainstBvhAabbs(const b3OpenCLArray<b3RayInfo>& rays,
b3OpenCLArray<int>& out_numRayRigidPairs, b3OpenCLArray<b3Int2>& out_rayRigidPairs)
{
B3_PROFILE("PLBVH testRaysAgainstBvhAabbs()");
int numRays = rays.size();
int maxRayRigidPairs = out_rayRigidPairs.size();
int reset = 0;
out_numRayRigidPairs.copyFromHostPointer(&reset, 1);
//
if( m_leafNodeAabbs.size() > 0 )
if (m_leafNodeAabbs.size() > 0)
{
B3_PROFILE("PLBVH ray test small AABB");
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ),
b3BufferInfoCL( m_rootNodeIndex.getBufferCL() ),
b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ),
b3BufferInfoCL( m_internalNodeAabbs.getBufferCL() ),
b3BufferInfoCL( m_internalNodeLeafIndexRanges.getBufferCL() ),
b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() ),
b3BufferInfoCL( rays.getBufferCL() ),
b3BufferInfoCL( out_numRayRigidPairs.getBufferCL() ),
b3BufferInfoCL( out_rayRigidPairs.getBufferCL() )
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(m_leafNodeAabbs.getBufferCL()),
b3BufferInfoCL(m_rootNodeIndex.getBufferCL()),
b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL()),
b3BufferInfoCL(m_internalNodeAabbs.getBufferCL()),
b3BufferInfoCL(m_internalNodeLeafIndexRanges.getBufferCL()),
b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL()),
b3BufferInfoCL(rays.getBufferCL()),
b3BufferInfoCL(out_numRayRigidPairs.getBufferCL()),
b3BufferInfoCL(out_rayRigidPairs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_plbvhRayTraverseKernel, "m_plbvhRayTraverseKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(maxRayRigidPairs);
launcher.setConst(numRays);
launcher.launch1D(numRays);
clFinish(m_queue);
}
int numLargeAabbRigids = m_largeAabbs.size();
if(numLargeAabbRigids > 0)
if (numLargeAabbRigids > 0)
{
B3_PROFILE("PLBVH ray test large AABB");
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( m_largeAabbs.getBufferCL() ),
b3BufferInfoCL( rays.getBufferCL() ),
b3BufferInfoCL( out_numRayRigidPairs.getBufferCL() ),
b3BufferInfoCL( out_rayRigidPairs.getBufferCL() )
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(m_largeAabbs.getBufferCL()),
b3BufferInfoCL(rays.getBufferCL()),
b3BufferInfoCL(out_numRayRigidPairs.getBufferCL()),
b3BufferInfoCL(out_rayRigidPairs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_plbvhLargeAabbRayTestKernel, "m_plbvhLargeAabbRayTestKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(numLargeAabbRigids);
launcher.setConst(maxRayRigidPairs);
launcher.setConst(numRays);
launcher.launch1D(numRays);
clFinish(m_queue);
}
//
int numRayRigidPairs = -1;
out_numRayRigidPairs.copyToHostPointer(&numRayRigidPairs, 1);
if(numRayRigidPairs > maxRayRigidPairs)
if (numRayRigidPairs > maxRayRigidPairs)
b3Error("Error running out of rayRigid pairs: numRayRigidPairs = %d, maxRayRigidPairs = %d.\n", numRayRigidPairs, maxRayRigidPairs);
}
void b3GpuParallelLinearBvh::constructBinaryRadixTree()
{
B3_PROFILE("b3GpuParallelLinearBvh::constructBinaryRadixTree()");
int numLeaves = m_leafNodeAabbs.size();
int numInternalNodes = numLeaves - 1;
//Each internal node is placed in between 2 leaf nodes.
//By using this arrangement and computing the common prefix between
//these 2 adjacent leaf nodes, it is possible to quickly construct a binary radix tree.
{
B3_PROFILE("m_computeAdjacentPairCommonPrefixKernel");
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() ),
b3BufferInfoCL( m_commonPrefixes.getBufferCL() ),
b3BufferInfoCL( m_commonPrefixLengths.getBufferCL() )
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL()),
b3BufferInfoCL(m_commonPrefixes.getBufferCL()),
b3BufferInfoCL(m_commonPrefixLengths.getBufferCL())};
b3LauncherCL launcher(m_queue, m_computeAdjacentPairCommonPrefixKernel, "m_computeAdjacentPairCommonPrefixKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(numInternalNodes);
launcher.launch1D(numInternalNodes);
clFinish(m_queue);
}
//For each leaf node, select its parent node by
//For each leaf node, select its parent node by
//comparing the 2 nearest internal nodes and assign child node indices
{
B3_PROFILE("m_buildBinaryRadixTreeLeafNodesKernel");
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( m_commonPrefixLengths.getBufferCL() ),
b3BufferInfoCL( m_leafNodeParentNodes.getBufferCL() ),
b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() )
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(m_commonPrefixLengths.getBufferCL()),
b3BufferInfoCL(m_leafNodeParentNodes.getBufferCL()),
b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL())};
b3LauncherCL launcher(m_queue, m_buildBinaryRadixTreeLeafNodesKernel, "m_buildBinaryRadixTreeLeafNodesKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(numLeaves);
launcher.launch1D(numLeaves);
clFinish(m_queue);
}
//For each internal node, perform 2 binary searches among the other internal nodes
//to its left and right to find its potential parent nodes and assign child node indices
{
B3_PROFILE("m_buildBinaryRadixTreeInternalNodesKernel");
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( m_commonPrefixes.getBufferCL() ),
b3BufferInfoCL( m_commonPrefixLengths.getBufferCL() ),
b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ),
b3BufferInfoCL( m_internalNodeParentNodes.getBufferCL() ),
b3BufferInfoCL( m_rootNodeIndex.getBufferCL() )
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(m_commonPrefixes.getBufferCL()),
b3BufferInfoCL(m_commonPrefixLengths.getBufferCL()),
b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL()),
b3BufferInfoCL(m_internalNodeParentNodes.getBufferCL()),
b3BufferInfoCL(m_rootNodeIndex.getBufferCL())};
b3LauncherCL launcher(m_queue, m_buildBinaryRadixTreeInternalNodesKernel, "m_buildBinaryRadixTreeInternalNodesKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(numInternalNodes);
launcher.launch1D(numInternalNodes);
clFinish(m_queue);
}
//Find the number of nodes seperating each internal node and the root node
//so that the AABBs can be set using the next kernel.
//Also determine the maximum number of nodes separating an internal node and the root node.
{
B3_PROFILE("m_findDistanceFromRootKernel");
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( m_rootNodeIndex.getBufferCL() ),
b3BufferInfoCL( m_internalNodeParentNodes.getBufferCL() ),
b3BufferInfoCL( m_maxDistanceFromRoot.getBufferCL() ),
b3BufferInfoCL( m_distanceFromRoot.getBufferCL() )
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(m_rootNodeIndex.getBufferCL()),
b3BufferInfoCL(m_internalNodeParentNodes.getBufferCL()),
b3BufferInfoCL(m_maxDistanceFromRoot.getBufferCL()),
b3BufferInfoCL(m_distanceFromRoot.getBufferCL())};
b3LauncherCL launcher(m_queue, m_findDistanceFromRootKernel, "m_findDistanceFromRootKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(numInternalNodes);
launcher.launch1D(numInternalNodes);
clFinish(m_queue);
}
//Starting from the internal nodes nearest to the leaf nodes, recursively move up
//the tree towards the root to set the AABBs of each internal node; each internal node
//checks its children and merges their AABBs
{
B3_PROFILE("m_buildBinaryRadixTreeAabbsRecursiveKernel");
int maxDistanceFromRoot = -1;
{
B3_PROFILE("copy maxDistanceFromRoot to CPU");
m_maxDistanceFromRoot.copyToHostPointer(&maxDistanceFromRoot, 1);
clFinish(m_queue);
}
for(int distanceFromRoot = maxDistanceFromRoot; distanceFromRoot >= 0; --distanceFromRoot)
for (int distanceFromRoot = maxDistanceFromRoot; distanceFromRoot >= 0; --distanceFromRoot)
{
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL( m_distanceFromRoot.getBufferCL() ),
b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() ),
b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ),
b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ),
b3BufferInfoCL( m_internalNodeAabbs.getBufferCL() )
};
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(m_distanceFromRoot.getBufferCL()),
b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL()),
b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL()),
b3BufferInfoCL(m_leafNodeAabbs.getBufferCL()),
b3BufferInfoCL(m_internalNodeAabbs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_buildBinaryRadixTreeAabbsRecursiveKernel, "m_buildBinaryRadixTreeAabbsRecursiveKernel");
launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
launcher.setConst(maxDistanceFromRoot);
launcher.setConst(distanceFromRoot);
launcher.setConst(numInternalNodes);
//It may seem inefficent to launch a thread for each internal node when a
//much smaller number of nodes is actually processed, but this is actually
//faster than determining the exact nodes that are ready to merge their child AABBs.
//faster than determining the exact nodes that are ready to merge their child AABBs.
launcher.launch1D(numInternalNodes);
}
clFinish(m_queue);
}
}

68
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.h vendored
View File

@ -37,10 +37,10 @@ subject to the following restrictions:
///"Maximizing Parallelism in the Construction of BVHs, Octrees, and k-d trees" [Karras 2012] \n
///@par
///The basic algorithm for building the BVH as presented in [Lauterbach et al. 2009] consists of 4 stages:
/// - [fully parallel] Assign morton codes for each AABB using its center (after quantizing the AABB centers into a virtual grid)
/// - [fully parallel] Assign morton codes for each AABB using its center (after quantizing the AABB centers into a virtual grid)
/// - [fully parallel] Sort morton codes
/// - [somewhat parallel] Build binary radix tree (assign parent/child pointers for internal nodes of the BVH)
/// - [somewhat parallel] Set internal node AABBs
/// - [somewhat parallel] Build binary radix tree (assign parent/child pointers for internal nodes of the BVH)
/// - [somewhat parallel] Set internal node AABBs
///@par
///[Karras 2012] improves on the algorithm by introducing fully parallel methods for the last 2 stages.
///The BVH implementation here shares many concepts with [Karras 2012], but a different method is used for constructing the tree.
@ -49,75 +49,75 @@ subject to the following restrictions:
class b3GpuParallelLinearBvh
{
cl_command_queue m_queue;
cl_program m_parallelLinearBvhProgram;
cl_kernel m_separateAabbsKernel;
cl_kernel m_findAllNodesMergedAabbKernel;
cl_kernel m_assignMortonCodesAndAabbIndiciesKernel;
//Binary radix tree construction kernels
cl_kernel m_computeAdjacentPairCommonPrefixKernel;
cl_kernel m_buildBinaryRadixTreeLeafNodesKernel;
cl_kernel m_buildBinaryRadixTreeInternalNodesKernel;
cl_kernel m_findDistanceFromRootKernel;
cl_kernel m_buildBinaryRadixTreeAabbsRecursiveKernel;
cl_kernel m_findLeafIndexRangesKernel;
//Traversal kernels
cl_kernel m_plbvhCalculateOverlappingPairsKernel;
cl_kernel m_plbvhRayTraverseKernel;
cl_kernel m_plbvhLargeAabbAabbTestKernel;
cl_kernel m_plbvhLargeAabbRayTestKernel;
b3RadixSort32CL m_radixSorter;
//1 element
b3OpenCLArray<int> m_rootNodeIndex; //Most significant bit(0x80000000) is set to indicate internal node
b3OpenCLArray<int> m_maxDistanceFromRoot; //Max number of internal nodes between an internal node and the root node
b3OpenCLArray<int> m_temp; //Used to hold the number of pairs in calculateOverlappingPairs()
b3OpenCLArray<int> m_rootNodeIndex; //Most significant bit(0x80000000) is set to indicate internal node
b3OpenCLArray<int> m_maxDistanceFromRoot; //Max number of internal nodes between an internal node and the root node
b3OpenCLArray<int> m_temp; //Used to hold the number of pairs in calculateOverlappingPairs()
//1 element per internal node (number_of_internal_nodes == number_of_leaves - 1)
b3OpenCLArray<b3SapAabb> m_internalNodeAabbs;
b3OpenCLArray<b3Int2> m_internalNodeLeafIndexRanges; //x == min leaf index, y == max leaf index
b3OpenCLArray<b3Int2> m_internalNodeChildNodes; //x == left child, y == right child; msb(0x80000000) is set to indicate internal node
b3OpenCLArray<int> m_internalNodeParentNodes; //For parent node index, msb(0x80000000) is not set since it is always internal
b3OpenCLArray<b3Int2> m_internalNodeLeafIndexRanges; //x == min leaf index, y == max leaf index
b3OpenCLArray<b3Int2> m_internalNodeChildNodes; //x == left child, y == right child; msb(0x80000000) is set to indicate internal node
b3OpenCLArray<int> m_internalNodeParentNodes; //For parent node index, msb(0x80000000) is not set since it is always internal
//1 element per internal node; for binary radix tree construction
b3OpenCLArray<b3Int64> m_commonPrefixes;
b3OpenCLArray<int> m_commonPrefixLengths;
b3OpenCLArray<int> m_distanceFromRoot; //Number of internal nodes between this node and the root
b3OpenCLArray<int> m_distanceFromRoot; //Number of internal nodes between this node and the root
//1 element per leaf node (leaf nodes only include small AABBs)
b3OpenCLArray<int> m_leafNodeParentNodes; //For parent node index, msb(0x80000000) is not set since it is always internal
b3OpenCLArray<b3SortData> m_mortonCodesAndAabbIndicies; //m_key == morton code, m_value == aabb index in m_leafNodeAabbs
b3OpenCLArray<b3SapAabb> m_mergedAabb; //m_mergedAabb[0] contains the merged AABB of all leaf nodes
b3OpenCLArray<b3SapAabb> m_leafNodeAabbs; //Contains only small AABBs
b3OpenCLArray<int> m_leafNodeParentNodes; //For parent node index, msb(0x80000000) is not set since it is always internal
b3OpenCLArray<b3SortData> m_mortonCodesAndAabbIndicies; //m_key == morton code, m_value == aabb index in m_leafNodeAabbs
b3OpenCLArray<b3SapAabb> m_mergedAabb; //m_mergedAabb[0] contains the merged AABB of all leaf nodes
b3OpenCLArray<b3SapAabb> m_leafNodeAabbs; //Contains only small AABBs
//1 element per large AABB, which is not stored in the BVH
b3OpenCLArray<b3SapAabb> m_largeAabbs;
public:
b3GpuParallelLinearBvh(cl_context context, cl_device_id device, cl_command_queue queue);
virtual ~b3GpuParallelLinearBvh();
///Must be called before any other function
void build(const b3OpenCLArray<b3SapAabb>& worldSpaceAabbs, const b3OpenCLArray<int>& smallAabbIndices,
const b3OpenCLArray<int>& largeAabbIndices);
void build(const b3OpenCLArray<b3SapAabb>& worldSpaceAabbs, const b3OpenCLArray<int>& smallAabbIndices,
const b3OpenCLArray<int>& largeAabbIndices);
///calculateOverlappingPairs() uses the worldSpaceAabbs parameter of b3GpuParallelLinearBvh::build() as the query AABBs.
///@param out_overlappingPairs The size() of this array is used to determine the max number of pairs.
///If the number of overlapping pairs is < out_overlappingPairs.size(), out_overlappingPairs is resized.
void calculateOverlappingPairs(b3OpenCLArray<b3Int4>& out_overlappingPairs);
///@param out_numRigidRayPairs Array of length 1; contains the number of detected ray-rigid AABB intersections;
///this value may be greater than out_rayRigidPairs.size() if out_rayRigidPairs is not large enough.
///@param out_rayRigidPairs Contains an array of rays intersecting rigid AABBs; x == ray index, y == rigid body index.
///If the size of this array is insufficient to hold all ray-rigid AABB intersections, additional intersections are discarded.
void testRaysAgainstBvhAabbs(const b3OpenCLArray<b3RayInfo>& rays,
b3OpenCLArray<int>& out_numRayRigidPairs, b3OpenCLArray<b3Int2>& out_rayRigidPairs);
void testRaysAgainstBvhAabbs(const b3OpenCLArray<b3RayInfo>& rays,
b3OpenCLArray<int>& out_numRayRigidPairs, b3OpenCLArray<b3Int2>& out_rayRigidPairs);
private:
void constructBinaryRadixTree();
};

44
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.cpp vendored
View File

@ -13,45 +13,44 @@ subject to the following restrictions:
#include "b3GpuParallelLinearBvhBroadphase.h"
b3GpuParallelLinearBvhBroadphase::b3GpuParallelLinearBvhBroadphase(cl_context context, cl_device_id device, cl_command_queue queue) :
m_plbvh(context, device, queue),
m_overlappingPairsGpu(context, queue),
m_aabbsGpu(context, queue),
m_smallAabbsMappingGpu(context, queue),
m_largeAabbsMappingGpu(context, queue)
b3GpuParallelLinearBvhBroadphase::b3GpuParallelLinearBvhBroadphase(cl_context context, cl_device_id device, cl_command_queue queue) : m_plbvh(context, device, queue),
m_overlappingPairsGpu(context, queue),
m_aabbsGpu(context, queue),
m_smallAabbsMappingGpu(context, queue),
m_largeAabbsMappingGpu(context, queue)
{
}
void b3GpuParallelLinearBvhBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
void b3GpuParallelLinearBvhBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
{
int newAabbIndex = m_aabbsCpu.size();
b3SapAabb aabb;
aabb.m_minVec = aabbMin;
aabb.m_maxVec = aabbMax;
aabb.m_minIndices[3] = userPtr;
aabb.m_signedMaxIndices[3] = newAabbIndex;
m_smallAabbsMappingCpu.push_back(newAabbIndex);
m_aabbsCpu.push_back(aabb);
}
void b3GpuParallelLinearBvhBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
void b3GpuParallelLinearBvhBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
{
int newAabbIndex = m_aabbsCpu.size();
b3SapAabb aabb;
aabb.m_minVec = aabbMin;
aabb.m_maxVec = aabbMax;
aabb.m_minIndices[3] = userPtr;
aabb.m_signedMaxIndices[3] = newAabbIndex;
m_largeAabbsMappingCpu.push_back(newAabbIndex);
m_aabbsCpu.push_back(aabb);
}
@ -59,22 +58,19 @@ void b3GpuParallelLinearBvhBroadphase::calculateOverlappingPairs(int maxPairs)
{
//Reconstruct BVH
m_plbvh.build(m_aabbsGpu, m_smallAabbsMappingGpu, m_largeAabbsMappingGpu);
//
m_overlappingPairsGpu.resize(maxPairs);
m_plbvh.calculateOverlappingPairs(m_overlappingPairsGpu);
}
void b3GpuParallelLinearBvhBroadphase::calculateOverlappingPairsHost(int maxPairs)
{
b3Assert(0); //CPU version not implemented
b3Assert(0); //CPU version not implemented
}
void b3GpuParallelLinearBvhBroadphase::writeAabbsToGpu()
{
m_aabbsGpu.copyFromHost(m_aabbsCpu);
void b3GpuParallelLinearBvhBroadphase::writeAabbsToGpu()
{
m_aabbsGpu.copyFromHost(m_aabbsCpu);
m_smallAabbsMappingGpu.copyFromHost(m_smallAabbsMappingCpu);
m_largeAabbsMappingGpu.copyFromHost(m_largeAabbsMappingCpu);
}

24
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.h vendored
View File

@ -21,42 +21,42 @@ subject to the following restrictions:
class b3GpuParallelLinearBvhBroadphase : public b3GpuBroadphaseInterface
{
b3GpuParallelLinearBvh m_plbvh;
b3OpenCLArray<b3Int4> m_overlappingPairsGpu;
b3OpenCLArray<b3SapAabb> m_aabbsGpu;
b3OpenCLArray<int> m_smallAabbsMappingGpu;
b3OpenCLArray<int> m_largeAabbsMappingGpu;
b3AlignedObjectArray<b3SapAabb> m_aabbsCpu;
b3AlignedObjectArray<int> m_smallAabbsMappingCpu;
b3AlignedObjectArray<int> m_largeAabbsMappingCpu;
public:
b3GpuParallelLinearBvhBroadphase(cl_context context, cl_device_id device, cl_command_queue queue);
virtual ~b3GpuParallelLinearBvhBroadphase() {}
virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
virtual void calculateOverlappingPairs(int maxPairs);
virtual void calculateOverlappingPairsHost(int maxPairs);
//call writeAabbsToGpu after done making all changes (createProxy etc)
virtual void writeAabbsToGpu();
virtual int getNumOverlap() { return m_overlappingPairsGpu.size(); }
virtual int getNumOverlap() { return m_overlappingPairsGpu.size(); }
virtual cl_mem getOverlappingPairBuffer() { return m_overlappingPairsGpu.getBufferCL(); }
virtual cl_mem getAabbBufferWS() { return m_aabbsGpu.getBufferCL(); }
virtual b3OpenCLArray<b3SapAabb>& getAllAabbsGPU() { return m_aabbsGpu; }
virtual b3OpenCLArray<b3Int4>& getOverlappingPairsGPU() { return m_overlappingPairsGpu; }
virtual b3OpenCLArray<int>& getSmallAabbIndicesGPU() { return m_smallAabbsMappingGpu; }
virtual b3OpenCLArray<int>& getLargeAabbIndicesGPU() { return m_largeAabbsMappingGpu; }
virtual b3AlignedObjectArray<b3SapAabb>& getAllAabbsCPU() { return m_aabbsCpu; }
static b3GpuBroadphaseInterface* CreateFunc(cl_context context, cl_device_id device, cl_command_queue queue)
{
return new b3GpuParallelLinearBvhBroadphase(context, device, queue);

928
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp vendored
View File

File diff suppressed because it is too large Load Diff

140
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h vendored
View File

@ -2,7 +2,7 @@
#define B3_GPU_SAP_BROADPHASE_H
#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
#include "Bullet3OpenCL/ParallelPrimitives/b3FillCL.h" //b3Int2
#include "Bullet3OpenCL/ParallelPrimitives/b3FillCL.h" //b3Int2
class b3Vector3;
#include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h"
@ -11,141 +11,133 @@ class b3Vector3;
#include "b3GpuBroadphaseInterface.h"
class b3GpuSapBroadphase : public b3GpuBroadphaseInterface
{
cl_context m_context;
cl_device_id m_device;
cl_command_queue m_queue;
cl_kernel m_flipFloatKernel;
cl_kernel m_scatterKernel ;
cl_kernel m_copyAabbsKernel;
cl_kernel m_sapKernel;
cl_kernel m_sap2Kernel;
cl_kernel m_prepareSumVarianceKernel;
cl_context m_context;
cl_device_id m_device;
cl_command_queue m_queue;
cl_kernel m_flipFloatKernel;
cl_kernel m_scatterKernel;
cl_kernel m_copyAabbsKernel;
cl_kernel m_sapKernel;
cl_kernel m_sap2Kernel;
cl_kernel m_prepareSumVarianceKernel;
class b3RadixSort32CL* m_sorter;
///test for 3d SAP
b3AlignedObjectArray<b3SortData> m_sortedAxisCPU[3][2];
b3AlignedObjectArray<b3UnsignedInt2> m_objectMinMaxIndexCPU[3][2];
b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis0;
b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis1;
b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis2;
b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis0prev;
b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis1prev;
b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis2prev;
b3AlignedObjectArray<b3SortData> m_sortedAxisCPU[3][2];
b3AlignedObjectArray<b3UnsignedInt2> m_objectMinMaxIndexCPU[3][2];
b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis0;
b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis1;
b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis2;
b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis0prev;
b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis1prev;
b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis2prev;
b3OpenCLArray<b3SortData> m_sortedAxisGPU0;
b3OpenCLArray<b3SortData> m_sortedAxisGPU1;
b3OpenCLArray<b3SortData> m_sortedAxisGPU2;
b3OpenCLArray<b3SortData> m_sortedAxisGPU0prev;
b3OpenCLArray<b3SortData> m_sortedAxisGPU1prev;
b3OpenCLArray<b3SortData> m_sortedAxisGPU2prev;
b3OpenCLArray<b3SortData> m_sortedAxisGPU0;
b3OpenCLArray<b3SortData> m_sortedAxisGPU1;
b3OpenCLArray<b3SortData> m_sortedAxisGPU2;
b3OpenCLArray<b3SortData> m_sortedAxisGPU0prev;
b3OpenCLArray<b3SortData> m_sortedAxisGPU1prev;
b3OpenCLArray<b3SortData> m_sortedAxisGPU2prev;
b3OpenCLArray<b3Int4> m_addedHostPairsGPU;
b3OpenCLArray<b3Int4> m_removedHostPairsGPU;
b3OpenCLArray<int> m_addedCountGPU;
b3OpenCLArray<int> m_removedCountGPU;
b3OpenCLArray<b3Int4> m_addedHostPairsGPU;
b3OpenCLArray<b3Int4> m_removedHostPairsGPU;
b3OpenCLArray<int> m_addedCountGPU;
b3OpenCLArray<int> m_removedCountGPU;
int m_currentBuffer;
int m_currentBuffer;
public:
b3OpenCLArray<int> m_pairCount;
b3OpenCLArray<b3SapAabb> m_allAabbsGPU;
b3AlignedObjectArray<b3SapAabb> m_allAabbsCPU;
b3OpenCLArray<b3SapAabb> m_allAabbsGPU;
b3AlignedObjectArray<b3SapAabb> m_allAabbsCPU;
virtual b3OpenCLArray<b3SapAabb>& getAllAabbsGPU()
virtual b3OpenCLArray<b3SapAabb>& getAllAabbsGPU()
{
return m_allAabbsGPU;
}
virtual b3AlignedObjectArray<b3SapAabb>& getAllAabbsCPU()
virtual b3AlignedObjectArray<b3SapAabb>& getAllAabbsCPU()
{
return m_allAabbsCPU;
}
b3OpenCLArray<b3Vector3> m_sum;
b3OpenCLArray<b3Vector3> m_sum2;
b3OpenCLArray<b3Vector3> m_dst;
b3OpenCLArray<b3Vector3> m_sum;
b3OpenCLArray<b3Vector3> m_sum2;
b3OpenCLArray<b3Vector3> m_dst;
b3OpenCLArray<int> m_smallAabbsMappingGPU;
b3OpenCLArray<int> m_smallAabbsMappingGPU;
b3AlignedObjectArray<int> m_smallAabbsMappingCPU;
b3OpenCLArray<int> m_largeAabbsMappingGPU;
b3OpenCLArray<int> m_largeAabbsMappingGPU;
b3AlignedObjectArray<int> m_largeAabbsMappingCPU;
b3OpenCLArray<b3Int4> m_overlappingPairs;
b3OpenCLArray<b3Int4> m_overlappingPairs;
//temporary gpu work memory
b3OpenCLArray<b3SortData> m_gpuSmallSortData;
b3OpenCLArray<b3SapAabb> m_gpuSmallSortedAabbs;
b3OpenCLArray<b3SortData> m_gpuSmallSortData;
b3OpenCLArray<b3SapAabb> m_gpuSmallSortedAabbs;
class b3PrefixScanFloat4CL* m_prefixScanFloat4;
class b3PrefixScanFloat4CL* m_prefixScanFloat4;
enum b3GpuSapKernelType
{
B3_GPU_SAP_KERNEL_BRUTE_FORCE_CPU=1,
B3_GPU_SAP_KERNEL_BRUTE_FORCE_CPU = 1,
B3_GPU_SAP_KERNEL_BRUTE_FORCE_GPU,
B3_GPU_SAP_KERNEL_ORIGINAL,
B3_GPU_SAP_KERNEL_BARRIER,
B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY
};
b3GpuSapBroadphase(cl_context ctx,cl_device_id device, cl_command_queue q , b3GpuSapKernelType kernelType=B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY);
b3GpuSapBroadphase(cl_context ctx, cl_device_id device, cl_command_queue q, b3GpuSapKernelType kernelType = B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY);
virtual ~b3GpuSapBroadphase();
static b3GpuBroadphaseInterface* CreateFuncBruteForceCpu(cl_context ctx,cl_device_id device, cl_command_queue q)
static b3GpuBroadphaseInterface* CreateFuncBruteForceCpu(cl_context ctx, cl_device_id device, cl_command_queue q)
{
return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_BRUTE_FORCE_CPU);
return new b3GpuSapBroadphase(ctx, device, q, B3_GPU_SAP_KERNEL_BRUTE_FORCE_CPU);
}
static b3GpuBroadphaseInterface* CreateFuncBruteForceGpu(cl_context ctx,cl_device_id device, cl_command_queue q)
static b3GpuBroadphaseInterface* CreateFuncBruteForceGpu(cl_context ctx, cl_device_id device, cl_command_queue q)
{
return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_BRUTE_FORCE_GPU);
return new b3GpuSapBroadphase(ctx, device, q, B3_GPU_SAP_KERNEL_BRUTE_FORCE_GPU);
}
static b3GpuBroadphaseInterface* CreateFuncOriginal(cl_context ctx,cl_device_id device, cl_command_queue q)
static b3GpuBroadphaseInterface* CreateFuncOriginal(cl_context ctx, cl_device_id device, cl_command_queue q)
{
return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_ORIGINAL);
return new b3GpuSapBroadphase(ctx, device, q, B3_GPU_SAP_KERNEL_ORIGINAL);
}
static b3GpuBroadphaseInterface* CreateFuncBarrier(cl_context ctx,cl_device_id device, cl_command_queue q)
static b3GpuBroadphaseInterface* CreateFuncBarrier(cl_context ctx, cl_device_id device, cl_command_queue q)
{
return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_BARRIER);
return new b3GpuSapBroadphase(ctx, device, q, B3_GPU_SAP_KERNEL_BARRIER);
}
static b3GpuBroadphaseInterface* CreateFuncLocalMemory(cl_context ctx,cl_device_id device, cl_command_queue q)
static b3GpuBroadphaseInterface* CreateFuncLocalMemory(cl_context ctx, cl_device_id device, cl_command_queue q)
{
return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY);
return new b3GpuSapBroadphase(ctx, device, q, B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY);
}
virtual void calculateOverlappingPairs(int maxPairs);
virtual void calculateOverlappingPairsHost(int maxPairs);
void reset();
virtual void calculateOverlappingPairs(int maxPairs);
virtual void calculateOverlappingPairsHost(int maxPairs);
void reset();
void init3dSap();
virtual void calculateOverlappingPairsHostIncremental3Sap();
virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask);
virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask);
virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
//call writeAabbsToGpu after done making all changes (createProxy etc)
virtual void writeAabbsToGpu();
virtual cl_mem getAabbBufferWS();
virtual int getNumOverlap();
virtual cl_mem getOverlappingPairBuffer();
virtual cl_mem getAabbBufferWS();
virtual int getNumOverlap();
virtual cl_mem getOverlappingPairBuffer();
virtual b3OpenCLArray<b3Int4>& getOverlappingPairsGPU();
virtual b3OpenCLArray<int>& getSmallAabbIndicesGPU();
virtual b3OpenCLArray<int>& getLargeAabbIndicesGPU();
};
#endif //B3_GPU_SAP_BROADPHASE_H
#endif //B3_GPU_SAP_BROADPHASE_H

9
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h vendored
View File

@ -5,10 +5,9 @@
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
///just make sure that the b3Aabb is 16-byte aligned
B3_ATTRIBUTE_ALIGNED16(struct) b3SapAabb : public b3Aabb
{
B3_ATTRIBUTE_ALIGNED16(struct)
b3SapAabb : public b3Aabb{
};
};
#endif //B3_SAP_AABB_H
#endif //B3_SAP_AABB_H

395
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphaseKernels.h vendored
View File

@ -1,199 +1,198 @@
//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
static const char* gridBroadphaseCL= \
"int getPosHash(int4 gridPos, __global float4* pParams)\n"
"{\n"
" int4 gridDim = *((__global int4*)(pParams + 1));\n"
" gridPos.x &= gridDim.x - 1;\n"
" gridPos.y &= gridDim.y - 1;\n"
" gridPos.z &= gridDim.z - 1;\n"
" int hash = gridPos.z * gridDim.y * gridDim.x + gridPos.y * gridDim.x + gridPos.x;\n"
" return hash;\n"
"} \n"
"int4 getGridPos(float4 worldPos, __global float4* pParams)\n"
"{\n"
" int4 gridPos;\n"
" int4 gridDim = *((__global int4*)(pParams + 1));\n"
" gridPos.x = (int)floor(worldPos.x * pParams[0].x) & (gridDim.x - 1);\n"
" gridPos.y = (int)floor(worldPos.y * pParams[0].y) & (gridDim.y - 1);\n"
" gridPos.z = (int)floor(worldPos.z * pParams[0].z) & (gridDim.z - 1);\n"
" return gridPos;\n"
"}\n"
"// calculate grid hash value for each body using its AABB\n"
"__kernel void kCalcHashAABB(int numObjects, __global float4* allpAABB, __global const int* smallAabbMapping, __global int2* pHash, __global float4* pParams )\n"
"{\n"
" int index = get_global_id(0);\n"
" if(index >= numObjects)\n"
" {\n"
" return;\n"
" }\n"
" float4 bbMin = allpAABB[smallAabbMapping[index]*2];\n"
" float4 bbMax = allpAABB[smallAabbMapping[index]*2 + 1];\n"
" float4 pos;\n"
" pos.x = (bbMin.x + bbMax.x) * 0.5f;\n"
" pos.y = (bbMin.y + bbMax.y) * 0.5f;\n"
" pos.z = (bbMin.z + bbMax.z) * 0.5f;\n"
" pos.w = 0.f;\n"
" // get address in grid\n"
" int4 gridPos = getGridPos(pos, pParams);\n"
" int gridHash = getPosHash(gridPos, pParams);\n"
" // store grid hash and body index\n"
" int2 hashVal;\n"
" hashVal.x = gridHash;\n"
" hashVal.y = index;\n"
" pHash[index] = hashVal;\n"
"}\n"
"__kernel void kClearCellStart( int numCells, \n"
" __global int* pCellStart )\n"
"{\n"
" int index = get_global_id(0);\n"
" if(index >= numCells)\n"
" {\n"
" return;\n"
" }\n"
" pCellStart[index] = -1;\n"
"}\n"
"__kernel void kFindCellStart(int numObjects, __global int2* pHash, __global int* cellStart )\n"
"{\n"
" __local int sharedHash[513];\n"
" int index = get_global_id(0);\n"
" int2 sortedData;\n"
" if(index < numObjects)\n"
" {\n"
" sortedData = pHash[index];\n"
" // Load hash data into shared memory so that we can look \n"
" // at neighboring body's hash value without loading\n"
" // two hash values per thread\n"
" sharedHash[get_local_id(0) + 1] = sortedData.x;\n"
" if((index > 0) && (get_local_id(0) == 0))\n"
" {\n"
" // first thread in block must load neighbor body hash\n"
" sharedHash[0] = pHash[index-1].x;\n"
" }\n"
" }\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" if(index < numObjects)\n"
" {\n"
" if((index == 0) || (sortedData.x != sharedHash[get_local_id(0)]))\n"
" {\n"
" cellStart[sortedData.x] = index;\n"
" }\n"
" }\n"
"}\n"
"int testAABBOverlap(float4 min0, float4 max0, float4 min1, float4 max1)\n"
"{\n"
" return (min0.x <= max1.x)&& (min1.x <= max0.x) && \n"
" (min0.y <= max1.y)&& (min1.y <= max0.y) && \n"
" (min0.z <= max1.z)&& (min1.z <= max0.z); \n"
"}\n"
"//search for AABB 'index' against other AABBs' in this cell\n"
"void findPairsInCell( int numObjects,\n"
" int4 gridPos,\n"
" int index,\n"
" __global int2* pHash,\n"
" __global int* pCellStart,\n"
" __global float4* allpAABB, \n"
" __global const int* smallAabbMapping,\n"
" __global float4* pParams,\n"
" volatile __global int* pairCount,\n"
" __global int4* pPairBuff2,\n"
" int maxPairs\n"
" )\n"
"{\n"
" int4 pGridDim = *((__global int4*)(pParams + 1));\n"
" int maxBodiesPerCell = pGridDim.w;\n"
" int gridHash = getPosHash(gridPos, pParams);\n"
" // get start of bucket for this cell\n"
" int bucketStart = pCellStart[gridHash];\n"
" if (bucketStart == -1)\n"
" {\n"
" return; // cell empty\n"
" }\n"
" // iterate over bodies in this cell\n"
" int2 sortedData = pHash[index];\n"
" int unsorted_indx = sortedData.y;\n"
" float4 min0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0]; \n"
" float4 max0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1];\n"
" int handleIndex = as_int(min0.w);\n"
" \n"
" int bucketEnd = bucketStart + maxBodiesPerCell;\n"
" bucketEnd = (bucketEnd > numObjects) ? numObjects : bucketEnd;\n"
" for(int index2 = bucketStart; index2 < bucketEnd; index2++) \n"
" {\n"
" int2 cellData = pHash[index2];\n"
" if (cellData.x != gridHash)\n"
" {\n"
" break; // no longer in same bucket\n"
" }\n"
" int unsorted_indx2 = cellData.y;\n"
" //if (unsorted_indx2 < unsorted_indx) // check not colliding with self\n"
" if (unsorted_indx2 != unsorted_indx) // check not colliding with self\n"
" { \n"
" float4 min1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 0];\n"
" float4 max1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 1];\n"
" if(testAABBOverlap(min0, max0, min1, max1))\n"
" {\n"
" if (pairCount)\n"
" {\n"
" int handleIndex2 = as_int(min1.w);\n"
" if (handleIndex<handleIndex2)\n"
" {\n"
" int curPair = atomic_add(pairCount,1);\n"
" if (curPair<maxPairs)\n"
" {\n"
" int4 newpair;\n"
" newpair.x = handleIndex;\n"
" newpair.y = handleIndex2;\n"
" newpair.z = -1;\n"
" newpair.w = -1;\n"
" pPairBuff2[curPair] = newpair;\n"
" }\n"
" }\n"
" \n"
" }\n"
" }\n"
" }\n"
" }\n"
"}\n"
"__kernel void kFindOverlappingPairs( int numObjects,\n"
" __global float4* allpAABB, \n"
" __global const int* smallAabbMapping,\n"
" __global int2* pHash, \n"
" __global int* pCellStart, \n"
" __global float4* pParams ,\n"
" volatile __global int* pairCount,\n"
" __global int4* pPairBuff2,\n"
" int maxPairs\n"
" )\n"
"{\n"
" int index = get_global_id(0);\n"
" if(index >= numObjects)\n"
" {\n"
" return;\n"
" }\n"
" int2 sortedData = pHash[index];\n"
" int unsorted_indx = sortedData.y;\n"
" float4 bbMin = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0];\n"
" float4 bbMax = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1];\n"
" float4 pos;\n"
" pos.x = (bbMin.x + bbMax.x) * 0.5f;\n"
" pos.y = (bbMin.y + bbMax.y) * 0.5f;\n"
" pos.z = (bbMin.z + bbMax.z) * 0.5f;\n"
" // get address in grid\n"
" int4 gridPosA = getGridPos(pos, pParams);\n"
" int4 gridPosB; \n"
" // examine only neighbouring cells\n"
" for(int z=-1; z<=1; z++) \n"
" {\n"
" gridPosB.z = gridPosA.z + z;\n"
" for(int y=-1; y<=1; y++) \n"
" {\n"
" gridPosB.y = gridPosA.y + y;\n"
" for(int x=-1; x<=1; x++) \n"
" {\n"
" gridPosB.x = gridPosA.x + x;\n"
" findPairsInCell(numObjects, gridPosB, index, pHash, pCellStart, allpAABB,smallAabbMapping, pParams, pairCount,pPairBuff2, maxPairs);\n"
" }\n"
" }\n"
" }\n"
"}\n"
;
static const char* gridBroadphaseCL =
"int getPosHash(int4 gridPos, __global float4* pParams)\n"
"{\n"
" int4 gridDim = *((__global int4*)(pParams + 1));\n"
" gridPos.x &= gridDim.x - 1;\n"
" gridPos.y &= gridDim.y - 1;\n"
" gridPos.z &= gridDim.z - 1;\n"
" int hash = gridPos.z * gridDim.y * gridDim.x + gridPos.y * gridDim.x + gridPos.x;\n"
" return hash;\n"
"} \n"
"int4 getGridPos(float4 worldPos, __global float4* pParams)\n"
"{\n"
" int4 gridPos;\n"
" int4 gridDim = *((__global int4*)(pParams + 1));\n"
" gridPos.x = (int)floor(worldPos.x * pParams[0].x) & (gridDim.x - 1);\n"
" gridPos.y = (int)floor(worldPos.y * pParams[0].y) & (gridDim.y - 1);\n"
" gridPos.z = (int)floor(worldPos.z * pParams[0].z) & (gridDim.z - 1);\n"
" return gridPos;\n"
"}\n"
"// calculate grid hash value for each body using its AABB\n"
"__kernel void kCalcHashAABB(int numObjects, __global float4* allpAABB, __global const int* smallAabbMapping, __global int2* pHash, __global float4* pParams )\n"
"{\n"
" int index = get_global_id(0);\n"
" if(index >= numObjects)\n"
" {\n"
" return;\n"
" }\n"
" float4 bbMin = allpAABB[smallAabbMapping[index]*2];\n"
" float4 bbMax = allpAABB[smallAabbMapping[index]*2 + 1];\n"
" float4 pos;\n"
" pos.x = (bbMin.x + bbMax.x) * 0.5f;\n"
" pos.y = (bbMin.y + bbMax.y) * 0.5f;\n"
" pos.z = (bbMin.z + bbMax.z) * 0.5f;\n"
" pos.w = 0.f;\n"
" // get address in grid\n"
" int4 gridPos = getGridPos(pos, pParams);\n"
" int gridHash = getPosHash(gridPos, pParams);\n"
" // store grid hash and body index\n"
" int2 hashVal;\n"
" hashVal.x = gridHash;\n"
" hashVal.y = index;\n"
" pHash[index] = hashVal;\n"
"}\n"
"__kernel void kClearCellStart( int numCells, \n"
" __global int* pCellStart )\n"
"{\n"
" int index = get_global_id(0);\n"
" if(index >= numCells)\n"
" {\n"
" return;\n"
" }\n"
" pCellStart[index] = -1;\n"
"}\n"
"__kernel void kFindCellStart(int numObjects, __global int2* pHash, __global int* cellStart )\n"
"{\n"
" __local int sharedHash[513];\n"
" int index = get_global_id(0);\n"
" int2 sortedData;\n"
" if(index < numObjects)\n"
" {\n"
" sortedData = pHash[index];\n"
" // Load hash data into shared memory so that we can look \n"
" // at neighboring body's hash value without loading\n"
" // two hash values per thread\n"
" sharedHash[get_local_id(0) + 1] = sortedData.x;\n"
" if((index > 0) && (get_local_id(0) == 0))\n"
" {\n"
" // first thread in block must load neighbor body hash\n"
" sharedHash[0] = pHash[index-1].x;\n"
" }\n"
" }\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" if(index < numObjects)\n"
" {\n"
" if((index == 0) || (sortedData.x != sharedHash[get_local_id(0)]))\n"
" {\n"
" cellStart[sortedData.x] = index;\n"
" }\n"
" }\n"
"}\n"
"int testAABBOverlap(float4 min0, float4 max0, float4 min1, float4 max1)\n"
"{\n"
" return (min0.x <= max1.x)&& (min1.x <= max0.x) && \n"
" (min0.y <= max1.y)&& (min1.y <= max0.y) && \n"
" (min0.z <= max1.z)&& (min1.z <= max0.z); \n"
"}\n"
"//search for AABB 'index' against other AABBs' in this cell\n"
"void findPairsInCell( int numObjects,\n"
" int4 gridPos,\n"
" int index,\n"
" __global int2* pHash,\n"
" __global int* pCellStart,\n"
" __global float4* allpAABB, \n"
" __global const int* smallAabbMapping,\n"
" __global float4* pParams,\n"
" volatile __global int* pairCount,\n"
" __global int4* pPairBuff2,\n"
" int maxPairs\n"
" )\n"
"{\n"
" int4 pGridDim = *((__global int4*)(pParams + 1));\n"
" int maxBodiesPerCell = pGridDim.w;\n"
" int gridHash = getPosHash(gridPos, pParams);\n"
" // get start of bucket for this cell\n"
" int bucketStart = pCellStart[gridHash];\n"
" if (bucketStart == -1)\n"
" {\n"
" return; // cell empty\n"
" }\n"
" // iterate over bodies in this cell\n"
" int2 sortedData = pHash[index];\n"
" int unsorted_indx = sortedData.y;\n"
" float4 min0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0]; \n"
" float4 max0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1];\n"
" int handleIndex = as_int(min0.w);\n"
" \n"
" int bucketEnd = bucketStart + maxBodiesPerCell;\n"
" bucketEnd = (bucketEnd > numObjects) ? numObjects : bucketEnd;\n"
" for(int index2 = bucketStart; index2 < bucketEnd; index2++) \n"
" {\n"
" int2 cellData = pHash[index2];\n"
" if (cellData.x != gridHash)\n"
" {\n"
" break; // no longer in same bucket\n"
" }\n"
" int unsorted_indx2 = cellData.y;\n"
" //if (unsorted_indx2 < unsorted_indx) // check not colliding with self\n"
" if (unsorted_indx2 != unsorted_indx) // check not colliding with self\n"
" { \n"
" float4 min1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 0];\n"
" float4 max1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 1];\n"
" if(testAABBOverlap(min0, max0, min1, max1))\n"
" {\n"
" if (pairCount)\n"
" {\n"
" int handleIndex2 = as_int(min1.w);\n"
" if (handleIndex<handleIndex2)\n"
" {\n"
" int curPair = atomic_add(pairCount,1);\n"
" if (curPair<maxPairs)\n"
" {\n"
" int4 newpair;\n"
" newpair.x = handleIndex;\n"
" newpair.y = handleIndex2;\n"
" newpair.z = -1;\n"
" newpair.w = -1;\n"
" pPairBuff2[curPair] = newpair;\n"
" }\n"
" }\n"
" \n"
" }\n"
" }\n"
" }\n"
" }\n"
"}\n"
"__kernel void kFindOverlappingPairs( int numObjects,\n"
" __global float4* allpAABB, \n"
" __global const int* smallAabbMapping,\n"
" __global int2* pHash, \n"
" __global int* pCellStart, \n"
" __global float4* pParams ,\n"
" volatile __global int* pairCount,\n"
" __global int4* pPairBuff2,\n"
" int maxPairs\n"
" )\n"
"{\n"
" int index = get_global_id(0);\n"
" if(index >= numObjects)\n"
" {\n"
" return;\n"
" }\n"
" int2 sortedData = pHash[index];\n"
" int unsorted_indx = sortedData.y;\n"
" float4 bbMin = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0];\n"
" float4 bbMax = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1];\n"
" float4 pos;\n"
" pos.x = (bbMin.x + bbMax.x) * 0.5f;\n"
" pos.y = (bbMin.y + bbMax.y) * 0.5f;\n"
" pos.z = (bbMin.z + bbMax.z) * 0.5f;\n"
" // get address in grid\n"
" int4 gridPosA = getGridPos(pos, pParams);\n"
" int4 gridPosB; \n"
" // examine only neighbouring cells\n"
" for(int z=-1; z<=1; z++) \n"
" {\n"
" gridPosB.z = gridPosA.z + z;\n"
" for(int y=-1; y<=1; y++) \n"
" {\n"
" gridPosB.y = gridPosA.y + y;\n"
" for(int x=-1; x<=1; x++) \n"
" {\n"
" gridPosB.x = gridPosA.x + x;\n"
" findPairsInCell(numObjects, gridPosB, index, pHash, pCellStart, allpAABB,smallAabbMapping, pParams, pairCount,pPairBuff2, maxPairs);\n"
" }\n"
" }\n"
" }\n"
"}\n";

1455
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvhKernels.h vendored
View File

File diff suppressed because it is too large Load Diff

681
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/kernels/sapKernels.h vendored
View File

@ -1,342 +1,341 @@
//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
static const char* sapCL= \
"/*\n"
"Copyright (c) 2012 Advanced Micro Devices, Inc. \n"
"This software is provided 'as-is', without any express or implied warranty.\n"
"In no event will the authors be held liable for any damages arising from the use of this software.\n"
"Permission is granted to anyone to use this software for any purpose, \n"
"including commercial applications, and to alter it and redistribute it freely, \n"
"subject to the following restrictions:\n"
"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n"
"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
"3. This notice may not be removed or altered from any source distribution.\n"
"*/\n"
"//Originally written by Erwin Coumans\n"
"#define NEW_PAIR_MARKER -1\n"
"typedef struct \n"
"{\n"
" union\n"
" {\n"
" float4 m_min;\n"
" float m_minElems[4];\n"
" int m_minIndices[4];\n"
" };\n"
" union\n"
" {\n"
" float4 m_max;\n"
" float m_maxElems[4];\n"
" int m_maxIndices[4];\n"
" };\n"
"} btAabbCL;\n"
"/// conservative test for overlap between two aabbs\n"
"bool TestAabbAgainstAabb2(const btAabbCL* aabb1, __local const btAabbCL* aabb2);\n"
"bool TestAabbAgainstAabb2(const btAabbCL* aabb1, __local const btAabbCL* aabb2)\n"
"{\n"
" bool overlap = true;\n"
" overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
" overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
" overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
" return overlap;\n"
"}\n"
"bool TestAabbAgainstAabb2GlobalGlobal(__global const btAabbCL* aabb1, __global const btAabbCL* aabb2);\n"
"bool TestAabbAgainstAabb2GlobalGlobal(__global const btAabbCL* aabb1, __global const btAabbCL* aabb2)\n"
"{\n"
" bool overlap = true;\n"
" overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
" overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
" overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
" return overlap;\n"
"}\n"
"bool TestAabbAgainstAabb2Global(const btAabbCL* aabb1, __global const btAabbCL* aabb2);\n"
"bool TestAabbAgainstAabb2Global(const btAabbCL* aabb1, __global const btAabbCL* aabb2)\n"
"{\n"
" bool overlap = true;\n"
" overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
" overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
" overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
" return overlap;\n"
"}\n"
"__kernel void computePairsKernelTwoArrays( __global const btAabbCL* unsortedAabbs, __global const int* unsortedAabbMapping, __global const int* unsortedAabbMapping2, volatile __global int4* pairsOut,volatile __global int* pairCount, int numUnsortedAabbs, int numUnSortedAabbs2, int axis, int maxPairs)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numUnsortedAabbs)\n"
" return;\n"
" int j = get_global_id(1);\n"
" if (j>=numUnSortedAabbs2)\n"
" return;\n"
" __global const btAabbCL* unsortedAabbPtr = &unsortedAabbs[unsortedAabbMapping[i]];\n"
" __global const btAabbCL* unsortedAabbPtr2 = &unsortedAabbs[unsortedAabbMapping2[j]];\n"
" if (TestAabbAgainstAabb2GlobalGlobal(unsortedAabbPtr,unsortedAabbPtr2))\n"
" {\n"
" int4 myPair;\n"
" \n"
" int xIndex = unsortedAabbPtr[0].m_minIndices[3];\n"
" int yIndex = unsortedAabbPtr2[0].m_minIndices[3];\n"
" if (xIndex>yIndex)\n"
" {\n"
" int tmp = xIndex;\n"
" xIndex=yIndex;\n"
" yIndex=tmp;\n"
" }\n"
" \n"
" myPair.x = xIndex;\n"
" myPair.y = yIndex;\n"
" myPair.z = NEW_PAIR_MARKER;\n"
" myPair.w = NEW_PAIR_MARKER;\n"
" int curPair = atomic_inc (pairCount);\n"
" if (curPair<maxPairs)\n"
" {\n"
" pairsOut[curPair] = myPair; //flush to main memory\n"
" }\n"
" }\n"
"}\n"
"__kernel void computePairsKernelBruteForce( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numObjects)\n"
" return;\n"
" for (int j=i+1;j<numObjects;j++)\n"
" {\n"
" if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j]))\n"
" {\n"
" int4 myPair;\n"
" myPair.x = aabbs[i].m_minIndices[3];\n"
" myPair.y = aabbs[j].m_minIndices[3];\n"
" myPair.z = NEW_PAIR_MARKER;\n"
" myPair.w = NEW_PAIR_MARKER;\n"
" int curPair = atomic_inc (pairCount);\n"
" if (curPair<maxPairs)\n"
" {\n"
" pairsOut[curPair] = myPair; //flush to main memory\n"
" }\n"
" }\n"
" }\n"
"}\n"
"__kernel void computePairsKernelOriginal( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numObjects)\n"
" return;\n"
" for (int j=i+1;j<numObjects;j++)\n"
" {\n"
" if(aabbs[i].m_maxElems[axis] < (aabbs[j].m_minElems[axis])) \n"
" {\n"
" break;\n"
" }\n"
" if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j]))\n"
" {\n"
" int4 myPair;\n"
" myPair.x = aabbs[i].m_minIndices[3];\n"
" myPair.y = aabbs[j].m_minIndices[3];\n"
" myPair.z = NEW_PAIR_MARKER;\n"
" myPair.w = NEW_PAIR_MARKER;\n"
" int curPair = atomic_inc (pairCount);\n"
" if (curPair<maxPairs)\n"
" {\n"
" pairsOut[curPair] = myPair; //flush to main memory\n"
" }\n"
" }\n"
" }\n"
"}\n"
"__kernel void computePairsKernelBarrier( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
"{\n"
" int i = get_global_id(0);\n"
" int localId = get_local_id(0);\n"
" __local int numActiveWgItems[1];\n"
" __local int breakRequest[1];\n"
" if (localId==0)\n"
" {\n"
" numActiveWgItems[0] = 0;\n"
" breakRequest[0] = 0;\n"
" }\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" atomic_inc(numActiveWgItems);\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" int localBreak = 0;\n"
" int j=i+1;\n"
" do\n"
" {\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" \n"
" if (j<numObjects)\n"
" {\n"
" if(aabbs[i].m_maxElems[axis] < (aabbs[j].m_minElems[axis])) \n"
" {\n"
" if (!localBreak)\n"
" {\n"
" atomic_inc(breakRequest);\n"
" localBreak = 1;\n"
" }\n"
" }\n"
" }\n"
" \n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" \n"
" if (j>=numObjects && !localBreak)\n"
" {\n"
" atomic_inc(breakRequest);\n"
" localBreak = 1;\n"
" }\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" \n"
" if (!localBreak)\n"
" {\n"
" if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j]))\n"
" {\n"
" int4 myPair;\n"
" myPair.x = aabbs[i].m_minIndices[3];\n"
" myPair.y = aabbs[j].m_minIndices[3];\n"
" myPair.z = NEW_PAIR_MARKER;\n"
" myPair.w = NEW_PAIR_MARKER;\n"
" int curPair = atomic_inc (pairCount);\n"
" if (curPair<maxPairs)\n"
" {\n"
" pairsOut[curPair] = myPair; //flush to main memory\n"
" }\n"
" }\n"
" }\n"
" j++;\n"
" } while (breakRequest[0]<numActiveWgItems[0]);\n"
"}\n"
"__kernel void computePairsKernelLocalSharedMemory( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
"{\n"
" int i = get_global_id(0);\n"
" int localId = get_local_id(0);\n"
" __local int numActiveWgItems[1];\n"
" __local int breakRequest[1];\n"
" __local btAabbCL localAabbs[128];// = aabbs[i];\n"
" \n"
" btAabbCL myAabb;\n"
" \n"
" myAabb = (i<numObjects)? aabbs[i]:aabbs[0];\n"
" float testValue = myAabb.m_maxElems[axis];\n"
" \n"
" if (localId==0)\n"
" {\n"
" numActiveWgItems[0] = 0;\n"
" breakRequest[0] = 0;\n"
" }\n"
" int localCount=0;\n"
" int block=0;\n"
" localAabbs[localId] = (i+block)<numObjects? aabbs[i+block] : aabbs[0];\n"
" localAabbs[localId+64] = (i+block+64)<numObjects? aabbs[i+block+64]: aabbs[0];\n"
" \n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" atomic_inc(numActiveWgItems);\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" int localBreak = 0;\n"
" \n"
" int j=i+1;\n"
" do\n"
" {\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" \n"
" if (j<numObjects)\n"
" {\n"
" if(testValue < (localAabbs[localCount+localId+1].m_minElems[axis])) \n"
" {\n"
" if (!localBreak)\n"
" {\n"
" atomic_inc(breakRequest);\n"
" localBreak = 1;\n"
" }\n"
" }\n"
" }\n"
" \n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" \n"
" if (j>=numObjects && !localBreak)\n"
" {\n"
" atomic_inc(breakRequest);\n"
" localBreak = 1;\n"
" }\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" \n"
" if (!localBreak)\n"
" {\n"
" if (TestAabbAgainstAabb2(&myAabb,&localAabbs[localCount+localId+1]))\n"
" {\n"
" int4 myPair;\n"
" myPair.x = myAabb.m_minIndices[3];\n"
" myPair.y = localAabbs[localCount+localId+1].m_minIndices[3];\n"
" myPair.z = NEW_PAIR_MARKER;\n"
" myPair.w = NEW_PAIR_MARKER;\n"
" int curPair = atomic_inc (pairCount);\n"
" if (curPair<maxPairs)\n"
" {\n"
" pairsOut[curPair] = myPair; //flush to main memory\n"
" }\n"
" }\n"
" }\n"
" \n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" localCount++;\n"
" if (localCount==64)\n"
" {\n"
" localCount = 0;\n"
" block+=64; \n"
" localAabbs[localId] = ((i+block)<numObjects) ? aabbs[i+block] : aabbs[0];\n"
" localAabbs[localId+64] = ((i+64+block)<numObjects) ? aabbs[i+block+64] : aabbs[0];\n"
" }\n"
" j++;\n"
" \n"
" } while (breakRequest[0]<numActiveWgItems[0]);\n"
" \n"
"}\n"
"//http://stereopsis.com/radix.html\n"
"unsigned int FloatFlip(float fl);\n"
"unsigned int FloatFlip(float fl)\n"
"{\n"
" unsigned int f = *(unsigned int*)&fl;\n"
" unsigned int mask = -(int)(f >> 31) | 0x80000000;\n"
" return f ^ mask;\n"
"}\n"
"float IFloatFlip(unsigned int f);\n"
"float IFloatFlip(unsigned int f)\n"
"{\n"
" unsigned int mask = ((f >> 31) - 1) | 0x80000000;\n"
" unsigned int fl = f ^ mask;\n"
" return *(float*)&fl;\n"
"}\n"
"__kernel void copyAabbsKernel( __global const btAabbCL* allAabbs, __global btAabbCL* destAabbs, int numObjects)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numObjects)\n"
" return;\n"
" int src = destAabbs[i].m_maxIndices[3];\n"
" destAabbs[i] = allAabbs[src];\n"
" destAabbs[i].m_maxIndices[3] = src;\n"
"}\n"
"__kernel void flipFloatKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, __global int2* sortData, int numObjects, int axis)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numObjects)\n"
" return;\n"
" \n"
" \n"
" sortData[i].x = FloatFlip(allAabbs[smallAabbMapping[i]].m_minElems[axis]);\n"
" sortData[i].y = i;\n"
" \n"
"}\n"
"__kernel void scatterKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, volatile __global const int2* sortData, __global btAabbCL* sortedAabbs, int numObjects)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numObjects)\n"
" return;\n"
" \n"
" sortedAabbs[i] = allAabbs[smallAabbMapping[sortData[i].y]];\n"
"}\n"
"__kernel void prepareSumVarianceKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, __global float4* sum, __global float4* sum2,int numAabbs)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numAabbs)\n"
" return;\n"
" \n"
" btAabbCL smallAabb = allAabbs[smallAabbMapping[i]];\n"
" \n"
" float4 s;\n"
" s = (smallAabb.m_max+smallAabb.m_min)*0.5f;\n"
" sum[i]=s;\n"
" sum2[i]=s*s; \n"
"}\n"
;
static const char* sapCL =
"/*\n"
"Copyright (c) 2012 Advanced Micro Devices, Inc. \n"
"This software is provided 'as-is', without any express or implied warranty.\n"
"In no event will the authors be held liable for any damages arising from the use of this software.\n"
"Permission is granted to anyone to use this software for any purpose, \n"
"including commercial applications, and to alter it and redistribute it freely, \n"
"subject to the following restrictions:\n"
"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n"
"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
"3. This notice may not be removed or altered from any source distribution.\n"
"*/\n"
"//Originally written by Erwin Coumans\n"
"#define NEW_PAIR_MARKER -1\n"
"typedef struct \n"
"{\n"
" union\n"
" {\n"
" float4 m_min;\n"
" float m_minElems[4];\n"
" int m_minIndices[4];\n"
" };\n"
" union\n"
" {\n"
" float4 m_max;\n"
" float m_maxElems[4];\n"
" int m_maxIndices[4];\n"
" };\n"
"} btAabbCL;\n"
"/// conservative test for overlap between two aabbs\n"
"bool TestAabbAgainstAabb2(const btAabbCL* aabb1, __local const btAabbCL* aabb2);\n"
"bool TestAabbAgainstAabb2(const btAabbCL* aabb1, __local const btAabbCL* aabb2)\n"
"{\n"
" bool overlap = true;\n"
" overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
" overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
" overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
" return overlap;\n"
"}\n"
"bool TestAabbAgainstAabb2GlobalGlobal(__global const btAabbCL* aabb1, __global const btAabbCL* aabb2);\n"
"bool TestAabbAgainstAabb2GlobalGlobal(__global const btAabbCL* aabb1, __global const btAabbCL* aabb2)\n"
"{\n"
" bool overlap = true;\n"
" overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
" overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
" overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
" return overlap;\n"
"}\n"
"bool TestAabbAgainstAabb2Global(const btAabbCL* aabb1, __global const btAabbCL* aabb2);\n"
"bool TestAabbAgainstAabb2Global(const btAabbCL* aabb1, __global const btAabbCL* aabb2)\n"
"{\n"
" bool overlap = true;\n"
" overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
" overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
" overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
" return overlap;\n"
"}\n"
"__kernel void computePairsKernelTwoArrays( __global const btAabbCL* unsortedAabbs, __global const int* unsortedAabbMapping, __global const int* unsortedAabbMapping2, volatile __global int4* pairsOut,volatile __global int* pairCount, int numUnsortedAabbs, int numUnSortedAabbs2, int axis, int maxPairs)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numUnsortedAabbs)\n"
" return;\n"
" int j = get_global_id(1);\n"
" if (j>=numUnSortedAabbs2)\n"
" return;\n"
" __global const btAabbCL* unsortedAabbPtr = &unsortedAabbs[unsortedAabbMapping[i]];\n"
" __global const btAabbCL* unsortedAabbPtr2 = &unsortedAabbs[unsortedAabbMapping2[j]];\n"
" if (TestAabbAgainstAabb2GlobalGlobal(unsortedAabbPtr,unsortedAabbPtr2))\n"
" {\n"
" int4 myPair;\n"
" \n"
" int xIndex = unsortedAabbPtr[0].m_minIndices[3];\n"
" int yIndex = unsortedAabbPtr2[0].m_minIndices[3];\n"
" if (xIndex>yIndex)\n"
" {\n"
" int tmp = xIndex;\n"
" xIndex=yIndex;\n"
" yIndex=tmp;\n"
" }\n"
" \n"
" myPair.x = xIndex;\n"
" myPair.y = yIndex;\n"
" myPair.z = NEW_PAIR_MARKER;\n"
" myPair.w = NEW_PAIR_MARKER;\n"
" int curPair = atomic_inc (pairCount);\n"
" if (curPair<maxPairs)\n"
" {\n"
" pairsOut[curPair] = myPair; //flush to main memory\n"
" }\n"
" }\n"
"}\n"
"__kernel void computePairsKernelBruteForce( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numObjects)\n"
" return;\n"
" for (int j=i+1;j<numObjects;j++)\n"
" {\n"
" if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j]))\n"
" {\n"
" int4 myPair;\n"
" myPair.x = aabbs[i].m_minIndices[3];\n"
" myPair.y = aabbs[j].m_minIndices[3];\n"
" myPair.z = NEW_PAIR_MARKER;\n"
" myPair.w = NEW_PAIR_MARKER;\n"
" int curPair = atomic_inc (pairCount);\n"
" if (curPair<maxPairs)\n"
" {\n"
" pairsOut[curPair] = myPair; //flush to main memory\n"
" }\n"
" }\n"
" }\n"
"}\n"
"__kernel void computePairsKernelOriginal( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numObjects)\n"
" return;\n"
" for (int j=i+1;j<numObjects;j++)\n"
" {\n"
" if(aabbs[i].m_maxElems[axis] < (aabbs[j].m_minElems[axis])) \n"
" {\n"
" break;\n"
" }\n"
" if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j]))\n"
" {\n"
" int4 myPair;\n"
" myPair.x = aabbs[i].m_minIndices[3];\n"
" myPair.y = aabbs[j].m_minIndices[3];\n"
" myPair.z = NEW_PAIR_MARKER;\n"
" myPair.w = NEW_PAIR_MARKER;\n"
" int curPair = atomic_inc (pairCount);\n"
" if (curPair<maxPairs)\n"
" {\n"
" pairsOut[curPair] = myPair; //flush to main memory\n"
" }\n"
" }\n"
" }\n"
"}\n"
"__kernel void computePairsKernelBarrier( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
"{\n"
" int i = get_global_id(0);\n"
" int localId = get_local_id(0);\n"
" __local int numActiveWgItems[1];\n"
" __local int breakRequest[1];\n"
" if (localId==0)\n"
" {\n"
" numActiveWgItems[0] = 0;\n"
" breakRequest[0] = 0;\n"
" }\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" atomic_inc(numActiveWgItems);\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" int localBreak = 0;\n"
" int j=i+1;\n"
" do\n"
" {\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" \n"
" if (j<numObjects)\n"
" {\n"
" if(aabbs[i].m_maxElems[axis] < (aabbs[j].m_minElems[axis])) \n"
" {\n"
" if (!localBreak)\n"
" {\n"
" atomic_inc(breakRequest);\n"
" localBreak = 1;\n"
" }\n"
" }\n"
" }\n"
" \n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" \n"
" if (j>=numObjects && !localBreak)\n"
" {\n"
" atomic_inc(breakRequest);\n"
" localBreak = 1;\n"
" }\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" \n"
" if (!localBreak)\n"
" {\n"
" if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j]))\n"
" {\n"
" int4 myPair;\n"
" myPair.x = aabbs[i].m_minIndices[3];\n"
" myPair.y = aabbs[j].m_minIndices[3];\n"
" myPair.z = NEW_PAIR_MARKER;\n"
" myPair.w = NEW_PAIR_MARKER;\n"
" int curPair = atomic_inc (pairCount);\n"
" if (curPair<maxPairs)\n"
" {\n"
" pairsOut[curPair] = myPair; //flush to main memory\n"
" }\n"
" }\n"
" }\n"
" j++;\n"
" } while (breakRequest[0]<numActiveWgItems[0]);\n"
"}\n"
"__kernel void computePairsKernelLocalSharedMemory( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
"{\n"
" int i = get_global_id(0);\n"
" int localId = get_local_id(0);\n"
" __local int numActiveWgItems[1];\n"
" __local int breakRequest[1];\n"
" __local btAabbCL localAabbs[128];// = aabbs[i];\n"
" \n"
" btAabbCL myAabb;\n"
" \n"
" myAabb = (i<numObjects)? aabbs[i]:aabbs[0];\n"
" float testValue = myAabb.m_maxElems[axis];\n"
" \n"
" if (localId==0)\n"
" {\n"
" numActiveWgItems[0] = 0;\n"
" breakRequest[0] = 0;\n"
" }\n"
" int localCount=0;\n"
" int block=0;\n"
" localAabbs[localId] = (i+block)<numObjects? aabbs[i+block] : aabbs[0];\n"
" localAabbs[localId+64] = (i+block+64)<numObjects? aabbs[i+block+64]: aabbs[0];\n"
" \n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" atomic_inc(numActiveWgItems);\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" int localBreak = 0;\n"
" \n"
" int j=i+1;\n"
" do\n"
" {\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" \n"
" if (j<numObjects)\n"
" {\n"
" if(testValue < (localAabbs[localCount+localId+1].m_minElems[axis])) \n"
" {\n"
" if (!localBreak)\n"
" {\n"
" atomic_inc(breakRequest);\n"
" localBreak = 1;\n"
" }\n"
" }\n"
" }\n"
" \n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" \n"
" if (j>=numObjects && !localBreak)\n"
" {\n"
" atomic_inc(breakRequest);\n"
" localBreak = 1;\n"
" }\n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" \n"
" if (!localBreak)\n"
" {\n"
" if (TestAabbAgainstAabb2(&myAabb,&localAabbs[localCount+localId+1]))\n"
" {\n"
" int4 myPair;\n"
" myPair.x = myAabb.m_minIndices[3];\n"
" myPair.y = localAabbs[localCount+localId+1].m_minIndices[3];\n"
" myPair.z = NEW_PAIR_MARKER;\n"
" myPair.w = NEW_PAIR_MARKER;\n"
" int curPair = atomic_inc (pairCount);\n"
" if (curPair<maxPairs)\n"
" {\n"
" pairsOut[curPair] = myPair; //flush to main memory\n"
" }\n"
" }\n"
" }\n"
" \n"
" barrier(CLK_LOCAL_MEM_FENCE);\n"
" localCount++;\n"
" if (localCount==64)\n"
" {\n"
" localCount = 0;\n"
" block+=64; \n"
" localAabbs[localId] = ((i+block)<numObjects) ? aabbs[i+block] : aabbs[0];\n"
" localAabbs[localId+64] = ((i+64+block)<numObjects) ? aabbs[i+block+64] : aabbs[0];\n"
" }\n"
" j++;\n"
" \n"
" } while (breakRequest[0]<numActiveWgItems[0]);\n"
" \n"
"}\n"
"//http://stereopsis.com/radix.html\n"
"unsigned int FloatFlip(float fl);\n"
"unsigned int FloatFlip(float fl)\n"
"{\n"
" unsigned int f = *(unsigned int*)&fl;\n"
" unsigned int mask = -(int)(f >> 31) | 0x80000000;\n"
" return f ^ mask;\n"
"}\n"
"float IFloatFlip(unsigned int f);\n"
"float IFloatFlip(unsigned int f)\n"
"{\n"
" unsigned int mask = ((f >> 31) - 1) | 0x80000000;\n"
" unsigned int fl = f ^ mask;\n"
" return *(float*)&fl;\n"
"}\n"
"__kernel void copyAabbsKernel( __global const btAabbCL* allAabbs, __global btAabbCL* destAabbs, int numObjects)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numObjects)\n"
" return;\n"
" int src = destAabbs[i].m_maxIndices[3];\n"
" destAabbs[i] = allAabbs[src];\n"
" destAabbs[i].m_maxIndices[3] = src;\n"
"}\n"
"__kernel void flipFloatKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, __global int2* sortData, int numObjects, int axis)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numObjects)\n"
" return;\n"
" \n"
" \n"
" sortData[i].x = FloatFlip(allAabbs[smallAabbMapping[i]].m_minElems[axis]);\n"
" sortData[i].y = i;\n"
" \n"
"}\n"
"__kernel void scatterKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, volatile __global const int2* sortData, __global btAabbCL* sortedAabbs, int numObjects)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numObjects)\n"
" return;\n"
" \n"
" sortedAabbs[i] = allAabbs[smallAabbMapping[sortData[i].y]];\n"
"}\n"
"__kernel void prepareSumVarianceKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, __global float4* sum, __global float4* sum2,int numAabbs)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numAabbs)\n"
" return;\n"
" \n"
" btAabbCL smallAabb = allAabbs[smallAabbMapping[i]];\n"
" \n"
" float4 s;\n"
" s = (smallAabb.m_max+smallAabb.m_min)*0.5f;\n"
" sum[i]=s;\n"
" sum2[i]=s*s; \n"
"}\n";

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