diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index 856b5d411..7d53f8624 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -1049,7 +1049,7 @@ btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, const btScalar length = delta.length(); results.normal = delta / length; results.witnesses[0] += results.normal * margin; - results.distance = length - margin; + results.distance = length - margin; return results.distance; } else diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 139d69c7d..4916848fe 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -282,7 +282,7 @@ void btDeformableBodySolver::updateVelocity() } else { - psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv; + psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv; } psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2()); ++counter; diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index 3b559bbf7..1e2c9f5bc 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -216,7 +216,7 @@ public: class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint { public: - btSoftBody::Face* m_face; + btSoftBody::Face* m_face; bool m_useStrainLimiting; btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting); btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 733e648c4..7f67260ce 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -452,8 +452,8 @@ void btDeformableContactProjection::checkConstraints(const TVStack& x) d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]); } } -// printf("d = %f, %f, %f\n", d[0], d[1], d[2]); -// printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]); + // printf("d = %f, %f, %f\n", d[0], d[1], d[2]); + // printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]); } } @@ -494,7 +494,7 @@ void btDeformableContactProjection::setLagrangeMultiplier() lm.m_dirs[2] = btVector3(0, 0, 1); m_lagrangeMultipliers.push_back(lm); } - + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) { if (!m_nodeRigidConstraints[i][j].m_binding) @@ -520,20 +520,20 @@ void btDeformableContactProjection::setLagrangeMultiplier() lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal; } m_lagrangeMultipliers.push_back(lm); - } - + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) { if (!m_faceRigidConstraints[i][j].m_binding) { continue; } - btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; LagrangeMultiplier lm; lm.m_num_nodes = 3; - + for (int k = 0; k < 3; ++k) { face->m_n[k]->m_constrained = true; @@ -542,7 +542,7 @@ void btDeformableContactProjection::setLagrangeMultiplier() } if (m_faceRigidConstraints[i][j].m_static) { - face->m_pcontact[3] = 1; + face->m_pcontact[3] = 1; lm.m_num_constraints = 3; lm.m_dirs[0] = btVector3(1, 0, 0); lm.m_dirs[1] = btVector3(0, 1, 0); @@ -550,7 +550,7 @@ void btDeformableContactProjection::setLagrangeMultiplier() } else { - face->m_pcontact[3] = 0; + face->m_pcontact[3] = 0; lm.m_num_constraints = 1; lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal; } diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 257483d64..dbde78944 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -401,7 +401,6 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) // At this point, dv should be golden for nodes in contact // proceed to solve deformable momentum equation - m_deformableBodySolver->solveDeformableConstraints(timeStep); } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index cca065afd..f71545215 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2818,7 +2818,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) : colObjWrap->getWorldTransform(); btScalar dst; - btGjkEpaSolver2::sResults results; + btGjkEpaSolver2::sResults results; // #define USE_QUADRATURE 1 #define CACHE_PREV_COLLISION @@ -2860,7 +2860,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO } #endif - // collision detection using x* + // collision detection using x* btTransform triangle_transform; triangle_transform.setIdentity(); triangle_transform.setOrigin(f.m_n[0]->m_q); @@ -2872,42 +2872,42 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO if (dst >= 0) return false; - // Use consistent barycenter to recalculate distance. - #ifdef CACHE_PREV_COLLISION - if (f.m_pcontact[3] != 0) - { - for (int i = 0; i < 3; ++i) - bary[i] = f.m_pcontact[i]; - contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - const btConvexShape* csh = static_cast(shp); - btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr,results); - cti.m_colObj = colObjWrap->getCollisionObject(); - dst = results.distance; - cti.m_normal = results.normal; - cti.m_offset = dst; - - //point-convex CD - wtr = colObjWrap->getWorldTransform(); - btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); - triangle_transform.setOrigin(f.m_n[0]->m_x); - btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); - - dst = results.distance - csh->getMargin() - margin; - return true; - } - #endif - - // Use triangle-convex CD. +// Use consistent barycenter to recalculate distance. +#ifdef CACHE_PREV_COLLISION + if (f.m_pcontact[3] != 0) + { + for (int i = 0; i < 3; ++i) + bary[i] = f.m_pcontact[i]; + contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + const btConvexShape* csh = static_cast(shp); + btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results); + cti.m_colObj = colObjWrap->getCollisionObject(); + dst = results.distance; + cti.m_normal = results.normal; + cti.m_offset = dst; + + //point-convex CD + wtr = colObjWrap->getWorldTransform(); + btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); + + dst = results.distance - csh->getMargin() - margin; + return true; + } +#endif + + // Use triangle-convex CD. wtr = colObjWrap->getWorldTransform(); btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); triangle_transform.setOrigin(f.m_n[0]->m_x); btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); contact_point = results.witnesses[0]; getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - - for (int i = 0; i < 3; ++i) - f.m_pcontact[i] = bary[i]; - + + for (int i = 0; i < 3; ++i) + f.m_pcontact[i] = bary[i]; + dst = results.distance - csh->getMargin() - margin; cti.m_colObj = colObjWrap->getCollisionObject(); cti.m_normal = results.normal; diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 62ec76769..c17bbb5cd 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1686,7 +1686,7 @@ struct btSoftColliders c.m_c2 = ima; c.m_c3 = fc; c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; - c.m_c5 = n.m_effectiveMass_inv; + c.m_c5 = n.m_effectiveMass_inv; if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { @@ -1838,9 +1838,9 @@ struct btSoftColliders psb->m_faceRigidContacts.push_back(c); } } - // Set caching barycenters to be false after collision detection. - // Only turn on when contact is static. - f.m_pcontact[3] = 0; + // Set caching barycenters to be false after collision detection. + // Only turn on when contact is static. + f.m_pcontact[3] = 0; } btSoftBody* psb; const btCollisionObjectWrapper* m_colObj1Wrap;