diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index fef5e4600..ff5efd70f 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -237,7 +237,7 @@ public: void applyForce(const btVector3& force, const btVector3& rel_pos) { applyCentralForce(force); - applyTorque(rel_pos.cross(force)); + applyTorque(rel_pos.cross(force)*m_angularFactor); } void applyCentralImpulse(const btVector3& impulse) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 0fa347d2f..3551d5e0c 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -95,8 +95,12 @@ btSoftBody::~btSoftBody() { //for now, delete the internal shape delete m_collisionShape; - for(int i=0;im_x, @@ -433,14 +440,14 @@ void btSoftBody::setTotalMass(btScalar mass,bool fromfaces) f.m_n[j]->m_im+=twicearea; } } - for(int i=0;i0 ? @@ -528,7 +537,7 @@ void btSoftBody::setPose(bool bvolume,bool bframe) /* Pos */ const btVector3 com=evaluateCom(); m_pose.m_pos.resize(m_nodes.size()); - for(int i=0,ni=m_nodes.size();i0) { + int i,ni; + const btVector3 org=m_nodes[0].m_x; - for(int i=0,ni=m_faces.size();im_x-org,cross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org)); @@ -658,6 +669,8 @@ cluster->m_ndimpulses++; // int btSoftBody::generateBendingConstraints(int distance,Material* mat) { + int i,j; + if(distance>1) { /* Build graph */ @@ -665,16 +678,16 @@ int btSoftBody::generateBendingConstraints(int distance,Material* mat) const unsigned inf=(~(unsigned)0)>>1; unsigned* adj=new unsigned[n*n]; #define IDX(_x_,_y_) ((_y_)*n+(_x_)) - for(int j=0;jsum) @@ -697,9 +710,9 @@ int btSoftBody::generateBendingConstraints(int distance,Material* mat) } /* Build links */ int nlinks=0; - for(int j=0;jm_leaf) m_cdbvt.remove(m_clusters[i]->m_leaf); btAlignedFree(m_clusters[i]); } m_clusters.resize(btMin(k,m_nodes.size())); -for(int i=0;im_collide= true; @@ -765,8 +785,10 @@ if(k>0) do { const btScalar w=2-btMin(1,iterations/slope); changed=false; - iterations++; - for(int i=0;im_nodes.size();++j) @@ -777,12 +799,12 @@ if(k>0) { c /= (btScalar)m_clusters[i]->m_nodes.size(); c = centers[i]+(c-centers[i])*w; - changed |= (c-centers[i]).length2()>SIMD_EPSILON; + changed |= ((c-centers[i]).length2()>SIMD_EPSILON); centers[i] = c; m_clusters[i]->m_nodes.resize(0); } } - for(int i=0;i0) /* Merge */ btAlignedObjectArray cids; cids.resize(m_nodes.size(),-1); - for(int i=0;im_nodes.size();++j) { cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i; } } - for(int i=0;i0) btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]); } /* Terminate */ - for(int i=0;im_nodes.size()==0) { @@ -862,12 +884,14 @@ return(0); // void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut) { -const Node* nbase(&m_nodes[0]); -int ncount(m_nodes.size()); +const Node* nbase = &m_nodes[0]; +int ncount = m_nodes.size(); btSymMatrix edges(ncount,-2); int newnodes=0; +int i,j,k,ni; + /* Filter out */ -for(int i=0;iEval(n[0]->m_x) ranks; btAlignedObjectArray todelete; ranks.resize(nnodes,0); - for(int i=0,ni=m_links.size();i=0;--i) + for(i=nnodes-1;i>=0;--i) { if(!ranks[i]) todelete.push_back(i); } @@ -1117,6 +1141,7 @@ 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); @@ -1129,7 +1154,7 @@ 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(int i=0,ni=m_links.size();im_gravity*m_sst.sdt); applyForces(); /* Integrate */ - for(int i=0,ni=m_nodes.size();im_q-l.m_n[0]->m_q; l.m_c2 = 1/(l.m_c3.length2()*l.m_c0); } /* Prepare anchors */ -for(int i=0,ni=m_anchors.size();igetWorldTransform().getBasis()*a.m_local; @@ -1331,7 +1361,7 @@ if(m_cfg.viterations>0) } } /* Update */ - for(int i=0,ni=m_nodes.size();i0) } } const btScalar vc=m_sst.isdt*(1-m_cfg.kDP); - for(int i=0,ni=m_nodes.size();i0) if(m_cfg.diterations>0) { const btScalar vcf=m_cfg.kVCF*m_sst.isdt; - for(int i=0,ni=m_nodes.size();i& bodies { const int nb=bodies.size(); int iterations=0; -for(int i=0;im_cfg.citerations); } -for(int i=0;iprepareClusters(iterations); } -for(int i=0;isolveClusters(sor); } } -for(int i=0;icleanupClusters(); } @@ -1494,19 +1526,21 @@ void btSoftBody::pointersToIndices() { #define PTR2IDX(_p_,_b_) reinterpret_cast((_p_)-(_b_)) btSoftBody::Node* base=&m_nodes[0]; - for(int i=0,ni=m_nodes.size();idata=*(void**)&i; } } - for(int i=0,ni=m_links.size();idata=*(void**)&i; } } - for(int i=0,ni=m_anchors.size();idata=&m_nodes[i]; } } - for(int i=0,ni=m_links.size();idata=&m_faces[i]; } } - for(int i=0,ni=m_anchors.size();im_x-f.m_n[0]->m_x, @@ -1681,7 +1719,7 @@ void btSoftBody::updateNormals() f.m_n[1]->m_n+=n; f.m_n[2]->m_n+=n; } - for(int i=0,ni=m_nodes.size();im_x,f.m_n[1]->m_x,f.m_n[2]->m_x); @@ -1773,11 +1813,11 @@ void btSoftBody::updateConstants() /* Area's */ btAlignedObjectArray counts; counts.resize(m_nodes.size(),0); - for(int i=0,ni=m_nodes.size();im_area+=btFabs(f.m_ra); } } - for(int i=0,ni=m_nodes.size();i0) m_nodes[i].m_area/=(btScalar)counts[i]; @@ -1799,7 +1839,9 @@ void btSoftBody::updateConstants() // void btSoftBody::initializeClusters() { -for(int i=0;im_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]; + 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]; @@ -1837,7 +1883,7 @@ for(int i=0;im_x-c.m_com; } @@ -1848,7 +1894,9 @@ for(int i=0;im_v*c.m_masses[i]; - c.m_lv += v; - c.m_av += cross(c.m_nodes[i]->m_x-c.m_com,v); + 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] = @@ -1990,9 +2042,11 @@ btAlignedObjectArray deltas; btAlignedObjectArray weights; deltas.resize(m_nodes.size(),btVector3(0,0,0)); weights.resize(m_nodes.size(),0); +int i; + if(drift) { - for(int i=0;i0) m_nodes[i].m_x+=deltas[i]/weights[i]; + if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i]; } } // void btSoftBody::dampClusters() { -for(int i=0;i0) @@ -2227,7 +2283,9 @@ void btSoftBody::applyForces() dvolumetv = (m_pose.m_volume-volume)*kVC; } /* Per vertex forces */ - for(int i=0,ni=m_nodes.size();i0) @@ -2278,7 +2336,7 @@ void btSoftBody::applyForces() } } /* Per face forces */ - for(int i=0,ni=m_faces.size();im_nodes.size();++i) + for(i=0;im_nodes.size();++i) { const btSoftBody::Node& n=psb->m_nodes[i]; if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; @@ -178,7 +180,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, /* Links */ if(0!=(drawflags&fDrawFlags::Links)) { - for(int i=0;im_links.size();++i) + for(i=0;im_links.size();++i) { const btSoftBody::Link& l=psb->m_links[i]; if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; @@ -188,7 +190,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, /* Normals */ if(0!=(drawflags&fDrawFlags::Normals)) { - for(int i=0;im_nodes.size();++i) + for(i=0;im_nodes.size();++i) { const btSoftBody::Node& n=psb->m_nodes[i]; if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; @@ -203,7 +205,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, static const btVector3 axis[]={btVector3(1,0,0), btVector3(0,1,0), btVector3(0,0,1)}; - for(int i=0;im_rcontacts.size();++i) + for(i=0;im_rcontacts.size();++i) { const btSoftBody::RContact& c=psb->m_rcontacts[i]; const btVector3 o= c.m_node->m_x-c.m_cti.m_normal* @@ -218,7 +220,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, /* Anchors */ if(0!=(drawflags&fDrawFlags::Anchors)) { - for(int i=0;im_anchors.size();++i) + for(i=0;im_anchors.size();++i) { const btSoftBody::Anchor& a=psb->m_anchors[i]; const btVector3 q=a.m_body->getWorldTransform()*a.m_local; @@ -226,7 +228,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, drawVertex(idraw,q,0.25,btVector3(0,1,0)); idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1)); } - for(int i=0;im_nodes.size();++i) + for(i=0;im_nodes.size();++i) { const btSoftBody::Node& n=psb->m_nodes[i]; if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; @@ -242,7 +244,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, const btScalar scl=(btScalar)0.8; const btScalar alp=(btScalar)1; const btVector3 col(0,(btScalar)0.7,0); - for(int i=0;im_faces.size();++i) + for(i=0;im_faces.size();++i) { const btSoftBody::Face& f=psb->m_faces[i]; if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; @@ -258,7 +260,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, if(0!=(drawflags&fDrawFlags::Clusters)) { srand(1806); - for(int i=0;im_clusters.size();++i) + for(i=0;im_clusters.size();++i) { if(psb->m_clusters[i]->m_collide) { @@ -268,7 +270,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, color=color.normalized()*0.75; btAlignedObjectArray vertices; vertices.resize(psb->m_clusters[i]->m_nodes.size()); - for(int j=0,nj=vertices.size();jm_clusters[i]->m_nodes[j]->m_x; } @@ -281,7 +283,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, add(hres.m_OutputVertices,-center); mul(hres.m_OutputVertices,(btScalar)1); add(hres.m_OutputVertices,center); - for(int j=0;j<(int)hres.mNumFaces;++j) + 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]], @@ -311,7 +313,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, /* Notes */ if(0!=(drawflags&fDrawFlags::Notes)) { - for(int i=0;im_notes.size();++i) + for(i=0;im_notes.size();++i) { const btSoftBody::Note& n=psb->m_notes[i]; btVector3 p=n.m_offset; @@ -331,7 +333,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, /* Joints */ if(0!=(drawflags&fDrawFlags::Joints)) { - for(int i=0;im_joints.size();++i) + for(i=0;im_joints.size();++i) { const btSoftBody::Joint* pj=psb->m_joints[i]; switch(pj->Type()) @@ -451,7 +453,9 @@ btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBody::btSoftBodyWorldInfo& wor const int r=res+2; btVector3* x=new btVector3[r]; btScalar* m=new btScalar[r]; - for(int i=0;iappendLink(i-1,i); } @@ -489,7 +493,9 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& wor const int tot=rx*ry; btVector3* x=new btVector3[tot]; btScalar* m=new btScalar[tot]; - for(int iy=0;iy vtx; chks.resize(maxidx*maxidx,false); vtx.resize(maxidx); - for(int i=0,j=0,ni=maxidx*3;i