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

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files

754
Demos/SoftDemo/SoftDemo.cpp
View File

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

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

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

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

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

1922
src/BulletSoftBody/btSoftBody.cpp
View File

File diff suppressed because it is too large Load Diff

188
src/BulletSoftBody/btSoftBody.h
View File

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

52
src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp
View File

@@ -50,24 +50,24 @@ btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
m_dispatcher(dispatcher),
m_dispatchInfoPtr(0)
m_dispatcher(dispatcher),
m_dispatchInfoPtr(0)
{
m_softBody = (btSoftBody*) (isSwapped? body1:body0);
m_triBody = isSwapped? body0:body1;
//
// create the manifold from the dispatcher 'manifold pool'
//
// m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
//
// create the manifold from the dispatcher 'manifold pool'
//
// m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
clearCache();
clearCache();
}
btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
{
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)
{
//just for debugging purposes
//just for debugging purposes
//printf("triangle %d",m_triangleCount++);
btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
btCollisionAlgorithmConstructionInfo ci;
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)
{
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);
// 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]);
normal.normalize();
normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION;
// other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
// other+=normal*22.f;
// other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
// other+=normal*22.f;
btVector3 pts[6] = {triangle[0]+normal,
triangle[1]+normal,
triangle[2]+normal,
triangle[0]-normal,
triangle[1]-normal,
triangle[2]-normal};
triangle[2]+normal,
triangle[0]-normal,
triangle[1]-normal,
triangle[2]-normal};
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]);
// tm.setMargin(m_collisionMarginTriangle);
// tm.setMargin(m_collisionMarginTriangle);
//copy over user pointers to temporary shape
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);
//m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
// cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
// cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
// cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
// cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
colAlgo->~btCollisionAlgorithm();
ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
@@ -229,22 +229,22 @@ void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* b
btCollisionObject* triOb = triBody;
btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
// if (convexBody->getCollisionShape()->isConvex())
// if (convexBody->getCollisionShape()->isConvex())
{
btScalar collisionMarginTriangle = concaveShape->getMargin();
// resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
// resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
//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_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
// m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
// resultOut->refreshContactPoints();
// resultOut->refreshContactPoints();
}

4
src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h
View File

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

472
src/BulletSoftBody/btSoftBodyHelpers.cpp
View File

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

90
src/BulletSoftBody/btSoftBodyHelpers.h
View File

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

698
src/BulletSoftBody/btSoftBodyInternals.h
View File

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

4
src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp
View File

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

4
src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h
View File

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

24
src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
View File

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

162
src/BulletSoftBody/btSparseSDF.h
View File

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