diff options
Diffstat (limited to 'extern/bullet2/src/BulletSoftBody/btSoftBody.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletSoftBody/btSoftBody.cpp | 2590 |
1 files changed, 2590 insertions, 0 deletions
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp new file mode 100644 index 00000000000..2553009fec2 --- /dev/null +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp @@ -0,0 +1,2590 @@ +/* +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(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_clusters.size();++i) + btAlignedFree(m_clusters[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) + { + btScalar len = m_nodes[i].m_n.length(); + if (len>SIMD_EPSILON) + m_nodes[i].m_n /= len; + } +} + +// +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=btScalar(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 ATTRIBUTE_ALIGNED16(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; + ATTRIBUTE_ALIGNED16(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; + } +} |