diff options
Diffstat (limited to 'extern/bullet2/src/BulletSoftBody/btSoftBody.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletSoftBody/btSoftBody.cpp | 2043 |
1 files changed, 1056 insertions, 987 deletions
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp index 1ec668c9c78..ee810c54082 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp @@ -150,98 +150,98 @@ bool btSoftBody::checkFace(int node0,int node1,int node2) const // btSoftBody::Material* btSoftBody::appendMaterial() { -Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material(); -if(m_materials.size()>0) - *pm=*m_materials[0]; + 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); + 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); + 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) + const btVector3& o, + Node* feature) { -appendNote(text,o,btVector4(1,0,0,0),feature); + appendNote(text,o,btVector4(1,0,0,0),feature); } // void btSoftBody::appendNote( const char* text, - const btVector3& o, - Link* feature) + 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]); + 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) + 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]); + 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()) + if(m_nodes.capacity()==m_nodes.size()) { - pointersToIndices(); - m_nodes.reserve(m_nodes.size()*2+1); - indicesToPointers(); + 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); + 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]; + 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); + m_links.push_back(l); } // @@ -273,12 +273,12 @@ void btSoftBody::appendLink( Node* node0, // void btSoftBody::appendFace(int model,Material* mat) { -Face f; -if(model>=0) + 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); + m_faces.push_back(f); } // @@ -306,9 +306,9 @@ void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat) } // -void btSoftBody::appendAnchor(int node,btRigidBody* body,bool disableCollisionWithBody=false) +void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies) { - if (disableCollisionWithBody) + if (disableCollisionBetweenLinkedBodies) { if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size()) { @@ -327,54 +327,54 @@ void btSoftBody::appendAnchor(int node,btRigidBody* body,bool disableCollision // 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); + 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); + appendLinearJoint(specs,m_clusters[0],body); } // void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body) { -appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]); + 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); + 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); + appendAngularJoint(specs,m_clusters[0],body); } // void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body) { -appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); + appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); } // @@ -463,8 +463,8 @@ void btSoftBody::setTotalMass(btScalar mass,bool fromfaces) { 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); + 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; @@ -494,13 +494,17 @@ void btSoftBody::setTotalDensity(btScalar density) void btSoftBody::transform(const btTransform& trs) { const btScalar margin=getCollisionShape()->getMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + 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)); + vol = btDbvtVolume::FromCR(n.m_x,margin); + + m_ndbvt.update(n.m_leaf,vol); } updateNormals(); updateBounds(); @@ -511,31 +515,34 @@ void btSoftBody::transform(const btTransform& trs) // void btSoftBody::translate(const btVector3& trs) { -btTransform t; -t.setIdentity(); -t.setOrigin(trs); -transform(t); + btTransform t; + t.setIdentity(); + t.setOrigin(trs); + transform(t); } // void btSoftBody::rotate( const btQuaternion& rot) { -btTransform t; -t.setIdentity(); -t.setRotation(rot); -transform(t); + btTransform t; + t.setIdentity(); + t.setRotation(rot); + transform(t); } // void btSoftBody::scale(const btVector3& scl) { const btScalar margin=getCollisionShape()->getMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + 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)); + vol = btDbvtVolume::FromCR(n.m_x,margin); + m_ndbvt.update(n.m_leaf,vol); } updateNormals(); updateBounds(); @@ -562,8 +569,8 @@ void btSoftBody::setPose(bool bvolume,bool bframe) { Node& n=m_nodes[i]; m_pose.m_wgh[i]= n.m_im>0 ? - 1/(m_nodes[i].m_im*tmass) : - kmass/tmass; + 1/(m_nodes[i].m_im*tmass) : + kmass/tmass; } /* Pos */ const btVector3 com=evaluateCom(); @@ -578,16 +585,16 @@ void btSoftBody::setPose(bool bvolume,bool bframe) 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); + 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(); } @@ -614,87 +621,87 @@ btScalar btSoftBody::getVolume() const // int btSoftBody::clusterCount() const { -return(m_clusters.size()); + 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) + 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]; + com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i]; } -return(com*cluster->m_imass); + return(com*cluster->m_imass); } // btVector3 btSoftBody::clusterCom(int cluster) const { -return(clusterCom(m_clusters[cluster])); + return(clusterCom(m_clusters[cluster])); } // btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos) { -return(cluster->m_lv+cross(cluster->m_av,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++; + 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++; + 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); + 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++; + 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++; + 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); + 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++; + cluster->m_dimpulses[0]+=impulse*cluster->m_imass; + cluster->m_ndimpulses++; } // @@ -762,9 +769,9 @@ int btSoftBody::generateBendingConstraints(int distance,Material* mat) // void btSoftBody::randomizeConstraints() { -unsigned long seed=243703; + unsigned long seed=243703; #define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff) -int i,ni; + int i,ni; for(i=0,ni=m_links.size();i<ni;++i) { @@ -780,493 +787,524 @@ int i,ni; // void btSoftBody::releaseCluster(int index) { -Cluster* c=m_clusters[index]; -if(c->m_leaf) m_cdbvt.remove(c->m_leaf); -c->~Cluster(); -btAlignedFree(c); -m_clusters.remove(c); + Cluster* c=m_clusters[index]; + if(c->m_leaf) m_cdbvt.remove(c->m_leaf); + c->~Cluster(); + btAlignedFree(c); + m_clusters.remove(c); } // void btSoftBody::releaseClusters() { -while(m_clusters.size()>0) releaseCluster(0); + while(m_clusters.size()>0) releaseCluster(0); } // int btSoftBody::generateClusters(int k,int maxiterations) { -int i; -releaseClusters(); -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) + int i; + releaseClusters(); + 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) + 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; + c+=m_clusters[i]->m_nodes[j]->m_x; } - if(m_clusters[i]->m_nodes.size()) + 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); + 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) + 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 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) + const btScalar d=ClusterMetric(centers[j],nx); + if(d<kdist) { - kbest=j; - kdist=d; + kbest=j; + kdist=d; } } - m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]); + 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) + /* 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) + for(int j=0;j<m_clusters[i]->m_nodes.size();++j) { - cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i; + cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i; } } - for(i=0;i<m_faces.size();++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 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 cid=cids[idx[j]]; + for(int q=1;q<3;++q) { - const int kid=idx[(j+q)%3]; - if(cids[kid]!=cid) + 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()) + 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]); + m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]); } } } } } - /* Master */ - if(m_clusters.size()>1) + /* 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]); + 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) + /* Terminate */ + for(i=0;i<m_clusters.size();++i) { - if(m_clusters[i]->m_nodes.size()==0) + if(m_clusters[i]->m_nodes.size()==0) { - releaseCluster(i--); + releaseCluster(i--); } } - - initializeClusters(); - updateClusters(); - return(m_clusters.size()); + + initializeClusters(); + updateClusters(); + + //for self-collision + m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size()); + { + for (int c0=0;c0<m_clusters.size();c0++) + { + m_clusters[c0]->m_clusterIndex=c0; + for (int c1=0;c1<m_clusters.size();c1++) + { + + bool connected=false; + Cluster* cla = m_clusters[c0]; + Cluster* clb = m_clusters[c1]; + for (int i=0;!connected&&i<cla->m_nodes.size();i++) + { + for (int j=0;j<clb->m_nodes.size();j++) + { + if (cla->m_nodes[i] == clb->m_nodes[j]) + { + connected=true; + break; + } + } + } + m_clusterConnectivity[c0+c1*m_clusters.size()]=connected; + } + } + } + + return(m_clusters.size()); } -return(0); + 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; + 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) + /* Filter out */ + for(i=0;i<m_links.size();++i) { - Link& l=m_links[i]; - if(l.m_bbending) + 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))) + 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; + 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) + /* 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; + 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) + 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; + 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) + /* Intersect */ + for(i=0;i<ncount;++i) { - for(j=i+1;j<ncount;++j) + for(j=i+1;j<ncount;++j) { - if(edges(i,j)==-1) + 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) + 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) + 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) + 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; + 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) + if(b.m_im>0) { b.m_im/=0.5;m=1/b.m_im; } else - m=0; + m=0; } - appendNode(x,m); - edges(i,j)=m_nodes.size()-1; - m_nodes[edges(i,j)].m_v=v; - ++newnodes; + 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) + 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)) + 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) + 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]]; + 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) + /* 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++) + 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)) + if((idx[j]<ncount)&&(idx[k]<ncount)) { - const int ni=edges(idx[j],idx[k]); - if(ni>0) + 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; + 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) + /* 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)) + 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; + 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) + 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]]) + 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; + appendLink(i); + todetach=m_links.size()-1; } - else + else { - if(( (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&& + if(( (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&& (ifn->Eval(m_nodes[id[1]].m_x)<accurary))) - todetach=i; + todetach=i; } - if(todetach) + if(todetach) { - Link& l=m_links[todetach]; - for(int j=0;j<2;++j) + 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]; + 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) + /* 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)) + 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) + for(int j=0;j<3;++j) { - int cn=cnodes[int(n[j]-nbase)]; - if(cn) n[j]=&m_nodes[cn]; + 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) + /* 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(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(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(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++; } - for(i=0;i<m_links.size();++i) + 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]) + 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; + --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 0 + for(i=nnodes-1;i>=0;--i) { - if(!ranks[i]) todelete.push_back(i); + if(!ranks[i]) todelete.push_back(i); } - if(todelete.size()) + 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) + 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; + 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); + IndicesToPointers(this,&map[0]); + m_nodes.resize(nnodes); } - #endif +#endif } -m_bUpdateRtCst=true; + 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)); + 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); + 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); + 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) + if(!done) { - m_ndbvt.remove(pn[0]->m_leaf); - m_ndbvt.remove(pn[1]->m_leaf); - m_nodes.pop_back(); - m_nodes.pop_back(); + m_ndbvt.remove(pn[0]->m_leaf); + m_ndbvt.remove(pn[1]->m_leaf); + m_nodes.pop_back(); + m_nodes.pop_back(); } -return(done); + return(done); } // -bool btSoftBody::rayCast(const btVector3& org, - const btVector3& dir, - sRayCast& results, - btScalar maxtime) +bool btSoftBody::rayTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results) { -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); + if(m_faces.size()&&m_fdbvt.empty()) + initializeFaceTree(); + + results.body = this; + results.fraction = 1.f; + results.feature = eFeature::None; + results.index = -1; + + return(rayTest(rayFrom,rayTo,results.fraction,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) + 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; + 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; + 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; } } @@ -1282,11 +1320,11 @@ void btSoftBody::predictMotion(btScalar dt) 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; @@ -1310,45 +1348,48 @@ void btSoftBody::predictMotion(btScalar dt) /* Bounds */ updateBounds(); /* Nodes */ + ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; for(i=0,ni=m_nodes.size();i<ni;++i) { Node& n=m_nodes[i]; + vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg); m_ndbvt.update( n.m_leaf, - btDbvtVolume::FromCR(n.m_x,m_sst.radmrg), - n.m_v*m_sst.velmrg, - m_sst.updmrg); + vol, + 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; + f.m_n[1]->m_v+ + f.m_n[2]->m_v)/3; + vol = VolumeOf(f,m_sst.radmrg); m_fdbvt.update( f.m_leaf, - VolumeOf(f,m_sst.radmrg), - v*m_sst.velmrg, - m_sst.updmrg); - } + vol, + 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); @@ -1361,104 +1402,104 @@ void btSoftBody::predictMotion(btScalar dt) // void btSoftBody::solveConstraints() { -/* Apply clusters */ -applyClusters(false); -/* Prepare links */ + /* Apply clusters */ + applyClusters(false); + /* Prepare links */ -int i,ni; + int i,ni; -for(i=0,ni=m_links.size();i<ni;++i) + 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); + 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) + /* 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(); + 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 velocities */ + if(m_cfg.viterations>0) { - /* Solve */ - for(int isolve=0;isolve<m_cfg.viterations;++isolve) + /* Solve */ + for(int isolve=0;isolve<m_cfg.viterations;++isolve) { - for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq) + for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq) { - getSolver(m_cfg.m_vsequence[iseq])(this,1); + getSolver(m_cfg.m_vsequence[iseq])(this,1); } } - /* Update */ - for(i=0,ni=m_nodes.size();i<ni;++i) + /* 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; + Node& n=m_nodes[i]; + n.m_x = n.m_q+n.m_v*m_sst.sdt; } } -/* Solve positions */ -if(m_cfg.piterations>0) + /* Solve positions */ + if(m_cfg.piterations>0) { - for(int isolve=0;isolve<m_cfg.piterations;++isolve) + 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) + 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); + 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) + 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); + 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) + /* 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) + 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; + Node& n=m_nodes[i]; + n.m_q = n.m_x; } - for(int idrift=0;idrift<m_cfg.diterations;++idrift) + for(int idrift=0;idrift<m_cfg.diterations;++idrift) { - for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq) + for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq) { - getSolver(m_cfg.m_dsequence[iseq])(this,1,0); + getSolver(m_cfg.m_dsequence[iseq])(this,1,0); } } - for(int i=0,ni=m_nodes.size();i<ni;++i) + 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; + Node& n=m_nodes[i]; + n.m_v += (n.m_x-n.m_q)*vcf; } } -/* Apply clusters */ -dampClusters(); -applyClusters(true); + /* Apply clusters */ + dampClusters(); + applyClusters(true); } // void btSoftBody::staticSolve(int iterations) { -for(int isolve=0;isolve<iterations;++isolve) + for(int isolve=0;isolve<iterations;++isolve) { - for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq) + for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq) { - getSolver(m_cfg.m_psequence[iseq])(this,1,0); + getSolver(m_cfg.m_psequence[iseq])(this,1,0); } } } @@ -1466,35 +1507,35 @@ for(int isolve=0;isolve<iterations;++isolve) // void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/) { -/// placeholder + /// placeholder } // void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies) { -const int nb=bodies.size(); -int iterations=0; -int i; + const int nb=bodies.size(); + int iterations=0; + int i; -for(i=0;i<nb;++i) + for(i=0;i<nb;++i) { - iterations=btMax(iterations,bodies[i]->m_cfg.citerations); + iterations=btMax(iterations,bodies[i]->m_cfg.citerations); } -for(i=0;i<nb;++i) + for(i=0;i<nb;++i) { - bodies[i]->prepareClusters(iterations); + bodies[i]->prepareClusters(iterations); } -for(i=0;i<iterations;++i) + for(i=0;i<iterations;++i) { - const btScalar sor=1; - for(int j=0;j<nb;++j) + const btScalar sor=1; + for(int j=0;j<nb;++j) { - bodies[j]->solveClusters(sor); + bodies[j]->solveClusters(sor); } } -for(i=0;i<nb;++i) + for(i=0;i<nb;++i) { - bodies[i]->cleanupClusters(); + bodies[i]->cleanupClusters(); } } @@ -1506,48 +1547,54 @@ void btSoftBody::integrateMotion() } // - btSoftBody::RayCaster::RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt) +btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt) { -o = org; -d = dir; -mint = mxt; -face = 0; -tests = 0; + m_rayFrom = rayFrom; + m_rayNormalizedDirection = (rayTo-rayFrom); + m_rayTo = rayTo; + m_mint = mxt; + m_face = 0; + m_tests = 0; } // -void btSoftBody::RayCaster::Process(const btDbvtNode* leaf) +void btSoftBody::RayFromToCaster::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; + btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data; + const btScalar t=rayFromToTriangle( m_rayFrom,m_rayTo,m_rayNormalizedDirection, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + m_mint); + if((t>0)&&(t<m_mint)) + { + m_mint=t;m_face=&f; + } + ++m_tests; } // -btScalar btSoftBody::RayCaster::rayTriangle( const btVector3& org, - const btVector3& dir, - const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar maxt) +btScalar btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayNormalizedDirection, + 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); + const btScalar den=dot(rayNormalizedDirection,n); if(!btFuzzyZero(den)) { - const btScalar num=dot(org,n)-d; + const btScalar num=dot(rayFrom,n)-d; const btScalar t=-num/den; if((t>teps)&&(t<maxt)) { - const btVector3 hit=org+dir*t; + const btVector3 hit=rayFrom+rayNormalizedDirection*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)) @@ -1569,9 +1616,9 @@ void btSoftBody::pointersToIndices() 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) { @@ -1584,21 +1631,21 @@ void btSoftBody::pointersToIndices() 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 } @@ -1606,16 +1653,16 @@ void btSoftBody::pointersToIndices() void btSoftBody::indicesToPointers(const int* map) { #define IDX2PTR(_p_,_b_) map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]): \ - (&(_b_)[(((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) { @@ -1628,110 +1675,116 @@ void btSoftBody::indicesToPointers(const int* map) 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, +int btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const { int cnt=0; if(bcountonly||m_fdbvt.empty()) - {/* Full search */ + {/* Full search */ + btVector3 dir = rayTo-rayFrom; + dir.normalize(); + 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); + + const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,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; + } + else + {/* Use dbvt */ + RayFromToCaster collider(rayFrom,rayTo,mint); + + btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider); + if(collider.m_face) + { + mint=collider.m_mint; feature=btSoftBody::eFeature::Face; - index=(int)(collider.face-&m_faces[0]); + index=(int)(collider.m_face-&m_faces[0]); cnt=1; - } } + } return(cnt); } // void btSoftBody::initializeFaceTree() { -m_fdbvt.clear(); -for(int i=0;i<m_faces.size();++i) + 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); + 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) + btVector3 com(0,0,0); + if(m_pose.m_bframe) { - for(int i=0,ni=m_nodes.size();i<ni;++i) + for(int i=0,ni=m_nodes.size();i<ni;++i) { - com+=m_nodes[i].m_x*m_pose.m_wgh[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 +bool btSoftBody::checkContact( btCollisionObject* colObj, + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti) const { btVector3 nrm; - btCollisionShape* shp=prb->getCollisionShape(); - const btTransform& wtr=prb->getInterpolationWorldTransform(); + btCollisionShape* shp=colObj->getCollisionShape(); + btRigidBody* tmpRigid = btRigidBody::upcast(colObj); + const btTransform& wtr=tmpRigid? tmpRigid->getInterpolationWorldTransform() : colObj->getWorldTransform(); btScalar dst=m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), - shp, - nrm, - margin); + shp, + nrm, + margin); if(dst<0) { - cti.m_body = prb; + cti.m_colObj = colObj; cti.m_normal = wtr.getBasis()*nrm; cti.m_offset = -dot( cti.m_normal, - x-cti.m_normal*dst); + x-cti.m_normal*dst); return(true); } return(false); @@ -1751,7 +1804,7 @@ void btSoftBody::updateNormals() { 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_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; @@ -1769,28 +1822,28 @@ void btSoftBody::updateNormals() 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... + 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); - } + if(0!=getBroadphaseHandle()) + { + m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); } - else - { + } + else + { m_bounds[0]= - m_bounds[1]=btVector3(0,0,0); - } + m_bounds[1]=btVector3(0,0,0); + } } @@ -1821,12 +1874,12 @@ void btSoftBody::updatePose() 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); + 1,m_cfg.maxvolume); pose.m_scl=Mul(pose.m_scl,idet); - } - + } + } } @@ -1881,53 +1934,53 @@ void btSoftBody::initializeClusters() { int i; -for( i=0;i<m_clusters.size();++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) + 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_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; + 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) + 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; + c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com; } } } @@ -1936,49 +1989,49 @@ for( i=0;i<m_clusters.size();++i) // 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) + 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; + 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 */ + 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); - + 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 */ +#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; @@ -1988,74 +2041,79 @@ for(i=0;i<m_clusters.size();++i) 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) +#endif +#endif + /* Velocities */ + c.m_lv=btVector3(0,0,0); + c.m_av=btVector3(0,0,0); { - 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); + 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) + 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) + 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); + 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) + } + /* Dbvt */ + if(c.m_collide) { - btVector3 mi=c.m_nodes[0]->m_x; - btVector3 mx=mi; - for(int j=1;j<n;++j) + 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); + 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); + 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); + c.m_leaf=m_cdbvt.insert(bounds,&c); } } } + + } + + + // void btSoftBody::cleanupClusters() { -for(int i=0;i<m_joints.size();++i) + for(int i=0;i<m_joints.size();++i) { - m_joints[i]->Terminate(m_sst.sdt); - if(m_joints[i]->m_delete) + m_joints[i]->Terminate(m_sst.sdt); + if(m_joints[i]->m_delete) { - btAlignedFree(m_joints[i]); - m_joints.remove(m_joints[i--]); + btAlignedFree(m_joints[i]); + m_joints.remove(m_joints[i--]); } } } @@ -2063,9 +2121,9 @@ for(int i=0;i<m_joints.size();++i) // void btSoftBody::prepareClusters(int iterations) { -for(int i=0;i<m_joints.size();++i) + for(int i=0;i<m_joints.size();++i) { - m_joints[i]->Prepare(m_sst.sdt,iterations); + m_joints[i]->Prepare(m_sst.sdt,iterations); } } @@ -2073,50 +2131,51 @@ for(int i=0;i<m_joints.size();++i) // void btSoftBody::solveClusters(btScalar sor) { -for(int i=0,ni=m_joints.size();i<ni;++i) + for(int i=0,ni=m_joints.size();i<ni;++i) { - m_joints[i]->Solve(m_sst.sdt,sor); + 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; + 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) + if(drift) { - for(i=0;i<m_clusters.size();++i) + for(i=0;i<m_clusters.size();++i) { - Cluster& c=*m_clusters[i]; - if(c.m_ndimpulses) + 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; + c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses; + c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses; } } } -for(i=0;i<m_clusters.size();++i) + + for(i=0;i<m_clusters.size();++i) { - Cluster& c=*m_clusters[i]; - if(0<(drift?c.m_ndimpulses:c.m_nvimpulses)) + 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 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; + 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; } } } @@ -2131,18 +2190,21 @@ void btSoftBody::dampClusters() { int i; -for(i=0;i<m_clusters.size();++i) + for(i=0;i<m_clusters.size();++i) { - Cluster& c=*m_clusters[i]; - if(c.m_ndamping>0) + Cluster& c=*m_clusters[i]; + if(c.m_ndamping>0) { - for(int j=0;j<c.m_nodes.size();++j) + for(int j=0;j<c.m_nodes.size();++j) { - Node& n=*c.m_nodes[j]; - if(n.m_im>0) + 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); + const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com); + if(vx.length2()<=n.m_v.length2()) + { + n.m_v += c.m_ndamping*(vx-n.m_v); + } } } } @@ -2152,154 +2214,155 @@ for(i=0;i<m_clusters.size();++i) // void btSoftBody::Joint::Prepare(btScalar dt,int) { -m_bodies[0].activate(); -m_bodies[1].activate(); + 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) + 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_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; } -m_drift /=(btScalar)iterations; + 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]); + 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) + if(m_split>0) { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + 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) + 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_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; } -m_drift /=(btScalar)iterations; + 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); + 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) + if(m_split>0) { - m_bodies[0].applyDAImpulse(-m_sdrift); - m_bodies[1].applyDAImpulse( m_sdrift); + 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) + 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_drift=m_drift*m_erp/dt; + if(m_split>0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; } - m_drift/=(btScalar)iterations; + m_drift/=(btScalar)iterations; } else { - m_drift=m_sdrift=btVector3(0,0,0); + 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 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; + 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]); + 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) + if(m_split>0) { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + 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; @@ -2311,14 +2374,14 @@ void btSoftBody::applyForces() const bool as_pressure=kPR!=0; const bool as_volume=kVC>0; const bool as_aero= as_lift || - as_drag ; + as_drag ; const bool as_vaero= as_aero && - (m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided); + (m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided); const bool as_faero= as_aero && - (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided); + (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided); const bool use_medium= as_aero; const bool use_volume= as_pressure || - as_volume ; + as_volume ; btScalar volume=0; btScalar ivolumetp=0; btScalar dvolumetv=0; @@ -2447,12 +2510,10 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti) for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i) { const RContact& c=psb->m_rcontacts[i]; - ///skip object that don't have collision response - if (!psb->getWorldInfo()->m_dispatcher->needsResponse(psb,c.m_cti.m_body)) - continue; - const sCti& cti=c.m_cti; - const btVector3 va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*dt; + btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj); + + const btVector3 va=tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); 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); @@ -2462,7 +2523,8 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti) 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); + if (tmpRigid) + tmpRigid->applyImpulse(impulse,c.m_c1); } } } @@ -2470,32 +2532,32 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti) // void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti) { -for(int i=0,ni=psb->m_scontacts.size();i<ni;++i) + 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 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; + 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()); + 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()); } } @@ -2522,107 +2584,114 @@ void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti) // void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst) { -for(int i=0,ni=psb->m_links.size();i<ni;++i) + 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); + 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) + 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); + return(0); } // btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) { -switch(solver) + switch(solver) { case eVSolver::Linear: return(&btSoftBody::VSolve_Links); } -return(0); + return(0); } // void btSoftBody::defaultCollisionHandler(btCollisionObject* pco) { -switch(m_cfg.collisions&fCollision::RVSmask) + 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; + btSoftColliders::CollideSDF_RS docollide; + btRigidBody* prb1=btRigidBody::upcast(pco); + btTransform wtr=prb1 ? prb1->getInterpolationWorldTransform() : pco->getWorldTransform(); + + const btTransform ctr=pco->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.m_colObj1 = pco; + docollide.m_rigidBody = prb1; + + docollide.dynmargin = basemargin+timemargin; + docollide.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide); + } + break; case fCollision::CL_RS: { - btSoftColliders::CollideCL_RS collider; - collider.Process(this,btRigidBody::upcast(pco)); + btSoftColliders::CollideCL_RS collider; + collider.Process(this,pco); } - break; + break; } } // void btSoftBody::defaultCollisionHandler(btSoftBody* psb) { -const int cf=m_cfg.collisions&psb->m_cfg.collisions; -switch(cf&fCollision::SVSmask) + 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); + btSoftColliders::CollideCL_SS docollide; + docollide.Process(this,psb); } - break; + 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; + //only self-collision for Cluster, not Vertex-Face yet + if (this!=psb) + { + 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; + docollide.psb[0]->m_ndbvt.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; + docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } + } + break; } } |