Autoformat selection for soft body, btDbvt related classes (no code change, just layout using Visual Studio)

This commit is contained in:
erwin.coumans
2008-10-15 18:39:27 +00:00
parent cac172d422
commit 60ce828419
20 changed files with 3941 additions and 3941 deletions

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

View File

@@ -26,19 +26,19 @@ subject to the following restrictions:
#if DBVT_BP_PROFILE #if DBVT_BP_PROFILE
struct ProfileScope struct ProfileScope
{ {
__forceinline ProfileScope(btClock& clock,unsigned long& value) : __forceinline ProfileScope(btClock& clock,unsigned long& value) :
m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
{ {
} }
__forceinline ~ProfileScope() __forceinline ~ProfileScope()
{ {
(*m_value)+=m_clock->getTimeMicroseconds()-m_base; (*m_value)+=m_clock->getTimeMicroseconds()-m_base;
} }
btClock* m_clock; btClock* m_clock;
unsigned long* m_value; unsigned long* m_value;
unsigned long m_base; unsigned long m_base;
}; };
#define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) #define SPC(_value_) ProfileScope spc_scope(m_clock,_value_)
#else #else
#define SPC(_value_) #define SPC(_value_)
@@ -52,35 +52,35 @@ struct ProfileScope
template <typename T> template <typename T>
static inline void listappend(T* item,T*& list) static inline void listappend(T* item,T*& list)
{ {
item->links[0]=0; item->links[0]=0;
item->links[1]=list; item->links[1]=list;
if(list) list->links[0]=item; if(list) list->links[0]=item;
list=item; list=item;
} }
// //
template <typename T> template <typename T>
static inline void listremove(T* item,T*& list) static inline void listremove(T* item,T*& list)
{ {
if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
if(item->links[1]) item->links[1]->links[0]=item->links[0]; if(item->links[1]) item->links[1]->links[0]=item->links[0];
} }
// //
template <typename T> template <typename T>
static inline int listcount(T* root) static inline int listcount(T* root)
{ {
int n=0; int n=0;
while(root) { ++n;root=root->links[1]; } while(root) { ++n;root=root->links[1]; }
return(n); return(n);
} }
// //
template <typename T> template <typename T>
static inline void clear(T& value) static inline void clear(T& value)
{ {
static const struct ZeroDummy : T {} zerodummy; static const struct ZeroDummy : T {} zerodummy;
value=zerodummy; value=zerodummy;
} }
// //
@@ -90,25 +90,25 @@ value=zerodummy;
/* Tree collider */ /* Tree collider */
struct btDbvtTreeCollider : btDbvt::ICollide struct btDbvtTreeCollider : btDbvt::ICollide
{ {
btDbvtBroadphase* pbp; btDbvtBroadphase* pbp;
btDbvtProxy* proxy; btDbvtProxy* proxy;
btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
void Process(const btDbvtNode* na,const btDbvtNode* nb) void Process(const btDbvtNode* na,const btDbvtNode* nb)
{ {
if(na!=nb) if(na!=nb)
{ {
btDbvtProxy* pa=(btDbvtProxy*)na->data; btDbvtProxy* pa=(btDbvtProxy*)na->data;
btDbvtProxy* pb=(btDbvtProxy*)nb->data; btDbvtProxy* pb=(btDbvtProxy*)nb->data;
#if DBVT_BP_SORTPAIRS #if DBVT_BP_SORTPAIRS
if(pa>pb) btSwap(pa,pb); if(pa>pb) btSwap(pa,pb);
#endif #endif
pbp->m_paircache->addOverlappingPair(pa,pb); pbp->m_paircache->addOverlappingPair(pa,pb);
++pbp->m_newpairs; ++pbp->m_newpairs;
} }
} }
void Process(const btDbvtNode* n) void Process(const btDbvtNode* n)
{ {
Process(n,proxy->leaf); Process(n,proxy->leaf);
} }
}; };
@@ -119,88 +119,88 @@ void Process(const btDbvtNode* n)
// //
btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
{ {
m_deferedcollide = false; m_deferedcollide = false;
m_needcleanup = true; m_needcleanup = true;
m_releasepaircache = (paircache!=0)?false:true; m_releasepaircache = (paircache!=0)?false:true;
m_prediction = 1/(btScalar)2; m_prediction = 1/(btScalar)2;
m_stageCurrent = 0; m_stageCurrent = 0;
m_fixedleft = 0; m_fixedleft = 0;
m_fupdates = 1; m_fupdates = 1;
m_dupdates = 0; m_dupdates = 0;
m_cupdates = 10; m_cupdates = 10;
m_newpairs = 1; m_newpairs = 1;
m_updates_call = 0; m_updates_call = 0;
m_updates_done = 0; m_updates_done = 0;
m_updates_ratio = 0; m_updates_ratio = 0;
m_paircache = paircache? m_paircache = paircache?
paircache : paircache :
new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
m_gid = 0; m_gid = 0;
m_pid = 0; m_pid = 0;
m_cid = 0; m_cid = 0;
for(int i=0;i<=STAGECOUNT;++i) for(int i=0;i<=STAGECOUNT;++i)
{ {
m_stageRoots[i]=0; m_stageRoots[i]=0;
} }
#if DBVT_BP_PROFILE #if DBVT_BP_PROFILE
clear(m_profiling); clear(m_profiling);
#endif #endif
} }
// //
btDbvtBroadphase::~btDbvtBroadphase() btDbvtBroadphase::~btDbvtBroadphase()
{ {
if(m_releasepaircache) if(m_releasepaircache)
{ {
m_paircache->~btOverlappingPairCache(); m_paircache->~btOverlappingPairCache();
btAlignedFree(m_paircache); btAlignedFree(m_paircache);
} }
} }
// //
btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin, btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin,
const btVector3& aabbMax, const btVector3& aabbMax,
int /*shapeType*/, int /*shapeType*/,
void* userPtr, void* userPtr,
short int collisionFilterGroup, short int collisionFilterGroup,
short int collisionFilterMask, short int collisionFilterMask,
btDispatcher* /*dispatcher*/, btDispatcher* /*dispatcher*/,
void* /*multiSapProxy*/) void* /*multiSapProxy*/)
{ {
btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( aabbMin,aabbMax,userPtr, btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( aabbMin,aabbMax,userPtr,
collisionFilterGroup, collisionFilterGroup,
collisionFilterMask); collisionFilterMask);
btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax);
//bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); //bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax);
proxy->stage = m_stageCurrent; proxy->stage = m_stageCurrent;
proxy->m_uniqueId = ++m_gid; proxy->m_uniqueId = ++m_gid;
proxy->leaf = m_sets[0].insert(aabb,proxy); proxy->leaf = m_sets[0].insert(aabb,proxy);
listappend(proxy,m_stageRoots[m_stageCurrent]); listappend(proxy,m_stageRoots[m_stageCurrent]);
if(!m_deferedcollide) if(!m_deferedcollide)
{ {
btDbvtTreeCollider collider(this); btDbvtTreeCollider collider(this);
collider.proxy=proxy; collider.proxy=proxy;
btDbvt::collideTV(m_sets[0].m_root,aabb,collider); btDbvt::collideTV(m_sets[0].m_root,aabb,collider);
btDbvt::collideTV(m_sets[1].m_root,aabb,collider); btDbvt::collideTV(m_sets[1].m_root,aabb,collider);
} }
return(proxy); return(proxy);
} }
// //
void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy, void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
btDispatcher* dispatcher) btDispatcher* dispatcher)
{ {
btDbvtProxy* proxy=(btDbvtProxy*)absproxy; btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
if(proxy->stage==STAGECOUNT) if(proxy->stage==STAGECOUNT)
m_sets[1].remove(proxy->leaf); m_sets[1].remove(proxy->leaf);
else else
m_sets[0].remove(proxy->leaf); m_sets[0].remove(proxy->leaf);
listremove(proxy,m_stageRoots[proxy->stage]); listremove(proxy,m_stageRoots[proxy->stage]);
m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
btAlignedFree(proxy); btAlignedFree(proxy);
m_needcleanup=true; m_needcleanup=true;
} }
void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const
@@ -230,79 +230,79 @@ void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
BroadphaseRayTester callback(rayCallback); BroadphaseRayTester callback(rayCallback);
m_sets[0].rayTest( m_sets[0].m_root, m_sets[0].rayTest( m_sets[0].m_root,
rayFrom, rayFrom,
rayTo, rayTo,
callback); callback);
m_sets[1].rayTest( m_sets[1].m_root, m_sets[1].rayTest( m_sets[1].m_root,
rayFrom, rayFrom,
rayTo, rayTo,
callback); callback);
} }
// //
void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy,
const btVector3& aabbMin, const btVector3& aabbMin,
const btVector3& aabbMax, const btVector3& aabbMax,
btDispatcher* /*dispatcher*/) btDispatcher* /*dispatcher*/)
{ {
btDbvtProxy* proxy=(btDbvtProxy*)absproxy; btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
#if DBVT_BP_PREVENTFALSEUPDATE #if DBVT_BP_PREVENTFALSEUPDATE
if(NotEqual(aabb,proxy->leaf->volume)) if(NotEqual(aabb,proxy->leaf->volume))
#endif #endif
{ {
bool docollide=false; bool docollide=false;
if(proxy->stage==STAGECOUNT) if(proxy->stage==STAGECOUNT)
{/* fixed -> dynamic set */ {/* fixed -> dynamic set */
m_sets[1].remove(proxy->leaf); m_sets[1].remove(proxy->leaf);
proxy->leaf=m_sets[0].insert(aabb,proxy); proxy->leaf=m_sets[0].insert(aabb,proxy);
docollide=true; docollide=true;
} }
else else
{/* dynamic set */ {/* dynamic set */
++m_updates_call; ++m_updates_call;
if(Intersect(proxy->leaf->volume,aabb)) if(Intersect(proxy->leaf->volume,aabb))
{/* Moving */ {/* Moving */
const btVector3 delta=aabbMin-proxy->m_aabbMin; const btVector3 delta=aabbMin-proxy->m_aabbMin;
btVector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction); btVector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction);
if(delta[0]<0) velocity[0]=-velocity[0]; if(delta[0]<0) velocity[0]=-velocity[0];
if(delta[1]<0) velocity[1]=-velocity[1]; if(delta[1]<0) velocity[1]=-velocity[1];
if(delta[2]<0) velocity[2]=-velocity[2]; if(delta[2]<0) velocity[2]=-velocity[2];
if ( if (
#ifdef DBVT_BP_MARGIN #ifdef DBVT_BP_MARGIN
m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
#else #else
m_sets[0].update(proxy->leaf,aabb,velocity) m_sets[0].update(proxy->leaf,aabb,velocity)
#endif #endif
) )
{ {
++m_updates_done; ++m_updates_done;
docollide=true; docollide=true;
} }
} }
else else
{/* Teleporting */ {/* Teleporting */
m_sets[0].update(proxy->leaf,aabb); m_sets[0].update(proxy->leaf,aabb);
++m_updates_done; ++m_updates_done;
docollide=true; docollide=true;
} }
} }
listremove(proxy,m_stageRoots[proxy->stage]); listremove(proxy,m_stageRoots[proxy->stage]);
proxy->m_aabbMin = aabbMin; proxy->m_aabbMin = aabbMin;
proxy->m_aabbMax = aabbMax; proxy->m_aabbMax = aabbMax;
proxy->stage = m_stageCurrent; proxy->stage = m_stageCurrent;
listappend(proxy,m_stageRoots[m_stageCurrent]); listappend(proxy,m_stageRoots[m_stageCurrent]);
if(docollide) if(docollide)
{ {
m_needcleanup=true; m_needcleanup=true;
if(!m_deferedcollide) if(!m_deferedcollide)
{ {
btDbvtTreeCollider collider(this); btDbvtTreeCollider collider(this);
btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider); btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider);
btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider); btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider);
} }
} }
} }
@@ -311,24 +311,24 @@ if(NotEqual(aabb,proxy->leaf->volume))
// //
void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
{ {
collide(dispatcher); collide(dispatcher);
#if DBVT_BP_PROFILE #if DBVT_BP_PROFILE
if(0==(m_pid%DBVT_BP_PROFILING_RATE)) if(0==(m_pid%DBVT_BP_PROFILING_RATE))
{ {
printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs()); printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
unsigned int total=m_profiling.m_total; unsigned int total=m_profiling.m_total;
if(total<=0) total=1; if(total<=0) total=1;
printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE); printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE); printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE); printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE); printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE);
const unsigned long sum=m_profiling.m_ddcollide+ const unsigned long sum=m_profiling.m_ddcollide+
m_profiling.m_fdcollide+ m_profiling.m_fdcollide+
m_profiling.m_cleanup; m_profiling.m_cleanup;
printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE); printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE)); printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE));
clear(m_profiling); clear(m_profiling);
m_clock.reset(); m_clock.reset();
} }
#endif #endif
} }
@@ -336,108 +336,108 @@ if(0==(m_pid%DBVT_BP_PROFILING_RATE))
// //
void btDbvtBroadphase::collide(btDispatcher* dispatcher) void btDbvtBroadphase::collide(btDispatcher* dispatcher)
{ {
SPC(m_profiling.m_total); SPC(m_profiling.m_total);
/* optimize */ /* optimize */
m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
if(m_fixedleft) if(m_fixedleft)
{ {
const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
m_fixedleft=btMax<int>(0,m_fixedleft-count); m_fixedleft=btMax<int>(0,m_fixedleft-count);
} }
/* dynamic -> fixed set */ /* dynamic -> fixed set */
m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
btDbvtProxy* current=m_stageRoots[m_stageCurrent]; btDbvtProxy* current=m_stageRoots[m_stageCurrent];
if(current) if(current)
{ {
btDbvtTreeCollider collider(this); btDbvtTreeCollider collider(this);
do { do {
btDbvtProxy* next=current->links[1]; btDbvtProxy* next=current->links[1];
listremove(current,m_stageRoots[current->stage]); listremove(current,m_stageRoots[current->stage]);
listappend(current,m_stageRoots[STAGECOUNT]); listappend(current,m_stageRoots[STAGECOUNT]);
#if DBVT_BP_ACCURATESLEEPING #if DBVT_BP_ACCURATESLEEPING
m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
collider.proxy=current; collider.proxy=current;
btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider);
btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
#endif #endif
m_sets[0].remove(current->leaf); m_sets[0].remove(current->leaf);
ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax); ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax);
current->leaf = m_sets[1].insert(curAabb,current); current->leaf = m_sets[1].insert(curAabb,current);
current->stage = STAGECOUNT; current->stage = STAGECOUNT;
current = next; current = next;
} while(current); } while(current);
m_fixedleft=m_sets[1].m_leaves; m_fixedleft=m_sets[1].m_leaves;
m_needcleanup=true; m_needcleanup=true;
} }
/* collide dynamics */ /* collide dynamics */
{ {
btDbvtTreeCollider collider(this); btDbvtTreeCollider collider(this);
if(m_deferedcollide) if(m_deferedcollide)
{ {
SPC(m_profiling.m_fdcollide); SPC(m_profiling.m_fdcollide);
btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider); btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider);
} }
if(m_deferedcollide) if(m_deferedcollide)
{ {
SPC(m_profiling.m_ddcollide); SPC(m_profiling.m_ddcollide);
btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider); btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider);
} }
} }
/* clean up */ /* clean up */
if(m_needcleanup) if(m_needcleanup)
{ {
SPC(m_profiling.m_cleanup); SPC(m_profiling.m_cleanup);
btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray();
if(pairs.size()>0) if(pairs.size()>0)
{ {
const int ci=pairs.size(); const int ci=pairs.size();
int ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100)); int ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
for(int i=0;i<ni;++i) for(int i=0;i<ni;++i)
{ {
btBroadphasePair& p=pairs[(m_cid+i)%ci]; btBroadphasePair& p=pairs[(m_cid+i)%ci];
btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0; btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0;
btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1; btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1;
if(!Intersect(pa->leaf->volume,pb->leaf->volume)) if(!Intersect(pa->leaf->volume,pb->leaf->volume))
{ {
#if DBVT_BP_SORTPAIRS #if DBVT_BP_SORTPAIRS
if(pa>pb) btSwap(pa,pb); if(pa>pb) btSwap(pa,pb);
#endif #endif
m_paircache->removeOverlappingPair(pa,pb,dispatcher); m_paircache->removeOverlappingPair(pa,pb,dispatcher);
--ni;--i; --ni;--i;
} }
} }
if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
} }
} }
++m_pid; ++m_pid;
m_newpairs=1; m_newpairs=1;
m_needcleanup=false; m_needcleanup=false;
if(m_updates_call>0) if(m_updates_call>0)
{ m_updates_ratio=m_updates_done/(btScalar)m_updates_call; } { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
else else
{ m_updates_ratio=0; } { m_updates_ratio=0; }
m_updates_done/=2; m_updates_done/=2;
m_updates_call/=2; m_updates_call/=2;
} }
// //
void btDbvtBroadphase::optimize() void btDbvtBroadphase::optimize()
{ {
m_sets[0].optimizeTopDown(); m_sets[0].optimizeTopDown();
m_sets[1].optimizeTopDown(); m_sets[1].optimizeTopDown();
} }
// //
btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache()
{ {
return(m_paircache); return(m_paircache);
} }
// //
const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const
{ {
return(m_paircache); return(m_paircache);
} }
// //
@@ -446,16 +446,16 @@ void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aab
ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds; ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds;
if(!m_sets[0].empty()) if(!m_sets[0].empty())
if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume,
m_sets[1].m_root->volume,bounds); m_sets[1].m_root->volume,bounds);
else else
bounds=m_sets[0].m_root->volume; bounds=m_sets[0].m_root->volume;
else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume;
else else
bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0); bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
aabbMin=bounds.Mins(); aabbMin=bounds.Mins();
aabbMax=bounds.Maxs(); aabbMax=bounds.Maxs();
} }
// //
@@ -466,9 +466,9 @@ void btDbvtBroadphase::printStats()
#if DBVT_BP_ENABLE_BENCHMARK #if DBVT_BP_ENABLE_BENCHMARK
struct btBroadphaseBenchmark struct btBroadphaseBenchmark
{ {
struct Experiment struct Experiment
{ {
const char* name; const char* name;
int object_count; int object_count;
int update_count; int update_count;
@@ -476,109 +476,109 @@ struct btBroadphaseBenchmark
int iterations; int iterations;
btScalar speed; btScalar speed;
btScalar amplitude; btScalar amplitude;
}; };
struct Object struct Object
{ {
btVector3 center; btVector3 center;
btVector3 extents; btVector3 extents;
btBroadphaseProxy* proxy; btBroadphaseProxy* proxy;
btScalar time; btScalar time;
void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi) void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
{ {
time += speed; time += speed;
center[0] = btCos(time*(btScalar)2.17)*amplitude+ center[0] = btCos(time*(btScalar)2.17)*amplitude+
btSin(time)*amplitude/2; btSin(time)*amplitude/2;
center[1] = btCos(time*(btScalar)1.38)*amplitude+ center[1] = btCos(time*(btScalar)1.38)*amplitude+
btSin(time)*amplitude; btSin(time)*amplitude;
center[2] = btSin(time*(btScalar)0.777)*amplitude; center[2] = btSin(time*(btScalar)0.777)*amplitude;
pbi->setAabb(proxy,center-extents,center+extents,0); pbi->setAabb(proxy,center-extents,center+extents,0);
} }
}; };
static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); }
static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); } static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); }
static void OutputTime(const char* name,btClock& c,unsigned count=0) static void OutputTime(const char* name,btClock& c,unsigned count=0)
{ {
const unsigned long us=c.getTimeMicroseconds(); const unsigned long us=c.getTimeMicroseconds();
const unsigned long ms=(us+500)/1000; const unsigned long ms=(us+500)/1000;
const btScalar sec=us/(btScalar)(1000*1000); const btScalar sec=us/(btScalar)(1000*1000);
if(count>0) if(count>0)
printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec); printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
else else
printf("%s : %u us (%u ms)\r\n",name,us,ms); printf("%s : %u us (%u ms)\r\n",name,us,ms);
} }
}; };
void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi) void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
{ {
static const btBroadphaseBenchmark::Experiment experiments[]= static const btBroadphaseBenchmark::Experiment experiments[]=
{ {
{"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100}, {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
/*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100}, /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
{"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/ {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
}; };
static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]); static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]);
btAlignedObjectArray<btBroadphaseBenchmark::Object*> objects; btAlignedObjectArray<btBroadphaseBenchmark::Object*> objects;
btClock wallclock; btClock wallclock;
/* Begin */ /* Begin */
for(int iexp=0;iexp<nexperiments;++iexp) for(int iexp=0;iexp<nexperiments;++iexp)
{ {
const btBroadphaseBenchmark::Experiment& experiment=experiments[iexp]; const btBroadphaseBenchmark::Experiment& experiment=experiments[iexp];
const int object_count=experiment.object_count; const int object_count=experiment.object_count;
const int update_count=(object_count*experiment.update_count)/100; const int update_count=(object_count*experiment.update_count)/100;
const int spawn_count=(object_count*experiment.spawn_count)/100; const int spawn_count=(object_count*experiment.spawn_count)/100;
const btScalar speed=experiment.speed; const btScalar speed=experiment.speed;
const btScalar amplitude=experiment.amplitude; const btScalar amplitude=experiment.amplitude;
printf("Experiment #%u '%s':\r\n",iexp,experiment.name); printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
printf("\tObjects: %u\r\n",object_count); printf("\tObjects: %u\r\n",object_count);
printf("\tUpdate: %u\r\n",update_count); printf("\tUpdate: %u\r\n",update_count);
printf("\tSpawn: %u\r\n",spawn_count); printf("\tSpawn: %u\r\n",spawn_count);
printf("\tSpeed: %f\r\n",speed); printf("\tSpeed: %f\r\n",speed);
printf("\tAmplitude: %f\r\n",amplitude); printf("\tAmplitude: %f\r\n",amplitude);
srand(180673); srand(180673);
/* Create objects */ /* Create objects */
wallclock.reset(); wallclock.reset();
objects.reserve(object_count); objects.reserve(object_count);
for(int i=0;i<object_count;++i) for(int i=0;i<object_count;++i)
{ {
btBroadphaseBenchmark::Object* po=new btBroadphaseBenchmark::Object(); btBroadphaseBenchmark::Object* po=new btBroadphaseBenchmark::Object();
po->center[0]=btBroadphaseBenchmark::UnitRand()*50; po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
po->center[1]=btBroadphaseBenchmark::UnitRand()*50; po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
po->center[2]=btBroadphaseBenchmark::UnitRand()*50; po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
po->time=btBroadphaseBenchmark::UnitRand()*2000; po->time=btBroadphaseBenchmark::UnitRand()*2000;
po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
objects.push_back(po); objects.push_back(po);
} }
btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock); btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
/* First update */ /* First update */
wallclock.reset(); wallclock.reset();
for(int i=0;i<objects.size();++i) for(int i=0;i<objects.size();++i)
{ {
objects[i]->update(speed,amplitude,pbi); objects[i]->update(speed,amplitude,pbi);
} }
btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock); btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
/* Updates */ /* Updates */
wallclock.reset(); wallclock.reset();
for(int i=0;i<experiment.iterations;++i) for(int i=0;i<experiment.iterations;++i)
{ {
for(int j=0;j<update_count;++j) for(int j=0;j<update_count;++j)
{ {
objects[j]->update(speed,amplitude,pbi); objects[j]->update(speed,amplitude,pbi);
} }
pbi->calculateOverlappingPairs(0); pbi->calculateOverlappingPairs(0);
} }
btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
/* Clean up */ /* Clean up */
wallclock.reset(); wallclock.reset();
for(int i=0;i<objects.size();++i) for(int i=0;i<objects.size();++i)
{ {
pbi->destroyProxy(objects[i]->proxy,0); pbi->destroyProxy(objects[i]->proxy,0);
delete objects[i]; delete objects[i];
} }
objects.resize(0); objects.resize(0);
btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
} }
} }

View File

@@ -31,8 +31,8 @@ subject to the following restrictions:
#define DBVT_BP_MARGIN (btScalar)0.05 #define DBVT_BP_MARGIN (btScalar)0.05
#if DBVT_BP_PROFILE #if DBVT_BP_PROFILE
#define DBVT_BP_PROFILING_RATE 256 #define DBVT_BP_PROFILING_RATE 256
#include "LinearMath/btQuickprof.h" #include "LinearMath/btQuickprof.h"
#endif #endif
// //
@@ -40,16 +40,16 @@ subject to the following restrictions:
// //
struct btDbvtProxy : btBroadphaseProxy struct btDbvtProxy : btBroadphaseProxy
{ {
/* Fields */ /* Fields */
//btDbvtAabbMm aabb; //btDbvtAabbMm aabb;
btDbvtNode* leaf; btDbvtNode* leaf;
btDbvtProxy* links[2]; btDbvtProxy* links[2];
int stage; int stage;
/* ctor */ /* ctor */
btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask) btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
{ {
links[0]=links[1]=0; links[0]=links[1]=0;
} }
}; };
@@ -60,60 +60,60 @@ typedef btAlignedObjectArray<btDbvtProxy*> btDbvtProxyArray;
///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3. ///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3.
struct btDbvtBroadphase : btBroadphaseInterface struct btDbvtBroadphase : btBroadphaseInterface
{ {
/* Config */ /* Config */
enum { enum {
DYNAMIC_SET = 0, /* Dynamic set index */ DYNAMIC_SET = 0, /* Dynamic set index */
FIXED_SET = 1, /* Fixed set index */ FIXED_SET = 1, /* Fixed set index */
STAGECOUNT = 2 /* Number of stages */ STAGECOUNT = 2 /* Number of stages */
}; };
/* Fields */ /* Fields */
btDbvt m_sets[2]; // Dbvt sets btDbvt m_sets[2]; // Dbvt sets
btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list
btOverlappingPairCache* m_paircache; // Pair cache btOverlappingPairCache* m_paircache; // Pair cache
btScalar m_prediction; // Velocity prediction btScalar m_prediction; // Velocity prediction
int m_stageCurrent; // Current stage int m_stageCurrent; // Current stage
int m_fupdates; // % of fixed updates per frame int m_fupdates; // % of fixed updates per frame
int m_dupdates; // % of dynamic updates per frame int m_dupdates; // % of dynamic updates per frame
int m_cupdates; // % of cleanup updates per frame int m_cupdates; // % of cleanup updates per frame
int m_newpairs; // Number of pairs created int m_newpairs; // Number of pairs created
int m_fixedleft; // Fixed optimization left int m_fixedleft; // Fixed optimization left
unsigned m_updates_call; // Number of updates call unsigned m_updates_call; // Number of updates call
unsigned m_updates_done; // Number of updates done unsigned m_updates_done; // Number of updates done
btScalar m_updates_ratio; // m_updates_done/m_updates_call btScalar m_updates_ratio; // m_updates_done/m_updates_call
int m_pid; // Parse id int m_pid; // Parse id
int m_cid; // Cleanup index int m_cid; // Cleanup index
int m_gid; // Gen id int m_gid; // Gen id
bool m_releasepaircache; // Release pair cache on delete bool m_releasepaircache; // Release pair cache on delete
bool m_deferedcollide; // Defere dynamic/static collision to collide call bool m_deferedcollide; // Defere dynamic/static collision to collide call
bool m_needcleanup; // Need to run cleanup? bool m_needcleanup; // Need to run cleanup?
#if DBVT_BP_PROFILE #if DBVT_BP_PROFILE
btClock m_clock; btClock m_clock;
struct { struct {
unsigned long m_total; unsigned long m_total;
unsigned long m_ddcollide; unsigned long m_ddcollide;
unsigned long m_fdcollide; unsigned long m_fdcollide;
unsigned long m_cleanup; unsigned long m_cleanup;
unsigned long m_jobcount; unsigned long m_jobcount;
} m_profiling; } m_profiling;
#endif #endif
/* Methods */ /* Methods */
btDbvtBroadphase(btOverlappingPairCache* paircache=0); btDbvtBroadphase(btOverlappingPairCache* paircache=0);
~btDbvtBroadphase(); ~btDbvtBroadphase();
void collide(btDispatcher* dispatcher); void collide(btDispatcher* dispatcher);
void optimize(); void optimize();
/* btBroadphaseInterface Implementation */ /* btBroadphaseInterface Implementation */
btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback); virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback);
virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
void calculateOverlappingPairs(btDispatcher* dispatcher); void calculateOverlappingPairs(btDispatcher* dispatcher);
btOverlappingPairCache* getOverlappingPairCache(); btOverlappingPairCache* getOverlappingPairCache();
const btOverlappingPairCache* getOverlappingPairCache() const; const btOverlappingPairCache* getOverlappingPairCache() const;
void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
void printStats(); void printStats();
static void benchmark(btBroadphaseInterface*); static void benchmark(btBroadphaseInterface*);
}; };
#endif #endif

