diff options
Diffstat (limited to 'extern/bullet2/src/BulletSoftBody')
16 files changed, 2217 insertions, 1869 deletions
diff --git a/extern/bullet2/src/BulletSoftBody/CMakeLists.txt b/extern/bullet2/src/BulletSoftBody/CMakeLists.txt index a725e10ff77..dbd87afea38 100644 --- a/extern/bullet2/src/BulletSoftBody/CMakeLists.txt +++ b/extern/bullet2/src/BulletSoftBody/CMakeLists.txt @@ -3,19 +3,41 @@ INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src } ) -ADD_LIBRARY(LibBulletSoftBody +SET(BulletSoftBody_SRCS btSoftBody.cpp - btSoftBody.h btSoftBodyHelpers.cpp - btSparseSDF.h - btSoftBodyHelpers.h btSoftBodyRigidBodyCollisionConfiguration.cpp btSoftRigidCollisionAlgorithm.cpp - btSoftRigidCollisionAlgorithm.h btSoftSoftCollisionAlgorithm.cpp - btSoftSoftCollisionAlgorithm.h btSoftBodyConcaveCollisionAlgorithm.cpp + btSoftRigidDynamicsWorld.cpp +) + +SET(BulletSoftBody_HDRS + btSoftBody.h + btSparseSDF.h + btSoftBodyHelpers.h + btSoftRigidCollisionAlgorithm.h + btSoftSoftCollisionAlgorithm.h btSoftBodyConcaveCollisionAlgorithm.h btSoftRigidDynamicsWorld.h - btSoftRigidDynamicsWorld.cpp ) + + + +ADD_LIBRARY(BulletSoftBody ${BulletSoftBody_SRCS} ${BulletSoftBody_HDRS}) +SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + TARGET_LINK_LIBRARIES(BulletSoftBody BulletDynamics) +ENDIF (BUILD_SHARED_LIBS) + +IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + INSTALL(TARGETS BulletSoftBody DESTINATION lib) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} DESTINATION include FILES_MATCHING PATTERN "*.h") +ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + +IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES PUBLIC_HEADER "${BulletSoftBody_HDRS}") +ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) 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; } } diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.h b/extern/bullet2/src/BulletSoftBody/btSoftBody.h index 743462d259a..a62c21883c8 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.h @@ -18,7 +18,6 @@ subject to the following restrictions: #define _BT_SOFT_BODY_H #include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btPoint3.h" #include "LinearMath/btTransform.h" #include "LinearMath/btIDebugDraw.h" #include "BulletDynamics/Dynamics/btRigidBody.h" @@ -45,7 +44,8 @@ struct btSoftBodyWorldInfo }; -/// btSoftBody is work-in-progress +///The btSoftBody is an class to simulate cloth and volumetric soft bodies. +///There is two-way interaction between btSoftBody and btRigidBody/btCollisionObject. class btSoftBody : public btCollisionObject { public: @@ -64,13 +64,13 @@ public: F_OneSided, ///Face normals are taken as it is END };}; - + ///eVSolver : velocities solvers struct eVSolver { enum _ { Linear, ///Linear solver END };}; - + ///ePSolver : positions solvers struct ePSolver { enum _ { Linear, ///Linear solver @@ -79,7 +79,7 @@ public: SContacts, ///Soft contacts solver END };}; - + ///eSolverPresets struct eSolverPresets { enum _ { Positions, @@ -87,7 +87,7 @@ public: Default = Positions, END };}; - + ///eFeature struct eFeature { enum _ { None, @@ -96,20 +96,20 @@ public: Face, END };}; - + typedef btAlignedObjectArray<eVSolver::_> tVSolverArray; typedef btAlignedObjectArray<ePSolver::_> tPSolverArray; - + // // Flags // - + ///fCollision struct fCollision { enum _ { RVSmask = 0x000f, ///Rigid versus soft mask SDF_RS = 0x0001, ///SDF based rigid vs soft CL_RS = 0x0002, ///Cluster vs convex rigid vs soft - + SVSmask = 0x00f0, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling @@ -117,7 +117,7 @@ public: Default = SDF_RS, END };}; - + ///fMaterial struct fMaterial { enum _ { DebugDraw = 0x0001, /// Enable debug draw @@ -125,26 +125,26 @@ public: Default = DebugDraw, END };}; - + // // API Types // - + /* sRayCast */ struct sRayCast { btSoftBody* body; /// soft body eFeature::_ feature; /// feature type int index; /// feature index - btScalar time; /// time of impact (rayorg+raydir*time) + btScalar fraction; /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction) }; - + /* ImplicitFn */ struct ImplicitFn { virtual btScalar Eval(const btVector3& x)=0; }; - + // // Internal types // @@ -155,7 +155,7 @@ public: /* sCti is Softbody contact info */ struct sCti { - btRigidBody* m_body; /* Rigid body */ + btCollisionObject* m_colObj; /* Rigid body */ btVector3 m_normal; /* Outward normal */ btScalar m_offset; /* Offset from origin */ }; @@ -182,7 +182,7 @@ public: btScalar m_kVST; // Volume stiffness coefficient [0,1] int m_flags; // Flags }; - + /* Feature */ struct Feature : Element { @@ -293,12 +293,13 @@ public: btVector3 m_lv; btVector3 m_av; btDbvtNode* m_leaf; - btScalar m_ndamping; - btScalar m_ldamping; - btScalar m_adamping; + btScalar m_ndamping; /* Node damping */ + btScalar m_ldamping; /* Linear damping */ + btScalar m_adamping; /* Angular damping */ btScalar m_matching; bool m_collide; - Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {} + int m_clusterIndex; + Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {} }; /* Impulse */ struct Impulse @@ -309,109 +310,115 @@ public: int m_asDrift:1; Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0) {} Impulse operator -() const - { + { Impulse i=*this; i.m_velocity=-i.m_velocity; i.m_drift=-i.m_drift; return(i); - } + } Impulse operator*(btScalar x) const - { + { Impulse i=*this; i.m_velocity*=x; i.m_drift*=x; return(i); - } + } }; /* Body */ struct Body { - Cluster* m_soft; - btRigidBody* m_rigid; - Body() : m_soft(0),m_rigid(0) {} - Body(Cluster* p) : m_soft(p),m_rigid(0) {} - Body(btRigidBody* p) : m_soft(0),m_rigid(p) {} + Cluster* m_soft; + btRigidBody* m_rigid; + btCollisionObject* m_collisionObject; + + Body() : m_soft(0),m_rigid(0),m_collisionObject(0) {} + Body(Cluster* p) : m_soft(p),m_rigid(0),m_collisionObject(0) {} + Body(btCollisionObject* colObj) : m_soft(0),m_collisionObject(colObj) + { + m_rigid = btRigidBody::upcast(m_collisionObject); + } + void activate() const - { + { if(m_rigid) m_rigid->activate(); - } + } const btMatrix3x3& invWorldInertia() const - { + { static const btMatrix3x3 iwi(0,0,0,0,0,0,0,0,0); if(m_rigid) return(m_rigid->getInvInertiaTensorWorld()); if(m_soft) return(m_soft->m_invwi); return(iwi); - } + } btScalar invMass() const - { + { if(m_rigid) return(m_rigid->getInvMass()); if(m_soft) return(m_soft->m_imass); return(0); - } + } const btTransform& xform() const - { + { static const btTransform identity=btTransform::getIdentity(); - if(m_rigid) return(m_rigid->getInterpolationWorldTransform()); + if(m_collisionObject) return(m_collisionObject->getInterpolationWorldTransform()); if(m_soft) return(m_soft->m_framexform); return(identity); - } + } btVector3 linearVelocity() const - { + { if(m_rigid) return(m_rigid->getLinearVelocity()); if(m_soft) return(m_soft->m_lv); return(btVector3(0,0,0)); - } + } btVector3 angularVelocity(const btVector3& rpos) const - { + { if(m_rigid) return(cross(m_rigid->getAngularVelocity(),rpos)); if(m_soft) return(cross(m_soft->m_av,rpos)); return(btVector3(0,0,0)); - } + } btVector3 angularVelocity() const - { + { if(m_rigid) return(m_rigid->getAngularVelocity()); if(m_soft) return(m_soft->m_av); return(btVector3(0,0,0)); - } + } btVector3 velocity(const btVector3& rpos) const - { + { return(linearVelocity()+angularVelocity(rpos)); - } + } void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const - { + { if(m_rigid) m_rigid->applyImpulse(impulse,rpos); if(m_soft) btSoftBody::clusterVImpulse(m_soft,rpos,impulse); - } + } void applyDImpulse(const btVector3& impulse,const btVector3& rpos) const - { + { if(m_rigid) m_rigid->applyImpulse(impulse,rpos); if(m_soft) btSoftBody::clusterDImpulse(m_soft,rpos,impulse); - } + } void applyImpulse(const Impulse& impulse,const btVector3& rpos) const - { + { if(impulse.m_asVelocity) applyVImpulse(impulse.m_velocity,rpos); if(impulse.m_asDrift) applyDImpulse(impulse.m_drift,rpos); - } + } void applyVAImpulse(const btVector3& impulse) const - { + { if(m_rigid) m_rigid->applyTorqueImpulse(impulse); if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse); - } + } void applyDAImpulse(const btVector3& impulse) const - { + { if(m_rigid) m_rigid->applyTorqueImpulse(impulse); if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse); - } + } void applyAImpulse(const Impulse& impulse) const - { + { if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift); - } + } void applyDCImpulse(const btVector3& impulse) const - { + { if(m_rigid) m_rigid->applyCentralImpulse(impulse); if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse); - } + } }; /* Joint */ struct Joint @@ -419,15 +426,15 @@ public: struct eType { enum _ { Linear, Angular, - Contact, + Contact };}; struct Specs - { - Specs() : erp(1),cfm(1),split(1) {} + { + Specs() : erp(1),cfm(1),split(1) {} btScalar erp; btScalar cfm; btScalar split; - }; + }; Body m_bodies[2]; btVector3 m_refs[2]; btScalar m_cfm; @@ -438,7 +445,7 @@ public: btMatrix3x3 m_massmatrix; bool m_delete; virtual ~Joint() {} - Joint() : m_delete(false) {} + Joint() : m_delete(false) {} virtual void Prepare(btScalar dt,int iterations); virtual void Solve(btScalar dt,btScalar sor)=0; virtual void Terminate(btScalar dt)=0; @@ -448,9 +455,9 @@ public: struct LJoint : Joint { struct Specs : Joint::Specs - { + { btVector3 position; - }; + }; btVector3 m_rpos[2]; void Prepare(btScalar dt,int iterations); void Solve(btScalar dt,btScalar sor); @@ -461,17 +468,17 @@ public: struct AJoint : Joint { struct IControl - { + { virtual void Prepare(AJoint*) {} virtual btScalar Speed(AJoint*,btScalar current) { return(current); } static IControl* Default() { static IControl def;return(&def); } - }; + }; struct Specs : Joint::Specs - { - Specs() : icontrol(IControl::Default()) {} + { + Specs() : icontrol(IControl::Default()) {} btVector3 axis; IControl* icontrol; - }; + }; btVector3 m_axis[2]; IControl* m_icontrol; void Prepare(btScalar dt,int iterations); @@ -534,23 +541,26 @@ public: btScalar radmrg; // radial margin btScalar updmrg; // Update margin }; - /* RayCaster */ - struct RayCaster : btDbvt::ICollide - { - btVector3 o; - btVector3 d; - btScalar mint; - Face* face; - int tests; - RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt); + /// RayFromToCaster takes a ray from, ray to (instead of direction!) + struct RayFromToCaster : btDbvt::ICollide + { + btVector3 m_rayFrom; + btVector3 m_rayTo; + btVector3 m_rayNormalizedDirection; + btScalar m_mint; + Face* m_face; + int m_tests; + RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt); void Process(const btDbvtNode* leaf); - static inline btScalar rayTriangle(const btVector3& org, - const btVector3& dir, - const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar maxt=SIMD_INFINITY); - }; + + static inline btScalar rayFromToTriangle(const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayNormalizedDirection, + const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar maxt=SIMD_INFINITY); + }; // // Typedef's @@ -596,16 +606,19 @@ public: btDbvt m_fdbvt; // Faces tree btDbvt m_cdbvt; // Clusters tree tClusterArray m_clusters; // Clusters - - btTransform m_initialWorldTransform; //used to attach constraints etc. + + btAlignedObjectArray<bool>m_clusterConnectivity;//cluster connectivity, for self-collision + + btTransform m_initialWorldTransform; + // // Api // - + /* ctor */ btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count, - const btVector3* x, - const btScalar* m); + const btVector3* x, + const btScalar* m); /* dtor */ virtual ~btSoftBody(); /* Check for existing link */ @@ -617,59 +630,60 @@ public: return m_worldInfo; } + ///@todo: avoid internal softbody shape hack and move collision code to collision library virtual void setCollisionShape(btCollisionShape* collisionShape) { - //don't do anything, due to the internal shape hack: todo: fix this + } bool checkLink( int node0, int node1) const; bool checkLink( const Node* node0, - const Node* node1) const; + const Node* node1) const; /* Check for existring face */ bool checkFace( int node0, - int node1, - int node2) const; + int node1, + int node2) const; /* Append material */ Material* appendMaterial(); /* Append note */ void appendNote( const char* text, - const btVector3& o, - const btVector4& c=btVector4(1,0,0,0), - Node* n0=0, - Node* n1=0, - Node* n2=0, - Node* n3=0); + const btVector3& o, + const btVector4& c=btVector4(1,0,0,0), + Node* n0=0, + Node* n1=0, + Node* n2=0, + Node* n3=0); void appendNote( const char* text, - const btVector3& o, - Node* feature); + const btVector3& o, + Node* feature); void appendNote( const char* text, - const btVector3& o, - Link* feature); + const btVector3& o, + Link* feature); void appendNote( const char* text, - const btVector3& o, - Face* feature); + const btVector3& o, + Face* feature); /* Append node */ void appendNode( const btVector3& x,btScalar m); /* Append link */ void appendLink(int model=-1,Material* mat=0); void appendLink( int node0, - int node1, - Material* mat=0, - bool bcheckexist=false); + int node1, + Material* mat=0, + bool bcheckexist=false); void appendLink( Node* node0, - Node* node1, - Material* mat=0, - bool bcheckexist=false); + Node* node1, + Material* mat=0, + bool bcheckexist=false); /* Append face */ void appendFace(int model=-1,Material* mat=0); void appendFace( int node0, - int node1, - int node2, - Material* mat=0); + int node1, + int node2, + Material* mat=0); /* Append anchor */ void appendAnchor( int node, - btRigidBody* body,bool disableCollision); + btRigidBody* body, bool disableCollisionBetweenLinkedBodies=false); /* Append linear joint */ void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1); void appendLinearJoint(const LJoint::Specs& specs,Body body=Body()); @@ -682,7 +696,7 @@ public: void addForce( const btVector3& force); /* Add force (or gravity) to a node of the body */ void addForce( const btVector3& force, - int node); + int node); /* Add velocity to the entire body */ void addVelocity( const btVector3& velocity); @@ -691,17 +705,17 @@ public: /* Add velocity to a node of the body */ void addVelocity( const btVector3& velocity, - int node); + int node); /* Set mass */ void setMass( int node, - btScalar mass); + btScalar mass); /* Get mass */ btScalar getMass( int node) const; /* Get total mass */ btScalar getTotalMass() const; /* Set total mass (weighted by previous masses) */ void setTotalMass( btScalar mass, - bool fromfaces=false); + bool fromfaces=false); /* Set total density */ void setTotalDensity(btScalar density); /* Transform */ @@ -714,7 +728,7 @@ public: void scale( const btVector3& scl); /* Set current state as pose */ void setPose( bool bvolume, - bool bframe); + bool bframe); /* Return the volume */ btScalar getVolume() const; /* Cluster count */ @@ -734,7 +748,7 @@ public: static void clusterDCImpulse(Cluster* cluster,const btVector3& impulse); /* Generate bending constraints based on distance in the adjency graph */ int generateBendingConstraints( int distance, - Material* mat=0); + Material* mat=0); /* Randomize constraints to reduce solver bias */ void randomizeConstraints(); /* Release clusters */ @@ -747,11 +761,11 @@ public: /* CutLink */ bool cutLink(int node0,int node1,btScalar position); bool cutLink(const Node* node0,const Node* node1,btScalar position); - /* Ray casting */ - bool rayCast(const btVector3& org, - const btVector3& dir, - sRayCast& results, - btScalar maxtime=SIMD_INFINITY); + + ///Ray casting using rayFrom and rayTo in worldspace, (not direction!) + bool rayTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results); /* Solver presets */ void setSolver(eSolverPresets::_ preset); /* predictMotion */ @@ -769,11 +783,11 @@ public: /* defaultCollisionHandlers */ void defaultCollisionHandler(btCollisionObject* pco); void defaultCollisionHandler(btSoftBody* psb); - + // // Cast // - + static const btSoftBody* upcast(const btCollisionObject* colObj) { if (colObj->getInternalType()==CO_SOFT_BODY) @@ -801,11 +815,12 @@ public: // void pointersToIndices(); void indicesToPointers(const int* map=0); - int rayCast(const btVector3& org,const btVector3& dir, - btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const; + + int rayTest(const btVector3& rayFrom,const btVector3& rayTo, + btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const; void initializeFaceTree(); btVector3 evaluateCom() const; - bool checkContact(btRigidBody* prb,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const; + bool checkContact(btCollisionObject* colObj,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const; void updateNormals(); void updateBounds(); void updatePose(); @@ -825,7 +840,7 @@ public: static void VSolve_Links(btSoftBody* psb,btScalar kst); static psolver_t getSolver(ePSolver::_ solver); static vsolver_t getSolver(eVSolver::_ solver); - + }; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp index 61a57ea5da9..f334e15e0d3 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @@ -32,7 +32,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletSoftBody/btSoftBody.h" -#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.3)//make this configurable +#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) : btCollisionAlgorithm(ci), @@ -50,27 +50,27 @@ btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm() btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped): - m_dispatcher(dispatcher), - m_dispatchInfoPtr(0) +m_dispatcher(dispatcher), +m_dispatchInfoPtr(0) { m_softBody = (btSoftBody*) (isSwapped? body1:body0); m_triBody = isSwapped? body0:body1; - - // - // create the manifold from the dispatcher 'manifold pool' - // -// m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody); - clearCache(); + // + // create the manifold from the dispatcher 'manifold pool' + // + // m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody); + + clearCache(); } btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback() { clearCache(); -// m_dispatcher->releaseManifold( m_manifoldPtr ); - + // m_dispatcher->releaseManifold( m_manifoldPtr ); + } - + void btSoftBodyTriangleCallback::clearCache() { @@ -83,18 +83,18 @@ void btSoftBodyTriangleCallback::clearCache() delete tmp->m_childShape; } m_shapeCache.clear(); -}; +} void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) { - //just for debugging purposes + //just for debugging purposes //printf("triangle %d",m_triangleCount++); btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody); btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = m_dispatcher; - ///debug drawing of the overlapping triangles + ///debug drawing of the overlapping triangles if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) { btVector3 color(255,255,0); @@ -107,7 +107,7 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, btTriIndex triIndex(partId,triangleIndex,0); btHashKey<btTriIndex> triKey(triIndex.getUid()); - + btTriIndex* shapeIndex = m_shapeCache[triKey]; if (shapeIndex) { @@ -116,13 +116,13 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, //copy over user pointers to temporary shape tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); - + btCollisionShape* tmpShape = ob->getCollisionShape(); ob->internalSetTemporaryCollisionShape( tm ); - + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); - + colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); @@ -133,56 +133,56 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, //aabb filter is already applied! //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject); - -// if (m_softBody->getCollisionShape()->getShapeType()== + + // if (m_softBody->getCollisionShape()->getShapeType()== { -// btVector3 other; + // btVector3 other; btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]); normal.normalize(); normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION; -// other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; -// other+=normal*22.f; - btVector3 pts[6] = {triangle[0], - triangle[1], - triangle[2], - triangle[0]-normal, - triangle[1]-normal, - triangle[2]-normal}; + // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; + // other+=normal*22.f; + btVector3 pts[6] = {triangle[0]+normal, + triangle[1]+normal, + triangle[2]+normal, + triangle[0]-normal, + triangle[1]-normal, + triangle[2]-normal}; btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6); -// btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); - + // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); + //btTriangleShape tm(triangle[0],triangle[1],triangle[2]); - // tm.setMargin(m_collisionMarginTriangle); - + // tm.setMargin(m_collisionMarginTriangle); + //copy over user pointers to temporary shape tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); - + btCollisionShape* tmpShape = ob->getCollisionShape(); ob->internalSetTemporaryCollisionShape( tm ); - + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); ///this should use the btDispatcher, so the actual registered algorithm is used // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); //m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex); - // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); -// cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); + // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); + // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); - - + + ob->internalSetTemporaryCollisionShape( tmpShape ); triIndex.m_childShape = tm; m_shapeCache.insert(triKey,triIndex); } - + } @@ -194,7 +194,7 @@ void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMargin m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION); m_resultOut = resultOut; - + btVector3 aabbWorldSpaceMin,aabbWorldSpaceMax; m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax); btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5); @@ -217,8 +217,8 @@ void btSoftBodyConcaveCollisionAlgorithm::clearCache() void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { - - + + btCollisionObject* convexBody = m_isSwapped ? body1 : body0; btCollisionObject* triBody = m_isSwapped ? body0 : body1; @@ -228,26 +228,26 @@ void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* b btCollisionObject* triOb = triBody; btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape()); - - // if (convexBody->getCollisionShape()->isConvex()) + + // if (convexBody->getCollisionShape()->isConvex()) { btScalar collisionMarginTriangle = concaveShape->getMargin(); - -// resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); + + // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut); //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); -// m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody); + // m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody); concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax()); - - // resultOut->refreshContactPoints(); - + + // resultOut->refreshContactPoints(); + } - + } } @@ -287,7 +287,7 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO btScalar m_ccdSphereRadius; btScalar m_hitFraction; - + LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) :m_ccdSphereFromTrans(from), @@ -296,8 +296,8 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO m_hitFraction(hitFraction) { } - - + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { (void)partId; @@ -327,9 +327,9 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO }; - - + + if (triBody->getCollisionShape()->isConcave()) { btVector3 rayAabbMin = convexFromLocal.getOrigin(); @@ -349,12 +349,12 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO btCollisionObject* concavebody = triBody; btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); - + if (triangleMesh) { triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); } - + if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h index 08ac3f11e20..a6ea33717bc 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h @@ -35,7 +35,7 @@ struct btTriIndex { int m_PartIdTriangleIndex; class btCollisionShape* m_childShape; - + btTriIndex(int partId,int triangleIndex,btCollisionShape* shape) { m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; @@ -75,11 +75,11 @@ class btSoftBodyTriangleCallback : public btTriangleCallback btScalar m_collisionMarginTriangle; btHashMap<btHashKey<btTriIndex>,btTriIndex> m_shapeCache; - + public: -int m_triangleCount; - -// btPersistentManifold* m_manifoldPtr; + int m_triangleCount; + + // btPersistentManifold* m_manifoldPtr; btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); @@ -88,7 +88,7 @@ int m_triangleCount; virtual ~btSoftBodyTriangleCallback(); virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - + void clearCache(); SIMD_FORCE_INLINE const btVector3& getAabbMin() const diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp index d9919967233..6ab93c16402 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -22,57 +22,57 @@ subject to the following restrictions: // static void drawVertex( btIDebugDraw* idraw, - const btVector3& x,btScalar s,const btVector3& c) - { - idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); - idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); - idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); - } + const btVector3& x,btScalar s,const btVector3& c) +{ + idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); + idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); + idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); +} // static void drawBox( btIDebugDraw* idraw, - const btVector3& mins, - const btVector3& maxs, - const btVector3& color) + const btVector3& mins, + const btVector3& maxs, + const btVector3& color) { -const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), - btVector3(maxs.x(),mins.y(),mins.z()), - btVector3(maxs.x(),maxs.y(),mins.z()), - btVector3(mins.x(),maxs.y(),mins.z()), - btVector3(mins.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),maxs.y(),maxs.z()), - btVector3(mins.x(),maxs.y(),maxs.z())}; -idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); -idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); -idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); -idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); -idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); -idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); + const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), + btVector3(maxs.x(),mins.y(),mins.z()), + btVector3(maxs.x(),maxs.y(),mins.z()), + btVector3(mins.x(),maxs.y(),mins.z()), + btVector3(mins.x(),mins.y(),maxs.z()), + btVector3(maxs.x(),mins.y(),maxs.z()), + btVector3(maxs.x(),maxs.y(),maxs.z()), + btVector3(mins.x(),maxs.y(),maxs.z())}; + idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); + idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); + idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); + idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); + idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); + idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); } // static void drawTree( btIDebugDraw* idraw, - const btDbvtNode* node, - int depth, - const btVector3& ncolor, - const btVector3& lcolor, - int mindepth, - int maxdepth) + const btDbvtNode* node, + int depth, + const btVector3& ncolor, + const btVector3& lcolor, + int mindepth, + int maxdepth) { -if(node) + if(node) { - if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0))) + if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0))) { - drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth); - drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); + drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth); + drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); } - if(depth>=mindepth) + if(depth>=mindepth) { - const btScalar scl=(btScalar)(node->isinternal()?1:1); - const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; - const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl; - drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); + const btScalar scl=(btScalar)(node->isinternal()?1:1); + const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; + const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl; + drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); } } } @@ -81,25 +81,25 @@ if(node) template <typename T> static inline T sum(const btAlignedObjectArray<T>& items) { -T v; -if(items.size()) + T v; + if(items.size()) { - v=items[0]; - for(int i=1,ni=items.size();i<ni;++i) + v=items[0]; + for(int i=1,ni=items.size();i<ni;++i) { - v+=items[i]; + v+=items[i]; } } -return(v); + return(v); } // template <typename T,typename Q> static inline void add(btAlignedObjectArray<T>& items,const Q& value) { -for(int i=0,ni=items.size();i<ni;++i) + for(int i=0,ni=items.size();i<ni;++i) { - items[i]+=value; + items[i]+=value; } } @@ -107,9 +107,9 @@ for(int i=0,ni=items.size();i<ni;++i) template <typename T,typename Q> static inline void mul(btAlignedObjectArray<T>& items,const Q& value) { -for(int i=0,ni=items.size();i<ni;++i) + for(int i=0,ni=items.size();i<ni;++i) { - items[i]*=value; + items[i]*=value; } } @@ -117,8 +117,8 @@ for(int i=0,ni=items.size();i<ni;++i) template <typename T> static inline T average(const btAlignedObjectArray<T>& items) { -const btScalar n=(btScalar)(items.size()>0?items.size():1); -return(sum(items)/n); + const btScalar n=(btScalar)(items.size()>0?items.size():1); + return(sum(items)/n); } // @@ -136,27 +136,27 @@ static inline btScalar tetravolume(const btVector3& x0, // #if 0 static btVector3 stresscolor(btScalar stress) - { +{ static const btVector3 spectrum[]= { btVector3(1,0,1), - btVector3(0,0,1), - btVector3(0,1,1), - btVector3(0,1,0), - btVector3(1,1,0), - btVector3(1,0,0), - btVector3(1,0,0)}; + btVector3(0,0,1), + btVector3(0,1,1), + btVector3(0,1,0), + btVector3(1,1,0), + btVector3(1,0,0), + btVector3(1,0,0)}; static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; static const btScalar one=1; stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors; const int sel=(int)stress; const btScalar frc=stress-sel; return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); - } +} #endif // void btSoftBodyHelpers::Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags) + btIDebugDraw* idraw, + int drawflags) { const btScalar scl=(btScalar)0.1; const btScalar nscl=scl*5; @@ -251,29 +251,29 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; const btVector3 c=(x[0]+x[1]+x[2])/3; idraw->drawTriangle((x[0]-c)*scl+c, - (x[1]-c)*scl+c, - (x[2]-c)*scl+c, - col,alp); + (x[1]-c)*scl+c, + (x[2]-c)*scl+c, + col,alp); } } /* Clusters */ if(0!=(drawflags&fDrawFlags::Clusters)) - { + { srand(1806); for(i=0;i<psb->m_clusters.size();++i) - { + { if(psb->m_clusters[i]->m_collide) - { + { btVector3 color( rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX); + rand()/(btScalar)RAND_MAX, + rand()/(btScalar)RAND_MAX); color=color.normalized()*0.75; btAlignedObjectArray<btVector3> vertices; vertices.resize(psb->m_clusters[i]->m_nodes.size()); for(j=0,nj=vertices.size();j<nj;++j) - { + { vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x; - } + } HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); HullResult hres; HullLibrary hlib; @@ -284,32 +284,32 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, mul(hres.m_OutputVertices,(btScalar)1); add(hres.m_OutputVertices,center); for(j=0;j<(int)hres.mNumFaces;++j) - { + { const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]}; idraw->drawTriangle(hres.m_OutputVertices[idx[0]], - hres.m_OutputVertices[idx[1]], - hres.m_OutputVertices[idx[2]], - color,1); - } - hlib.ReleaseResult(hres); + hres.m_OutputVertices[idx[1]], + hres.m_OutputVertices[idx[2]], + color,1); } + hlib.ReleaseResult(hres); + } /* Velocities */ - #if 0 +#if 0 for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j) - { + { const btSoftBody::Cluster& c=psb->m_clusters[i]; const btVector3 r=c.m_nodes[j]->m_x-c.m_com; const btVector3 v=c.m_lv+cross(c.m_av,r); idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0)); - } - #endif + } +#endif /* Frame */ btSoftBody::Cluster& c=*psb->m_clusters[i]; idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); - } } + } /* Notes */ if(0!=(drawflags&fDrawFlags::Notes)) { @@ -318,9 +318,9 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, const btSoftBody::Note& n=psb->m_notes[i]; btVector3 p=n.m_offset; for(int j=0;j<n.m_rank;++j) - { + { p+=n.m_nodes[j]->m_x*n.m_coords[j]; - } + } idraw->draw3dText(p,n.m_text); } } @@ -333,33 +333,33 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, /* Joints */ if(0!=(drawflags&fDrawFlags::Joints)) { - for(i=0;i<psb->m_joints.size();++i) + for(i=0;i<psb->m_joints.size();++i) { - const btSoftBody::Joint* pj=psb->m_joints[i]; - switch(pj->Type()) + const btSoftBody::Joint* pj=psb->m_joints[i]; + switch(pj->Type()) { case btSoftBody::Joint::eType::Linear: { - const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; - const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; - idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); - idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); - drawVertex(idraw,a0,0.25,btVector3(1,1,0)); - drawVertex(idraw,a1,0.25,btVector3(0,1,1)); + const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; + const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; + const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; + idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); + idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); + drawVertex(idraw,a0,0.25,btVector3(1,1,0)); + drawVertex(idraw,a1,0.25,btVector3(0,1,1)); } - break; + break; case btSoftBody::Joint::eType::Angular: { - const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; - const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); - const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); - const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; - idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); - idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); - idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); - idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); + const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; + const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); + const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); + const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; + const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; + idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); + idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); + idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); + idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); } } } @@ -368,10 +368,10 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, // void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool /*stress*/) + btIDebugDraw* idraw, + bool masses, + bool areas, + bool /*stress*/) { for(int i=0;i<psb->m_nodes.size();++i) { @@ -394,34 +394,34 @@ void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, // void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { -drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); + drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); } // void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { -drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); + drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); } // void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { -drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); + drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); } // void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw) + btIDebugDraw* idraw) { if(psb->m_pose.m_bframe) { @@ -445,9 +445,9 @@ void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, // btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from, - const btVector3& to, - int res, - int fixeds) + const btVector3& to, + int res, + int fixeds) { /* Create nodes */ const int r=res+2; @@ -477,13 +477,13 @@ btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, cons // btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags) + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags) { #define IDX(_x_,_y_) ((_y_)*rx+(_x_)) /* Create nodes */ @@ -553,9 +553,215 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const } // +btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags, + float* tex_coords) +{ + + /* + * + * corners: + * + * [0][0] corner00 ------- corner01 [resx][0] + * | | + * | | + * [0][resy] corner10 -------- corner11 [resx][resy] + * + * + * + * + * + * + * "fixedgs" map: + * + * corner00 --> +1 + * corner01 --> +2 + * corner10 --> +4 + * corner11 --> +8 + * upper middle --> +16 + * left middle --> +32 + * right middle --> +64 + * lower middle --> +128 + * center --> +256 + * + * + * tex_coords size (resx-1)*(resy-1)*12 + * + * + * + * SINGLE QUAD INTERNALS + * + * 1) btSoftBody's nodes and links, + * diagonal link is optional ("gendiags") + * + * + * node00 ------ node01 + * | . + * | . + * | . + * | . + * | . + * node10 node11 + * + * + * + * 2) Faces: + * two triangles, + * UV Coordinates (hier example for single quad) + * + * (0,1) (0,1) (1,1) + * 1 |\ 3 \-----| 2 + * | \ \ | + * | \ \ | + * | \ \ | + * | \ \ | + * 2 |-----\ 3 \| 1 + * (0,0) (1,0) (1,0) + * + * + * + * + * + * + */ + +#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) + /* Create nodes */ + if((resx<2)||(resy<2)) return(0); + const int rx=resx; + const int ry=resy; + const int tot=rx*ry; + btVector3* x=new btVector3[tot]; + btScalar* m=new btScalar[tot]; + + int iy; + + for(iy=0;iy<ry;++iy) + { + const btScalar ty=iy/(btScalar)(ry-1); + const btVector3 py0=lerp(corner00,corner01,ty); + const btVector3 py1=lerp(corner10,corner11,ty); + for(int ix=0;ix<rx;++ix) + { + const btScalar tx=ix/(btScalar)(rx-1); + x[IDX(ix,iy)]=lerp(py0,py1,tx); + m[IDX(ix,iy)]=1; + } + } + btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m); + if(fixeds&1) psb->setMass(IDX(0,0),0); + if(fixeds&2) psb->setMass(IDX(rx-1,0),0); + if(fixeds&4) psb->setMass(IDX(0,ry-1),0); + if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); + if(fixeds&16) psb->setMass(IDX((rx-1)/2,0),0); + if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),0); + if(fixeds&64) psb->setMass(IDX(rx-1,(ry-1)/2),0); + if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),0); + if(fixeds&256) psb->setMass(IDX((rx-1)/2,(ry-1)/2),0); + delete[] x; + delete[] m; + + + int z = 0; + /* Create links and faces */ + for(iy=0;iy<ry;++iy) + { + for(int ix=0;ix<rx;++ix) + { + const bool mdx=(ix+1)<rx; + const bool mdy=(iy+1)<ry; + + int node00=IDX(ix,iy); + int node01=IDX(ix+1,iy); + int node10=IDX(ix,iy+1); + int node11=IDX(ix+1,iy+1); + + if(mdx) psb->appendLink(node00,node01); + if(mdy) psb->appendLink(node00,node10); + if(mdx&&mdy) + { + psb->appendFace(node00,node10,node11); + if (tex_coords) { + tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0); + tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1); + tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0); + tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2); + tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3); + tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2); + } + psb->appendFace(node11,node01,node00); + if (tex_coords) { + tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3); + tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2); + tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3); + tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1); + tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0); + tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1); + } + if (gendiags) psb->appendLink(node00,node11); + z += 12; + } + } + } + /* Finished */ +#undef IDX + return(psb); +} + +float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) +{ + + /* + * + * + * node00 --- node01 + * | | + * node10 --- node11 + * + * + * ID map: + * + * node00 s --> 0 + * node00 t --> 1 + * + * node01 s --> 3 + * node01 t --> 1 + * + * node10 s --> 0 + * node10 t --> 2 + * + * node11 s --> 3 + * node11 t --> 2 + * + * + */ + + float tc=0.0f; + if (id == 0) { + tc = (1.0f/((resx-1))*ix); + } + else if (id==1) { + tc = (1.0f/((resy-1))*(resy-1-iy)); + } + else if (id==2) { + tc = (1.0f/((resy-1))*(resy-1-iy-1)); + } + else if (id==3) { + tc = (1.0f/((resx-1))*(ix+1)); + } + return tc; +} +// btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, - const btVector3& radius, - int res) + const btVector3& radius, + int res) { struct Hammersley { @@ -586,8 +792,8 @@ btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,c // btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, - const int* triangles, - int ntriangles) + const int* triangles, + int ntriangles) { int maxidx=0; int i,j,ni; @@ -615,7 +821,7 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo if(!chks[IDX(idx[j],idx[k])]) { chks[IDX(idx[j],idx[k])]=true; - chks[IDX(idx[k],idx[k])]=true; + chks[IDX(idx[k],idx[j])]=true; psb->appendLink(idx[j],idx[k]); } } @@ -628,7 +834,7 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo // btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, - int nvertices) + int nvertices) { HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); HullResult hres; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h index e9c6cb20657..0e3b50397ee 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h @@ -39,69 +39,81 @@ struct fDrawFlags { enum _ { Joints = 0x1000, /* presets */ Std = Links+Faces+Tetras+Anchors+Notes+Joints, - StdTetra = Std-Faces+Tetras, + StdTetra = Std-Faces+Tetras };}; struct btSoftBodyHelpers { /* Draw body */ static void Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags=fDrawFlags::Std); + btIDebugDraw* idraw, + int drawflags=fDrawFlags::Std); /* Draw body infos */ static void DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool stress); + btIDebugDraw* idraw, + bool masses, + bool areas, + bool stress); /* Draw node tree */ static void DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); + btIDebugDraw* idraw, + int mindepth=0, + int maxdepth=-1); /* Draw face tree */ static void DrawFaceTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); + btIDebugDraw* idraw, + int mindepth=0, + int maxdepth=-1); /* Draw cluster tree */ static void DrawClusterTree(btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); + btIDebugDraw* idraw, + int mindepth=0, + int maxdepth=-1); /* Draw rigid frame */ static void DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw); + btIDebugDraw* idraw); /* Create a rope */ static btSoftBody* CreateRope( btSoftBodyWorldInfo& worldInfo, - const btVector3& from, - const btVector3& to, - int res, - int fixeds); + const btVector3& from, + const btVector3& to, + int res, + int fixeds); /* Create a patch */ static btSoftBody* CreatePatch(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags); + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags); + /* Create a patch with UV Texture Coordinates */ + static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo, + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags, + float* tex_coords=0); + static float CalculateUV(int resx,int resy,int ix,int iy,int id); /* Create an ellipsoid */ static btSoftBody* CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, - const btVector3& center, - const btVector3& radius, - int res); + const btVector3& center, + const btVector3& radius, + int res); /* Create from trimesh */ static btSoftBody* CreateFromTriMesh( btSoftBodyWorldInfo& worldInfo, - const btScalar* vertices, - const int* triangles, - int ntriangles); + const btScalar* vertices, + const int* triangles, + int ntriangles); /* Create from convex-hull */ static btSoftBody* CreateFromConvexHull( btSoftBodyWorldInfo& worldInfo, - const btVector3* vertices, - int nvertices); + const btVector3* vertices, + int nvertices); }; #endif //SOFT_BODY_HELPERS_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h index 8fcf633fecc..5f0f7d54318 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h @@ -31,14 +31,14 @@ subject to the following restrictions: template <typename T> struct btSymMatrix { - btSymMatrix() : dim(0) {} - btSymMatrix(int n,const T& init=T()) { resize(n,init); } -void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } -int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); } -T& operator()(int c,int r) { return(store[index(c,r)]); } -const T& operator()(int c,int r) const { return(store[index(c,r)]); } -btAlignedObjectArray<T> store; -int dim; + btSymMatrix() : dim(0) {} + btSymMatrix(int n,const T& init=T()) { resize(n,init); } + void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } + int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); } + T& operator()(int c,int r) { return(store[index(c,r)]); } + const T& operator()(int c,int r) const { return(store[index(c,r)]); } + btAlignedObjectArray<T> store; + int dim; }; // @@ -48,10 +48,11 @@ class btSoftBodyCollisionShape : public btConcaveShape { public: btSoftBody* m_body; - + btSoftBodyCollisionShape(btSoftBody* backptr) { - m_body=backptr; + m_shapeType = SOFTBODY_SHAPE_PROXYTYPE; + m_body=backptr; } virtual ~btSoftBodyCollisionShape() @@ -68,37 +69,34 @@ public: ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { - /* t should be identity, but better be safe than...fast? */ - const btVector3 mins=m_body->m_bounds[0]; - const btVector3 maxs=m_body->m_bounds[1]; - const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),maxs.y(),maxs.z()), - t*btVector3(mins.x(),maxs.y(),maxs.z())}; - aabbMin=aabbMax=crns[0]; - for(int i=1;i<8;++i) + /* t should be identity, but better be safe than...fast? */ + const btVector3 mins=m_body->m_bounds[0]; + const btVector3 maxs=m_body->m_bounds[1]; + const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), + t*btVector3(maxs.x(),mins.y(),mins.z()), + t*btVector3(maxs.x(),maxs.y(),mins.z()), + t*btVector3(mins.x(),maxs.y(),mins.z()), + t*btVector3(mins.x(),mins.y(),maxs.z()), + t*btVector3(maxs.x(),mins.y(),maxs.z()), + t*btVector3(maxs.x(),maxs.y(),maxs.z()), + t*btVector3(mins.x(),maxs.y(),maxs.z())}; + aabbMin=aabbMax=crns[0]; + for(int i=1;i<8;++i) { - aabbMin.setMin(crns[i]); - aabbMax.setMax(crns[i]); + aabbMin.setMin(crns[i]); + aabbMax.setMax(crns[i]); } } - virtual int getShapeType() const - { - return SOFTBODY_SHAPE_PROXYTYPE; - } + virtual void setLocalScaling(const btVector3& /*scaling*/) { ///na } virtual const btVector3& getLocalScaling() const { - static const btVector3 dummy(1,1,1); - return dummy; + static const btVector3 dummy(1,1,1); + return dummy; } virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const { @@ -121,24 +119,24 @@ public: const btSoftBody::Cluster* m_cluster; btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } - - + + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const - { + { btSoftBody::Node* const * n=&m_cluster->m_nodes[0]; btScalar d=dot(vec,n[0]->m_x); int j=0; for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i) - { + { const btScalar k=dot(vec,n[i]->m_x); if(k>d) { d=k;j=i; } - } - return(n[j]->m_x); } + return(n[j]->m_x); + } virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const - { + { return(localGetSupportingVertex(vec)); - } + } //notice that the vectors should be unit length virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const {} @@ -151,7 +149,7 @@ public: {} virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } - + //debugging virtual const char* getName()const {return "SOFTCLUSTER";} @@ -173,8 +171,8 @@ public: template <typename T> static inline void ZeroInitialize(T& value) { -static const T zerodummy; -value=zerodummy; + static const T zerodummy; + value=zerodummy; } // template <typename T> @@ -194,23 +192,23 @@ static inline T InvLerp(const T& a,const T& b,btScalar t) { return((b+a*t-b*t)/(a*b)); } // static inline btMatrix3x3 Lerp( const btMatrix3x3& a, - const btMatrix3x3& b, - btScalar t) -{ -btMatrix3x3 r; -r[0]=Lerp(a[0],b[0],t); -r[1]=Lerp(a[1],b[1],t); -r[2]=Lerp(a[2],b[2],t); -return(r); + const btMatrix3x3& b, + btScalar t) +{ + btMatrix3x3 r; + r[0]=Lerp(a[0],b[0],t); + r[1]=Lerp(a[1],b[1],t); + r[2]=Lerp(a[2],b[2],t); + return(r); } // static inline btVector3 Clamp(const btVector3& v,btScalar maxlength) { -const btScalar sql=v.length2(); -if(sql>(maxlength*maxlength)) - return((v*maxlength)/btSqrt(sql)); + const btScalar sql=v.length2(); + if(sql>(maxlength*maxlength)) + return((v*maxlength)/btSqrt(sql)); else - return(v); + return(v); } // template <typename T> @@ -235,8 +233,8 @@ static inline bool SameSign(const T& x,const T& y) // static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y) { -const btVector3 d=x-y; -return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); + const btVector3 d=x-y; + return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); } // static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) @@ -273,7 +271,7 @@ static inline btMatrix3x3 Diagonal(btScalar x) } // static inline btMatrix3x3 Add(const btMatrix3x3& a, - const btMatrix3x3& b) + const btMatrix3x3& b) { btMatrix3x3 r; for(int i=0;i<3;++i) r[i]=a[i]+b[i]; @@ -281,7 +279,7 @@ static inline btMatrix3x3 Add(const btMatrix3x3& a, } // static inline btMatrix3x3 Sub(const btMatrix3x3& a, - const btMatrix3x3& b) + const btMatrix3x3& b) { btMatrix3x3 r; for(int i=0;i<3;++i) r[i]=a[i]-b[i]; @@ -289,7 +287,7 @@ static inline btMatrix3x3 Sub(const btMatrix3x3& a, } // static inline btMatrix3x3 Mul(const btMatrix3x3& a, - btScalar b) + btScalar b) { btMatrix3x3 r; for(int i=0;i<3;++i) r[i]=a[i]*b; @@ -298,9 +296,9 @@ static inline btMatrix3x3 Mul(const btMatrix3x3& a, // static inline void Orthogonalize(btMatrix3x3& m) { -m[2]=cross(m[0],m[1]).normalized(); -m[1]=cross(m[2],m[0]).normalized(); -m[0]=cross(m[1],m[2]).normalized(); + m[2]=cross(m[0],m[1]).normalized(); + m[1]=cross(m[2],m[0]).normalized(); + m[0]=cross(m[1],m[2]).normalized(); } // static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) @@ -311,90 +309,90 @@ static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const bt // static inline btMatrix3x3 ImpulseMatrix( btScalar dt, - btScalar ima, - btScalar imb, - const btMatrix3x3& iwi, - const btVector3& r) + btScalar ima, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) { return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); } // static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra, - btScalar imb,const btMatrix3x3& iib,const btVector3& rb) + btScalar imb,const btMatrix3x3& iib,const btVector3& rb) { -return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); + return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); } // static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, - const btMatrix3x3& iib) + const btMatrix3x3& iib) { -return(Add(iia,iib).inverse()); + return(Add(iia,iib).inverse()); } // static inline btVector3 ProjectOnAxis( const btVector3& v, - const btVector3& a) + const btVector3& a) { return(a*dot(v,a)); } // static inline btVector3 ProjectOnPlane( const btVector3& v, - const btVector3& a) + const btVector3& a) { return(v-ProjectOnAxis(v,a)); } // static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - btVector3& prj, - btScalar& sqd) + const btVector3& b, + btVector3& prj, + btScalar& sqd) { -const btVector3 d=b-a; -const btScalar m2=d.length2(); -if(m2>SIMD_EPSILON) + const btVector3 d=b-a; + const btScalar m2=d.length2(); + if(m2>SIMD_EPSILON) { - const btScalar t=Clamp<btScalar>(-dot(a,d)/m2,0,1); - const btVector3 p=a+d*t; - const btScalar l2=p.length2(); - if(l2<sqd) + const btScalar t=Clamp<btScalar>(-dot(a,d)/m2,0,1); + const btVector3 p=a+d*t; + const btScalar l2=p.length2(); + if(l2<sqd) { - prj=p; - sqd=l2; + prj=p; + sqd=l2; } } } // static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btVector3& prj, - btScalar& sqd) -{ -const btVector3& q=cross(b-a,c-a); -const btScalar m2=q.length2(); -if(m2>SIMD_EPSILON) + const btVector3& b, + const btVector3& c, + btVector3& prj, + btScalar& sqd) +{ + const btVector3& q=cross(b-a,c-a); + const btScalar m2=q.length2(); + if(m2>SIMD_EPSILON) { - const btVector3 n=q/btSqrt(m2); - const btScalar k=dot(a,n); - const btScalar k2=k*k; - if(k2<sqd) + const btVector3 n=q/btSqrt(m2); + const btScalar k=dot(a,n); + const btScalar k2=k*k; + if(k2<sqd) { - const btVector3 p=n*k; - if( (dot(cross(a-p,b-p),q)>0)&& - (dot(cross(b-p,c-p),q)>0)&& - (dot(cross(c-p,a-p),q)>0)) + const btVector3 p=n*k; + if( (dot(cross(a-p,b-p),q)>0)&& + (dot(cross(b-p,c-p),q)>0)&& + (dot(cross(c-p,a-p),q)>0)) { - prj=p; - sqd=k2; + prj=p; + sqd=k2; } else { - ProjectOrigin(a,b,prj,sqd); - ProjectOrigin(b,c,prj,sqd); - ProjectOrigin(c,a,prj,sqd); + ProjectOrigin(a,b,prj,sqd); + ProjectOrigin(b,c,prj,sqd); + ProjectOrigin(c,a,prj,sqd); } } } @@ -403,53 +401,53 @@ if(m2>SIMD_EPSILON) // template <typename T> static inline T BaryEval( const T& a, - const T& b, - const T& c, - const btVector3& coord) + const T& b, + const T& c, + const btVector3& coord) { return(a*coord.x()+b*coord.y()+c*coord.z()); } // static inline btVector3 BaryCoord( const btVector3& a, - const btVector3& b, - const btVector3& c, - const btVector3& p) -{ -const btScalar w[]={ cross(a-p,b-p).length(), - cross(b-p,c-p).length(), - cross(c-p,a-p).length()}; -const btScalar isum=1/(w[0]+w[1]+w[2]); -return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); + const btVector3& b, + const btVector3& c, + const btVector3& p) +{ + const btScalar w[]={ cross(a-p,b-p).length(), + cross(b-p,c-p).length(), + cross(c-p,a-p).length()}; + const btScalar isum=1/(w[0]+w[1]+w[2]); + return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); } // static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, - const btVector3& a, - const btVector3& b, - const btScalar accuracy, - const int maxiterations=256) -{ -btScalar span[2]={0,1}; -btScalar values[2]={fn->Eval(a),fn->Eval(b)}; -if(values[0]>values[1]) + const btVector3& a, + const btVector3& b, + const btScalar accuracy, + const int maxiterations=256) +{ + btScalar span[2]={0,1}; + btScalar values[2]={fn->Eval(a),fn->Eval(b)}; + if(values[0]>values[1]) { - btSwap(span[0],span[1]); - btSwap(values[0],values[1]); + btSwap(span[0],span[1]); + btSwap(values[0],values[1]); } -if(values[0]>-accuracy) return(-1); -if(values[1]<+accuracy) return(-1); -for(int i=0;i<maxiterations;++i) + if(values[0]>-accuracy) return(-1); + if(values[1]<+accuracy) return(-1); + for(int i=0;i<maxiterations;++i) { - const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1])); - const btScalar v=fn->Eval(Lerp(a,b,t)); - if((t<=0)||(t>=1)) break; - if(btFabs(v)<accuracy) return(t); - if(v<0) + const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1])); + const btScalar v=fn->Eval(Lerp(a,b,t)); + if((t<=0)||(t>=1)) break; + if(btFabs(v)<accuracy) return(t); + if(v<0) { span[0]=t;values[0]=v; } else { span[1]=t;values[1]=v; } } -return(-1); + return(-1); } // @@ -464,26 +462,26 @@ static inline btVector3 NormalizeAny(const btVector3& v) // static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f, - btScalar margin) -{ -const btVector3* pts[]={ &f.m_n[0]->m_x, - &f.m_n[1]->m_x, - &f.m_n[2]->m_x}; -btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); -vol.Expand(btVector3(margin,margin,margin)); -return(vol); + btScalar margin) +{ + const btVector3* pts[]={ &f.m_n[0]->m_x, + &f.m_n[1]->m_x, + &f.m_n[2]->m_x}; + btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); + vol.Expand(btVector3(margin,margin,margin)); + return(vol); } // static inline btVector3 CenterOf( const btSoftBody::Face& f) { -return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); + return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); } // static inline btScalar AreaOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2) + const btVector3& x1, + const btVector3& x2) { const btVector3 a=x1-x0; const btVector3 b=x2-x0; @@ -494,9 +492,9 @@ static inline btScalar AreaOf( const btVector3& x0, // static inline btScalar VolumeOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) + const btVector3& x1, + const btVector3& x2, + const btVector3& x3) { const btVector3 a=x1-x0; const btVector3 b=x2-x0; @@ -506,8 +504,8 @@ static inline btScalar VolumeOf( const btVector3& x0, // static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, - const btVector3& x, - btSoftBody::sMedium& medium) + const btVector3& x, + btSoftBody::sMedium& medium) { medium.m_velocity = btVector3(0,0,0); medium.m_pressure = 0; @@ -525,8 +523,8 @@ static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, // static inline void ApplyClampedForce( btSoftBody::Node& n, - const btVector3& f, - btScalar dt) + const btVector3& f, + btScalar dt) { const btScalar dtim=dt*n.m_im; if((f*dtim).length2()>n.m_v.length2()) @@ -541,13 +539,13 @@ static inline void ApplyClampedForce( btSoftBody::Node& n, // static inline int MatchEdge( const btSoftBody::Node* a, - const btSoftBody::Node* b, - const btSoftBody::Node* ma, - const btSoftBody::Node* mb) + const btSoftBody::Node* b, + const btSoftBody::Node* ma, + const btSoftBody::Node* mb) { -if((a==ma)&&(b==mb)) return(0); -if((a==mb)&&(b==ma)) return(1); -return(-1); + if((a==ma)&&(b==mb)) return(0); + if((a==mb)&&(b==ma)) return(1); + return(-1); } // @@ -557,56 +555,56 @@ return(-1); // struct btEigen { -static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) + static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) { - static const int maxiterations=16; - static const btScalar accuracy=(btScalar)0.0001; - btMatrix3x3& v=*vectors; - int iterations=0; - vectors->setIdentity(); - do { - int p=0,q=1; - if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; } - if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; } - if(btFabs(a[p][q])>accuracy) + static const int maxiterations=16; + static const btScalar accuracy=(btScalar)0.0001; + btMatrix3x3& v=*vectors; + int iterations=0; + vectors->setIdentity(); + do { + int p=0,q=1; + if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; } + if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; } + if(btFabs(a[p][q])>accuracy) { - const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); - const btScalar z=btFabs(w); - const btScalar t=w/(z*(btSqrt(1+w*w)+z)); - if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ + const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); + const btScalar z=btFabs(w); + const btScalar t=w/(z*(btSqrt(1+w*w)+z)); + if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ { - const btScalar c=1/btSqrt(t*t+1); - const btScalar s=c*t; - mulPQ(a,c,s,p,q); - mulTPQ(a,c,s,p,q); - mulPQ(v,c,s,p,q); + const btScalar c=1/btSqrt(t*t+1); + const btScalar s=c*t; + mulPQ(a,c,s,p,q); + mulTPQ(a,c,s,p,q); + mulPQ(v,c,s,p,q); } else break; } else break; } while((++iterations)<maxiterations); - if(values) + if(values) { - *values=btVector3(a[0][0],a[1][1],a[2][2]); + *values=btVector3(a[0][0],a[1][1],a[2][2]); } - return(iterations); + return(iterations); } private: -static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) + static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) { - const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]}, - {a[q][0],a[q][1],a[q][2]}}; - int i; + const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]}, + {a[q][0],a[q][1],a[q][2]}}; + int i; - for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i]; - for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i]; + for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i]; + for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i]; } -static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) + static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) { - const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]}, - {a[0][q],a[1][q],a[2][q]}}; - int i; + const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]}, + {a[0][q],a[1][q],a[2][q]}}; + int i; - for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i]; - for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i]; + for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i]; + for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i]; } }; @@ -641,7 +639,7 @@ static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix q.setIdentity(); s.setIdentity(); } -return(i); + return(i); } // @@ -654,54 +652,54 @@ struct btSoftColliders // struct ClusterBase : btDbvt::ICollide { - btScalar erp; - btScalar idt; - btScalar margin; - btScalar friction; - btScalar threshold; - ClusterBase() + btScalar erp; + btScalar idt; + btScalar margin; + btScalar friction; + btScalar threshold; + ClusterBase() { - erp =(btScalar)1; - idt =0; - margin =0; - friction =0; - threshold =(btScalar)0; + erp =(btScalar)1; + idt =0; + margin =0; + friction =0; + threshold =(btScalar)0; } - bool SolveContact( const btGjkEpaSolver2::sResults& res, - btSoftBody::Body ba,btSoftBody::Body bb, - btSoftBody::CJoint& joint) + bool SolveContact( const btGjkEpaSolver2::sResults& res, + btSoftBody::Body ba,btSoftBody::Body bb, + btSoftBody::CJoint& joint) { - if(res.distance<margin) + if(res.distance<margin) { - const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin(); - const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin(); - const btVector3 va=ba.velocity(ra); - const btVector3 vb=bb.velocity(rb); - const btVector3 vrel=va-vb; - const btScalar rvac=dot(vrel,res.normal); - const btScalar depth=res.distance-margin; - const btVector3 iv=res.normal*rvac; - const btVector3 fv=vrel-iv; - joint.m_bodies[0] = ba; - joint.m_bodies[1] = bb; - joint.m_refs[0] = ra*ba.xform().getBasis(); - joint.m_refs[1] = rb*bb.xform().getBasis(); - joint.m_rpos[0] = ra; - joint.m_rpos[1] = rb; - joint.m_cfm = 1; - joint.m_erp = 1; - joint.m_life = 0; - joint.m_maxlife = 0; - joint.m_split = 1; - joint.m_drift = depth*res.normal; - joint.m_normal = res.normal; - joint.m_delete = false; - joint.m_friction = fv.length2()<(-rvac*friction)?1:friction; - joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0], - bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]); - return(true); + const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin(); + const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin(); + const btVector3 va=ba.velocity(ra); + const btVector3 vb=bb.velocity(rb); + const btVector3 vrel=va-vb; + const btScalar rvac=dot(vrel,res.normal); + const btScalar depth=res.distance-margin; + const btVector3 iv=res.normal*rvac; + const btVector3 fv=vrel-iv; + joint.m_bodies[0] = ba; + joint.m_bodies[1] = bb; + joint.m_refs[0] = ra*ba.xform().getBasis(); + joint.m_refs[1] = rb*bb.xform().getBasis(); + joint.m_rpos[0] = ra; + joint.m_rpos[1] = rb; + joint.m_cfm = 1; + joint.m_erp = 1; + joint.m_life = 0; + joint.m_maxlife = 0; + joint.m_split = 1; + joint.m_drift = depth*res.normal; + joint.m_normal = res.normal; + joint.m_delete = false; + joint.m_friction = fv.length2()<(-rvac*friction)?1:friction; + joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0], + bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]); + return(true); } - return(false); + return(false); } }; // @@ -709,52 +707,53 @@ struct btSoftColliders // struct CollideCL_RS : ClusterBase { - btSoftBody* psb; - btRigidBody* prb; - void Process(const btDbvtNode* leaf) + btSoftBody* psb; + + btCollisionObject* m_colObj; + void Process(const btDbvtNode* leaf) { - btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data; - btSoftClusterCollisionShape cshape(cluster); - const btConvexShape* rshape=(const btConvexShape*)prb->getCollisionShape(); - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), - rshape,prb->getInterpolationWorldTransform(), - btVector3(1,0,0),res)) + btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data; + btSoftClusterCollisionShape cshape(cluster); + const btConvexShape* rshape=(const btConvexShape*)m_colObj->getCollisionShape(); + btGjkEpaSolver2::sResults res; + if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), + rshape,m_colObj->getInterpolationWorldTransform(), + btVector3(1,0,0),res)) { - btSoftBody::CJoint joint; - if(SolveContact(res,cluster,prb,joint)) + btSoftBody::CJoint joint; + if(SolveContact(res,cluster,m_colObj,joint))//prb,joint)) { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;psb->m_joints.push_back(pj); - if(prb->isStaticOrKinematicObject()) + btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); + *pj=joint;psb->m_joints.push_back(pj); + if(m_colObj->isStaticOrKinematicObject()) { - pj->m_erp *= psb->m_cfg.kSKHR_CL; - pj->m_split *= psb->m_cfg.kSK_SPLT_CL; + pj->m_erp *= psb->m_cfg.kSKHR_CL; + pj->m_split *= psb->m_cfg.kSK_SPLT_CL; } else { - pj->m_erp *= psb->m_cfg.kSRHR_CL; - pj->m_split *= psb->m_cfg.kSR_SPLT_CL; + pj->m_erp *= psb->m_cfg.kSRHR_CL; + pj->m_split *= psb->m_cfg.kSR_SPLT_CL; } } } } - void Process(btSoftBody* ps,btRigidBody* pr) + void Process(btSoftBody* ps,btCollisionObject* colOb) { - psb = ps; - prb = pr; - idt = ps->m_sst.isdt; - margin = ps->getCollisionShape()->getMargin()+ - pr->getCollisionShape()->getMargin(); - friction = btMin(psb->m_cfg.kDF,prb->getFriction()); - btVector3 mins; - btVector3 maxs; - - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(1,1,1)*margin); - btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this); + psb = ps; + m_colObj = colOb; + idt = ps->m_sst.isdt; + margin = m_colObj->getCollisionShape()->getMargin(); + ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful. + friction = btMin(psb->m_cfg.kDF,m_colObj->getFriction()); + btVector3 mins; + btVector3 maxs; + + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; + colOb->getCollisionShape()->getAabb(colOb->getInterpolationWorldTransform(),mins,maxs); + volume=btDbvtVolume::FromMM(mins,maxs); + volume.Expand(btVector3(1,1,1)*margin); + ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this); } }; // @@ -762,36 +761,53 @@ struct btSoftColliders // struct CollideCL_SS : ClusterBase { - btSoftBody* bodies[2]; - void Process(const btDbvtNode* la,const btDbvtNode* lb) + btSoftBody* bodies[2]; + void Process(const btDbvtNode* la,const btDbvtNode* lb) { - btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; - btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; - btSoftClusterCollisionShape csa(cla); - btSoftClusterCollisionShape csb(clb); - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), - &csb,btTransform::getIdentity(), - cla->m_com-clb->m_com,res)) + btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; + btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; + + + bool connected=false; + if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size())) { - btSoftBody::CJoint joint; - if(SolveContact(res,cla,clb,joint)) + connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex]; + } + + if (!connected) + { + btSoftClusterCollisionShape csa(cla); + btSoftClusterCollisionShape csb(clb); + btGjkEpaSolver2::sResults res; + if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), + &csb,btTransform::getIdentity(), + cla->m_com-clb->m_com,res)) { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;bodies[0]->m_joints.push_back(pj); - pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); - pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; + btSoftBody::CJoint joint; + if(SolveContact(res,cla,clb,joint)) + { + btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); + *pj=joint;bodies[0]->m_joints.push_back(pj); + pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); + pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; + } } + } else + { + static int count=0; + count++; + //printf("count=%d\n",count); + } } - void Process(btSoftBody* psa,btSoftBody* psb) + void Process(btSoftBody* psa,btSoftBody* psb) { - idt = psa->m_sst.isdt; - margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; - friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); - bodies[0] = psa; - bodies[1] = psb; - btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); + idt = psa->m_sst.isdt; + margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; + friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); + bodies[0] = psa; + bodies[1] = psb; + psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); } }; // @@ -799,96 +815,99 @@ struct btSoftColliders // struct CollideSDF_RS : btDbvt::ICollide { - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; - DoNode(*node); + btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; + DoNode(*node); } - void DoNode(btSoftBody::Node& n) const + void DoNode(btSoftBody::Node& n) const { - const btScalar m=n.m_im>0?dynmargin:stamargin; - btSoftBody::RContact c; - if( (!n.m_battach)&& - psb->checkContact(prb,n.m_x,m,c.m_cti)) + const btScalar m=n.m_im>0?dynmargin:stamargin; + btSoftBody::RContact c; + if( (!n.m_battach)&& + psb->checkContact(m_colObj1,n.m_x,m,c.m_cti)) { - const btScalar ima=n.m_im; - const btScalar imb=prb->getInvMass(); - const btScalar ms=ima+imb; - if(ms>0) + const btScalar ima=n.m_im; + const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f; + const btScalar ms=ima+imb; + if(ms>0) { - const btTransform& wtr=prb->getInterpolationWorldTransform(); - const btMatrix3x3& iwi=prb->getInvInertiaTensorWorld(); - const btVector3 ra=n.m_x-wtr.getOrigin(); - const btVector3 va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt; - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=vb-va; - const btScalar dn=dot(vr,c.m_cti.m_normal); - const btVector3 fv=vr-c.m_cti.m_normal*dn; - const btScalar fc=psb->m_cfg.kDF*prb->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); - c.m_c1 = ra; - c.m_c2 = ima*psb->m_sst.sdt; - c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; - c.m_c4 = prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; - psb->m_rcontacts.push_back(c); - prb->activate(); + const btTransform& wtr=m_rigidBody?m_rigidBody->getInterpolationWorldTransform() : m_colObj1->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0); + const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra=n.m_x-wtr.getOrigin(); + const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0); + const btVector3 vb=n.m_x-n.m_q; + const btVector3 vr=vb-va; + const btScalar dn=dot(vr,c.m_cti.m_normal); + const btVector3 fv=vr-c.m_cti.m_normal*dn; + const btScalar fc=psb->m_cfg.kDF*m_colObj1->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); + c.m_c1 = ra; + c.m_c2 = ima*psb->m_sst.sdt; + c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; + c.m_c4 = m_colObj1->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; + psb->m_rcontacts.push_back(c); + if (m_rigidBody) + m_rigidBody->activate(); } } } - btSoftBody* psb; - btRigidBody* prb; - btScalar dynmargin; - btScalar stamargin; + btSoftBody* psb; + btCollisionObject* m_colObj1; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; }; // // CollideVF_SS // struct CollideVF_SS : btDbvt::ICollide { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) { - btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; - btSoftBody::Face* face=(btSoftBody::Face*)lface->data; - btVector3 o=node->m_x; - btVector3 p; - btScalar d=SIMD_INFINITY; - ProjectOrigin( face->m_n[0]->m_x-o, - face->m_n[1]->m_x-o, - face->m_n[2]->m_x-o, - p,d); - const btScalar m=mrg+(o-node->m_q).length()*2; - if(d<(m*m)) + btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; + btSoftBody::Face* face=(btSoftBody::Face*)lface->data; + btVector3 o=node->m_x; + btVector3 p; + btScalar d=SIMD_INFINITY; + ProjectOrigin( face->m_n[0]->m_x-o, + face->m_n[1]->m_x-o, + face->m_n[2]->m_x-o, + p,d); + const btScalar m=mrg+(o-node->m_q).length()*2; + if(d<(m*m)) { - const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; - const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); - const btScalar ma=node->m_im; - btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); - if( (n[0]->m_im<=0)|| - (n[1]->m_im<=0)|| - (n[2]->m_im<=0)) + const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; + const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); + const btScalar ma=node->m_im; + btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); + if( (n[0]->m_im<=0)|| + (n[1]->m_im<=0)|| + (n[2]->m_im<=0)) { - mb=0; + mb=0; } - const btScalar ms=ma+mb; - if(ms>0) + const btScalar ms=ma+mb; + if(ms>0) { - btSoftBody::SContact c; - c.m_normal = p/-btSqrt(d); - c.m_margin = m; - c.m_node = node; - c.m_face = face; - c.m_weights = w; - c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); - c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; - c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; - psb[0]->m_scontacts.push_back(c); + btSoftBody::SContact c; + c.m_normal = p/-btSqrt(d); + c.m_margin = m; + c.m_node = node; + c.m_face = face; + c.m_weights = w; + c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); + c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; + c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; + psb[0]->m_scontacts.push_back(c); } } } - btSoftBody* psb[2]; - btScalar mrg; + btSoftBody* psb[2]; + btScalar mrg; }; }; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp index 53ac2782583..f5a67f6d895 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp @@ -29,10 +29,10 @@ btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfigura mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16); m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc; - + mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; - + mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; m_swappedSoftRigidConvexCreateFunc->m_swapped=true; @@ -40,26 +40,27 @@ btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfigura #ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc; - + mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc; m_swappedSoftRigidConcaveCreateFunc->m_swapped=true; #endif //replace pool by a new one, with potential larger size - + if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool) { int curElemSize = m_collisionAlgorithmPool->getElementSize(); ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool - - + + int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm); int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm); int maxSize2 = sizeof(btSoftBodyConcaveCollisionAlgorithm); int collisionAlgorithmMaxElementSize = btMax(maxSize0,maxSize1); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); + if (collisionAlgorithmMaxElementSize > curElemSize) { m_collisionAlgorithmPool->~btPoolAllocator(); @@ -69,9 +70,6 @@ btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfigura } } - - - } btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfiguration() @@ -93,7 +91,7 @@ btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfigur btAlignedFree( m_swappedSoftRigidConcaveCreateFunc); #endif } - + ///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) { diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h index 41c3af96939..21addcfe2e1 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h @@ -32,7 +32,7 @@ class btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfi btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConvexCreateFunc; btCollisionAlgorithmCreateFunc* m_softRigidConcaveCreateFunc; btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConcaveCreateFunc; - + public: btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp index e5feb5ef749..11ad9e7daba 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp @@ -35,13 +35,13 @@ m_isSwapped(isSwapped) btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm() { - + //m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject); /*if (m_ownManifold) { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); } */ diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h index 74327e6c635..adc3844e363 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h @@ -28,15 +28,15 @@ class btSoftBody; /// btSoftRigidCollisionAlgorithm provides collision detection between btSoftBody and btRigidBody class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm { -// bool m_ownManifold; -// btPersistentManifold* m_manifoldPtr; + // bool m_ownManifold; + // btPersistentManifold* m_manifoldPtr; btSoftBody* m_softBody; btCollisionObject* m_rigidCollisionObject; ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean bool m_isSwapped; - + public: btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); @@ -52,7 +52,7 @@ public: //we don't add any manifolds } - + struct CreateFunc :public btCollisionAlgorithmCreateFunc { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index b363a2efbc1..a0069b95145 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -28,17 +28,17 @@ subject to the following restrictions: btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) { -m_drawFlags = fDrawFlags::Std; -m_drawNodeTree = true; -m_drawFaceTree = false; -m_drawClusterTree = false; -m_sbi.m_broadphase = pairCache; -m_sbi.m_dispatcher = dispatcher; -m_sbi.m_sparsesdf.Initialize(); -m_sbi.m_sparsesdf.Reset(); + m_drawFlags = fDrawFlags::Std; + m_drawNodeTree = true; + m_drawFaceTree = false; + m_drawClusterTree = false; + m_sbi.m_broadphase = pairCache; + m_sbi.m_dispatcher = dispatcher; + m_sbi.m_sparsesdf.Initialize(); + m_sbi.m_sparsesdf.Reset(); } - + btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() { @@ -55,7 +55,7 @@ void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) psb->predictMotion(timeStep); } } - + void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) { btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); @@ -63,6 +63,13 @@ void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) ///solve soft bodies constraints solveSoftBodiesConstraints(); + //self collisions + for ( int i=0;i<m_softBodies.size();i++) + { + btSoftBody* psb=(btSoftBody*)m_softBodies[i]; + psb->defaultCollisionHandler(psb); + } + ///update soft bodies updateSoftBodies(); @@ -71,7 +78,7 @@ void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) void btSoftRigidDynamicsWorld::updateSoftBodies() { BT_PROFILE("updateSoftBodies"); - + for ( int i=0;i<m_softBodies.size();i++) { btSoftBody* psb=(btSoftBody*)m_softBodies[i]; @@ -82,12 +89,12 @@ void btSoftRigidDynamicsWorld::updateSoftBodies() void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints() { BT_PROFILE("solveSoftConstraints"); - + if(m_softBodies.size()) - { + { btSoftBody::solveClusters(m_softBodies); - } - + } + for(int i=0;i<m_softBodies.size();++i) { btSoftBody* psb=(btSoftBody*)m_softBodies[i]; @@ -100,8 +107,8 @@ void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body) m_softBodies.push_back(body); btCollisionWorld::addCollisionObject(body, - btBroadphaseProxy::DefaultFilter, - btBroadphaseProxy::AllFilter); + btBroadphaseProxy::DefaultFilter, + btBroadphaseProxy::AllFilter); } diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h index aa8795d897b..edb848e2481 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h @@ -23,7 +23,7 @@ typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld { - + btSoftBodyArray m_softBodies; int m_drawFlags; bool m_drawNodeTree; @@ -32,9 +32,9 @@ class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld btSoftBodyWorldInfo m_sbi; protected: - + virtual void predictUnconstraintMotion(btScalar timeStep); - + virtual void internalSingleStepSimulation( btScalar timeStep); void updateSoftBodies(); @@ -43,17 +43,17 @@ protected: public: - + btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); virtual ~btSoftRigidDynamicsWorld(); - + virtual void debugDrawWorld(); - + void addSoftBody(btSoftBody* body); void removeSoftBody(btSoftBody* body); - + int getDrawFlags() const { return(m_drawFlags); } void setDrawFlags(int f) { m_drawFlags=f; } @@ -66,7 +66,7 @@ public: return m_sbi; } - + btSoftBodyArray& getSoftBodyArray() { return m_softBodies; @@ -76,7 +76,7 @@ public: { return m_softBodies; } - + }; #endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h index 7ca9c3415c9..1b34e0af60f 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h @@ -33,7 +33,7 @@ class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm btSoftBody* m_softBody0; btSoftBody* m_softBody1; - + public: btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btCollisionAlgorithm(ci) {} diff --git a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h index eafe74be1ae..cc4266732ae 100644 --- a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h +++ b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h @@ -23,139 +23,139 @@ subject to the following restrictions: // Modified Paul Hsieh hash template <const int DWORDLEN> unsigned int HsiehHash(const void* pdata) - { +{ const unsigned short* data=(const unsigned short*)pdata; unsigned hash=DWORDLEN<<2,tmp; for(int i=0;i<DWORDLEN;++i) - { + { hash += data[0]; tmp = (data[1]<<11)^hash; hash = (hash<<16)^tmp; data += 2; hash += hash>>11; - } + } hash^=hash<<3;hash+=hash>>5; hash^=hash<<4;hash+=hash>>17; hash^=hash<<25;hash+=hash>>6; return(hash); - } +} template <const int CELLSIZE> struct btSparseSdf - { +{ // // Inner types // struct IntFrac - { + { int b; int i; btScalar f; - }; + }; struct Cell - { + { btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; int c[3]; int puid; unsigned hash; btCollisionShape* pclient; Cell* next; - }; + }; // // Fields // - + btAlignedObjectArray<Cell*> cells; btScalar voxelsz; int puid; int ncells; int nprobes; int nqueries; - + // // Methods // - + // void Initialize(int hashsize=2383) - { + { cells.resize(hashsize,0); Reset(); - } + } // void Reset() - { + { for(int i=0,ni=cells.size();i<ni;++i) - { + { Cell* pc=cells[i]; cells[i]=0; while(pc) - { + { Cell* pn=pc->next; delete pc; pc=pn; - } } + } voxelsz =0.25; puid =0; ncells =0; nprobes =1; nqueries =1; - } + } // void GarbageCollect(int lifetime=256) - { + { const int life=puid-lifetime; for(int i=0;i<cells.size();++i) - { + { Cell*& root=cells[i]; Cell* pp=0; Cell* pc=root; while(pc) - { + { Cell* pn=pc->next; if(pc->puid<life) - { + { if(pp) pp->next=pn; else root=pn; delete pc;pc=pp;--ncells; - } - pp=pc;pc=pn; } + pp=pc;pc=pn; } + } //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries); nqueries=1; nprobes=1; - ++puid; /* TODO: Reset puid's when int range limit is reached */ - /* else setup a priority list... */ - } + ++puid; ///@todo: Reset puid's when int range limit is reached */ + /* else setup a priority list... */ + } // int RemoveReferences(btCollisionShape* pcs) - { + { int refcount=0; for(int i=0;i<cells.size();++i) - { + { Cell*& root=cells[i]; Cell* pp=0; Cell* pc=root; while(pc) - { + { Cell* pn=pc->next; if(pc->pclient==pcs) - { + { if(pp) pp->next=pn; else root=pn; delete pc;pc=pp;++refcount; - } - pp=pc;pc=pn; } + pp=pc;pc=pn; } - return(refcount); } + return(refcount); + } // btScalar Evaluate( const btVector3& x, - btCollisionShape* shape, - btVector3& normal, - btScalar margin) - { + btCollisionShape* shape, + btVector3& normal, + btScalar margin) + { /* Lookup cell */ const btVector3 scx=x/voxelsz; const IntFrac ix=Decompose(scx.x()); @@ -166,19 +166,19 @@ struct btSparseSdf Cell* c=root; ++nqueries; while(c) - { + { ++nprobes; if( (c->hash==h) && (c->c[0]==ix.b) && (c->c[1]==iy.b) && (c->c[2]==iz.b) && (c->pclient==shape)) - { break; } - else - { c=c->next; } - } + { break; } + else + { c=c->next; } + } if(!c) - { + { ++nprobes; ++ncells; c=new Cell(); @@ -187,82 +187,82 @@ struct btSparseSdf c->hash=h; c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b; BuildCell(*c); - } + } c->puid=puid; /* Extract infos */ const int o[]={ ix.i,iy.i,iz.i}; const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0], - c->d[o[0]+1][o[1]+0][o[2]+0], - c->d[o[0]+1][o[1]+1][o[2]+0], - c->d[o[0]+0][o[1]+1][o[2]+0], - c->d[o[0]+0][o[1]+0][o[2]+1], - c->d[o[0]+1][o[1]+0][o[2]+1], - c->d[o[0]+1][o[1]+1][o[2]+1], - c->d[o[0]+0][o[1]+1][o[2]+1]}; + c->d[o[0]+1][o[1]+0][o[2]+0], + c->d[o[0]+1][o[1]+1][o[2]+0], + c->d[o[0]+0][o[1]+1][o[2]+0], + c->d[o[0]+0][o[1]+0][o[2]+1], + c->d[o[0]+1][o[1]+0][o[2]+1], + c->d[o[0]+1][o[1]+1][o[2]+1], + c->d[o[0]+0][o[1]+1][o[2]+1]}; /* Normal */ - #if 1 +#if 1 const btScalar gx[]={ d[1]-d[0],d[2]-d[3], - d[5]-d[4],d[6]-d[7]}; + d[5]-d[4],d[6]-d[7]}; const btScalar gy[]={ d[3]-d[0],d[2]-d[1], - d[7]-d[4],d[6]-d[5]}; + d[7]-d[4],d[6]-d[5]}; const btScalar gz[]={ d[4]-d[0],d[5]-d[1], - d[7]-d[3],d[6]-d[2]}; + d[7]-d[3],d[6]-d[2]}; normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f), - Lerp(gx[2],gx[3],iy.f),iz.f)); + Lerp(gx[2],gx[3],iy.f),iz.f)); normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f), - Lerp(gy[2],gy[3],ix.f),iz.f)); + Lerp(gy[2],gy[3],ix.f),iz.f)); normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f), - Lerp(gz[2],gz[3],ix.f),iy.f)); + Lerp(gz[2],gz[3],ix.f),iy.f)); normal = normal.normalized(); - #else +#else normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized(); - #endif +#endif /* Distance */ const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f), - Lerp(d[3],d[2],ix.f),iy.f); + Lerp(d[3],d[2],ix.f),iy.f); const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f), - Lerp(d[7],d[6],ix.f),iy.f); + Lerp(d[7],d[6],ix.f),iy.f); return(Lerp(d0,d1,iz.f)-margin); - } + } // void BuildCell(Cell& c) - { + { const btVector3 org=btVector3( (btScalar)c.c[0], - (btScalar)c.c[1], - (btScalar)c.c[2]) * - CELLSIZE*voxelsz; + (btScalar)c.c[1], + (btScalar)c.c[2]) * + CELLSIZE*voxelsz; for(int k=0;k<=CELLSIZE;++k) - { + { const btScalar z=voxelsz*k+org.z(); for(int j=0;j<=CELLSIZE;++j) - { + { const btScalar y=voxelsz*j+org.y(); for(int i=0;i<=CELLSIZE;++i) - { + { const btScalar x=voxelsz*i+org.x(); c.d[i][j][k]=DistanceToShape( btVector3(x,y,z), - c.pclient); - } + c.pclient); } } } + } // static inline btScalar DistanceToShape(const btVector3& x, - btCollisionShape* shape) - { + btCollisionShape* shape) + { btTransform unit; unit.setIdentity(); if(shape->isConvex()) - { + { btGjkEpaSolver2::sResults res; btConvexShape* csh=static_cast<btConvexShape*>(shape); return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res)); - } - return(0); } + return(0); + } // static inline IntFrac Decompose(btScalar x) - { + { /* That one need a lot of improvements... */ /* Remove test, faster floor... */ IntFrac r; @@ -272,18 +272,18 @@ struct btSparseSdf const btScalar k=(x-r.b)*CELLSIZE; r.i=(int)k;r.f=k-r.i;r.b-=o; return(r); - } + } // static inline btScalar Lerp(btScalar a,btScalar b,btScalar t) - { + { return(a+(b-a)*t); - } + } + - // static inline unsigned int Hash(int x,int y,int z,btCollisionShape* shape) - { + { struct btS { int x,y,z; @@ -291,16 +291,16 @@ struct btSparseSdf }; btS myset; - + myset.x=x;myset.y=y;myset.z=z;myset.p=shape; const void* ptr = &myset; unsigned int result = HsiehHash<sizeof(btS)/4> (ptr); - + return result; - } + } }; - + #endif |