diff options
author | Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com> | 2019-07-15 10:48:20 -0700 |
---|---|---|
committer | Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com> | 2019-08-02 13:12:51 -0700 |
commit | bac7d461c5a9d318a04b9741cccf7f1759132bf3 (patch) | |
tree | c056906eebaae83221a31d184fc7aa804de7a927 | |
parent | ac628f4d394f39d10cd178e005b63ad7dda0e1f0 (diff) | |
download | bullet3-bac7d461c5a9d318a04b9741cccf7f1759132bf3.tar.gz |
fix bugs in poststablize and projection of colinear constraints
-rw-r--r-- | src/BulletSoftBody/btContactProjection.cpp | 31 | ||||
-rw-r--r-- | src/BulletSoftBody/btContactProjection.h | 25 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableBodySolver.cpp | 37 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableBodySolver.h | 5 | ||||
-rw-r--r-- | src/BulletSoftBody/btSoftBodyInternals.h | 60 |
5 files changed, 89 insertions, 69 deletions
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index b90c749f3..6082f4dad 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -11,6 +11,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity) { ///solve rigid body constraints + m_world->getSolverInfo().m_numIterations = 5; m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo()); // loop through constraints to set constrained values @@ -258,21 +259,21 @@ void btContactProjection::setConstraintDirections() } else { - // prune out collinear constraints - const btVector3& first_dir = c[0].m_direction; - int i = 1; - while (i < c.size()) - { - if (std::abs(std::abs(first_dir.dot(c[i].m_direction)) - 1) < 4*SIMD_EPSILON) - c.removeAtIndex(i); - else - ++i; - } - if (c.size() == 3) - { - if (std::abs(std::abs(c[1].m_direction.dot(c[2].m_direction)) - 1) < 4*SIMD_EPSILON) - c.removeAtIndex(2); - } +// // prune out collinear constraints +// const btVector3& first_dir = c[0].m_direction; +// int i = 1; +// while (i < c.size()) +// { +// if (std::abs(std::abs(first_dir.dot(c[i].m_direction)) - 1) < 4*SIMD_EPSILON) +// c.removeAtIndex(i); +// else +// ++i; +// } +// if (c.size() == 3) +// { +// if (std::abs(std::abs(c[1].m_direction.dot(c[2].m_direction)) - 1) < 4*SIMD_EPSILON) +// c.removeAtIndex(2); +// } } } } diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index 29d0e6632..8ce3cb578 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -47,8 +47,13 @@ public: { // TODO : friction btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction); - free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir; + if (free_dir.norm() < SIMD_EPSILON) + x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; + else + { + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir; + } } else x[i].setZero(); @@ -69,7 +74,8 @@ public: if (constraints.size() == 1) { x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; - x[i] += constraints[0].m_value * constraints[0].m_direction; + btVector3 diff = constraints[0].m_value * constraints[0].m_direction; + x[i] += diff; if (friction.m_direction.norm() > SIMD_EPSILON) { x[i] -= x[i].dot(friction.m_direction) * friction.m_direction; @@ -79,8 +85,17 @@ public: else if (constraints.size() == 2) { btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction); - free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir + constraints[0].m_direction * constraints[0].m_value + constraints[1].m_direction * constraints[1].m_value; + if (free_dir.norm() < SIMD_EPSILON) + { + x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; + btVector3 diff = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction; + x[i] += diff; + } + else + { + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir + constraints[0].m_direction * constraints[0].m_value + constraints[1].m_direction * constraints[1].m_value; + } } else x[i] = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction + constraints[2].m_value * constraints[2].m_direction; diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 41a5df70d..e9bcf226a 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -77,8 +77,8 @@ void btDeformableBodySolver::postStabilize() const btVector3 vr = vb - va; const btScalar dn = btDot(vr, cti.m_normal); - const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg); - + btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg); +// dp += mrg; // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient btScalar dvn = dn * c.m_c4; @@ -86,23 +86,27 @@ void btDeformableBodySolver::postStabilize() // TODO: only contact is considered here, add friction later if (dp < 0) { - c.m_node->m_x -= dp * cti.m_normal * c.m_c4; -// c.m_node->m_x -= impulse * c.m_c2; - - //// - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - if (rigidCol) - rigidCol->applyImpulse(impulse, c.m_c1); - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + bool two_way = false; + if (two_way) { - if (multibodyLinkCol) + c.m_node->m_x -= impulse * c.m_c2; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { - double multiplier = 0.5; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); + if (rigidCol) + rigidCol->applyImpulse(impulse, c.m_c1); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + if (multibodyLinkCol) + { + double multiplier = 0.5; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); + } } } + else + c.m_node->m_x -= dp * cti.m_normal * c.m_c4; } } } @@ -115,7 +119,6 @@ void btDeformableBodySolver::solveConstraints(float solverdt) bool nodeUpdated = updateNodes(); reinitialize(nodeUpdated); backupVelocity(); - postStabilize(); for (int i = 0; i < m_solveIterations; ++i) { m_objective->computeResidual(solverdt, m_residual); @@ -123,7 +126,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt) updateVelocity(); } advect(solverdt); -// postStabilize(); + postStabilize(); } void btDeformableBodySolver::reinitialize(bool nodeUpdated) diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 540a2b6cb..3fac0743d 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -2,7 +2,7 @@ // btDeformableBodySolver.h // BulletSoftBody // -// Created by Chuyuan Fu on 7/1/19. +// Created by Xuchen Han on 7/1/19. // #ifndef BT_DEFORMABLE_BODY_SOLVERS_H @@ -101,8 +101,7 @@ public: for (int j = 0; j < psb->m_nodes.size(); ++j) { auto& node = psb->m_nodes[j]; - node.m_x += dt * m_dv[counter++]; -// node.m_x = node.m_q + dt * node.m_v; + node.m_x = node.m_q + dt * node.m_v; } } } diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 12d96171f..b892498ea 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -869,35 +869,37 @@ struct btSoftColliders 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_q - 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_c3 = 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(); - } + if (!n.m_battach) + { + if (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_c3 = 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; |