File diff suppressed because it is too large Load Diff

View File

@@ -296,7 +296,7 @@ public:
btScalar m_adamping; btScalar m_adamping;
btScalar m_matching; btScalar m_matching;
bool m_collide; bool m_collide;
Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {} Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {}
}; };
/* Impulse */ /* Impulse */
struct Impulse struct Impulse
@@ -307,109 +307,109 @@ public:
int m_asDrift:1; int m_asDrift:1;
Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0) {} Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0) {}
Impulse operator -() const Impulse operator -() const
{ {
Impulse i=*this; Impulse i=*this;
i.m_velocity=-i.m_velocity; i.m_velocity=-i.m_velocity;
i.m_drift=-i.m_drift; i.m_drift=-i.m_drift;
return(i); return(i);
} }
Impulse operator*(btScalar x) const Impulse operator*(btScalar x) const
{ {
Impulse i=*this; Impulse i=*this;
i.m_velocity*=x; i.m_velocity*=x;
i.m_drift*=x; i.m_drift*=x;
return(i); return(i);
} }
}; };
/* Body */ /* Body */
struct Body struct Body
{ {
Cluster* m_soft; Cluster* m_soft;
btRigidBody* m_rigid; btRigidBody* m_rigid;
Body() : m_soft(0),m_rigid(0) {} Body() : m_soft(0),m_rigid(0) {}
Body(Cluster* p) : m_soft(p),m_rigid(0) {} Body(Cluster* p) : m_soft(p),m_rigid(0) {}
Body(btRigidBody* p) : m_soft(0),m_rigid(p) {} Body(btRigidBody* p) : m_soft(0),m_rigid(p) {}
void activate() const void activate() const
{ {
if(m_rigid) m_rigid->activate(); if(m_rigid) m_rigid->activate();
} }
const btMatrix3x3& invWorldInertia() const const btMatrix3x3& invWorldInertia() const
{ {
static const btMatrix3x3 iwi(0,0,0,0,0,0,0,0,0); static const btMatrix3x3 iwi(0,0,0,0,0,0,0,0,0);
if(m_rigid) return(m_rigid->getInvInertiaTensorWorld()); if(m_rigid) return(m_rigid->getInvInertiaTensorWorld());
if(m_soft) return(m_soft->m_invwi); if(m_soft) return(m_soft->m_invwi);
return(iwi); return(iwi);
} }
btScalar invMass() const btScalar invMass() const
{ {
if(m_rigid) return(m_rigid->getInvMass()); if(m_rigid) return(m_rigid->getInvMass());
if(m_soft) return(m_soft->m_imass); if(m_soft) return(m_soft->m_imass);
return(0); return(0);
} }
const btTransform& xform() const const btTransform& xform() const
{ {
static const btTransform identity=btTransform::getIdentity(); static const btTransform identity=btTransform::getIdentity();
if(m_rigid) return(m_rigid->getInterpolationWorldTransform()); if(m_rigid) return(m_rigid->getInterpolationWorldTransform());
if(m_soft) return(m_soft->m_framexform); if(m_soft) return(m_soft->m_framexform);
return(identity); return(identity);
} }
btVector3 linearVelocity() const btVector3 linearVelocity() const
{ {
if(m_rigid) return(m_rigid->getLinearVelocity()); if(m_rigid) return(m_rigid->getLinearVelocity());
if(m_soft) return(m_soft->m_lv); if(m_soft) return(m_soft->m_lv);
return(btVector3(0,0,0)); return(btVector3(0,0,0));
} }
btVector3 angularVelocity(const btVector3& rpos) const btVector3 angularVelocity(const btVector3& rpos) const
{ {
if(m_rigid) return(cross(m_rigid->getAngularVelocity(),rpos)); if(m_rigid) return(cross(m_rigid->getAngularVelocity(),rpos));
if(m_soft) return(cross(m_soft->m_av,rpos)); if(m_soft) return(cross(m_soft->m_av,rpos));
return(btVector3(0,0,0)); return(btVector3(0,0,0));
} }
btVector3 angularVelocity() const btVector3 angularVelocity() const
{ {
if(m_rigid) return(m_rigid->getAngularVelocity()); if(m_rigid) return(m_rigid->getAngularVelocity());
if(m_soft) return(m_soft->m_av); if(m_soft) return(m_soft->m_av);
return(btVector3(0,0,0)); return(btVector3(0,0,0));
} }
btVector3 velocity(const btVector3& rpos) const btVector3 velocity(const btVector3& rpos) const
{ {
return(linearVelocity()+angularVelocity(rpos)); return(linearVelocity()+angularVelocity(rpos));
} }
void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const
{ {
if(m_rigid) m_rigid->applyImpulse(impulse,rpos); if(m_rigid) m_rigid->applyImpulse(impulse,rpos);
if(m_soft) btSoftBody::clusterVImpulse(m_soft,rpos,impulse); if(m_soft) btSoftBody::clusterVImpulse(m_soft,rpos,impulse);
} }
void applyDImpulse(const btVector3& impulse,const btVector3& rpos) const void applyDImpulse(const btVector3& impulse,const btVector3& rpos) const
{ {
if(m_rigid) m_rigid->applyImpulse(impulse,rpos); if(m_rigid) m_rigid->applyImpulse(impulse,rpos);
if(m_soft) btSoftBody::clusterDImpulse(m_soft,rpos,impulse); if(m_soft) btSoftBody::clusterDImpulse(m_soft,rpos,impulse);
} }
void applyImpulse(const Impulse& impulse,const btVector3& rpos) const void applyImpulse(const Impulse& impulse,const btVector3& rpos) const
{ {
if(impulse.m_asVelocity) applyVImpulse(impulse.m_velocity,rpos); if(impulse.m_asVelocity) applyVImpulse(impulse.m_velocity,rpos);
if(impulse.m_asDrift) applyDImpulse(impulse.m_drift,rpos); if(impulse.m_asDrift) applyDImpulse(impulse.m_drift,rpos);
} }
void applyVAImpulse(const btVector3& impulse) const void applyVAImpulse(const btVector3& impulse) const
{ {
if(m_rigid) m_rigid->applyTorqueImpulse(impulse); if(m_rigid) m_rigid->applyTorqueImpulse(impulse);
if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse); if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse);
} }
void applyDAImpulse(const btVector3& impulse) const void applyDAImpulse(const btVector3& impulse) const
{ {
if(m_rigid) m_rigid->applyTorqueImpulse(impulse); if(m_rigid) m_rigid->applyTorqueImpulse(impulse);
if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse); if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse);
} }
void applyAImpulse(const Impulse& impulse) const void applyAImpulse(const Impulse& impulse) const
{ {
if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity);
if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift); if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift);
} }
void applyDCImpulse(const btVector3& impulse) const void applyDCImpulse(const btVector3& impulse) const
{ {
if(m_rigid) m_rigid->applyCentralImpulse(impulse); if(m_rigid) m_rigid->applyCentralImpulse(impulse);
if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse); if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse);
} }
}; };
/* Joint */ /* Joint */
struct Joint struct Joint
@@ -420,12 +420,12 @@ public:
Contact, Contact,
};}; };};
struct Specs struct Specs
{ {
Specs() : erp(1),cfm(1),split(1) {} Specs() : erp(1),cfm(1),split(1) {}
btScalar erp; btScalar erp;
btScalar cfm; btScalar cfm;
btScalar split; btScalar split;
}; };
Body m_bodies[2]; Body m_bodies[2];
btVector3 m_refs[2]; btVector3 m_refs[2];
btScalar m_cfm; btScalar m_cfm;
@@ -436,7 +436,7 @@ public:
btMatrix3x3 m_massmatrix; btMatrix3x3 m_massmatrix;
bool m_delete; bool m_delete;
virtual ~Joint() {} virtual ~Joint() {}
Joint() : m_delete(false) {} Joint() : m_delete(false) {}
virtual void Prepare(btScalar dt,int iterations); virtual void Prepare(btScalar dt,int iterations);
virtual void Solve(btScalar dt,btScalar sor)=0; virtual void Solve(btScalar dt,btScalar sor)=0;
virtual void Terminate(btScalar dt)=0; virtual void Terminate(btScalar dt)=0;
@@ -446,9 +446,9 @@ public:
struct LJoint : Joint struct LJoint : Joint
{ {
struct Specs : Joint::Specs struct Specs : Joint::Specs
{ {
btVector3 position; btVector3 position;
}; };
btVector3 m_rpos[2]; btVector3 m_rpos[2];
void Prepare(btScalar dt,int iterations); void Prepare(btScalar dt,int iterations);
void Solve(btScalar dt,btScalar sor); void Solve(btScalar dt,btScalar sor);
@@ -459,17 +459,17 @@ public:
struct AJoint : Joint struct AJoint : Joint
{ {
struct IControl struct IControl
{ {
virtual void Prepare(AJoint*) {} virtual void Prepare(AJoint*) {}
virtual btScalar Speed(AJoint*,btScalar current) { return(current); } virtual btScalar Speed(AJoint*,btScalar current) { return(current); }
static IControl* Default() { static IControl def;return(&def); } static IControl* Default() { static IControl def;return(&def); }
}; };
struct Specs : Joint::Specs struct Specs : Joint::Specs
{ {
Specs() : icontrol(IControl::Default()) {} Specs() : icontrol(IControl::Default()) {}
btVector3 axis; btVector3 axis;
IControl* icontrol; IControl* icontrol;
}; };
btVector3 m_axis[2]; btVector3 m_axis[2];
IControl* m_icontrol; IControl* m_icontrol;
void Prepare(btScalar dt,int iterations); void Prepare(btScalar dt,int iterations);
@@ -534,24 +534,24 @@ public:
}; };
/// RayFromToCaster takes a ray from, ray to (instead of direction!) /// RayFromToCaster takes a ray from, ray to (instead of direction!)
struct RayFromToCaster : btDbvt::ICollide struct RayFromToCaster : btDbvt::ICollide
{ {
btVector3 m_rayFrom; btVector3 m_rayFrom;
btVector3 m_rayTo; btVector3 m_rayTo;
btVector3 m_rayNormalizedDirection; btVector3 m_rayNormalizedDirection;
btScalar m_mint; btScalar m_mint;
Face* m_face; Face* m_face;
int m_tests; int m_tests;
RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt); RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt);
void Process(const btDbvtNode* leaf); void Process(const btDbvtNode* leaf);
static inline btScalar rayFromToTriangle(const btVector3& rayFrom, static inline btScalar rayFromToTriangle(const btVector3& rayFrom,
const btVector3& rayTo, const btVector3& rayTo,
const btVector3& rayNormalizedDirection, const btVector3& rayNormalizedDirection,
const btVector3& a, const btVector3& a,
const btVector3& b, const btVector3& b,
const btVector3& c, const btVector3& c,
btScalar maxt=SIMD_INFINITY); btScalar maxt=SIMD_INFINITY);
}; };
// //
// Typedef's // Typedef's
@@ -604,8 +604,8 @@ public:
/* ctor */ /* ctor */
btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count, btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count,
const btVector3* x, const btVector3* x,
const btScalar* m); const btScalar* m);
/* dtor */ /* dtor */
virtual ~btSoftBody(); virtual ~btSoftBody();
/* Check for existing link */ /* Check for existing link */
@@ -625,51 +625,51 @@ public:
bool checkLink( int node0, bool checkLink( int node0,
int node1) const; int node1) const;
bool checkLink( const Node* node0, bool checkLink( const Node* node0,
const Node* node1) const; const Node* node1) const;
/* Check for existring face */ /* Check for existring face */
bool checkFace( int node0, bool checkFace( int node0,
int node1, int node1,
int node2) const; int node2) const;
/* Append material */ /* Append material */
Material* appendMaterial(); Material* appendMaterial();
/* Append note */ /* Append note */
void appendNote( const char* text, void appendNote( const char* text,
const btVector3& o, const btVector3& o,
const btVector4& c=btVector4(1,0,0,0), const btVector4& c=btVector4(1,0,0,0),
Node* n0=0, Node* n0=0,
Node* n1=0, Node* n1=0,
Node* n2=0, Node* n2=0,
Node* n3=0); Node* n3=0);
void appendNote( const char* text, void appendNote( const char* text,
const btVector3& o, const btVector3& o,
Node* feature); Node* feature);
void appendNote( const char* text, void appendNote( const char* text,
const btVector3& o, const btVector3& o,
Link* feature); Link* feature);
void appendNote( const char* text, void appendNote( const char* text,
const btVector3& o, const btVector3& o,
Face* feature); Face* feature);
/* Append node */ /* Append node */
void appendNode( const btVector3& x,btScalar m); void appendNode( const btVector3& x,btScalar m);
/* Append link */ /* Append link */
void appendLink(int model=-1,Material* mat=0); void appendLink(int model=-1,Material* mat=0);
void appendLink( int node0, void appendLink( int node0,
int node1, int node1,
Material* mat=0, Material* mat=0,
bool bcheckexist=false); bool bcheckexist=false);
void appendLink( Node* node0, void appendLink( Node* node0,
Node* node1, Node* node1,
Material* mat=0, Material* mat=0,
bool bcheckexist=false); bool bcheckexist=false);
/* Append face */ /* Append face */
void appendFace(int model=-1,Material* mat=0); void appendFace(int model=-1,Material* mat=0);
void appendFace( int node0, void appendFace( int node0,
int node1, int node1,
int node2, int node2,
Material* mat=0); Material* mat=0);
/* Append anchor */ /* Append anchor */
void appendAnchor( int node, void appendAnchor( int node,
btRigidBody* body); btRigidBody* body);
/* Append linear joint */ /* Append linear joint */
void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1); void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1);
void appendLinearJoint(const LJoint::Specs& specs,Body body=Body()); void appendLinearJoint(const LJoint::Specs& specs,Body body=Body());
@@ -682,7 +682,7 @@ public:
void addForce( const btVector3& force); void addForce( const btVector3& force);
/* Add force (or gravity) to a node of the body */ /* Add force (or gravity) to a node of the body */
void addForce( const btVector3& force, void addForce( const btVector3& force,
int node); int node);
/* Add velocity to the entire body */ /* Add velocity to the entire body */
void addVelocity( const btVector3& velocity); void addVelocity( const btVector3& velocity);
@@ -691,17 +691,17 @@ public:
/* Add velocity to a node of the body */ /* Add velocity to a node of the body */
void addVelocity( const btVector3& velocity, void addVelocity( const btVector3& velocity,
int node); int node);
/* Set mass */ /* Set mass */
void setMass( int node, void setMass( int node,
btScalar mass); btScalar mass);
/* Get mass */ /* Get mass */
btScalar getMass( int node) const; btScalar getMass( int node) const;
/* Get total mass */ /* Get total mass */
btScalar getTotalMass() const; btScalar getTotalMass() const;
/* Set total mass (weighted by previous masses) */ /* Set total mass (weighted by previous masses) */
void setTotalMass( btScalar mass, void setTotalMass( btScalar mass,
bool fromfaces=false); bool fromfaces=false);
/* Set total density */ /* Set total density */
void setTotalDensity(btScalar density); void setTotalDensity(btScalar density);
/* Transform */ /* Transform */
@@ -714,7 +714,7 @@ public:
void scale( const btVector3& scl); void scale( const btVector3& scl);
/* Set current state as pose */ /* Set current state as pose */
void setPose( bool bvolume, void setPose( bool bvolume,
bool bframe); bool bframe);
/* Return the volume */ /* Return the volume */
btScalar getVolume() const; btScalar getVolume() const;
/* Cluster count */ /* Cluster count */
@@ -734,7 +734,7 @@ public:
static void clusterDCImpulse(Cluster* cluster,const btVector3& impulse); static void clusterDCImpulse(Cluster* cluster,const btVector3& impulse);
/* Generate bending constraints based on distance in the adjency graph */ /* Generate bending constraints based on distance in the adjency graph */
int generateBendingConstraints( int distance, int generateBendingConstraints( int distance,
Material* mat=0); Material* mat=0);
/* Randomize constraints to reduce solver bias */ /* Randomize constraints to reduce solver bias */
void randomizeConstraints(); void randomizeConstraints();
/* Release clusters */ /* Release clusters */
@@ -750,8 +750,8 @@ public:
///Ray casting using rayFrom and rayTo in worldspace, (not direction!) ///Ray casting using rayFrom and rayTo in worldspace, (not direction!)
bool rayTest(const btVector3& rayFrom, bool rayTest(const btVector3& rayFrom,
const btVector3& rayTo, const btVector3& rayTo,
sRayCast& results); sRayCast& results);
/* Solver presets */ /* Solver presets */
void setSolver(eSolverPresets::_ preset); void setSolver(eSolverPresets::_ preset);
/* predictMotion */ /* predictMotion */
@@ -803,7 +803,7 @@ public:
void indicesToPointers(const int* map=0); void indicesToPointers(const int* map=0);
int rayTest(const btVector3& rayFrom,const btVector3& rayTo, int rayTest(const btVector3& rayFrom,const btVector3& rayTo,
btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const; btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const;
void initializeFaceTree(); void initializeFaceTree();
btVector3 evaluateCom() const; btVector3 evaluateCom() const;
bool checkContact(btRigidBody* prb,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const; bool checkContact(btRigidBody* prb,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const;

View File

@@ -50,24 +50,24 @@ btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped): btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
m_dispatcher(dispatcher), m_dispatcher(dispatcher),
m_dispatchInfoPtr(0) m_dispatchInfoPtr(0)
{ {
m_softBody = (btSoftBody*) (isSwapped? body1:body0); m_softBody = (btSoftBody*) (isSwapped? body1:body0);
m_triBody = isSwapped? body0:body1; m_triBody = isSwapped? body0:body1;
// //
// create the manifold from the dispatcher 'manifold pool' // create the manifold from the dispatcher 'manifold pool'
// //
// m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody); // m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
clearCache(); clearCache();
} }
btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback() btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
{ {
clearCache(); clearCache();
// m_dispatcher->releaseManifold( m_manifoldPtr ); // m_dispatcher->releaseManifold( m_manifoldPtr );
} }
@@ -88,13 +88,13 @@ void btSoftBodyTriangleCallback::clearCache()
void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
{ {
//just for debugging purposes //just for debugging purposes
//printf("triangle %d",m_triangleCount++); //printf("triangle %d",m_triangleCount++);
btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody); btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
btCollisionAlgorithmConstructionInfo ci; btCollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher1 = m_dispatcher; ci.m_dispatcher1 = m_dispatcher;
///debug drawing of the overlapping triangles ///debug drawing of the overlapping triangles
if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
{ {
btVector3 color(255,255,0); btVector3 color(255,255,0);
@@ -134,28 +134,28 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId,
//btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject); //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
// if (m_softBody->getCollisionShape()->getShapeType()== // if (m_softBody->getCollisionShape()->getShapeType()==
{ {
// btVector3 other; // btVector3 other;
btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]); btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
normal.normalize(); normal.normalize();
normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION; normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION;
// other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
// other+=normal*22.f; // other+=normal*22.f;
btVector3 pts[6] = {triangle[0]+normal, btVector3 pts[6] = {triangle[0]+normal,
triangle[1]+normal, triangle[1]+normal,
triangle[2]+normal, triangle[2]+normal,
triangle[0]-normal, triangle[0]-normal,
triangle[1]-normal, triangle[1]-normal,
triangle[2]-normal}; triangle[2]-normal};
btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6); btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6);
// btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
//btTriangleShape tm(triangle[0],triangle[1],triangle[2]); //btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
// tm.setMargin(m_collisionMarginTriangle); // tm.setMargin(m_collisionMarginTriangle);
//copy over user pointers to temporary shape //copy over user pointers to temporary shape
tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer());
@@ -169,8 +169,8 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId,
// btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
//m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex); //m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
// cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
// cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
colAlgo->~btCollisionAlgorithm(); colAlgo->~btCollisionAlgorithm();
ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
@@ -229,22 +229,22 @@ void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* b
btCollisionObject* triOb = triBody; btCollisionObject* triOb = triBody;
btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape()); btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
// if (convexBody->getCollisionShape()->isConvex()) // if (convexBody->getCollisionShape()->isConvex())
{ {
btScalar collisionMarginTriangle = concaveShape->getMargin(); btScalar collisionMarginTriangle = concaveShape->getMargin();
// resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut); m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
//m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
// m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody); // m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax()); concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
// resultOut->refreshContactPoints(); // resultOut->refreshContactPoints();
} }

View File

@@ -77,9 +77,9 @@ class btSoftBodyTriangleCallback : public btTriangleCallback
btHashMap<btHashKey<btTriIndex>,btTriIndex> m_shapeCache; btHashMap<btHashKey<btTriIndex>,btTriIndex> m_shapeCache;
public: public:
int m_triangleCount; int m_triangleCount;
// btPersistentManifold* m_manifoldPtr; // btPersistentManifold* m_manifoldPtr;
btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);

View File

@@ -22,57 +22,57 @@ subject to the following restrictions:
// //
static void drawVertex( btIDebugDraw* idraw, static void drawVertex( btIDebugDraw* idraw,
const btVector3& x,btScalar s,const btVector3& c) const btVector3& x,btScalar s,const btVector3& c)
{ {
idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
} }
// //
static void drawBox( btIDebugDraw* idraw, static void drawBox( btIDebugDraw* idraw,
const btVector3& mins, const btVector3& mins,
const btVector3& maxs, const btVector3& maxs,
const btVector3& color) const btVector3& color)
{ {
const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()),
btVector3(maxs.x(),mins.y(),mins.z()), btVector3(maxs.x(),mins.y(),mins.z()),
btVector3(maxs.x(),maxs.y(),mins.z()), btVector3(maxs.x(),maxs.y(),mins.z()),
btVector3(mins.x(),maxs.y(),mins.z()), btVector3(mins.x(),maxs.y(),mins.z()),
btVector3(mins.x(),mins.y(),maxs.z()), btVector3(mins.x(),mins.y(),maxs.z()),
btVector3(maxs.x(),mins.y(),maxs.z()), btVector3(maxs.x(),mins.y(),maxs.z()),
btVector3(maxs.x(),maxs.y(),maxs.z()), btVector3(maxs.x(),maxs.y(),maxs.z()),
btVector3(mins.x(),maxs.y(),maxs.z())}; btVector3(mins.x(),maxs.y(),maxs.z())};
idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
} }
// //
static void drawTree( btIDebugDraw* idraw, static void drawTree( btIDebugDraw* idraw,
const btDbvtNode* node, const btDbvtNode* node,
int depth, int depth,
const btVector3& ncolor, const btVector3& ncolor,
const btVector3& lcolor, const btVector3& lcolor,
int mindepth, int mindepth,
int maxdepth) int maxdepth)
{ {
if(node) if(node)
{ {
if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0))) if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
{ {
drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth); drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
} }
if(depth>=mindepth) if(depth>=mindepth)
{ {
const btScalar scl=(btScalar)(node->isinternal()?1:1); const btScalar scl=(btScalar)(node->isinternal()?1:1);
const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl;
const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl; const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl;
drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
} }
} }
} }
@@ -81,25 +81,25 @@ if(node)
template <typename T> template <typename T>
static inline T sum(const btAlignedObjectArray<T>& items) static inline T sum(const btAlignedObjectArray<T>& items)
{ {
T v; T v;
if(items.size()) if(items.size())
{ {
v=items[0]; v=items[0];
for(int i=1,ni=items.size();i<ni;++i) for(int i=1,ni=items.size();i<ni;++i)
{ {
v+=items[i]; v+=items[i];
} }
} }
return(v); return(v);
} }
// //
template <typename T,typename Q> template <typename T,typename Q>
static inline void add(btAlignedObjectArray<T>& items,const Q& value) static inline void add(btAlignedObjectArray<T>& items,const Q& value)
{ {
for(int i=0,ni=items.size();i<ni;++i) for(int i=0,ni=items.size();i<ni;++i)
{ {
items[i]+=value; items[i]+=value;
} }
} }
@@ -107,9 +107,9 @@ for(int i=0,ni=items.size();i<ni;++i)
template <typename T,typename Q> template <typename T,typename Q>
static inline void mul(btAlignedObjectArray<T>& items,const Q& value) static inline void mul(btAlignedObjectArray<T>& items,const Q& value)
{ {
for(int i=0,ni=items.size();i<ni;++i) for(int i=0,ni=items.size();i<ni;++i)
{ {
items[i]*=value; items[i]*=value;
} }
} }
@@ -117,8 +117,8 @@ for(int i=0,ni=items.size();i<ni;++i)
template <typename T> template <typename T>
static inline T average(const btAlignedObjectArray<T>& items) static inline T average(const btAlignedObjectArray<T>& items)
{ {
const btScalar n=(btScalar)(items.size()>0?items.size():1); const btScalar n=(btScalar)(items.size()>0?items.size():1);
return(sum(items)/n); return(sum(items)/n);
} }
// //
@@ -136,27 +136,27 @@ static inline btScalar tetravolume(const btVector3& x0,
// //
#if 0 #if 0
static btVector3 stresscolor(btScalar stress) static btVector3 stresscolor(btScalar stress)
{ {
static const btVector3 spectrum[]= { btVector3(1,0,1), static const btVector3 spectrum[]= { btVector3(1,0,1),
btVector3(0,0,1), btVector3(0,0,1),
btVector3(0,1,1), btVector3(0,1,1),
btVector3(0,1,0), btVector3(0,1,0),
btVector3(1,1,0), btVector3(1,1,0),
btVector3(1,0,0), btVector3(1,0,0),
btVector3(1,0,0)}; btVector3(1,0,0)};
static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
static const btScalar one=1; static const btScalar one=1;
stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors; stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;
const int sel=(int)stress; const int sel=(int)stress;
const btScalar frc=stress-sel; const btScalar frc=stress-sel;
return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
} }
#endif #endif
// //
void btSoftBodyHelpers::Draw( btSoftBody* psb, void btSoftBodyHelpers::Draw( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
int drawflags) int drawflags)
{ {
const btScalar scl=(btScalar)0.1; const btScalar scl=(btScalar)0.1;
const btScalar nscl=scl*5; const btScalar nscl=scl*5;
@@ -251,29 +251,29 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x};
const btVector3 c=(x[0]+x[1]+x[2])/3; const btVector3 c=(x[0]+x[1]+x[2])/3;
idraw->drawTriangle((x[0]-c)*scl+c, idraw->drawTriangle((x[0]-c)*scl+c,
(x[1]-c)*scl+c, (x[1]-c)*scl+c,
(x[2]-c)*scl+c, (x[2]-c)*scl+c,
col,alp); col,alp);
} }
} }
/* Clusters */ /* Clusters */
if(0!=(drawflags&fDrawFlags::Clusters)) if(0!=(drawflags&fDrawFlags::Clusters))
{ {
srand(1806); srand(1806);
for(i=0;i<psb->m_clusters.size();++i) for(i=0;i<psb->m_clusters.size();++i)
{ {
if(psb->m_clusters[i]->m_collide) if(psb->m_clusters[i]->m_collide)
{ {
btVector3 color( rand()/(btScalar)RAND_MAX, btVector3 color( rand()/(btScalar)RAND_MAX,
rand()/(btScalar)RAND_MAX, rand()/(btScalar)RAND_MAX,
rand()/(btScalar)RAND_MAX); rand()/(btScalar)RAND_MAX);
color=color.normalized()*0.75; color=color.normalized()*0.75;
btAlignedObjectArray<btVector3> vertices; btAlignedObjectArray<btVector3> vertices;
vertices.resize(psb->m_clusters[i]->m_nodes.size()); vertices.resize(psb->m_clusters[i]->m_nodes.size());
for(j=0,nj=vertices.size();j<nj;++j) for(j=0,nj=vertices.size();j<nj;++j)
{ {
vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x; vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
} }
HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
HullResult hres; HullResult hres;
HullLibrary hlib; HullLibrary hlib;
@@ -284,32 +284,32 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
mul(hres.m_OutputVertices,(btScalar)1); mul(hres.m_OutputVertices,(btScalar)1);
add(hres.m_OutputVertices,center); add(hres.m_OutputVertices,center);
for(j=0;j<(int)hres.mNumFaces;++j) for(j=0;j<(int)hres.mNumFaces;++j)
{ {
const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]}; const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
idraw->drawTriangle(hres.m_OutputVertices[idx[0]], idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
hres.m_OutputVertices[idx[1]], hres.m_OutputVertices[idx[1]],
hres.m_OutputVertices[idx[2]], hres.m_OutputVertices[idx[2]],
color,1); color,1);
}
hlib.ReleaseResult(hres);
} }
hlib.ReleaseResult(hres);
}
/* Velocities */ /* Velocities */
#if 0 #if 0
for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j) for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
{ {
const btSoftBody::Cluster& c=psb->m_clusters[i]; const btSoftBody::Cluster& c=psb->m_clusters[i];
const btVector3 r=c.m_nodes[j]->m_x-c.m_com; const btVector3 r=c.m_nodes[j]->m_x-c.m_com;
const btVector3 v=c.m_lv+cross(c.m_av,r); const btVector3 v=c.m_lv+cross(c.m_av,r);
idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0)); idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
} }
#endif #endif
/* Frame */ /* Frame */
btSoftBody::Cluster& c=*psb->m_clusters[i]; btSoftBody::Cluster& c=*psb->m_clusters[i];
idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
}
} }
}
/* Notes */ /* Notes */
if(0!=(drawflags&fDrawFlags::Notes)) if(0!=(drawflags&fDrawFlags::Notes))
{ {
@@ -318,9 +318,9 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
const btSoftBody::Note& n=psb->m_notes[i]; const btSoftBody::Note& n=psb->m_notes[i];
btVector3 p=n.m_offset; btVector3 p=n.m_offset;
for(int j=0;j<n.m_rank;++j) for(int j=0;j<n.m_rank;++j)
{ {
p+=n.m_nodes[j]->m_x*n.m_coords[j]; p+=n.m_nodes[j]->m_x*n.m_coords[j];
} }
idraw->draw3dText(p,n.m_text); idraw->draw3dText(p,n.m_text);
} }
} }
@@ -333,33 +333,33 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
/* Joints */ /* Joints */
if(0!=(drawflags&fDrawFlags::Joints)) if(0!=(drawflags&fDrawFlags::Joints))
{ {
for(i=0;i<psb->m_joints.size();++i) for(i=0;i<psb->m_joints.size();++i)
{ {
const btSoftBody::Joint* pj=psb->m_joints[i]; const btSoftBody::Joint* pj=psb->m_joints[i];
switch(pj->Type()) switch(pj->Type())
{ {
case btSoftBody::Joint::eType::Linear: case btSoftBody::Joint::eType::Linear:
{ {
const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj;
const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
drawVertex(idraw,a0,0.25,btVector3(1,1,0)); drawVertex(idraw,a0,0.25,btVector3(1,1,0));
drawVertex(idraw,a1,0.25,btVector3(0,1,1)); drawVertex(idraw,a1,0.25,btVector3(0,1,1));
} }
break; break;
case btSoftBody::Joint::eType::Angular: case btSoftBody::Joint::eType::Angular:
{ {
const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj;
const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); const btVector3 o0=pj->m_bodies[0].xform().getOrigin();
const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); const btVector3 o1=pj->m_bodies[1].xform().getOrigin();
const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
} }
} }
} }
@@ -368,10 +368,10 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
// //
void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, void btSoftBodyHelpers::DrawInfos( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
bool masses, bool masses,
bool areas, bool areas,
bool /*stress*/) bool /*stress*/)
{ {
for(int i=0;i<psb->m_nodes.size();++i) for(int i=0;i<psb->m_nodes.size();++i)
{ {
@@ -394,34 +394,34 @@ void btSoftBodyHelpers::DrawInfos( btSoftBody* psb,
// //
void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
int mindepth, int mindepth,
int maxdepth) int maxdepth)
{ {
drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
} }
// //
void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
int mindepth, int mindepth,
int maxdepth) int maxdepth)
{ {
drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
} }
// //
void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
int mindepth, int mindepth,
int maxdepth) int maxdepth)
{ {
drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
} }
// //
void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, void btSoftBodyHelpers::DrawFrame( btSoftBody* psb,
btIDebugDraw* idraw) btIDebugDraw* idraw)
{ {
if(psb->m_pose.m_bframe) if(psb->m_pose.m_bframe)
{ {
@@ -445,9 +445,9 @@ void btSoftBodyHelpers::DrawFrame( btSoftBody* psb,
// //
btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from, btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from,
const btVector3& to, const btVector3& to,
int res, int res,
int fixeds) int fixeds)
{ {
/* Create nodes */ /* Create nodes */
const int r=res+2; const int r=res+2;
@@ -477,13 +477,13 @@ btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, cons
// //
btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00,
const btVector3& corner10, const btVector3& corner10,
const btVector3& corner01, const btVector3& corner01,
const btVector3& corner11, const btVector3& corner11,
int resx, int resx,
int resy, int resy,
int fixeds, int fixeds,
bool gendiags) bool gendiags)
{ {
#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) #define IDX(_x_,_y_) ((_y_)*rx+(_x_))
/* Create nodes */ /* Create nodes */
@@ -554,83 +554,83 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const
// //
btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
const btVector3& corner00, const btVector3& corner00,
const btVector3& corner10, const btVector3& corner10,
const btVector3& corner01, const btVector3& corner01,
const btVector3& corner11, const btVector3& corner11,
int resx, int resx,
int resy, int resy,
int fixeds, int fixeds,
bool gendiags, bool gendiags,
float* tex_coords) float* tex_coords)
{ {
/* /*
* *
* corners: * corners:
* *
* [0][0] corner00 ------- corner01 [resx][0] * [0][0] corner00 ------- corner01 [resx][0]
* | | * | |
* | | * | |
* [0][resy] corner10 -------- corner11 [resx][resy] * [0][resy] corner10 -------- corner11 [resx][resy]
* *
* *
* *
* *
* *
* *
* "fixedgs" map: * "fixedgs" map:
* *
* corner00 --> +1 * corner00 --> +1
* corner01 --> +2 * corner01 --> +2
* corner10 --> +4 * corner10 --> +4
* corner11 --> +8 * corner11 --> +8
* upper middle --> +16 * upper middle --> +16
* left middle --> +32 * left middle --> +32
* right middle --> +64 * right middle --> +64
* lower middle --> +128 * lower middle --> +128
* center --> +256 * center --> +256
* *
* *
* tex_coords size (resx-1)*(resy-1)*12 * tex_coords size (resx-1)*(resy-1)*12
* *
* *
* *
* SINGLE QUAD INTERNALS * SINGLE QUAD INTERNALS
* *
* 1) btSoftBody's nodes and links, * 1) btSoftBody's nodes and links,
* diagonal link is optional ("gendiags") * diagonal link is optional ("gendiags")
* *
* *
* node00 ------ node01 * node00 ------ node01
* | . * | .
* | . * | .
* | . * | .
* | . * | .
* | . * | .
* node10 node11 * node10 node11
* *
* *
* *
* 2) Faces: * 2) Faces:
* two triangles, * two triangles,
* UV Coordinates (hier example for single quad) * UV Coordinates (hier example for single quad)
* *
* (0,1) (0,1) (1,1) * (0,1) (0,1) (1,1)
* 1 |\ 3 \-----| 2 * 1 |\ 3 \-----| 2
* | \ \ | * | \ \ |
* | \ \ | * | \ \ |
* | \ \ | * | \ \ |
* | \ \ | * | \ \ |
* 2 |-----\ 3 \| 1 * 2 |-----\ 3 \| 1
* (0,0) (1,0) (1,0) * (0,0) (1,0) (1,0)
* *
* *
* *
* *
* *
* *
*/ */
#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) #define IDX(_x_,_y_) ((_y_)*rx+(_x_))
/* Create nodes */ /* Create nodes */
@@ -716,32 +716,32 @@ btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id)
{ {
/* /*
* *
* *
* node00 --- node01 * node00 --- node01
* | | * | |
* node10 --- node11 * node10 --- node11
* *
* *
* ID map: * ID map:
* *
* node00 s --> 0 * node00 s --> 0
* node00 t --> 1 * node00 t --> 1
* *
* node01 s --> 3 * node01 s --> 3
* node01 t --> 1 * node01 t --> 1
* *
* node10 s --> 0 * node10 s --> 0
* node10 t --> 2 * node10 t --> 2
* *
* node11 s --> 3 * node11 s --> 3
* node11 t --> 2 * node11 t --> 2
* *
* *
*/ */
float tc=0.0f; float tc=0.0f;
if (id == 0) { if (id == 0) {
tc = (1.0f/((resx-1))*ix); tc = (1.0f/((resx-1))*ix);
} }
@@ -754,12 +754,12 @@ float tc=0.0f;
else if (id==3) { else if (id==3) {
tc = (1.0f/((resx-1))*(ix+1)); tc = (1.0f/((resx-1))*(ix+1));
} }
return tc; return tc;
} }
// //
btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
const btVector3& radius, const btVector3& radius,
int res) int res)
{ {
struct Hammersley struct Hammersley
{ {
@@ -790,8 +790,8 @@ btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,c
// //
btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices,
const int* triangles, const int* triangles,
int ntriangles) int ntriangles)
{ {
int maxidx=0; int maxidx=0;
int i,j,ni; int i,j,ni;
@@ -832,7 +832,7 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo
// //
btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
int nvertices) int nvertices)
{ {
HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); HullDesc hdsc(QF_TRIANGLES,nvertices,vertices);
HullResult hres; HullResult hres;

View File

@@ -46,74 +46,74 @@ struct btSoftBodyHelpers
{ {
/* Draw body */ /* Draw body */
static void Draw( btSoftBody* psb, static void Draw( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
int drawflags=fDrawFlags::Std); int drawflags=fDrawFlags::Std);
/* Draw body infos */ /* Draw body infos */
static void DrawInfos( btSoftBody* psb, static void DrawInfos( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
bool masses, bool masses,
bool areas, bool areas,
bool stress); bool stress);
/* Draw node tree */ /* Draw node tree */
static void DrawNodeTree( btSoftBody* psb, static void DrawNodeTree( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
int mindepth=0, int mindepth=0,
int maxdepth=-1); int maxdepth=-1);
/* Draw face tree */ /* Draw face tree */
static void DrawFaceTree( btSoftBody* psb, static void DrawFaceTree( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
int mindepth=0, int mindepth=0,
int maxdepth=-1); int maxdepth=-1);
/* Draw cluster tree */ /* Draw cluster tree */
static void DrawClusterTree(btSoftBody* psb, static void DrawClusterTree(btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
int mindepth=0, int mindepth=0,
int maxdepth=-1); int maxdepth=-1);
/* Draw rigid frame */ /* Draw rigid frame */
static void DrawFrame( btSoftBody* psb, static void DrawFrame( btSoftBody* psb,
btIDebugDraw* idraw); btIDebugDraw* idraw);
/* Create a rope */ /* Create a rope */
static btSoftBody* CreateRope( btSoftBodyWorldInfo& worldInfo, static btSoftBody* CreateRope( btSoftBodyWorldInfo& worldInfo,
const btVector3& from, const btVector3& from,
const btVector3& to, const btVector3& to,
int res, int res,
int fixeds); int fixeds);
/* Create a patch */ /* Create a patch */
static btSoftBody* CreatePatch(btSoftBodyWorldInfo& worldInfo, static btSoftBody* CreatePatch(btSoftBodyWorldInfo& worldInfo,
const btVector3& corner00, const btVector3& corner00,
const btVector3& corner10, const btVector3& corner10,
const btVector3& corner01, const btVector3& corner01,
const btVector3& corner11, const btVector3& corner11,
int resx, int resx,
int resy, int resy,
int fixeds, int fixeds,
bool gendiags); bool gendiags);
/* Create a patch with UV Texture Coordinates */ /* Create a patch with UV Texture Coordinates */
static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo, static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
const btVector3& corner00, const btVector3& corner00,
const btVector3& corner10, const btVector3& corner10,
const btVector3& corner01, const btVector3& corner01,
const btVector3& corner11, const btVector3& corner11,
int resx, int resx,
int resy, int resy,
int fixeds, int fixeds,
bool gendiags, bool gendiags,
float* tex_coords=0); float* tex_coords=0);
static float CalculateUV(int resx,int resy,int ix,int iy,int id); static float CalculateUV(int resx,int resy,int ix,int iy,int id);
/* Create an ellipsoid */ /* Create an ellipsoid */
static btSoftBody* CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, static btSoftBody* CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,
const btVector3& center, const btVector3& center,
const btVector3& radius, const btVector3& radius,
int res); int res);
/* Create from trimesh */ /* Create from trimesh */
static btSoftBody* CreateFromTriMesh( btSoftBodyWorldInfo& worldInfo, static btSoftBody* CreateFromTriMesh( btSoftBodyWorldInfo& worldInfo,
const btScalar* vertices, const btScalar* vertices,
const int* triangles, const int* triangles,
int ntriangles); int ntriangles);
/* Create from convex-hull */ /* Create from convex-hull */
static btSoftBody* CreateFromConvexHull( btSoftBodyWorldInfo& worldInfo, static btSoftBody* CreateFromConvexHull( btSoftBodyWorldInfo& worldInfo,
const btVector3* vertices, const btVector3* vertices,
int nvertices); int nvertices);
}; };
#endif //SOFT_BODY_HELPERS_H #endif //SOFT_BODY_HELPERS_H

View File

@@ -31,14 +31,14 @@ subject to the following restrictions:
template <typename T> template <typename T>
struct btSymMatrix struct btSymMatrix
{ {
btSymMatrix() : dim(0) {} btSymMatrix() : dim(0) {}
btSymMatrix(int n,const T& init=T()) { resize(n,init); } btSymMatrix(int n,const T& init=T()) { resize(n,init); }
void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); }
int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); } int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
T& operator()(int c,int r) { return(store[index(c,r)]); } T& operator()(int c,int r) { return(store[index(c,r)]); }
const T& operator()(int c,int r) const { return(store[index(c,r)]); } const T& operator()(int c,int r) const { return(store[index(c,r)]); }
btAlignedObjectArray<T> store; btAlignedObjectArray<T> store;
int dim; int dim;
}; };
// //
@@ -69,22 +69,22 @@ public:
///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{ {
/* t should be identity, but better be safe than...fast? */ /* t should be identity, but better be safe than...fast? */
const btVector3 mins=m_body->m_bounds[0]; const btVector3 mins=m_body->m_bounds[0];
const btVector3 maxs=m_body->m_bounds[1]; const btVector3 maxs=m_body->m_bounds[1];
const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
t*btVector3(maxs.x(),mins.y(),mins.z()), t*btVector3(maxs.x(),mins.y(),mins.z()),
t*btVector3(maxs.x(),maxs.y(),mins.z()), t*btVector3(maxs.x(),maxs.y(),mins.z()),
t*btVector3(mins.x(),maxs.y(),mins.z()), t*btVector3(mins.x(),maxs.y(),mins.z()),
t*btVector3(mins.x(),mins.y(),maxs.z()), t*btVector3(mins.x(),mins.y(),maxs.z()),
t*btVector3(maxs.x(),mins.y(),maxs.z()), t*btVector3(maxs.x(),mins.y(),maxs.z()),
t*btVector3(maxs.x(),maxs.y(),maxs.z()), t*btVector3(maxs.x(),maxs.y(),maxs.z()),
t*btVector3(mins.x(),maxs.y(),maxs.z())}; t*btVector3(mins.x(),maxs.y(),maxs.z())};
aabbMin=aabbMax=crns[0]; aabbMin=aabbMax=crns[0];
for(int i=1;i<8;++i) for(int i=1;i<8;++i)
{ {
aabbMin.setMin(crns[i]); aabbMin.setMin(crns[i]);
aabbMax.setMax(crns[i]); aabbMax.setMax(crns[i]);
} }
} }
@@ -95,8 +95,8 @@ public:
} }
virtual const btVector3& getLocalScaling() const virtual const btVector3& getLocalScaling() const
{ {
static const btVector3 dummy(1,1,1); static const btVector3 dummy(1,1,1);
return dummy; return dummy;
} }
virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
{ {
@@ -122,21 +122,21 @@ public:
virtual btVector3 localGetSupportingVertex(const btVector3& vec) const virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
{ {
btSoftBody::Node* const * n=&m_cluster->m_nodes[0]; btSoftBody::Node* const * n=&m_cluster->m_nodes[0];
btScalar d=dot(vec,n[0]->m_x); btScalar d=dot(vec,n[0]->m_x);
int j=0; int j=0;
for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i) for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
{ {
const btScalar k=dot(vec,n[i]->m_x); const btScalar k=dot(vec,n[i]->m_x);
if(k>d) { d=k;j=i; } if(k>d) { d=k;j=i; }
} }
return(n[j]->m_x); return(n[j]->m_x);
} }
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
{ {
return(localGetSupportingVertex(vec)); return(localGetSupportingVertex(vec));
} }
//notice that the vectors should be unit length //notice that the vectors should be unit length
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
{} {}
@@ -171,8 +171,8 @@ public:
template <typename T> template <typename T>
static inline void ZeroInitialize(T& value) static inline void ZeroInitialize(T& value)
{ {
static const T zerodummy; static const T zerodummy;
value=zerodummy; value=zerodummy;
} }
// //
template <typename T> template <typename T>
@@ -192,23 +192,23 @@ static inline T InvLerp(const T& a,const T& b,btScalar t)
{ return((b+a*t-b*t)/(a*b)); } { return((b+a*t-b*t)/(a*b)); }
// //
static inline btMatrix3x3 Lerp( const btMatrix3x3& a, static inline btMatrix3x3 Lerp( const btMatrix3x3& a,
const btMatrix3x3& b, const btMatrix3x3& b,
btScalar t) btScalar t)
{ {
btMatrix3x3 r; btMatrix3x3 r;
r[0]=Lerp(a[0],b[0],t); r[0]=Lerp(a[0],b[0],t);
r[1]=Lerp(a[1],b[1],t); r[1]=Lerp(a[1],b[1],t);
r[2]=Lerp(a[2],b[2],t); r[2]=Lerp(a[2],b[2],t);
return(r); return(r);
} }
// //
static inline btVector3 Clamp(const btVector3& v,btScalar maxlength) static inline btVector3 Clamp(const btVector3& v,btScalar maxlength)
{ {
const btScalar sql=v.length2(); const btScalar sql=v.length2();
if(sql>(maxlength*maxlength)) if(sql>(maxlength*maxlength))
return((v*maxlength)/btSqrt(sql)); return((v*maxlength)/btSqrt(sql));
else else
return(v); return(v);
} }
// //
template <typename T> template <typename T>
@@ -233,8 +233,8 @@ static inline bool SameSign(const T& x,const T& y)
// //
static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y) static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y)
{ {
const btVector3 d=x-y; const btVector3 d=x-y;
return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
} }
// //
static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s)
@@ -271,7 +271,7 @@ static inline btMatrix3x3 Diagonal(btScalar x)
} }
// //
static inline btMatrix3x3 Add(const btMatrix3x3& a, static inline btMatrix3x3 Add(const btMatrix3x3& a,
const btMatrix3x3& b) const btMatrix3x3& b)
{ {
btMatrix3x3 r; btMatrix3x3 r;
for(int i=0;i<3;++i) r[i]=a[i]+b[i]; for(int i=0;i<3;++i) r[i]=a[i]+b[i];
@@ -279,7 +279,7 @@ static inline btMatrix3x3 Add(const btMatrix3x3& a,
} }
// //
static inline btMatrix3x3 Sub(const btMatrix3x3& a, static inline btMatrix3x3 Sub(const btMatrix3x3& a,
const btMatrix3x3& b) const btMatrix3x3& b)
{ {
btMatrix3x3 r; btMatrix3x3 r;
for(int i=0;i<3;++i) r[i]=a[i]-b[i]; for(int i=0;i<3;++i) r[i]=a[i]-b[i];
@@ -287,7 +287,7 @@ static inline btMatrix3x3 Sub(const btMatrix3x3& a,
} }
// //
static inline btMatrix3x3 Mul(const btMatrix3x3& a, static inline btMatrix3x3 Mul(const btMatrix3x3& a,
btScalar b) btScalar b)
{ {
btMatrix3x3 r; btMatrix3x3 r;
for(int i=0;i<3;++i) r[i]=a[i]*b; for(int i=0;i<3;++i) r[i]=a[i]*b;
@@ -296,9 +296,9 @@ static inline btMatrix3x3 Mul(const btMatrix3x3& a,
// //
static inline void Orthogonalize(btMatrix3x3& m) static inline void Orthogonalize(btMatrix3x3& m)
{ {
m[2]=cross(m[0],m[1]).normalized(); m[2]=cross(m[0],m[1]).normalized();
m[1]=cross(m[2],m[0]).normalized(); m[1]=cross(m[2],m[0]).normalized();
m[0]=cross(m[1],m[2]).normalized(); m[0]=cross(m[1],m[2]).normalized();
} }
// //
static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
@@ -309,90 +309,90 @@ static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const bt
// //
static inline btMatrix3x3 ImpulseMatrix( btScalar dt, static inline btMatrix3x3 ImpulseMatrix( btScalar dt,
btScalar ima, btScalar ima,
btScalar imb, btScalar imb,
const btMatrix3x3& iwi, const btMatrix3x3& iwi,
const btVector3& r) const btVector3& r)
{ {
return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
} }
// //
static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra, static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
btScalar imb,const btMatrix3x3& iib,const btVector3& rb) btScalar imb,const btMatrix3x3& iib,const btVector3& rb)
{ {
return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
} }
// //
static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia,
const btMatrix3x3& iib) const btMatrix3x3& iib)
{ {
return(Add(iia,iib).inverse()); return(Add(iia,iib).inverse());
} }
// //
static inline btVector3 ProjectOnAxis( const btVector3& v, static inline btVector3 ProjectOnAxis( const btVector3& v,
const btVector3& a) const btVector3& a)
{ {
return(a*dot(v,a)); return(a*dot(v,a));
} }
// //
static inline btVector3 ProjectOnPlane( const btVector3& v, static inline btVector3 ProjectOnPlane( const btVector3& v,
const btVector3& a) const btVector3& a)
{ {
return(v-ProjectOnAxis(v,a)); return(v-ProjectOnAxis(v,a));
} }
// //
static inline void ProjectOrigin( const btVector3& a, static inline void ProjectOrigin( const btVector3& a,
const btVector3& b, const btVector3& b,
btVector3& prj, btVector3& prj,
btScalar& sqd) btScalar& sqd)
{ {
const btVector3 d=b-a; const btVector3 d=b-a;
const btScalar m2=d.length2(); const btScalar m2=d.length2();
if(m2>SIMD_EPSILON) if(m2>SIMD_EPSILON)
{ {
const btScalar t=Clamp<btScalar>(-dot(a,d)/m2,0,1); const btScalar t=Clamp<btScalar>(-dot(a,d)/m2,0,1);
const btVector3 p=a+d*t; const btVector3 p=a+d*t;
const btScalar l2=p.length2(); const btScalar l2=p.length2();
if(l2<sqd) if(l2<sqd)
{ {
prj=p; prj=p;
sqd=l2; sqd=l2;
} }
} }
} }
// //
static inline void ProjectOrigin( const btVector3& a, static inline void ProjectOrigin( const btVector3& a,
const btVector3& b, const btVector3& b,
const btVector3& c, const btVector3& c,
btVector3& prj, btVector3& prj,
btScalar& sqd) btScalar& sqd)
{ {
const btVector3& q=cross(b-a,c-a); const btVector3& q=cross(b-a,c-a);
const btScalar m2=q.length2(); const btScalar m2=q.length2();
if(m2>SIMD_EPSILON) if(m2>SIMD_EPSILON)
{ {
const btVector3 n=q/btSqrt(m2); const btVector3 n=q/btSqrt(m2);
const btScalar k=dot(a,n); const btScalar k=dot(a,n);
const btScalar k2=k*k; const btScalar k2=k*k;
if(k2<sqd) if(k2<sqd)
{ {
const btVector3 p=n*k; const btVector3 p=n*k;
if( (dot(cross(a-p,b-p),q)>0)&& if( (dot(cross(a-p,b-p),q)>0)&&
(dot(cross(b-p,c-p),q)>0)&& (dot(cross(b-p,c-p),q)>0)&&
(dot(cross(c-p,a-p),q)>0)) (dot(cross(c-p,a-p),q)>0))
{ {
prj=p; prj=p;
sqd=k2; sqd=k2;
} }
else else
{ {
ProjectOrigin(a,b,prj,sqd); ProjectOrigin(a,b,prj,sqd);
ProjectOrigin(b,c,prj,sqd); ProjectOrigin(b,c,prj,sqd);
ProjectOrigin(c,a,prj,sqd); ProjectOrigin(c,a,prj,sqd);
} }
} }
} }
@@ -401,53 +401,53 @@ if(m2>SIMD_EPSILON)
// //
template <typename T> template <typename T>
static inline T BaryEval( const T& a, static inline T BaryEval( const T& a,
const T& b, const T& b,
const T& c, const T& c,
const btVector3& coord) const btVector3& coord)
{ {
return(a*coord.x()+b*coord.y()+c*coord.z()); return(a*coord.x()+b*coord.y()+c*coord.z());
} }
// //
static inline btVector3 BaryCoord( const btVector3& a, static inline btVector3 BaryCoord( const btVector3& a,
const btVector3& b, const btVector3& b,
const btVector3& c, const btVector3& c,
const btVector3& p) const btVector3& p)
{ {
const btScalar w[]={ cross(a-p,b-p).length(), const btScalar w[]={ cross(a-p,b-p).length(),
cross(b-p,c-p).length(), cross(b-p,c-p).length(),
cross(c-p,a-p).length()}; cross(c-p,a-p).length()};
const btScalar isum=1/(w[0]+w[1]+w[2]); const btScalar isum=1/(w[0]+w[1]+w[2]);
return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
} }
// //
static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn,
const btVector3& a, const btVector3& a,
const btVector3& b, const btVector3& b,
const btScalar accuracy, const btScalar accuracy,
const int maxiterations=256) const int maxiterations=256)
{ {
btScalar span[2]={0,1}; btScalar span[2]={0,1};
btScalar values[2]={fn->Eval(a),fn->Eval(b)}; btScalar values[2]={fn->Eval(a),fn->Eval(b)};
if(values[0]>values[1]) if(values[0]>values[1])
{ {
btSwap(span[0],span[1]); btSwap(span[0],span[1]);
btSwap(values[0],values[1]); btSwap(values[0],values[1]);
} }
if(values[0]>-accuracy) return(-1); if(values[0]>-accuracy) return(-1);
if(values[1]<+accuracy) return(-1); if(values[1]<+accuracy) return(-1);
for(int i=0;i<maxiterations;++i) for(int i=0;i<maxiterations;++i)
{ {
const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1])); const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
const btScalar v=fn->Eval(Lerp(a,b,t)); const btScalar v=fn->Eval(Lerp(a,b,t));
if((t<=0)||(t>=1)) break; if((t<=0)||(t>=1)) break;
if(btFabs(v)<accuracy) return(t); if(btFabs(v)<accuracy) return(t);
if(v<0) if(v<0)
{ span[0]=t;values[0]=v; } { span[0]=t;values[0]=v; }
else else
{ span[1]=t;values[1]=v; } { span[1]=t;values[1]=v; }
} }
return(-1); return(-1);
} }
// //
@@ -462,26 +462,26 @@ static inline btVector3 NormalizeAny(const btVector3& v)
// //
static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f, static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f,
btScalar margin) btScalar margin)
{ {
const btVector3* pts[]={ &f.m_n[0]->m_x, const btVector3* pts[]={ &f.m_n[0]->m_x,
&f.m_n[1]->m_x, &f.m_n[1]->m_x,
&f.m_n[2]->m_x}; &f.m_n[2]->m_x};
btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3);
vol.Expand(btVector3(margin,margin,margin)); vol.Expand(btVector3(margin,margin,margin));
return(vol); return(vol);
} }
// //
static inline btVector3 CenterOf( const btSoftBody::Face& f) static inline btVector3 CenterOf( const btSoftBody::Face& f)
{ {
return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
} }
// //
static inline btScalar AreaOf( const btVector3& x0, static inline btScalar AreaOf( const btVector3& x0,
const btVector3& x1, const btVector3& x1,
const btVector3& x2) const btVector3& x2)
{ {
const btVector3 a=x1-x0; const btVector3 a=x1-x0;
const btVector3 b=x2-x0; const btVector3 b=x2-x0;
@@ -492,9 +492,9 @@ static inline btScalar AreaOf( const btVector3& x0,
// //
static inline btScalar VolumeOf( const btVector3& x0, static inline btScalar VolumeOf( const btVector3& x0,
const btVector3& x1, const btVector3& x1,
const btVector3& x2, const btVector3& x2,
const btVector3& x3) const btVector3& x3)
{ {
const btVector3 a=x1-x0; const btVector3 a=x1-x0;
const btVector3 b=x2-x0; const btVector3 b=x2-x0;
@@ -504,8 +504,8 @@ static inline btScalar VolumeOf( const btVector3& x0,
// //
static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, static void EvaluateMedium( const btSoftBodyWorldInfo* wfi,
const btVector3& x, const btVector3& x,
btSoftBody::sMedium& medium) btSoftBody::sMedium& medium)
{ {
medium.m_velocity = btVector3(0,0,0); medium.m_velocity = btVector3(0,0,0);
medium.m_pressure = 0; medium.m_pressure = 0;
@@ -523,8 +523,8 @@ static void EvaluateMedium( const btSoftBodyWorldInfo* wfi,
// //
static inline void ApplyClampedForce( btSoftBody::Node& n, static inline void ApplyClampedForce( btSoftBody::Node& n,
const btVector3& f, const btVector3& f,
btScalar dt) btScalar dt)
{ {
const btScalar dtim=dt*n.m_im; const btScalar dtim=dt*n.m_im;
if((f*dtim).length2()>n.m_v.length2()) if((f*dtim).length2()>n.m_v.length2())
@@ -539,13 +539,13 @@ static inline void ApplyClampedForce( btSoftBody::Node& n,
// //
static inline int MatchEdge( const btSoftBody::Node* a, static inline int MatchEdge( const btSoftBody::Node* a,
const btSoftBody::Node* b, const btSoftBody::Node* b,
const btSoftBody::Node* ma, const btSoftBody::Node* ma,
const btSoftBody::Node* mb) const btSoftBody::Node* mb)
{ {
if((a==ma)&&(b==mb)) return(0); if((a==ma)&&(b==mb)) return(0);
if((a==mb)&&(b==ma)) return(1); if((a==mb)&&(b==ma)) return(1);
return(-1); return(-1);
} }
// //
@@ -555,56 +555,56 @@ return(-1);
// //
struct btEigen struct btEigen
{ {
static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
{ {
static const int maxiterations=16; static const int maxiterations=16;
static const btScalar accuracy=(btScalar)0.0001; static const btScalar accuracy=(btScalar)0.0001;
btMatrix3x3& v=*vectors; btMatrix3x3& v=*vectors;
int iterations=0; int iterations=0;
vectors->setIdentity(); vectors->setIdentity();
do { do {
int p=0,q=1; int p=0,q=1;
if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; } if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; } if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
if(btFabs(a[p][q])>accuracy) if(btFabs(a[p][q])>accuracy)
{ {
const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]);
const btScalar z=btFabs(w); const btScalar z=btFabs(w);
const btScalar t=w/(z*(btSqrt(1+w*w)+z)); const btScalar t=w/(z*(btSqrt(1+w*w)+z));
if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */
{ {
const btScalar c=1/btSqrt(t*t+1); const btScalar c=1/btSqrt(t*t+1);
const btScalar s=c*t; const btScalar s=c*t;
mulPQ(a,c,s,p,q); mulPQ(a,c,s,p,q);
mulTPQ(a,c,s,p,q); mulTPQ(a,c,s,p,q);
mulPQ(v,c,s,p,q); mulPQ(v,c,s,p,q);
} else break; } else break;
} else break; } else break;
} while((++iterations)<maxiterations); } while((++iterations)<maxiterations);
if(values) if(values)
{ {
*values=btVector3(a[0][0],a[1][1],a[2][2]); *values=btVector3(a[0][0],a[1][1],a[2][2]);
} }
return(iterations); return(iterations);
} }
private: private:
static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
{ {
const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]}, const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]},
{a[q][0],a[q][1],a[q][2]}}; {a[q][0],a[q][1],a[q][2]}};
int i; int i;
for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i]; for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i]; for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
} }
static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
{ {
const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]}, const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]},
{a[0][q],a[1][q],a[2][q]}}; {a[0][q],a[1][q],a[2][q]}};
int i; int i;
for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i]; for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i]; for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
} }
}; };
@@ -639,7 +639,7 @@ static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix
q.setIdentity(); q.setIdentity();
s.setIdentity(); s.setIdentity();
} }
return(i); return(i);
} }
// //
@@ -652,54 +652,54 @@ struct btSoftColliders
// //
struct ClusterBase : btDbvt::ICollide struct ClusterBase : btDbvt::ICollide
{ {
btScalar erp; btScalar erp;
btScalar idt; btScalar idt;
btScalar margin; btScalar margin;
btScalar friction; btScalar friction;
btScalar threshold; btScalar threshold;
ClusterBase() ClusterBase()
{ {
erp =(btScalar)1; erp =(btScalar)1;
idt =0; idt =0;
margin =0; margin =0;
friction =0; friction =0;
threshold =(btScalar)0; threshold =(btScalar)0;
} }
bool SolveContact( const btGjkEpaSolver2::sResults& res, bool SolveContact( const btGjkEpaSolver2::sResults& res,
btSoftBody::Body ba,btSoftBody::Body bb, btSoftBody::Body ba,btSoftBody::Body bb,
btSoftBody::CJoint& joint) btSoftBody::CJoint& joint)
{ {
if(res.distance<margin) if(res.distance<margin)
{ {
const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin(); const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin();
const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin(); const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin();
const btVector3 va=ba.velocity(ra); const btVector3 va=ba.velocity(ra);
const btVector3 vb=bb.velocity(rb); const btVector3 vb=bb.velocity(rb);
const btVector3 vrel=va-vb; const btVector3 vrel=va-vb;
const btScalar rvac=dot(vrel,res.normal); const btScalar rvac=dot(vrel,res.normal);
const btScalar depth=res.distance-margin; const btScalar depth=res.distance-margin;
const btVector3 iv=res.normal*rvac; const btVector3 iv=res.normal*rvac;
const btVector3 fv=vrel-iv; const btVector3 fv=vrel-iv;
joint.m_bodies[0] = ba; joint.m_bodies[0] = ba;
joint.m_bodies[1] = bb; joint.m_bodies[1] = bb;
joint.m_refs[0] = ra*ba.xform().getBasis(); joint.m_refs[0] = ra*ba.xform().getBasis();
joint.m_refs[1] = rb*bb.xform().getBasis(); joint.m_refs[1] = rb*bb.xform().getBasis();
joint.m_rpos[0] = ra; joint.m_rpos[0] = ra;
joint.m_rpos[1] = rb; joint.m_rpos[1] = rb;
joint.m_cfm = 1; joint.m_cfm = 1;
joint.m_erp = 1; joint.m_erp = 1;
joint.m_life = 0; joint.m_life = 0;
joint.m_maxlife = 0; joint.m_maxlife = 0;
joint.m_split = 1; joint.m_split = 1;
joint.m_drift = depth*res.normal; joint.m_drift = depth*res.normal;
joint.m_normal = res.normal; joint.m_normal = res.normal;
joint.m_delete = false; joint.m_delete = false;
joint.m_friction = fv.length2()<(-rvac*friction)?1:friction; joint.m_friction = fv.length2()<(-rvac*friction)?1:friction;
joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0], joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]); bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
return(true); return(true);
} }
return(false); return(false);
} }
}; };
// //
@@ -707,52 +707,52 @@ struct btSoftColliders
// //
struct CollideCL_RS : ClusterBase struct CollideCL_RS : ClusterBase
{ {
btSoftBody* psb; btSoftBody* psb;
btRigidBody* prb; btRigidBody* prb;
void Process(const btDbvtNode* leaf) void Process(const btDbvtNode* leaf)
{ {
btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data; btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data;
btSoftClusterCollisionShape cshape(cluster); btSoftClusterCollisionShape cshape(cluster);
const btConvexShape* rshape=(const btConvexShape*)prb->getCollisionShape(); const btConvexShape* rshape=(const btConvexShape*)prb->getCollisionShape();
btGjkEpaSolver2::sResults res; btGjkEpaSolver2::sResults res;
if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(),
rshape,prb->getInterpolationWorldTransform(), rshape,prb->getInterpolationWorldTransform(),
btVector3(1,0,0),res)) btVector3(1,0,0),res))
{ {
btSoftBody::CJoint joint; btSoftBody::CJoint joint;
if(SolveContact(res,cluster,prb,joint)) if(SolveContact(res,cluster,prb,joint))
{ {
btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
*pj=joint;psb->m_joints.push_back(pj); *pj=joint;psb->m_joints.push_back(pj);
if(prb->isStaticOrKinematicObject()) if(prb->isStaticOrKinematicObject())
{ {
pj->m_erp *= psb->m_cfg.kSKHR_CL; pj->m_erp *= psb->m_cfg.kSKHR_CL;
pj->m_split *= psb->m_cfg.kSK_SPLT_CL; pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
} }
else else
{ {
pj->m_erp *= psb->m_cfg.kSRHR_CL; pj->m_erp *= psb->m_cfg.kSRHR_CL;
pj->m_split *= psb->m_cfg.kSR_SPLT_CL; pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
} }
} }
} }
} }
void Process(btSoftBody* ps,btRigidBody* pr) void Process(btSoftBody* ps,btRigidBody* pr)
{ {
psb = ps; psb = ps;
prb = pr; prb = pr;
idt = ps->m_sst.isdt; idt = ps->m_sst.isdt;
margin = ps->getCollisionShape()->getMargin()+ margin = ps->getCollisionShape()->getMargin()+
pr->getCollisionShape()->getMargin(); pr->getCollisionShape()->getMargin();
friction = btMin(psb->m_cfg.kDF,prb->getFriction()); friction = btMin(psb->m_cfg.kDF,prb->getFriction());
btVector3 mins; btVector3 mins;
btVector3 maxs; btVector3 maxs;
ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; ATTRIBUTE_ALIGNED16(btDbvtVolume) volume;
pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs); pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs);
volume=btDbvtVolume::FromMM(mins,maxs); volume=btDbvtVolume::FromMM(mins,maxs);
volume.Expand(btVector3(1,1,1)*margin); volume.Expand(btVector3(1,1,1)*margin);
btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this); btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this);
} }
}; };
// //
@@ -760,36 +760,36 @@ struct btSoftColliders
// //
struct CollideCL_SS : ClusterBase struct CollideCL_SS : ClusterBase
{ {
btSoftBody* bodies[2]; btSoftBody* bodies[2];
void Process(const btDbvtNode* la,const btDbvtNode* lb) void Process(const btDbvtNode* la,const btDbvtNode* lb)
{ {
btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data;
btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data;
btSoftClusterCollisionShape csa(cla); btSoftClusterCollisionShape csa(cla);
btSoftClusterCollisionShape csb(clb); btSoftClusterCollisionShape csb(clb);
btGjkEpaSolver2::sResults res; btGjkEpaSolver2::sResults res;
if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(),
&csb,btTransform::getIdentity(), &csb,btTransform::getIdentity(),
cla->m_com-clb->m_com,res)) cla->m_com-clb->m_com,res))
{ {
btSoftBody::CJoint joint; btSoftBody::CJoint joint;
if(SolveContact(res,cla,clb,joint)) if(SolveContact(res,cla,clb,joint))
{ {
btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
*pj=joint;bodies[0]->m_joints.push_back(pj); *pj=joint;bodies[0]->m_joints.push_back(pj);
pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
} }
} }
} }
void Process(btSoftBody* psa,btSoftBody* psb) void Process(btSoftBody* psa,btSoftBody* psb)
{ {
idt = psa->m_sst.isdt; idt = psa->m_sst.isdt;
margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
bodies[0] = psa; bodies[0] = psa;
bodies[1] = psb; bodies[1] = psb;
btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
} }
}; };
// //
@@ -797,96 +797,96 @@ struct btSoftColliders
// //
struct CollideSDF_RS : btDbvt::ICollide struct CollideSDF_RS : btDbvt::ICollide
{ {
void Process(const btDbvtNode* leaf) void Process(const btDbvtNode* leaf)
{ {
btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; btSoftBody::Node* node=(btSoftBody::Node*)leaf->data;
DoNode(*node); DoNode(*node);
} }
void DoNode(btSoftBody::Node& n) const void DoNode(btSoftBody::Node& n) const
{ {
const btScalar m=n.m_im>0?dynmargin:stamargin; const btScalar m=n.m_im>0?dynmargin:stamargin;
btSoftBody::RContact c; btSoftBody::RContact c;
if( (!n.m_battach)&& if( (!n.m_battach)&&
psb->checkContact(prb,n.m_x,m,c.m_cti)) psb->checkContact(prb,n.m_x,m,c.m_cti))
{ {
const btScalar ima=n.m_im; const btScalar ima=n.m_im;
const btScalar imb=prb->getInvMass(); const btScalar imb=prb->getInvMass();
const btScalar ms=ima+imb; const btScalar ms=ima+imb;
if(ms>0) if(ms>0)
{ {
const btTransform& wtr=prb->getInterpolationWorldTransform(); const btTransform& wtr=prb->getInterpolationWorldTransform();
const btMatrix3x3& iwi=prb->getInvInertiaTensorWorld(); const btMatrix3x3& iwi=prb->getInvInertiaTensorWorld();
const btVector3 ra=n.m_x-wtr.getOrigin(); const btVector3 ra=n.m_x-wtr.getOrigin();
const btVector3 va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt; const btVector3 va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt;
const btVector3 vb=n.m_x-n.m_q; const btVector3 vb=n.m_x-n.m_q;
const btVector3 vr=vb-va; const btVector3 vr=vb-va;
const btScalar dn=dot(vr,c.m_cti.m_normal); const btScalar dn=dot(vr,c.m_cti.m_normal);
const btVector3 fv=vr-c.m_cti.m_normal*dn; const btVector3 fv=vr-c.m_cti.m_normal*dn;
const btScalar fc=psb->m_cfg.kDF*prb->getFriction(); const btScalar fc=psb->m_cfg.kDF*prb->getFriction();
c.m_node = &n; c.m_node = &n;
c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
c.m_c1 = ra; c.m_c1 = ra;
c.m_c2 = ima*psb->m_sst.sdt; c.m_c2 = ima*psb->m_sst.sdt;
c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc;
c.m_c4 = prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; c.m_c4 = prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
psb->m_rcontacts.push_back(c); psb->m_rcontacts.push_back(c);
prb->activate(); prb->activate();
} }
} }
} }
btSoftBody* psb; btSoftBody* psb;
btRigidBody* prb; btRigidBody* prb;
btScalar dynmargin; btScalar dynmargin;
btScalar stamargin; btScalar stamargin;
}; };
// //
// CollideVF_SS // CollideVF_SS
// //
struct CollideVF_SS : btDbvt::ICollide struct CollideVF_SS : btDbvt::ICollide
{ {
void Process(const btDbvtNode* lnode, void Process(const btDbvtNode* lnode,
const btDbvtNode* lface) const btDbvtNode* lface)
{ {
btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; btSoftBody::Node* node=(btSoftBody::Node*)lnode->data;
btSoftBody::Face* face=(btSoftBody::Face*)lface->data; btSoftBody::Face* face=(btSoftBody::Face*)lface->data;
btVector3 o=node->m_x; btVector3 o=node->m_x;
btVector3 p; btVector3 p;
btScalar d=SIMD_INFINITY; btScalar d=SIMD_INFINITY;
ProjectOrigin( face->m_n[0]->m_x-o, ProjectOrigin( face->m_n[0]->m_x-o,
face->m_n[1]->m_x-o, face->m_n[1]->m_x-o,
face->m_n[2]->m_x-o, face->m_n[2]->m_x-o,
p,d); p,d);
const btScalar m=mrg+(o-node->m_q).length()*2; const btScalar m=mrg+(o-node->m_q).length()*2;
if(d<(m*m)) if(d<(m*m))
{ {
const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
const btScalar ma=node->m_im; const btScalar ma=node->m_im;
btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
if( (n[0]->m_im<=0)|| if( (n[0]->m_im<=0)||
(n[1]->m_im<=0)|| (n[1]->m_im<=0)||
(n[2]->m_im<=0)) (n[2]->m_im<=0))
{ {
mb=0; mb=0;
} }
const btScalar ms=ma+mb; const btScalar ms=ma+mb;
if(ms>0) if(ms>0)
{ {
btSoftBody::SContact c; btSoftBody::SContact c;
c.m_normal = p/-btSqrt(d); c.m_normal = p/-btSqrt(d);
c.m_margin = m; c.m_margin = m;
c.m_node = node; c.m_node = node;
c.m_face = face; c.m_face = face;
c.m_weights = w; c.m_weights = w;
c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR;
c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR;
psb[0]->m_scontacts.push_back(c); psb[0]->m_scontacts.push_back(c);
} }
} }
} }
btSoftBody* psb[2]; btSoftBody* psb[2];
btScalar mrg; btScalar mrg;
}; };
}; };

