diff options
author | Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com> | 2019-08-06 10:52:19 -0700 |
---|---|---|
committer | Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com> | 2019-08-06 10:52:19 -0700 |
commit | e5231b5cc57d617a38bb0144e5a34661b54a8811 (patch) | |
tree | f094280eeda43d9073dc99a1b34472220b52164b | |
parent | 6a599bde87e53c22fb71cb6d35c718e9ea8083dc (diff) | |
download | bullet3-e5231b5cc57d617a38bb0144e5a34661b54a8811.tar.gz |
restore behaviors of btSoftBody
-rw-r--r-- | examples/DeformableContact/DeformableContact.cpp | 1 | ||||
-rw-r--r-- | examples/DeformableDemo/DeformableDemo.cpp | 1 | ||||
-rw-r--r-- | examples/Pinch/Pinch.cpp | 1 | ||||
-rw-r--r-- | examples/VolumetricDeformable/VolumetricDeformable.cpp | 1 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableBackwardEulerObjective.h | 1 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableBodySolver.cpp | 3 | ||||
-rw-r--r-- | src/BulletSoftBody/btSoftBody.cpp | 262 | ||||
-rw-r--r-- | src/BulletSoftBody/btSoftBody.h | 4 | ||||
-rw-r--r-- | src/BulletSoftBody/btSoftBodyInternals.h | 60 |
9 files changed, 221 insertions, 113 deletions
diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp index c40564867..102ad40a9 100644 --- a/examples/DeformableContact/DeformableContact.cpp +++ b/examples/DeformableContact/DeformableContact.cpp @@ -228,6 +228,7 @@ void DeformableContact::initPhysics() psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = .1; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp index 2411670cf..a3cdbf847 100644 --- a/examples/DeformableDemo/DeformableDemo.cpp +++ b/examples/DeformableDemo/DeformableDemo.cpp @@ -235,6 +235,7 @@ void DeformableDemo::initPhysics() psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 1; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); diff --git a/examples/Pinch/Pinch.cpp b/examples/Pinch/Pinch.cpp index 1e4afb748..c6e26ddb1 100644 --- a/examples/Pinch/Pinch.cpp +++ b/examples/Pinch/Pinch.cpp @@ -342,6 +342,7 @@ void Pinch::initPhysics() psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 2; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); diff --git a/examples/VolumetricDeformable/VolumetricDeformable.cpp b/examples/VolumetricDeformable/VolumetricDeformable.cpp index 15fb712f6..b30e4ac4d 100644 --- a/examples/VolumetricDeformable/VolumetricDeformable.cpp +++ b/examples/VolumetricDeformable/VolumetricDeformable.cpp @@ -241,6 +241,7 @@ void VolumetricDeformable::initPhysics() psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 0.5; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 82af57f6d..30c2d995e 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -26,7 +26,6 @@ class btDeformableRigidDynamicsWorld; class btDeformableBackwardEulerObjective { public: -// using TVStack = btAlignedObjectArray<btVector3>; typedef btAlignedObjectArray<btVector3> TVStack; btScalar m_dt; btDeformableRigidDynamicsWorld* m_world; diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 242b0a882..fc792bd7f 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -75,7 +75,6 @@ void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world) void btDeformableBodySolver::updateVelocity() { - // serial implementation int counter = 0; for (int i = 0; i < m_softBodySet.size(); ++i) { @@ -90,7 +89,6 @@ void btDeformableBodySolver::updateVelocity() void btDeformableBodySolver::backupVelocity() { - // serial implementation int counter = 0; for (int i = 0; i < m_softBodySet.size(); ++i) { @@ -104,7 +102,6 @@ void btDeformableBodySolver::backupVelocity() void btDeformableBodySolver::revertVelocity() { - // serial implementation int counter = 0; for (int i = 0; i < m_softBodySet.size(); ++i) { diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 8a62d31bb..86a48bf62 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -1758,115 +1758,115 @@ void btSoftBody::setSolver(eSolverPresets::_ preset) } } -// void btSoftBody::predictMotion(btScalar dt) { - int i, ni; - - /* Update */ - if (m_bUpdateRtCst) - { - m_bUpdateRtCst = false; - updateConstants(); - m_fdbvt.clear(); - if (m_cfg.collisions & fCollision::VF_SS) - { - initializeFaceTree(); - } - } - - /* Prepare */ - m_sst.sdt = dt * m_cfg.timescale; - m_sst.isdt = 1 / m_sst.sdt; - m_sst.velmrg = m_sst.sdt * 3; - m_sst.radmrg = getCollisionShape()->getMargin(); - m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; - /* Forces */ -// addVelocity(m_worldInfo->m_gravity * m_sst.sdt); - applyForces(); - /* Integrate */ - for (i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - n.m_q = n.m_x; - btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; - { - btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement / m_sst.sdt; - for (int c = 0; c < 3; c++) - { - if (deltaV[c] > clampDeltaV) - { - deltaV[c] = clampDeltaV; - } - if (deltaV[c] < -clampDeltaV) - { - deltaV[c] = -clampDeltaV; - } - } - } + int i, ni; + + /* Update */ + if (m_bUpdateRtCst) + { + m_bUpdateRtCst = false; + updateConstants(); + m_fdbvt.clear(); + if (m_cfg.collisions & fCollision::VF_SS) + { + initializeFaceTree(); + } + } + + /* Prepare */ + m_sst.sdt = dt * m_cfg.timescale; + m_sst.isdt = 1 / m_sst.sdt; + m_sst.velmrg = m_sst.sdt * 3; + m_sst.radmrg = getCollisionShape()->getMargin(); + m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; + /* Forces */ + addVelocity(m_worldInfo->m_gravity * m_sst.sdt); + applyForces(); + /* Integrate */ + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + n.m_q = n.m_x; + btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; + { + btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / m_sst.sdt; + for (int c = 0; c < 3; c++) + { + if (deltaV[c] > clampDeltaV) + { + deltaV[c] = clampDeltaV; + } + if (deltaV[c] < -clampDeltaV) + { + deltaV[c] = -clampDeltaV; + } + } + } n.m_v += deltaV; n.m_x += n.m_v * m_sst.sdt; - n.m_f = btVector3(0, 0, 0); - } - /* Clusters */ - updateClusters(); - /* 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, - 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; - vol = VolumeOf(f, m_sst.radmrg); - m_fdbvt.update(f.m_leaf, - 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); - /* Optimize dbvt's */ - m_ndbvt.optimizeIncremental(1); - m_fdbvt.optimizeIncremental(1); - m_cdbvt.optimizeIncremental(1); + n.m_f = btVector3(0, 0, 0); + } + /* Clusters */ + updateClusters(); + /* 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, + 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; + vol = VolumeOf(f, m_sst.radmrg); + m_fdbvt.update(f.m_leaf, + 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); + /* Optimize dbvt's */ + m_ndbvt.optimizeIncremental(1); + m_fdbvt.optimizeIncremental(1); + m_cdbvt.optimizeIncremental(1); } + // void btSoftBody::solveConstraints() { @@ -2262,8 +2262,35 @@ btVector3 btSoftBody::evaluateCom() const return (com); } -// bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti) const +{ + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); + //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); + const btTransform& wtr = colObjWrap->getWorldTransform(); + //todo: check which transform is needed here + + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(x), + shp, + nrm, + margin); + if (dst < 0) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); + return (true); + } + return (false); +} +// +bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict) const @@ -3262,6 +3289,33 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap collider.ProcessColObj(this, pcoWrap); } break; + case fCollision::SDF_RD: + { + btSoftColliders::CollideSDF_RD docollide; + btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); + btTransform wtr = pcoWrap->getWorldTransform(); + + const btTransform ctr = pcoWrap->getWorldTransform(); + const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length(); + const btScalar basemargin = getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(), + mins, + maxs); + volume = btDbvtVolume::FromMM(mins, maxs); + volume.Expand(btVector3(basemargin, basemargin, basemargin)); + docollide.psb = this; + docollide.m_colObj1Wrap = pcoWrap; + docollide.m_rigidBody = prb1; + + docollide.dynmargin = basemargin + timemargin; + docollide.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide); + } + break; } } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 87633a83c..f2934c94a 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -160,6 +160,7 @@ public: RVSmask = 0x000f, ///Rigid versus soft mask SDF_RS = 0x0001, ///SDF based rigid vs soft CL_RS = 0x0002, ///Cluster vs convex rigid vs soft + SDF_RD = 0x0003, ///DF based rigid vs deformable SVSmask = 0x0030, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling @@ -1006,7 +1007,8 @@ public: btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const; void initializeFaceTree(); btVector3 evaluateCom() const; - bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; + bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; + bool checkContact(const btCollisionObjectWrapper* colObjWrap, 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 eb487a56f..80545ebad 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -928,10 +928,62 @@ struct btSoftColliders psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this); } }; + // + // CollideSDF_RS + // + struct CollideSDF_RS : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Node* node = (btSoftBody::Node*)leaf->data; + DoNode(*node); + } + void DoNode(btSoftBody::Node& n) const + { + const btScalar m = n.m_im > 0 ? dynmargin : stamargin; + btSoftBody::RContact c; + + if ((!n.m_battach) && + psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) + { + 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 = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->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 = btDot(vr, c.m_cti.m_normal); + const btVector3 fv = vr - c.m_cti.m_normal * dn; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->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() < (dn * fc * dn * fc) ? 0 : 1 - fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + psb->m_rcontacts.push_back(c); + if (m_rigidBody) + m_rigidBody->activate(); + } + } + } + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; + // - // CollideSDF_RS + // CollideSDF_RD // - struct CollideSDF_RS : btDbvt::ICollide + struct CollideSDF_RD : btDbvt::ICollide { void Process(const btDbvtNode* leaf) { @@ -946,7 +998,7 @@ struct btSoftColliders if (!n.m_battach) { // check for collision at x_{n+1}^* - if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true)) + if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true)) { const btScalar ima = n.m_im; const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; @@ -954,7 +1006,7 @@ struct btSoftColliders if (ms > 0) { // resolve contact at x_n - psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false); + psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false); btSoftBody::sCti& cti = c.m_cti; c.m_node = &n; const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); |