diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 1af3a19bb..ae7119761 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -344,6 +344,8 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, if (numSimulationSubSteps) { + findActiveObjects(); + saveKinematicState(fixedTimeStep); applyGravity(); diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 7d2a03712..435b0cdb7 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -1728,21 +1728,22 @@ btVector3 btSoftBody::evaluateCom() const } // -bool btSoftBody::checkContact( btRigidBody* prb, +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); 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); @@ -2466,7 +2467,9 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti) { const RContact& c=psb->m_rcontacts[i]; 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); @@ -2476,7 +2479,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); } } } @@ -2577,9 +2581,10 @@ void btSoftBody::defaultCollisionHandler(btCollisionObject* pco) case fCollision::SDF_RS: { btSoftColliders::CollideSDF_RS docollide; - btRigidBody* prb=btRigidBody::upcast(pco); - const btTransform wtr=prb->getInterpolationWorldTransform(); - const btTransform ctr=prb->getWorldTransform(); + 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; @@ -2591,7 +2596,9 @@ void btSoftBody::defaultCollisionHandler(btCollisionObject* pco) volume=btDbvtVolume::FromMM(mins,maxs); volume.Expand(btVector3(basemargin,basemargin,basemargin)); docollide.psb = this; - docollide.prb = prb; + docollide.m_colObj1 = pco; + docollide.m_rigidBody = prb1; + docollide.dynmargin = basemargin+timemargin; docollide.stamargin = basemargin; m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide); diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 4521f6bcb..7954a195c 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -153,7 +153,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 */ }; @@ -813,7 +813,7 @@ public: 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(); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index d14128e94..3ea095379 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -808,35 +808,38 @@ struct btSoftColliders 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)) + psb->checkContact(m_colObj1,n.m_x,m,c.m_cti)) { const btScalar ima=n.m_im; - const btScalar imb=prb->getInvMass(); + 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 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=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt; + 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*prb->getFriction(); + 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 = prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; + c.m_c4 = m_colObj1->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; psb->m_rcontacts.push_back(c); - prb->activate(); + if (m_rigidBody) + m_rigidBody->activate(); } } } btSoftBody* psb; - btRigidBody* prb; + btCollisionObject* m_colObj1; + btRigidBody* m_rigidBody; btScalar dynmargin; btScalar stamargin; };