View File

@@ -40,8 +40,8 @@ btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm()
/*if (m_ownManifold) /*if (m_ownManifold)
{ {
if (m_manifoldPtr) if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr); m_dispatcher->releaseManifold(m_manifoldPtr);
} }
*/ */

View File

@@ -28,8 +28,8 @@ class btSoftBody;
/// btSoftRigidCollisionAlgorithm provides collision detection between btSoftBody and btRigidBody /// btSoftRigidCollisionAlgorithm provides collision detection between btSoftBody and btRigidBody
class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm
{ {
// bool m_ownManifold; // bool m_ownManifold;
// btPersistentManifold* m_manifoldPtr; // btPersistentManifold* m_manifoldPtr;
btSoftBody* m_softBody; btSoftBody* m_softBody;
btCollisionObject* m_rigidCollisionObject; btCollisionObject* m_rigidCollisionObject;

View File

@@ -28,14 +28,14 @@ subject to the following restrictions:
btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
{ {
m_drawFlags = fDrawFlags::Std; m_drawFlags = fDrawFlags::Std;
m_drawNodeTree = true; m_drawNodeTree = true;
m_drawFaceTree = false; m_drawFaceTree = false;
m_drawClusterTree = false; m_drawClusterTree = false;
m_sbi.m_broadphase = pairCache; m_sbi.m_broadphase = pairCache;
m_sbi.m_dispatcher = dispatcher; m_sbi.m_dispatcher = dispatcher;
m_sbi.m_sparsesdf.Initialize(); m_sbi.m_sparsesdf.Initialize();
m_sbi.m_sparsesdf.Reset(); m_sbi.m_sparsesdf.Reset();
} }
@@ -84,9 +84,9 @@ void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints()
BT_PROFILE("solveSoftConstraints"); BT_PROFILE("solveSoftConstraints");
if(m_softBodies.size()) if(m_softBodies.size())
{ {
btSoftBody::solveClusters(m_softBodies); btSoftBody::solveClusters(m_softBodies);
} }
for(int i=0;i<m_softBodies.size();++i) for(int i=0;i<m_softBodies.size();++i)
{ {
@@ -100,8 +100,8 @@ void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body)
m_softBodies.push_back(body); m_softBodies.push_back(body);
btCollisionWorld::addCollisionObject(body, btCollisionWorld::addCollisionObject(body,
btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::DefaultFilter,
btBroadphaseProxy::AllFilter); btBroadphaseProxy::AllFilter);
} }

View File

@@ -23,44 +23,44 @@ subject to the following restrictions:
// Modified Paul Hsieh hash // Modified Paul Hsieh hash
template <const int DWORDLEN> template <const int DWORDLEN>
unsigned int HsiehHash(const void* pdata) unsigned int HsiehHash(const void* pdata)
{ {
const unsigned short* data=(const unsigned short*)pdata; const unsigned short* data=(const unsigned short*)pdata;
unsigned hash=DWORDLEN<<2,tmp; unsigned hash=DWORDLEN<<2,tmp;
for(int i=0;i<DWORDLEN;++i) for(int i=0;i<DWORDLEN;++i)
{ {
hash += data[0]; hash += data[0];
tmp = (data[1]<<11)^hash; tmp = (data[1]<<11)^hash;
hash = (hash<<16)^tmp; hash = (hash<<16)^tmp;
data += 2; data += 2;
hash += hash>>11; hash += hash>>11;
} }
hash^=hash<<3;hash+=hash>>5; hash^=hash<<3;hash+=hash>>5;
hash^=hash<<4;hash+=hash>>17; hash^=hash<<4;hash+=hash>>17;
hash^=hash<<25;hash+=hash>>6; hash^=hash<<25;hash+=hash>>6;
return(hash); return(hash);
} }
template <const int CELLSIZE> template <const int CELLSIZE>
struct btSparseSdf struct btSparseSdf
{ {
// //
// Inner types // Inner types
// //
struct IntFrac struct IntFrac
{ {
int b; int b;
int i; int i;
btScalar f; btScalar f;
}; };
struct Cell struct Cell
{ {
btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1];
int c[3]; int c[3];
int puid; int puid;
unsigned hash; unsigned hash;
btCollisionShape* pclient; btCollisionShape* pclient;
Cell* next; Cell* next;
}; };
// //
// Fields // Fields
// //
@@ -78,84 +78,84 @@ struct btSparseSdf
// //
void Initialize(int hashsize=2383) void Initialize(int hashsize=2383)
{ {
cells.resize(hashsize,0); cells.resize(hashsize,0);
Reset(); Reset();
} }
// //
void Reset() void Reset()
{ {
for(int i=0,ni=cells.size();i<ni;++i) for(int i=0,ni=cells.size();i<ni;++i)
{ {
Cell* pc=cells[i]; Cell* pc=cells[i];
cells[i]=0; cells[i]=0;
while(pc) while(pc)
{ {
Cell* pn=pc->next; Cell* pn=pc->next;
delete pc; delete pc;
pc=pn; pc=pn;
}
} }
}
voxelsz =0.25; voxelsz =0.25;
puid =0; puid =0;
ncells =0; ncells =0;
nprobes =1; nprobes =1;
nqueries =1; nqueries =1;
} }
// //
void GarbageCollect(int lifetime=256) void GarbageCollect(int lifetime=256)
{ {
const int life=puid-lifetime; const int life=puid-lifetime;
for(int i=0;i<cells.size();++i) for(int i=0;i<cells.size();++i)
{ {
Cell*& root=cells[i]; Cell*& root=cells[i];
Cell* pp=0; Cell* pp=0;
Cell* pc=root; Cell* pc=root;
while(pc) while(pc)
{ {
Cell* pn=pc->next; Cell* pn=pc->next;
if(pc->puid<life) if(pc->puid<life)
{ {
if(pp) pp->next=pn; else root=pn; if(pp) pp->next=pn; else root=pn;
delete pc;pc=pp;--ncells; delete pc;pc=pp;--ncells;
}
pp=pc;pc=pn;
} }
pp=pc;pc=pn;
} }
}
//printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries); //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
nqueries=1; nqueries=1;
nprobes=1; nprobes=1;
++puid; /* TODO: Reset puid's when int range limit is reached */ ++puid; /* TODO: Reset puid's when int range limit is reached */
/* else setup a priority list... */ /* else setup a priority list... */
} }
// //
int RemoveReferences(btCollisionShape* pcs) int RemoveReferences(btCollisionShape* pcs)
{ {
int refcount=0; int refcount=0;
for(int i=0;i<cells.size();++i) for(int i=0;i<cells.size();++i)
{ {
Cell*& root=cells[i]; Cell*& root=cells[i];
Cell* pp=0; Cell* pp=0;
Cell* pc=root; Cell* pc=root;
while(pc) while(pc)
{ {
Cell* pn=pc->next; Cell* pn=pc->next;
if(pc->pclient==pcs) if(pc->pclient==pcs)
{ {
if(pp) pp->next=pn; else root=pn; if(pp) pp->next=pn; else root=pn;
delete pc;pc=pp;++refcount; delete pc;pc=pp;++refcount;
}
pp=pc;pc=pn;
} }
pp=pc;pc=pn;
} }
return(refcount);
} }
return(refcount);
}
// //
btScalar Evaluate( const btVector3& x, btScalar Evaluate( const btVector3& x,
btCollisionShape* shape, btCollisionShape* shape,
btVector3& normal, btVector3& normal,
btScalar margin) btScalar margin)
{ {
/* Lookup cell */ /* Lookup cell */
const btVector3 scx=x/voxelsz; const btVector3 scx=x/voxelsz;
const IntFrac ix=Decompose(scx.x()); const IntFrac ix=Decompose(scx.x());
@@ -166,19 +166,19 @@ struct btSparseSdf
Cell* c=root; Cell* c=root;
++nqueries; ++nqueries;
while(c) while(c)
{ {
++nprobes; ++nprobes;
if( (c->hash==h) && if( (c->hash==h) &&
(c->c[0]==ix.b) && (c->c[0]==ix.b) &&
(c->c[1]==iy.b) && (c->c[1]==iy.b) &&
(c->c[2]==iz.b) && (c->c[2]==iz.b) &&
(c->pclient==shape)) (c->pclient==shape))
{ break; } { break; }
else else
{ c=c->next; } { c=c->next; }
} }
if(!c) if(!c)
{ {
++nprobes; ++nprobes;
++ncells; ++ncells;
c=new Cell(); c=new Cell();
@@ -187,82 +187,82 @@ struct btSparseSdf
c->hash=h; c->hash=h;
c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b; c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b;
BuildCell(*c); BuildCell(*c);
} }
c->puid=puid; c->puid=puid;
/* Extract infos */ /* Extract infos */
const int o[]={ ix.i,iy.i,iz.i}; const int o[]={ ix.i,iy.i,iz.i};
const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0], const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0],
c->d[o[0]+1][o[1]+0][o[2]+0], c->d[o[0]+1][o[1]+0][o[2]+0],
c->d[o[0]+1][o[1]+1][o[2]+0], c->d[o[0]+1][o[1]+1][o[2]+0],
c->d[o[0]+0][o[1]+1][o[2]+0], c->d[o[0]+0][o[1]+1][o[2]+0],
c->d[o[0]+0][o[1]+0][o[2]+1], c->d[o[0]+0][o[1]+0][o[2]+1],
c->d[o[0]+1][o[1]+0][o[2]+1], c->d[o[0]+1][o[1]+0][o[2]+1],
c->d[o[0]+1][o[1]+1][o[2]+1], c->d[o[0]+1][o[1]+1][o[2]+1],
c->d[o[0]+0][o[1]+1][o[2]+1]}; c->d[o[0]+0][o[1]+1][o[2]+1]};
/* Normal */ /* Normal */
#if 1 #if 1
const btScalar gx[]={ d[1]-d[0],d[2]-d[3], const btScalar gx[]={ d[1]-d[0],d[2]-d[3],
d[5]-d[4],d[6]-d[7]}; d[5]-d[4],d[6]-d[7]};
const btScalar gy[]={ d[3]-d[0],d[2]-d[1], const btScalar gy[]={ d[3]-d[0],d[2]-d[1],
d[7]-d[4],d[6]-d[5]}; d[7]-d[4],d[6]-d[5]};
const btScalar gz[]={ d[4]-d[0],d[5]-d[1], const btScalar gz[]={ d[4]-d[0],d[5]-d[1],
d[7]-d[3],d[6]-d[2]}; d[7]-d[3],d[6]-d[2]};
normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f), normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f),
Lerp(gx[2],gx[3],iy.f),iz.f)); Lerp(gx[2],gx[3],iy.f),iz.f));
normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f), normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f),
Lerp(gy[2],gy[3],ix.f),iz.f)); Lerp(gy[2],gy[3],ix.f),iz.f));
normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f), normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f),
Lerp(gz[2],gz[3],ix.f),iy.f)); Lerp(gz[2],gz[3],ix.f),iy.f));
normal = normal.normalized(); normal = normal.normalized();
#else #else
normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized(); normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized();
#endif #endif
/* Distance */ /* Distance */
const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f), const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f),
Lerp(d[3],d[2],ix.f),iy.f); Lerp(d[3],d[2],ix.f),iy.f);
const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f), const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f),
Lerp(d[7],d[6],ix.f),iy.f); Lerp(d[7],d[6],ix.f),iy.f);
return(Lerp(d0,d1,iz.f)-margin); return(Lerp(d0,d1,iz.f)-margin);
} }
// //
void BuildCell(Cell& c) void BuildCell(Cell& c)
{ {
const btVector3 org=btVector3( (btScalar)c.c[0], const btVector3 org=btVector3( (btScalar)c.c[0],
(btScalar)c.c[1], (btScalar)c.c[1],
(btScalar)c.c[2]) * (btScalar)c.c[2]) *
CELLSIZE*voxelsz; CELLSIZE*voxelsz;
for(int k=0;k<=CELLSIZE;++k) for(int k=0;k<=CELLSIZE;++k)
{ {
const btScalar z=voxelsz*k+org.z(); const btScalar z=voxelsz*k+org.z();
for(int j=0;j<=CELLSIZE;++j) for(int j=0;j<=CELLSIZE;++j)
{ {
const btScalar y=voxelsz*j+org.y(); const btScalar y=voxelsz*j+org.y();
for(int i=0;i<=CELLSIZE;++i) for(int i=0;i<=CELLSIZE;++i)
{ {
const btScalar x=voxelsz*i+org.x(); const btScalar x=voxelsz*i+org.x();
c.d[i][j][k]=DistanceToShape( btVector3(x,y,z), c.d[i][j][k]=DistanceToShape( btVector3(x,y,z),
c.pclient); c.pclient);
}
} }
} }
} }
}
// //
static inline btScalar DistanceToShape(const btVector3& x, static inline btScalar DistanceToShape(const btVector3& x,
btCollisionShape* shape) btCollisionShape* shape)
{ {
btTransform unit; btTransform unit;
unit.setIdentity(); unit.setIdentity();
if(shape->isConvex()) if(shape->isConvex())
{ {
btGjkEpaSolver2::sResults res; btGjkEpaSolver2::sResults res;
btConvexShape* csh=static_cast<btConvexShape*>(shape); btConvexShape* csh=static_cast<btConvexShape*>(shape);
return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res)); return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res));
}
return(0);
} }
return(0);
}
// //
static inline IntFrac Decompose(btScalar x) static inline IntFrac Decompose(btScalar x)
{ {
/* That one need a lot of improvements... */ /* That one need a lot of improvements... */
/* Remove test, faster floor... */ /* Remove test, faster floor... */
IntFrac r; IntFrac r;
@@ -272,18 +272,18 @@ struct btSparseSdf
const btScalar k=(x-r.b)*CELLSIZE; const btScalar k=(x-r.b)*CELLSIZE;
r.i=(int)k;r.f=k-r.i;r.b-=o; r.i=(int)k;r.f=k-r.i;r.b-=o;
return(r); return(r);
} }
// //
static inline btScalar Lerp(btScalar a,btScalar b,btScalar t) static inline btScalar Lerp(btScalar a,btScalar b,btScalar t)
{ {
return(a+(b-a)*t); return(a+(b-a)*t);
} }
// //
static inline unsigned int Hash(int x,int y,int z,btCollisionShape* shape) static inline unsigned int Hash(int x,int y,int z,btCollisionShape* shape)
{ {
struct btS struct btS
{ {
int x,y,z; int x,y,z;
@@ -299,7 +299,7 @@ struct btSparseSdf
return result; return result;
} }
}; };