Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Files
3ca977e39f006a84d01e1f12e9d7745d422afc73
bullet3/src/BulletSoftBody/btSoftBody.cpp
erwin.coumans 3ca977e39f Patch by Nathanael Presson: (needs some more work, some of the demos don't show properly (RayCaster, CollisionInterfaceDemo,'g' key is already occupied in ConcaveDemo etc) 
btDbvtBroadphase:
- Fixed a performance issues reported by 'reltham'
- Added btDbvtBroadphase::optimize() for people who want good performances right away or don't do dynamics.
- fixed compilation issues when DBVT_BP_PROFILE was set.

btSoftBody:
- Fixed singular matrix issues related to polar decomposition (flat meshes).

DemoApplication:
- Shadows (enable/disable through 'g' or DemoApplication::setShadows(bool)).
- Texture can be enable/disable through 'u'

CDFramework:
- fixed compilation issues.
2008-07-28 04:33:55 +00:00

2587 lines
61 KiB
C++
Raw Blame History

/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///btSoftBody implementation by Nathanael Presson
#include "btSoftBodyInternals.h"
//
btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m)
:m_worldInfo(worldInfo)
{
/* Init */
m_internalType = CO_SOFT_BODY;
m_cfg.aeromodel = eAeroModel::V_Point;
m_cfg.kVCF = 1;
m_cfg.kDG = 0;
m_cfg.kLF = 0;
m_cfg.kDP = 0;
m_cfg.kPR = 0;
m_cfg.kVC = 0;
m_cfg.kDF = (btScalar)0.2;
m_cfg.kMT = 0;
m_cfg.kCHR = (btScalar)1.0;
m_cfg.kKHR = (btScalar)0.1;
m_cfg.kSHR = (btScalar)1.0;
m_cfg.kAHR = (btScalar)0.7;
m_cfg.kSRHR_CL = (btScalar)0.1;
m_cfg.kSKHR_CL = (btScalar)1;
m_cfg.kSSHR_CL = (btScalar)0.5;
m_cfg.kSR_SPLT_CL = (btScalar)0.5;
m_cfg.kSK_SPLT_CL = (btScalar)0.5;
m_cfg.kSS_SPLT_CL = (btScalar)0.5;
m_cfg.maxvolume = (btScalar)1;
m_cfg.timescale = 1;
m_cfg.viterations = 0;
m_cfg.piterations = 1;
m_cfg.diterations = 0;
m_cfg.citerations = 4;
m_cfg.collisions = fCollision::Default;
m_pose.m_bvolume = false;
m_pose.m_bframe = false;
m_pose.m_volume = 0;
m_pose.m_com = btVector3(0,0,0);
m_pose.m_rot.setIdentity();
m_pose.m_scl.setIdentity();
m_tag = 0;
m_timeacc = 0;
m_bUpdateRtCst = true;
m_bounds[0] = btVector3(0,0,0);
m_bounds[1] = btVector3(0,0,0);
m_worldTransform.setIdentity();
setSolver(eSolverPresets::Positions);
/* Default material */
Material* pm=appendMaterial();
pm->m_kLST = 1;
pm->m_kAST = 1;
pm->m_kVST = 1;
pm->m_flags = fMaterial::Default;
/* Collision shape */
///for now, create a collision shape internally
setCollisionShape(new btSoftBodyCollisionShape(this));
m_collisionShape->setMargin(0.25);
/* Nodes */
const btScalar margin=getCollisionShape()->getMargin();
m_nodes.resize(node_count);
for(int i=0,ni=node_count;i<ni;++i)
{
Node& n=m_nodes[i];
ZeroInitialize(n);
n.m_x = x?*x++:btVector3(0,0,0);
n.m_q = n.m_x;
n.m_im = m?*m++:1;
n.m_im = n.m_im>0?1/n.m_im:0;
n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
n.m_material= pm;
}
updateBounds();
}
//
btSoftBody::~btSoftBody()
{
//for now, delete the internal shape
delete m_collisionShape;
int i;
for(i=0;i<m_materials.size();++i)
btAlignedFree(m_materials[i]);
for(i=0;i<m_joints.size();++i)
btAlignedFree(m_joints[i]);
}
//
bool btSoftBody::checkLink(int node0,int node1) const
{
return(checkLink(&m_nodes[node0],&m_nodes[node1]));
}
//
bool btSoftBody::checkLink(const Node* node0,const Node* node1) const
{
const Node* n[]={node0,node1};
for(int i=0,ni=m_links.size();i<ni;++i)
{
const Link& l=m_links[i];
if( (l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
(l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
{
return(true);
}
}
return(false);
}
//
bool btSoftBody::checkFace(int node0,int node1,int node2) const
{
const Node* n[]={ &m_nodes[node0],
&m_nodes[node1],
&m_nodes[node2]};
for(int i=0,ni=m_faces.size();i<ni;++i)
{
const Face& f=m_faces[i];
int c=0;
for(int j=0;j<3;++j)
{
if( (f.m_n[j]==n[0])||
(f.m_n[j]==n[1])||
(f.m_n[j]==n[2])) c|=1<<j; else break;
}
if(c==7) return(true);
}
return(false);
}
//
btSoftBody::Material* btSoftBody::appendMaterial()
{
Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
if(m_materials.size()>0)
*pm=*m_materials[0];
else
ZeroInitialize(*pm);
m_materials.push_back(pm);
return(pm);
}
//
void btSoftBody::appendNote( const char* text,
const btVector3& o,
const btVector4& c,
Node* n0,
Node* n1,
Node* n2,
Node* n3)
{
Note n;
ZeroInitialize(n);
n.m_rank = 0;
n.m_text = text;
n.m_offset = o;
n.m_coords[0] = c.x();
n.m_coords[1] = c.y();
n.m_coords[2] = c.z();
n.m_coords[3] = c.w();
n.m_nodes[0] = n0;n.m_rank+=n0?1:0;
n.m_nodes[1] = n1;n.m_rank+=n1?1:0;
n.m_nodes[2] = n2;n.m_rank+=n2?1:0;
n.m_nodes[3] = n3;n.m_rank+=n3?1:0;
m_notes.push_back(n);
}
//
void btSoftBody::appendNote( const char* text,
const btVector3& o,
Node* feature)
{
appendNote(text,o,btVector4(1,0,0,0),feature);
}
//
void btSoftBody::appendNote( const char* text,
const btVector3& o,
Link* feature)
{
static const btScalar w=1/(btScalar)2;
appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0],
feature->m_n[1]);
}
//
void btSoftBody::appendNote( const char* text,
const btVector3& o,
Face* feature)
{
static const btScalar w=1/(btScalar)3;
appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0],
feature->m_n[1],
feature->m_n[2]);
}
//
void btSoftBody::appendNode( const btVector3& x,btScalar m)
{
if(m_nodes.capacity()==m_nodes.size())
{
pointersToIndices();
m_nodes.reserve(m_nodes.size()*2+1);
indicesToPointers();
}
const btScalar margin=getCollisionShape()->getMargin();
m_nodes.push_back(Node());
Node& n=m_nodes[m_nodes.size()-1];
ZeroInitialize(n);
n.m_x = x;
n.m_q = n.m_x;
n.m_im = m>0?1/m:0;
n.m_material = m_materials[0];
n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
}
//
void btSoftBody::appendLink(int model,Material* mat)
{
Link l;
if(model>=0)
l=m_links[model];
else
{ ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
m_links.push_back(l);
}
//
void btSoftBody::appendLink( int node0,
int node1,
Material* mat,
bool bcheckexist)
{
appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
}
//
void btSoftBody::appendLink( Node* node0,
Node* node1,
Material* mat,
bool bcheckexist)
{
if((!bcheckexist)||(!checkLink(node0,node1)))
{
appendLink(-1,mat);
Link& l=m_links[m_links.size()-1];
l.m_n[0] = node0;
l.m_n[1] = node1;
l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
m_bUpdateRtCst=true;
}
}
//
void btSoftBody::appendFace(int model,Material* mat)
{
Face f;
if(model>=0)
{ f=m_faces[model]; }
else
{ ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
m_faces.push_back(f);
}
//
void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
{
appendFace(-1,mat);
Face& f=m_faces[m_faces.size()-1];
btAssert(node0!=node1);
btAssert(node1!=node2);
btAssert(node2!=node0);
f.m_n[0] = &m_nodes[node0];
f.m_n[1] = &m_nodes[node1];
f.m_n[2] = &m_nodes[node2];
f.m_ra = AreaOf( f.m_n[0]->m_x,
f.m_n[1]->m_x,
f.m_n[2]->m_x);
m_bUpdateRtCst=true;
}
//
void btSoftBody::appendAnchor(int node,btRigidBody* body)
{
Anchor a;
a.m_node = &m_nodes[node];
a.m_body = body;
a.m_local = body->getInterpolationWorldTransform().inverse()*a.m_node->m_x;
a.m_node->m_battach = 1;
m_anchors.push_back(a);
}
//
void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
{
LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
pj->m_bodies[0] = body0;
pj->m_bodies[1] = body1;
pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position;
pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position;
pj->m_cfm = specs.cfm;
pj->m_erp = specs.erp;
pj->m_split = specs.split;
m_joints.push_back(pj);
}
//
void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
{
appendLinearJoint(specs,m_clusters[0],body);
}
//
void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
{
appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
}
//
void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
{
AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
pj->m_bodies[0] = body0;
pj->m_bodies[1] = body1;
pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
pj->m_cfm = specs.cfm;
pj->m_erp = specs.erp;
pj->m_split = specs.split;
pj->m_icontrol = specs.icontrol;
m_joints.push_back(pj);
}
//
void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
{
appendAngularJoint(specs,m_clusters[0],body);
}
//
void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
{
appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
}
//
void btSoftBody::addForce(const btVector3& force)
{
for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
}
//
void btSoftBody::addForce(const btVector3& force,int node)
{
Node& n=m_nodes[node];
if(n.m_im>0)
{
n.m_f += force;
}
}
//
void btSoftBody::addVelocity(const btVector3& velocity)
{
for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
}
//
void btSoftBody::addVelocity(const btVector3& velocity,int node)
{
Node& n=m_nodes[node];
if(n.m_im>0)
{
n.m_v += velocity;
}
}
//
void btSoftBody::setMass(int node,btScalar mass)
{
m_nodes[node].m_im=mass>0?1/mass:0;
m_bUpdateRtCst=true;
}
//
btScalar btSoftBody::getMass(int node) const
{
return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
}
//
btScalar btSoftBody::getTotalMass() const
{
btScalar mass=0;
for(int i=0;i<m_nodes.size();++i)
{
mass+=getMass(i);
}
return(mass);
}
//
void btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
{
int i;
if(fromfaces)
{
for(i=0;i<m_nodes.size();++i)
{
m_nodes[i].m_im=0;
}
for(i=0;i<m_faces.size();++i)
{
const Face& f=m_faces[i];
const btScalar twicearea=AreaOf( f.m_n[0]->m_x,
f.m_n[1]->m_x,
f.m_n[2]->m_x);
for(int j=0;j<3;++j)
{
f.m_n[j]->m_im+=twicearea;
}
}
for( i=0;i<m_nodes.size();++i)
{
m_nodes[i].m_im=1/m_nodes[i].m_im;
}
}
const btScalar tm=getTotalMass();
const btScalar itm=1/tm;
for( i=0;i<m_nodes.size();++i)
{
m_nodes[i].m_im/=itm*mass;
}
m_bUpdateRtCst=true;
}
//
void btSoftBody::setTotalDensity(btScalar density)
{
setTotalMass(getVolume()*density,true);
}
//
void btSoftBody::transform(const btTransform& trs)
{
const btScalar margin=getCollisionShape()->getMargin();
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_x=trs*n.m_x;
n.m_q=trs*n.m_q;
n.m_n=trs.getBasis()*n.m_n;
m_ndbvt.update(n.m_leaf,btDbvtVolume::FromCR(n.m_x,margin));
}
updateNormals();
updateBounds();
updateConstants();
}
//
void btSoftBody::translate(const btVector3& trs)
{
btTransform t;
t.setIdentity();
t.setOrigin(trs);
transform(t);
}
//
void btSoftBody::rotate( const btQuaternion& rot)
{
btTransform t;
t.setIdentity();
t.setRotation(rot);
transform(t);
}
//
void btSoftBody::scale(const btVector3& scl)
{
const btScalar margin=getCollisionShape()->getMargin();
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_x*=scl;
n.m_q*=scl;
m_ndbvt.update(n.m_leaf,btDbvtVolume::FromCR(n.m_x,margin));
}
updateNormals();
updateBounds();
updateConstants();
}
//
void btSoftBody::setPose(bool bvolume,bool bframe)
{
m_pose.m_bvolume = bvolume;
m_pose.m_bframe = bframe;
int i,ni;
/* Weights */
const btScalar omass=getTotalMass();
const btScalar kmass=omass*m_nodes.size()*1000;
btScalar tmass=omass;
m_pose.m_wgh.resize(m_nodes.size());
for(i=0,ni=m_nodes.size();i<ni;++i)
{
if(m_nodes[i].m_im<=0) tmass+=kmass;
}
for( i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
m_pose.m_wgh[i]= n.m_im>0 ?
1/(m_nodes[i].m_im*tmass) :
kmass/tmass;
}
/* Pos */
const btVector3 com=evaluateCom();
m_pose.m_pos.resize(m_nodes.size());
for( i=0,ni=m_nodes.size();i<ni;++i)
{
m_pose.m_pos[i]=m_nodes[i].m_x-com;
}
m_pose.m_volume = bvolume?getVolume():0;
m_pose.m_com = com;
m_pose.m_rot.setIdentity();
m_pose.m_scl.setIdentity();
/* Aqq */
m_pose.m_aqq[0] =
m_pose.m_aqq[1] =
m_pose.m_aqq[2] = btVector3(0,0,0);
for( i=0,ni=m_nodes.size();i<ni;++i)
{
const btVector3& q=m_pose.m_pos[i];
const btVector3 mq=m_pose.m_wgh[i]*q;
m_pose.m_aqq[0]+=mq.x()*q;
m_pose.m_aqq[1]+=mq.y()*q;
m_pose.m_aqq[2]+=mq.z()*q;
}
m_pose.m_aqq=m_pose.m_aqq.inverse();
updateConstants();
}
//
btScalar btSoftBody::getVolume() const
{
btScalar vol=0;
if(m_nodes.size()>0)
{
int i,ni;
const btVector3 org=m_nodes[0].m_x;
for(i=0,ni=m_faces.size();i<ni;++i)
{
const Face& f=m_faces[i];
vol+=dot(f.m_n[0]->m_x-org,cross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
}
vol/=(btScalar)6;
}
return(vol);
}
//
int btSoftBody::clusterCount() const
{
return(m_clusters.size());
}
//
btVector3 btSoftBody::clusterCom(const Cluster* cluster)
{
btVector3 com(0,0,0);
for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
{
com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
}
return(com*cluster->m_imass);
}
//
btVector3 btSoftBody::clusterCom(int cluster) const
{
return(clusterCom(m_clusters[cluster]));
}
//
btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
{
return(cluster->m_lv+cross(cluster->m_av,rpos));
}
//
void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
{
const btVector3 li=cluster->m_imass*impulse;
const btVector3 ai=cluster->m_invwi*cross(rpos,impulse);
cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
cluster->m_nvimpulses++;
}
//
void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
{
const btVector3 li=cluster->m_imass*impulse;
const btVector3 ai=cluster->m_invwi*cross(rpos,impulse);
cluster->m_dimpulses[0]+=li;
cluster->m_dimpulses[1]+=ai;
cluster->m_ndimpulses++;
}
//
void btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
{
if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity);
if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift);
}
//
void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
{
const btVector3 ai=cluster->m_invwi*impulse;
cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
cluster->m_nvimpulses++;
}
//
void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
{
const btVector3 ai=cluster->m_invwi*impulse;
cluster->m_dimpulses[1]+=ai;
cluster->m_ndimpulses++;
}
//
void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
{
if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity);
if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift);
}
//
void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
{
cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
cluster->m_ndimpulses++;
}
//
int btSoftBody::generateBendingConstraints(int distance,Material* mat)
{
int i,j;
if(distance>1)
{
/* Build graph */
const int n=m_nodes.size();
const unsigned inf=(~(unsigned)0)>>1;
unsigned* adj=new unsigned[n*n];
#define IDX(_x_,_y_) ((_y_)*n+(_x_))
for(j=0;j<n;++j)
{
for(i=0;i<n;++i)
{
if(i!=j) adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
else
adj[IDX(i,j)]=adj[IDX(j,i)]=0;
}
}
for( i=0;i<m_links.size();++i)
{
const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
adj[IDX(ia,ib)]=1;
adj[IDX(ib,ia)]=1;
}
for(int k=0;k<n;++k)
{
for(j=0;j<n;++j)
{
for(i=j+1;i<n;++i)
{
const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)];
if(adj[IDX(i,j)]>sum)
{
adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
}
}
}
}
/* Build links */
int nlinks=0;
for(j=0;j<n;++j)
{
for(i=j+1;i<n;++i)
{
if(adj[IDX(i,j)]==(unsigned)distance)
{
appendLink(i,j,mat);
m_links[m_links.size()-1].m_bbending=1;
++nlinks;
}
}
}
delete[] adj;
return(nlinks);
}
return(0);
}
//
void btSoftBody::randomizeConstraints()
{
unsigned long seed=243703;
#define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
int i,ni;
for(i=0,ni=m_links.size();i<ni;++i)
{
btSwap(m_links[i],m_links[NEXTRAND%ni]);
}
for(i=0,ni=m_faces.size();i<ni;++i)
{
btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
}
#undef NEXTRAND
}
//
int btSoftBody::generateClusters(int k,int maxiterations)
{
int i;
for(i=0;i<m_clusters.size();++i)
{
if(m_clusters[i]->m_leaf) m_cdbvt.remove(m_clusters[i]->m_leaf);
btAlignedFree(m_clusters[i]);
}
m_clusters.resize(btMin(k,m_nodes.size()));
for(i=0;i<m_clusters.size();++i)
{
m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
m_clusters[i]->m_collide= true;
}
k=m_clusters.size();
if(k>0)
{
/* Initialize */
btAlignedObjectArray<btVector3> centers;
btVector3 cog(0,0,0);
int i;
for(i=0;i<m_nodes.size();++i)
{
cog+=m_nodes[i].m_x;
m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
}
cog/=(btScalar)m_nodes.size();
centers.resize(k,cog);
/* Iterate */
const btScalar slope=16;
bool changed;
int iterations=0;
do {
const btScalar w=2-btMin<btScalar>(1,iterations/slope);
changed=false;
iterations++;
int i;
for(i=0;i<k;++i)
{
btVector3 c(0,0,0);
for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
{
c+=m_clusters[i]->m_nodes[j]->m_x;
}
if(m_clusters[i]->m_nodes.size())
{
c /= (btScalar)m_clusters[i]->m_nodes.size();
c = centers[i]+(c-centers[i])*w;
changed |= ((c-centers[i]).length2()>SIMD_EPSILON);
centers[i] = c;
m_clusters[i]->m_nodes.resize(0);
}
}
for(i=0;i<m_nodes.size();++i)
{
const btVector3 nx=m_nodes[i].m_x;
int kbest=0;
btScalar kdist=ClusterMetric(centers[0],nx);
for(int j=1;j<k;++j)
{
const btScalar d=ClusterMetric(centers[j],nx);
if(d<kdist)
{
kbest=j;
kdist=d;
}
}
m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
}
} while(changed&&(iterations<maxiterations));
/* Merge */
btAlignedObjectArray<int> cids;
cids.resize(m_nodes.size(),-1);
for(i=0;i<m_clusters.size();++i)
{
for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
{
cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
}
}
for(i=0;i<m_faces.size();++i)
{
const int idx[]={ int(m_faces[i].m_n[0]-&m_nodes[0]),
int(m_faces[i].m_n[1]-&m_nodes[0]),
int(m_faces[i].m_n[2]-&m_nodes[0])};
for(int j=0;j<3;++j)
{
const int cid=cids[idx[j]];
for(int q=1;q<3;++q)
{
const int kid=idx[(j+q)%3];
if(cids[kid]!=cid)
{
if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
{
m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
}
}
}
}
}
/* Master */
if(m_clusters.size()>1)
{
Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
pmaster->m_collide = false;
pmaster->m_nodes.reserve(m_nodes.size());
for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
m_clusters.push_back(pmaster);
btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
}
/* Terminate */
for(i=0;i<m_clusters.size();++i)
{
if(m_clusters[i]->m_nodes.size()==0)
{
btAlignedFree(m_clusters[i]);
btSwap(m_clusters[i],m_clusters[m_clusters.size()-1]);
m_clusters.pop_back();
--i;
}
}
initializeClusters();
updateClusters();
return(m_clusters.size());
}
return(0);
}
//
void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
{
const Node* nbase = &m_nodes[0];
int ncount = m_nodes.size();
btSymMatrix<int> edges(ncount,-2);
int newnodes=0;
int i,j,k,ni;
/* Filter out */
for(i=0;i<m_links.size();++i)
{
Link& l=m_links[i];
if(l.m_bbending)
{
if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
{
btSwap(m_links[i],m_links[m_links.size()-1]);
m_links.pop_back();--i;
}
}
}
/* Fill edges */
for(i=0;i<m_links.size();++i)
{
Link& l=m_links[i];
edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
}
for(i=0;i<m_faces.size();++i)
{
Face& f=m_faces[i];
edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
}
/* Intersect */
for(i=0;i<ncount;++i)
{
for(j=i+1;j<ncount;++j)
{
if(edges(i,j)==-1)
{
Node& a=m_nodes[i];
Node& b=m_nodes[j];
const btScalar t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
if(t>0)
{
const btVector3 x=Lerp(a.m_x,b.m_x,t);
const btVector3 v=Lerp(a.m_v,b.m_v,t);
btScalar m=0;
if(a.m_im>0)
{
if(b.m_im>0)
{
const btScalar ma=1/a.m_im;
const btScalar mb=1/b.m_im;
const btScalar mc=Lerp(ma,mb,t);
const btScalar f=(ma+mb)/(ma+mb+mc);
a.m_im=1/(ma*f);
b.m_im=1/(mb*f);
m=mc*f;
}
else
{ a.m_im/=0.5;m=1/a.m_im; }
}
else
{
if(b.m_im>0)
{ b.m_im/=0.5;m=1/b.m_im; }
else
m=0;
}
appendNode(x,m);
edges(i,j)=m_nodes.size()-1;
m_nodes[edges(i,j)].m_v=v;
++newnodes;
}
}
}
}
nbase=&m_nodes[0];
/* Refine links */
for(i=0,ni=m_links.size();i<ni;++i)
{
Link& feat=m_links[i];
const int idx[]={ int(feat.m_n[0]-nbase),
int(feat.m_n[1]-nbase)};
if((idx[0]<ncount)&&(idx[1]<ncount))
{
const int ni=edges(idx[0],idx[1]);
if(ni>0)
{
appendLink(i);
Link* pft[]={ &m_links[i],
&m_links[m_links.size()-1]};
pft[0]->m_n[0]=&m_nodes[idx[0]];
pft[0]->m_n[1]=&m_nodes[ni];
pft[1]->m_n[0]=&m_nodes[ni];
pft[1]->m_n[1]=&m_nodes[idx[1]];
}
}
}
/* Refine faces */
for(i=0;i<m_faces.size();++i)
{
const Face& feat=m_faces[i];
const int idx[]={ int(feat.m_n[0]-nbase),
int(feat.m_n[1]-nbase),
int(feat.m_n[2]-nbase)};
for(j=2,k=0;k<3;j=k++)
{
if((idx[j]<ncount)&&(idx[k]<ncount))
{
const int ni=edges(idx[j],idx[k]);
if(ni>0)
{
appendFace(i);
const int l=(k+1)%3;
Face* pft[]={ &m_faces[i],
&m_faces[m_faces.size()-1]};
pft[0]->m_n[0]=&m_nodes[idx[l]];
pft[0]->m_n[1]=&m_nodes[idx[j]];
pft[0]->m_n[2]=&m_nodes[ni];
pft[1]->m_n[0]=&m_nodes[ni];
pft[1]->m_n[1]=&m_nodes[idx[k]];
pft[1]->m_n[2]=&m_nodes[idx[l]];
appendLink(ni,idx[l],pft[0]->m_material);
--i;break;
}
}
}
}
/* Cut */
if(cut)
{
btAlignedObjectArray<int> cnodes;
const int pcount=ncount;
int i;
ncount=m_nodes.size();
cnodes.resize(ncount,0);
/* Nodes */
for(i=0;i<ncount;++i)
{
const btVector3 x=m_nodes[i].m_x;
if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
{
const btVector3 v=m_nodes[i].m_v;
btScalar m=getMass(i);
if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; }
appendNode(x,m);
cnodes[i]=m_nodes.size()-1;
m_nodes[cnodes[i]].m_v=v;
}
}
nbase=&m_nodes[0];
/* Links */
for(i=0,ni=m_links.size();i<ni;++i)
{
const int id[]={ int(m_links[i].m_n[0]-nbase),
int(m_links[i].m_n[1]-nbase)};
int todetach=0;
if(cnodes[id[0]]&&cnodes[id[1]])
{
appendLink(i);
todetach=m_links.size()-1;
}
else
{
if(( (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
(ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
todetach=i;
}
if(todetach)
{
Link& l=m_links[todetach];
for(int j=0;j<2;++j)
{
int cn=cnodes[int(l.m_n[j]-nbase)];
if(cn) l.m_n[j]=&m_nodes[cn];
}
}
}
/* Faces */
for(i=0,ni=m_faces.size();i<ni;++i)
{
Node** n= m_faces[i].m_n;
if( (ifn->Eval(n[0]->m_x)<accurary)&&
(ifn->Eval(n[1]->m_x)<accurary)&&
(ifn->Eval(n[2]->m_x)<accurary))
{
for(int j=0;j<3;++j)
{
int cn=cnodes[int(n[j]-nbase)];
if(cn) n[j]=&m_nodes[cn];
}
}
}
/* Clean orphans */
int nnodes=m_nodes.size();
btAlignedObjectArray<int> ranks;
btAlignedObjectArray<int> todelete;
ranks.resize(nnodes,0);
for(i=0,ni=m_links.size();i<ni;++i)
{
for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
}
for(i=0,ni=m_faces.size();i<ni;++i)
{
for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
}
for(i=0;i<m_links.size();++i)
{
const int id[]={ int(m_links[i].m_n[0]-nbase),
int(m_links[i].m_n[1]-nbase)};
const bool sg[]={ ranks[id[0]]==1,
ranks[id[1]]==1};
if(sg[0]||sg[1])
{
--ranks[id[0]];
--ranks[id[1]];
btSwap(m_links[i],m_links[m_links.size()-1]);
m_links.pop_back();--i;
}
}
#if 0
for(i=nnodes-1;i>=0;--i)
{
if(!ranks[i]) todelete.push_back(i);
}
if(todelete.size())
{
btAlignedObjectArray<int>& map=ranks;
for(int i=0;i<nnodes;++i) map[i]=i;
PointersToIndices(this);
for(int i=0,ni=todelete.size();i<ni;++i)
{
int j=todelete[i];
int& a=map[j];
int& b=map[--nnodes];
m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
btSwap(m_nodes[a],m_nodes[b]);
j=a;a=b;b=j;
}
IndicesToPointers(this,&map[0]);
m_nodes.resize(nnodes);
}
#endif
}
m_bUpdateRtCst=true;
}
//
bool btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
{
return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
}
//
bool btSoftBody::cutLink(int node0,int node1,btScalar position)
{
bool done=false;
int i,ni;
const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
const btScalar m=1;
appendNode(x,m);
appendNode(x,m);
Node* pa=&m_nodes[node0];
Node* pb=&m_nodes[node1];
Node* pn[2]={ &m_nodes[m_nodes.size()-2],
&m_nodes[m_nodes.size()-1]};
pn[0]->m_v=v;
pn[1]->m_v=v;
for(i=0,ni=m_links.size();i<ni;++i)
{
const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
if(mtch!=-1)
{
appendLink(i);
Link* pft[]={&m_links[i],&m_links[m_links.size()-1]};
pft[0]->m_n[1]=pn[mtch];
pft[1]->m_n[0]=pn[1-mtch];
done=true;
}
}
for(i=0,ni=m_faces.size();i<ni;++i)
{
for(int k=2,l=0;l<3;k=l++)
{
const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
if(mtch!=-1)
{
appendFace(i);
Face* pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
pft[0]->m_n[l]=pn[mtch];
pft[1]->m_n[k]=pn[1-mtch];
appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
}
}
}
if(!done)
{
m_ndbvt.remove(pn[0]->m_leaf);
m_ndbvt.remove(pn[1]->m_leaf);
m_nodes.pop_back();
m_nodes.pop_back();
}
return(done);
}
//
bool btSoftBody::rayCast(const btVector3& org,
const btVector3& dir,
sRayCast& results,
btScalar maxtime)
{
if(m_faces.size()&&m_fdbvt.empty()) initializeFaceTree();
results.body = this;
results.time = maxtime;
results.feature = eFeature::None;
results.index = -1;
return(rayCast(org,dir,results.time,results.feature,results.index,false)!=0);
}
//
void btSoftBody::setSolver(eSolverPresets::_ preset)
{
m_cfg.m_vsequence.clear();
m_cfg.m_psequence.clear();
m_cfg.m_dsequence.clear();
switch(preset)
{
case eSolverPresets::Positions:
m_cfg.m_psequence.push_back(ePSolver::Anchors);
m_cfg.m_psequence.push_back(ePSolver::RContacts);
m_cfg.m_psequence.push_back(ePSolver::SContacts);
m_cfg.m_psequence.push_back(ePSolver::Linear);
break;
case eSolverPresets::Velocities:
m_cfg.m_vsequence.push_back(eVSolver::Linear);
m_cfg.m_psequence.push_back(ePSolver::Anchors);
m_cfg.m_psequence.push_back(ePSolver::RContacts);
m_cfg.m_psequence.push_back(ePSolver::SContacts);
m_cfg.m_dsequence.push_back(ePSolver::Linear);
break;
}
}
//
void btSoftBody::predictMotion(btScalar dt)
{
int i,ni;
/* Update */
if(m_bUpdateRtCst)
{
m_bUpdateRtCst=false;
updateConstants();
m_fdbvt.clear();
if(m_cfg.collisions&fCollision::VF_SS)
{
initializeFaceTree();
}
}
/* Prepare */
m_sst.sdt = dt*m_cfg.timescale;
m_sst.isdt = 1/m_sst.sdt;
m_sst.velmrg = m_sst.sdt*3;
m_sst.radmrg = getCollisionShape()->getMargin();
m_sst.updmrg = m_sst.radmrg*(btScalar)0.25;
/* Forces */
addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
applyForces();
/* Integrate */
for(i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_q = n.m_x;
n.m_v += n.m_f*n.m_im*m_sst.sdt;
n.m_x += n.m_v*m_sst.sdt;
n.m_f = btVector3(0,0,0);
}
/* Clusters */
updateClusters();
/* Bounds */
updateBounds();
/* Nodes */
for(i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
m_ndbvt.update( n.m_leaf,
btDbvtVolume::FromCR(n.m_x,m_sst.radmrg),
n.m_v*m_sst.velmrg,
m_sst.updmrg);
}
/* Faces */
if(!m_fdbvt.empty())
{
for(int i=0;i<m_faces.size();++i)
{
Face& f=m_faces[i];
const btVector3 v=( f.m_n[0]->m_v+
f.m_n[1]->m_v+
f.m_n[2]->m_v)/3;
m_fdbvt.update( f.m_leaf,
VolumeOf(f,m_sst.radmrg),
v*m_sst.velmrg,
m_sst.updmrg);
}
}
/* Pose */
updatePose();
/* Match */
if(m_pose.m_bframe&&(m_cfg.kMT>0))
{
const btMatrix3x3 posetrs=m_pose.m_rot;
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
if(n.m_im>0)
{
const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
}
}
}
/* Clear contacts */
m_rcontacts.resize(0);
m_scontacts.resize(0);
/* Optimize dbvt's */
m_ndbvt.optimizeIncremental(1);
m_fdbvt.optimizeIncremental(1);
m_cdbvt.optimizeIncremental(1);
}
//
void btSoftBody::solveConstraints()
{
/* Apply clusters */
applyClusters(false);
/* Prepare links */
int i,ni;
for(i=0,ni=m_links.size();i<ni;++i)
{
Link& l=m_links[i];
l.m_c3 = l.m_n[1]->m_q-l.m_n[0]->m_q;
l.m_c2 = 1/(l.m_c3.length2()*l.m_c0);
}
/* Prepare anchors */
for(i=0,ni=m_anchors.size();i<ni;++i)
{
Anchor& a=m_anchors[i];
const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
a.m_c0 = ImpulseMatrix( m_sst.sdt,
a.m_node->m_im,
a.m_body->getInvMass(),
a.m_body->getInvInertiaTensorWorld(),
ra);
a.m_c1 = ra;
a.m_c2 = m_sst.sdt*a.m_node->m_im;
a.m_body->activate();
}
/* Solve velocities */
if(m_cfg.viterations>0)
{
/* Solve */
for(int isolve=0;isolve<m_cfg.viterations;++isolve)
{
for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
{
getSolver(m_cfg.m_vsequence[iseq])(this,1);
}
}
/* Update */
for(i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_x = n.m_q+n.m_v*m_sst.sdt;
}
}
/* Solve positions */
if(m_cfg.piterations>0)
{
for(int isolve=0;isolve<m_cfg.piterations;++isolve)
{
const btScalar ti=isolve/(btScalar)m_cfg.piterations;
for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
{
getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
}
}
const btScalar vc=m_sst.isdt*(1-m_cfg.kDP);
for(i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_v = (n.m_x-n.m_q)*vc;
n.m_f = btVector3(0,0,0);
}
}
/* Solve drift */
if(m_cfg.diterations>0)
{
const btScalar vcf=m_cfg.kVCF*m_sst.isdt;
for(i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_q = n.m_x;
}
for(int idrift=0;idrift<m_cfg.diterations;++idrift)
{
for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
{
getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
}
}
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_v += (n.m_x-n.m_q)*vcf;
}
}
/* Apply clusters */
dampClusters();
applyClusters(true);
}
//
void btSoftBody::staticSolve(int iterations)
{
for(int isolve=0;isolve<iterations;++isolve)
{
for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
{
getSolver(m_cfg.m_psequence[iseq])(this,1,0);
}
}
}
//
void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
{
/// placeholder
}
//
void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
{
const int nb=bodies.size();
int iterations=0;
int i;
for(i=0;i<nb;++i)
{
iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
}
for(i=0;i<nb;++i)
{
bodies[i]->prepareClusters(iterations);
}
for(i=0;i<iterations;++i)
{
const btScalar sor=1;
for(int j=0;j<nb;++j)
{
bodies[j]->solveClusters(sor);
}
}
for(i=0;i<nb;++i)
{
bodies[i]->cleanupClusters();
}
}
//
void btSoftBody::integrateMotion()
{
/* Update */
updateNormals();
}
//
btSoftBody::RayCaster::RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt)
{
o = org;
d = dir;
mint = mxt;
face = 0;
tests = 0;
}
//
void btSoftBody::RayCaster::Process(const btDbvtNode* leaf)
{
btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data;
const btScalar t=rayTriangle( o,d,
f.m_n[0]->m_x,
f.m_n[1]->m_x,
f.m_n[2]->m_x,
mint);
if((t>0)&&(t<mint)) { mint=t;face=&f; }
++tests;
}
//
btScalar btSoftBody::RayCaster::rayTriangle( const btVector3& org,
const btVector3& dir,
const btVector3& a,
const btVector3& b,
const btVector3& c,
btScalar maxt)
{
static const btScalar ceps=-SIMD_EPSILON*10;
static const btScalar teps=SIMD_EPSILON*10;
const btVector3 n=cross(b-a,c-a);
const btScalar d=dot(a,n);
const btScalar den=dot(dir,n);
if(!btFuzzyZero(den))
{
const btScalar num=dot(org,n)-d;
const btScalar t=-num/den;
if((t>teps)&&(t<maxt))
{
const btVector3 hit=org+dir*t;
if( (dot(n,cross(a-hit,b-hit))>ceps) &&
(dot(n,cross(b-hit,c-hit))>ceps) &&
(dot(n,cross(c-hit,a-hit))>ceps))
{
return(t);
}
}
}
return(-1);
}
//
void btSoftBody::pointersToIndices()
{
#define PTR2IDX(_p_,_b_) reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
btSoftBody::Node* base=&m_nodes[0];
int i,ni;
for(i=0,ni=m_nodes.size();i<ni;++i)
{
if(m_nodes[i].m_leaf)
{
m_nodes[i].m_leaf->data=*(void**)&i;
}
}
for(i=0,ni=m_links.size();i<ni;++i)
{
m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
}
for(i=0,ni=m_faces.size();i<ni;++i)
{
m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
if(m_faces[i].m_leaf)
{
m_faces[i].m_leaf->data=*(void**)&i;
}
}
for(i=0,ni=m_anchors.size();i<ni;++i)
{
m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
}
for(i=0,ni=m_notes.size();i<ni;++i)
{
for(int j=0;j<m_notes[i].m_rank;++j)
{
m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
}
}
#undef PTR2IDX
}
//
void btSoftBody::indicesToPointers(const int* map)
{
#define IDX2PTR(_p_,_b_) map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]): \
(&(_b_)[(((char*)_p_)-(char*)0)])
btSoftBody::Node* base=&m_nodes[0];
int i,ni;
for(i=0,ni=m_nodes.size();i<ni;++i)
{
if(m_nodes[i].m_leaf)
{
m_nodes[i].m_leaf->data=&m_nodes[i];
}
}
for(i=0,ni=m_links.size();i<ni;++i)
{
m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
}
for(i=0,ni=m_faces.size();i<ni;++i)
{
m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
if(m_faces[i].m_leaf)
{
m_faces[i].m_leaf->data=&m_faces[i];
}
}
for(i=0,ni=m_anchors.size();i<ni;++i)
{
m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
}
for(i=0,ni=m_notes.size();i<ni;++i)
{
for(int j=0;j<m_notes[i].m_rank;++j)
{
m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
}
}
#undef IDX2PTR
}
//
int btSoftBody::rayCast(const btVector3& org,const btVector3& dir,
btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
{
int cnt=0;
if(bcountonly||m_fdbvt.empty())
{/* Full search */
for(int i=0,ni=m_faces.size();i<ni;++i)
{
const btSoftBody::Face& f=m_faces[i];
const btScalar t=RayCaster::rayTriangle( org,dir,
f.m_n[0]->m_x,
f.m_n[1]->m_x,
f.m_n[2]->m_x,
mint);
if(t>0)
{
++cnt;
if(!bcountonly)
{
feature=btSoftBody::eFeature::Face;
index=i;
mint=t;
}
}
}
}
else
{/* Use dbvt */
RayCaster collider(org,dir,mint);
btDbvt::collideRAY(m_fdbvt.m_root,org,dir,collider);
if(collider.face)
{
mint=collider.mint;
feature=btSoftBody::eFeature::Face;
index=(int)(collider.face-&m_faces[0]);
cnt=1;
}
}
return(cnt);
}
//
void btSoftBody::initializeFaceTree()
{
m_fdbvt.clear();
for(int i=0;i<m_faces.size();++i)
{
Face& f=m_faces[i];
f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
}
}
//
btVector3 btSoftBody::evaluateCom() const
{
btVector3 com(0,0,0);
if(m_pose.m_bframe)
{
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
com+=m_nodes[i].m_x*m_pose.m_wgh[i];
}
}
return(com);
}
//
bool btSoftBody::checkContact( btRigidBody* prb,
const btVector3& x,
btScalar margin,
btSoftBody::sCti& cti) const
{
btVector3 nrm;
btCollisionShape* shp=prb->getCollisionShape();
const btTransform& wtr=prb->getInterpolationWorldTransform();
btScalar dst=m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x),
shp,
nrm,
margin);
if(dst<0)
{
cti.m_body = prb;
cti.m_normal = wtr.getBasis()*nrm;
cti.m_offset = -dot( cti.m_normal,
x-cti.m_normal*dst);
return(true);
}
return(false);
}
//
void btSoftBody::updateNormals()
{
const btVector3 zv(0,0,0);
int i,ni;
for(i=0,ni=m_nodes.size();i<ni;++i)
{
m_nodes[i].m_n=zv;
}
for(i=0,ni=m_faces.size();i<ni;++i)
{
btSoftBody::Face& f=m_faces[i];
const btVector3 n=cross(f.m_n[1]->m_x-f.m_n[0]->m_x,
f.m_n[2]->m_x-f.m_n[0]->m_x);
f.m_normal=n.normalized();
f.m_n[0]->m_n+=n;
f.m_n[1]->m_n+=n;
f.m_n[2]->m_n+=n;
}
for(i=0,ni=m_nodes.size();i<ni;++i)
{
m_nodes[i].m_n.normalize();
}
}
//
void btSoftBody::updateBounds()
{
if(m_ndbvt.m_root)
{
const btVector3& mins=m_ndbvt.m_root->volume.Mins();
const btVector3& maxs=m_ndbvt.m_root->volume.Maxs();
const btScalar csm=getCollisionShape()->getMargin();
const btVector3 mrg=btVector3( csm,
csm,
csm)*1; // ??? to investigate...
m_bounds[0]=mins-mrg;
m_bounds[1]=maxs+mrg;
if(0!=getBroadphaseHandle())
{
m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(),
m_bounds[0],
m_bounds[1],
m_worldInfo->m_dispatcher);
}
}
else
{
m_bounds[0]=
m_bounds[1]=btVector3(0,0,0);
}
}
//
void btSoftBody::updatePose()
{
if(m_pose.m_bframe)
{
btSoftBody::Pose& pose=m_pose;
const btVector3 com=evaluateCom();
/* Com */
pose.m_com = com;
/* Rotation */
btMatrix3x3 Apq;
const btScalar eps=SIMD_EPSILON;
Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
const btVector3 a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
const btVector3& b=pose.m_pos[i];
Apq[0]+=a.x()*b;
Apq[1]+=a.y()*b;
Apq[2]+=a.z()*b;
}
btMatrix3x3 r,s;
PolarDecompose(Apq,r,s);
pose.m_rot=r;
pose.m_scl=pose.m_aqq*r.transpose()*Apq;
if(m_cfg.maxvolume>1)
{
const btScalar idet=Clamp<btScalar>( 1/pose.m_scl.determinant(),
1,m_cfg.maxvolume);
pose.m_scl=Mul(pose.m_scl,idet);
}
}
}
//
void btSoftBody::updateConstants()
{
int i,ni;
/* Links */
for(i=0,ni=m_links.size();i<ni;++i)
{
Link& l=m_links[i];
Material& m=*l.m_material;
l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
l.m_c1 = l.m_rl*l.m_rl;
}
/* Faces */
for(i=0,ni=m_faces.size();i<ni;++i)
{
Face& f=m_faces[i];
f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
}
/* Area's */
btAlignedObjectArray<int> counts;
counts.resize(m_nodes.size(),0);
for(i=0,ni=m_nodes.size();i<ni;++i)
{
m_nodes[i].m_area = 0;
}
for(i=0,ni=m_faces.size();i<ni;++i)
{
btSoftBody::Face& f=m_faces[i];
for(int j=0;j<3;++j)
{
const int index=(int)(f.m_n[j]-&m_nodes[0]);
counts[index]++;
f.m_n[j]->m_area+=btFabs(f.m_ra);
}
}
for(i=0,ni=m_nodes.size();i<ni;++i)
{
if(counts[i]>0)
m_nodes[i].m_area/=(btScalar)counts[i];
else
m_nodes[i].m_area=0;
}
}
//
void btSoftBody::initializeClusters()
{
int i;
for( i=0;i<m_clusters.size();++i)
{
Cluster& c=*m_clusters[i];
c.m_imass=0;
c.m_masses.resize(c.m_nodes.size());
for(int j=0;j<c.m_nodes.size();++j)
{
c.m_masses[j] = c.m_nodes[j]->m_im>0?1/c.m_nodes[j]->m_im:0;
c.m_imass += c.m_masses[j];
}
c.m_imass = 1/c.m_imass;
c.m_com = btSoftBody::clusterCom(&c);
c.m_lv = btVector3(0,0,0);
c.m_av = btVector3(0,0,0);
c.m_leaf = 0;
/* Inertia */
btMatrix3x3& ii=c.m_locii;
ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
{
int i,ni;
for(i=0,ni=c.m_nodes.size();i<ni;++i)
{
const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
const btVector3 q=k*k;
const btScalar m=c.m_masses[i];
ii[0][0] += m*(q[1]+q[2]);
ii[1][1] += m*(q[0]+q[2]);
ii[2][2] += m*(q[0]+q[1]);
ii[0][1] -= m*k[0]*k[1];
ii[0][2] -= m*k[0]*k[2];
ii[1][2] -= m*k[1]*k[2];
}
}
ii[1][0]=ii[0][1];
ii[2][0]=ii[0][2];
ii[2][1]=ii[1][2];
ii=ii.inverse();
/* Frame */
c.m_framexform.setIdentity();
c.m_framexform.setOrigin(c.m_com);
c.m_framerefs.resize(c.m_nodes.size());
{
int i;
for(i=0;i<c.m_framerefs.size();++i)
{
c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
}
}
}
}
//
void btSoftBody::updateClusters()
{
BT_PROFILE("UpdateClusters");
int i;
for(i=0;i<m_clusters.size();++i)
{
btSoftBody::Cluster& c=*m_clusters[i];
const int n=c.m_nodes.size();
const btScalar invn=1/(btScalar)n;
if(n)
{
/* Frame */
const btScalar eps=0.0001;
btMatrix3x3 m,r,s;
m[0]=m[1]=m[2]=btVector3(0,0,0);
m[0][0]=eps*1;
m[1][1]=eps*2;
m[2][2]=eps*3;
c.m_com=clusterCom(&c);
for(int i=0;i<c.m_nodes.size();++i)
{
const btVector3 a=c.m_nodes[i]->m_x-c.m_com;
const btVector3& b=c.m_framerefs[i];
m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
}
PolarDecompose(m,r,s);
c.m_framexform.setOrigin(c.m_com);
c.m_framexform.setBasis(r);
/* Inertia */
#if 1/* Constant */
c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
#else
#if 0/* Sphere */
const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass);
const btVector3 inertia(rk,rk,rk);
const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
#else/* Actual */
c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
for(int i=0;i<n;++i)
{
const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
const btVector3 q=k*k;
const btScalar m=1/c.m_nodes[i]->m_im;
c.m_invwi[0][0] += m*(q[1]+q[2]);
c.m_invwi[1][1] += m*(q[0]+q[2]);
c.m_invwi[2][2] += m*(q[0]+q[1]);
c.m_invwi[0][1] -= m*k[0]*k[1];
c.m_invwi[0][2] -= m*k[0]*k[2];
c.m_invwi[1][2] -= m*k[1]*k[2];
}
c.m_invwi[1][0]=c.m_invwi[0][1];
c.m_invwi[2][0]=c.m_invwi[0][2];
c.m_invwi[2][1]=c.m_invwi[1][2];
c.m_invwi=c.m_invwi.inverse();
#endif
#endif
/* Velocities */
c.m_lv=btVector3(0,0,0);
c.m_av=btVector3(0,0,0);
{
int i;
for(i=0;i<n;++i)
{
const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
c.m_lv += v;
c.m_av += cross(c.m_nodes[i]->m_x-c.m_com,v);
}
}
c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
c.m_vimpulses[0] =
c.m_vimpulses[1] = btVector3(0,0,0);
c.m_dimpulses[0] =
c.m_dimpulses[1] = btVector3(0,0,0);
c.m_nvimpulses = 0;
c.m_ndimpulses = 0;
/* Matching */
if(c.m_matching>0)
{
for(int j=0;j<c.m_nodes.size();++j)
{
Node& n=*c.m_nodes[j];
const btVector3 x=c.m_framexform*c.m_framerefs[j];
n.m_x=Lerp(n.m_x,x,c.m_matching);
}
}
/* Dbvt */
if(c.m_collide)
{
btVector3 mi=c.m_nodes[0]->m_x;
btVector3 mx=mi;
for(int j=1;j<n;++j)
{
mi.setMin(c.m_nodes[j]->m_x);
mx.setMax(c.m_nodes[j]->m_x);
}
const btDbvtVolume bounds=btDbvtVolume::FromMM(mi,mx);
if(c.m_leaf)
m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
else
c.m_leaf=m_cdbvt.insert(bounds,&c);
}
}
}
}
//
void btSoftBody::cleanupClusters()
{
for(int i=0;i<m_joints.size();++i)
{
m_joints[i]->Terminate(m_sst.sdt);
if(m_joints[i]->m_delete)
{
btAlignedFree(m_joints[i]);
m_joints.remove(m_joints[i--]);
}
}
}
//
void btSoftBody::prepareClusters(int iterations)
{
for(int i=0;i<m_joints.size();++i)
{
m_joints[i]->Prepare(m_sst.sdt,iterations);
}
}
//
void btSoftBody::solveClusters(btScalar sor)
{
for(int i=0,ni=m_joints.size();i<ni;++i)
{
m_joints[i]->Solve(m_sst.sdt,sor);
}
}
//
void btSoftBody::applyClusters(bool drift)
{
BT_PROFILE("ApplyClusters");
const btScalar f0=m_sst.sdt;
const btScalar f1=f0/2;
btAlignedObjectArray<btVector3> deltas;
btAlignedObjectArray<btScalar> weights;
deltas.resize(m_nodes.size(),btVector3(0,0,0));
weights.resize(m_nodes.size(),0);
int i;
if(drift)
{
for(i=0;i<m_clusters.size();++i)
{
Cluster& c=*m_clusters[i];
if(c.m_ndimpulses)
{
c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
}
}
}
for(i=0;i<m_clusters.size();++i)
{
Cluster& c=*m_clusters[i];
if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
{
const btVector3 v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
const btVector3 w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
for(int j=0;j<c.m_nodes.size();++j)
{
const int idx=int(c.m_nodes[j]-&m_nodes[0]);
const btVector3& x=c.m_nodes[j]->m_x;
const btScalar q=c.m_masses[j];
deltas[idx] += (v+cross(w,x-c.m_com))*q;
weights[idx] += q;
}
}
}
for(i=0;i<deltas.size();++i)
{
if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i];
}
}
//
void btSoftBody::dampClusters()
{
int i;
for(i=0;i<m_clusters.size();++i)
{
Cluster& c=*m_clusters[i];
if(c.m_ndamping>0)
{
for(int j=0;j<c.m_nodes.size();++j)
{
Node& n=*c.m_nodes[j];
if(n.m_im>0)
{
const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
n.m_v += c.m_ndamping*(vx-n.m_v);
}
}
}
}
}
//
void btSoftBody::Joint::Prepare(btScalar dt,int)
{
m_bodies[0].activate();
m_bodies[1].activate();
}
//
void btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
{
static const btScalar maxdrift=4;
Joint::Prepare(dt,iterations);
m_rpos[0] = m_bodies[0].xform()*m_refs[0];
m_rpos[1] = m_bodies[1].xform()*m_refs[1];
m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
m_rpos[0] -= m_bodies[0].xform().getOrigin();
m_rpos[1] -= m_bodies[1].xform().getOrigin();
m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
if(m_split>0)
{
m_sdrift = m_massmatrix*(m_drift*m_split);
m_drift *= 1-m_split;
}
m_drift /=(btScalar)iterations;
}
//
void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
{
const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
const btVector3 vr=va-vb;
btSoftBody::Impulse impulse;
impulse.m_asVelocity = 1;
impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor;
m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
m_bodies[1].applyImpulse( impulse,m_rpos[1]);
}
//
void btSoftBody::LJoint::Terminate(btScalar dt)
{
if(m_split>0)
{
m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
}
}
//
void btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
{
static const btScalar maxdrift=SIMD_PI/16;
m_icontrol->Prepare(this);
Joint::Prepare(dt,iterations);
m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0];
m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1];
m_drift = NormalizeAny(cross(m_axis[1],m_axis[0]));
m_drift *= btMin(maxdrift,btAcos(Clamp<btScalar>(dot(m_axis[0],m_axis[1]),-1,+1)));
m_drift *= m_erp/dt;
m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
if(m_split>0)
{
m_sdrift = m_massmatrix*(m_drift*m_split);
m_drift *= 1-m_split;
}
m_drift /=(btScalar)iterations;
}
//
void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
{
const btVector3 va=m_bodies[0].angularVelocity();
const btVector3 vb=m_bodies[1].angularVelocity();
const btVector3 vr=va-vb;
const btScalar sp=dot(vr,m_axis[0]);
const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
btSoftBody::Impulse impulse;
impulse.m_asVelocity = 1;
impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor;
m_bodies[0].applyAImpulse(-impulse);
m_bodies[1].applyAImpulse( impulse);
}
//
void btSoftBody::AJoint::Terminate(btScalar dt)
{
if(m_split>0)
{
m_bodies[0].applyDAImpulse(-m_sdrift);
m_bodies[1].applyDAImpulse( m_sdrift);
}
}
//
void btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
{
Joint::Prepare(dt,iterations);
const bool dodrift=(m_life==0);
m_delete=(++m_life)>m_maxlife;
if(dodrift)
{
m_drift=m_drift*m_erp/dt;
if(m_split>0)
{
m_sdrift = m_massmatrix*(m_drift*m_split);
m_drift *= 1-m_split;
}
m_drift/=(btScalar)iterations;
}
else
{
m_drift=m_sdrift=btVector3(0,0,0);
}
}
//
void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
{
const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
const btVector3 vrel=va-vb;
const btScalar rvac=dot(vrel,m_normal);
btSoftBody::Impulse impulse;
impulse.m_asVelocity = 1;
impulse.m_velocity = m_drift;
if(rvac<0)
{
const btVector3 iv=m_normal*rvac;
const btVector3 fv=vrel-iv;
impulse.m_velocity += iv+fv*m_friction;
}
impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
m_bodies[1].applyImpulse( impulse,m_rpos[1]);
}
//
void btSoftBody::CJoint::Terminate(btScalar dt)
{
if(m_split>0)
{
m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
}
}
//
void btSoftBody::applyForces()
{
BT_PROFILE("SoftBody applyForces");
const btScalar dt=m_sst.sdt;
const btScalar kLF=m_cfg.kLF;
const btScalar kDG=m_cfg.kDG;
const btScalar kPR=m_cfg.kPR;
const btScalar kVC=m_cfg.kVC;
const bool as_lift=kLF>0;
const bool as_drag=kDG>0;
const bool as_pressure=kPR!=0;
const bool as_volume=kVC>0;
const bool as_aero= as_lift ||
as_drag ;
const bool as_vaero= as_aero &&
(m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
const bool as_faero= as_aero &&
(m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided);
const bool use_medium= as_aero;
const bool use_volume= as_pressure ||
as_volume ;
btScalar volume=0;
btScalar ivolumetp=0;
btScalar dvolumetv=0;
btSoftBody::sMedium medium;
if(use_volume)
{
volume = getVolume();
ivolumetp = 1/btFabs(volume)*kPR;
dvolumetv = (m_pose.m_volume-volume)*kVC;
}
/* Per vertex forces */
int i,ni;
for(i=0,ni=m_nodes.size();i<ni;++i)
{
btSoftBody::Node& n=m_nodes[i];
if(n.m_im>0)
{
if(use_medium)
{
EvaluateMedium(m_worldInfo,n.m_x,medium);
/* Aerodynamics */
if(as_vaero)
{
const btVector3 rel_v=n.m_v-medium.m_velocity;
const btScalar rel_v2=rel_v.length2();
if(rel_v2>SIMD_EPSILON)
{
btVector3 nrm=n.m_n;
/* Setup normal */
switch(m_cfg.aeromodel)
{
case btSoftBody::eAeroModel::V_Point:
nrm=NormalizeAny(rel_v);break;
case btSoftBody::eAeroModel::V_TwoSided:
nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
}
const btScalar dvn=dot(rel_v,nrm);
/* Compute forces */
if(dvn>0)
{
btVector3 force(0,0,0);
const btScalar c0 = n.m_area*dvn*rel_v2/2;
const btScalar c1 = c0*medium.m_density;
force += nrm*(-c1*kLF);
force += rel_v.normalized()*(-c1*kDG);
ApplyClampedForce(n,force,dt);
}
}
}
}
/* Pressure */
if(as_pressure)
{
n.m_f += n.m_n*(n.m_area*ivolumetp);
}
/* Volume */
if(as_volume)
{
n.m_f += n.m_n*(n.m_area*dvolumetv);
}
}
}
/* Per face forces */
for(i=0,ni=m_faces.size();i<ni;++i)
{
btSoftBody::Face& f=m_faces[i];
if(as_faero)
{
const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
EvaluateMedium(m_worldInfo,x,medium);
const btVector3 rel_v=v-medium.m_velocity;
const btScalar rel_v2=rel_v.length2();
if(rel_v2>SIMD_EPSILON)
{
btVector3 nrm=f.m_normal;
/* Setup normal */
switch(m_cfg.aeromodel)
{
case btSoftBody::eAeroModel::F_TwoSided:
nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
}
const btScalar dvn=dot(rel_v,nrm);
/* Compute forces */
if(dvn>0)
{
btVector3 force(0,0,0);
const btScalar c0 = f.m_ra*dvn*rel_v2;
const btScalar c1 = c0*medium.m_density;
force += nrm*(-c1*kLF);
force += rel_v.normalized()*(-c1*kDG);
force /= 3;
for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
}
}
}
}
}
//
void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
{
const btScalar kAHR=psb->m_cfg.kAHR*kst;
const btScalar dt=psb->m_sst.sdt;
for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
{
const Anchor& a=psb->m_anchors[i];
const btTransform& t=a.m_body->getInterpolationWorldTransform();
Node& n=*a.m_node;
const btVector3 wa=t*a.m_local;
const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
const btVector3 vb=n.m_x-n.m_q;
const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR;
const btVector3 impulse=a.m_c0*vr;
n.m_x+=impulse*a.m_c2;
a.m_body->applyImpulse(-impulse,a.m_c1);
}
}
//
void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti)
{
const btScalar dt=psb->m_sst.sdt;
const btScalar mrg=psb->getCollisionShape()->getMargin();
for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
{
const RContact& c=psb->m_rcontacts[i];
const sCti& cti=c.m_cti;
const btVector3 va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*dt;
const btVector3 vb=c.m_node->m_x-c.m_node->m_q;
const btVector3 vr=vb-va;
const btScalar dn=dot(vr,cti.m_normal);
if(dn<=SIMD_EPSILON)
{
const btScalar dp=btMin(dot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg);
const btVector3 fv=vr-cti.m_normal*dn;
const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst);
c.m_node->m_x-=impulse*c.m_c2;
c.m_cti.m_body->applyImpulse(impulse,c.m_c1);
}
}
}
//
void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
{
for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
{
const SContact& c=psb->m_scontacts[i];
const btVector3& nr=c.m_normal;
Node& n=*c.m_node;
Face& f=*c.m_face;
const btVector3 p=BaryEval( f.m_n[0]->m_x,
f.m_n[1]->m_x,
f.m_n[2]->m_x,
c.m_weights);
const btVector3 q=BaryEval( f.m_n[0]->m_q,
f.m_n[1]->m_q,
f.m_n[2]->m_q,
c.m_weights);
const btVector3 vr=(n.m_x-n.m_q)-(p-q);
btVector3 corr(0,0,0);
if(dot(vr,nr)<0)
{
const btScalar j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p));
corr+=c.m_normal*j;
}
corr -= ProjectOnPlane(vr,nr)*c.m_friction;
n.m_x += corr*c.m_cfm[0];
f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x());
f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y());
f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z());
}
}
//
void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
{
for(int i=0,ni=psb->m_links.size();i<ni;++i)
{
Link& l=psb->m_links[i];
if(l.m_c0>0)
{
Node& a=*l.m_n[0];
Node& b=*l.m_n[1];
const btVector3 del=b.m_x-a.m_x;
const btScalar len=del.length2();
const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
const btScalar t=k*a.m_im;
a.m_x-=del*(k*a.m_im);
b.m_x+=del*(k*b.m_im);
}
}
}
//
void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
{
for(int i=0,ni=psb->m_links.size();i<ni;++i)
{
Link& l=psb->m_links[i];
Node** n=l.m_n;
const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
n[0]->m_v+= l.m_c3*(j*n[0]->m_im);
n[1]->m_v-= l.m_c3*(j*n[1]->m_im);
}
}
//
btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver)
{
switch(solver)
{
case ePSolver::Anchors: return(&btSoftBody::PSolve_Anchors);
case ePSolver::Linear: return(&btSoftBody::PSolve_Links);
case ePSolver::RContacts: return(&btSoftBody::PSolve_RContacts);
case ePSolver::SContacts: return(&btSoftBody::PSolve_SContacts);
}
return(0);
}
//
btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver)
{
switch(solver)
{
case eVSolver::Linear: return(&btSoftBody::VSolve_Links);
}
return(0);
}
//
void btSoftBody::defaultCollisionHandler(btCollisionObject* pco)
{
switch(m_cfg.collisions&fCollision::RVSmask)
{
case fCollision::SDF_RS:
{
btSoftColliders::CollideSDF_RS docollide;
btRigidBody* prb=btRigidBody::upcast(pco);
const btTransform wtr=prb->getInterpolationWorldTransform();
const btTransform ctr=prb->getWorldTransform();
const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
const btScalar basemargin=getCollisionShape()->getMargin();
btVector3 mins;
btVector3 maxs;
btDbvtVolume volume;
pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(),
mins,
maxs);
volume=btDbvtVolume::FromMM(mins,maxs);
volume.Expand(btVector3(basemargin,basemargin,basemargin));
docollide.psb = this;
docollide.prb = prb;
docollide.dynmargin = basemargin+timemargin;
docollide.stamargin = basemargin;
btDbvt::collideTV(m_ndbvt.m_root,volume,docollide);
}
break;
case fCollision::CL_RS:
{
btSoftColliders::CollideCL_RS collider;
collider.Process(this,btRigidBody::upcast(pco));
}
break;
}
}
//
void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
{
const int cf=m_cfg.collisions&psb->m_cfg.collisions;
switch(cf&fCollision::SVSmask)
{
case fCollision::CL_SS:
{
btSoftColliders::CollideCL_SS docollide;
docollide.Process(this,psb);
}
break;
case fCollision::VF_SS:
{
btSoftColliders::CollideVF_SS docollide;
/* common */
docollide.mrg= getCollisionShape()->getMargin()+
psb->getCollisionShape()->getMargin();
/* psb0 nodes vs psb1 faces */
docollide.psb[0]=this;
docollide.psb[1]=psb;
btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root,
docollide.psb[1]->m_fdbvt.m_root,
docollide);
/* psb1 nodes vs psb0 faces */
docollide.psb[0]=psb;
docollide.psb[1]=this;
btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root,
docollide.psb[1]->m_fdbvt.m_root,
docollide);
}
break;
}
}
Reference in New Issue View Git Blame Copy Permalink