diff --git a/.ci/docker/ubuntu-bionic b/.ci/docker/ubuntu-bionic index b7643438e..9b389a42c 100644 --- a/.ci/docker/ubuntu-bionic +++ b/.ci/docker/ubuntu-bionic @@ -13,6 +13,7 @@ RUN apt-get install -y \ lsb-release \ pkg-config \ python3 \ + python3-dev \ python3-distutils \ software-properties-common \ sudo diff --git a/.ci/docker/ubuntu-xenial b/.ci/docker/ubuntu-xenial index 693e8a1fd..bce1f2b75 100644 --- a/.ci/docker/ubuntu-xenial +++ b/.ci/docker/ubuntu-xenial @@ -13,5 +13,6 @@ RUN apt-get install -y \ lsb-release \ pkg-config \ python3 \ + python3-dev \ software-properties-common \ sudo diff --git a/.ci/script.sh b/.ci/script.sh index 7fff323bf..66b814ed6 100755 --- a/.ci/script.sh +++ b/.ci/script.sh @@ -7,9 +7,10 @@ if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then $SUDO apt-get update $SUDO apt-get install -y python3 $SUDO apt-get install -y python3-pip + $SUDO apt-get install python3-dev $SUDO pip3 install -U wheel $SUDO pip3 install -U setuptools - $SUDO python3 setup.py install + python3 setup.py install --user python3 examples/pybullet/unittests/unittests.py --verbose python3 examples/pybullet/unittests/userDataTest.py --verbose python3 examples/pybullet/unittests/saveRestoreStateTest.py --verbose diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 3fb7052ae..8dfca94d9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6594,34 +6594,37 @@ bool PhysicsServerCommandProcessor::processCollisionFilterCommand(const struct S if (clientCmd.m_updateFlags & B3_COLLISION_FILTER_GROUP_MASK) { InternalBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_collisionFilterArgs.m_bodyUniqueIdA); - btCollisionObject* colObj = 0; - if (body->m_multiBody) + if (body) { - if (clientCmd.m_collisionFilterArgs.m_linkIndexA == -1) + btCollisionObject* colObj = 0; + if (body->m_multiBody) { - colObj = body->m_multiBody->getBaseCollider(); + if (clientCmd.m_collisionFilterArgs.m_linkIndexA == -1) + { + colObj = body->m_multiBody->getBaseCollider(); + } + else + { + if (clientCmd.m_collisionFilterArgs.m_linkIndexA >= 0 && clientCmd.m_collisionFilterArgs.m_linkIndexA < body->m_multiBody->getNumLinks()) + { + colObj = body->m_multiBody->getLinkCollider(clientCmd.m_collisionFilterArgs.m_linkIndexA); + } + } } else { - if (clientCmd.m_collisionFilterArgs.m_linkIndexA >= 0 && clientCmd.m_collisionFilterArgs.m_linkIndexA < body->m_multiBody->getNumLinks()) + if (body->m_rigidBody) { - colObj = body->m_multiBody->getLinkCollider(clientCmd.m_collisionFilterArgs.m_linkIndexA); + colObj = body->m_rigidBody; } } - } - else - { - if (body->m_rigidBody) + if (colObj) { - colObj = body->m_rigidBody; + colObj->getBroadphaseHandle()->m_collisionFilterGroup = clientCmd.m_collisionFilterArgs.m_collisionFilterGroup; + colObj->getBroadphaseHandle()->m_collisionFilterMask = clientCmd.m_collisionFilterArgs.m_collisionFilterMask; + m_data->m_dynamicsWorld->refreshBroadphaseProxy(colObj); } } - if (colObj) - { - colObj->getBroadphaseHandle()->m_collisionFilterGroup = clientCmd.m_collisionFilterArgs.m_collisionFilterGroup; - colObj->getBroadphaseHandle()->m_collisionFilterMask = clientCmd.m_collisionFilterArgs.m_collisionFilterMask; - m_data->m_dynamicsWorld->refreshBroadphaseProxy(colObj); - } } } return true; @@ -8065,6 +8068,21 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) { + const btCollisionObject* colObj = (btCollisionObject*)colObj0Wrap->getCollisionObject(); + const btMultiBodyLinkCollider* mbl = btMultiBodyLinkCollider::upcast(colObj); + int bodyUniqueId = -1; + if (mbl) + { + bodyUniqueId = mbl->m_multiBody->getUserIndex2(); + } + else + { + bodyUniqueId = colObj->getUserIndex2(); + } + + + bool isSwapped = m_bodyUniqueIdA != bodyUniqueId; + if (cp.m_distance1 <= m_closestDistanceThreshold) { b3ContactPointData pt; @@ -8077,9 +8095,18 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand pt.m_linkIndexB = m_linkIndexB; for (int j = 0; j < 3; j++) { - pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; - pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; - pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + if (isSwapped) + { + pt.m_contactNormalOnBInWS[j] = -srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnB()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnA()[j]; + } + else + { + pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + } } pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime; pt.m_linearFrictionForce1 = srcPt.m_appliedImpulseLateral1 / m_deltaTime; diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index 45d181713..7d53f8624 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -1049,7 +1049,8 @@ btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, const btScalar length = delta.length(); results.normal = delta / length; results.witnesses[0] += results.normal * margin; - return (length - margin); + results.distance = length - margin; + return results.distance; } else { diff --git a/src/BulletSoftBody/btConjugateResidual.h b/src/BulletSoftBody/btConjugateResidual.h index e3bca6e12..614612036 100644 --- a/src/BulletSoftBody/btConjugateResidual.h +++ b/src/BulletSoftBody/btConjugateResidual.h @@ -30,7 +30,7 @@ class btConjugateResidual : public btKrylovSolver public: btConjugateResidual(const int max_it_in) - : Base(max_it_in, 1e-4) + : Base(max_it_in, 1e-8) { } diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index dc7077377..1e2c9f5bc 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -216,7 +216,7 @@ public: class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint { public: - const 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 584b9d6ec..7f67260ce 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -452,7 +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("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]); } } @@ -493,6 +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) @@ -519,17 +521,19 @@ void btDeformableContactProjection::setLagrangeMultiplier() } m_lagrangeMultipliers.push_back(lm); } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) { if (!m_faceRigidConstraints[i][j].m_binding) { continue; } - const 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; @@ -538,6 +542,7 @@ void btDeformableContactProjection::setLagrangeMultiplier() } if (m_faceRigidConstraints[i][j].m_static) { + 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); @@ -545,6 +550,7 @@ void btDeformableContactProjection::setLagrangeMultiplier() } else { + 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/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 427c14954..98c6f5759 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2818,46 +2818,10 @@ 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; - //#define USE_QUADRATURE 1 - //#define CACHE_PREV_COLLISION - - // use the contact position of the previous collision -#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); - dst = m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(contact_point), - shp, - nrm, - margin); - nrm = wtr.getBasis() * nrm; - cti.m_colObj = colObjWrap->getCollisionObject(); - // use cached contact point - } - else - { - btGjkEpaSolver2::sResults results; - btTransform triangle_transform; - triangle_transform.setIdentity(); - triangle_transform.setOrigin(f.m_n[0]->m_x); - btTriangleShape triangle(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); - btVector3 guess(0, 0, 0); - const btConvexShape* csh = static_cast(shp); - btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); - dst = results.distance - margin; - 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); - nrm = results.normal; - cti.m_colObj = colObjWrap->getCollisionObject(); - for (int i = 0; i < 3; ++i) - f.m_pcontact[i] = bary[i]; - } - return (dst < 0); -#endif +// #define USE_QUADRATURE 1 +//#define CACHE_PREV_COLLISION // use collision quadrature point #ifdef USE_QUADRATURE @@ -2895,7 +2859,8 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO return (dst < 0); } #endif - btGjkEpaSolver2::sResults results; + + // collision detection using x* btTransform triangle_transform; triangle_transform.setIdentity(); triangle_transform.setOrigin(f.m_n[0]->m_q); @@ -2906,19 +2871,50 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO dst = results.distance - 2.0 * csh->getMargin() - margin; // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable 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. 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]; + dst = results.distance - csh->getMargin() - margin; cti.m_colObj = colObjWrap->getCollisionObject(); cti.m_normal = results.normal; cti.m_offset = dst; return true; } -// + void btSoftBody::updateNormals() { const btVector3 zv(0, 0, 0); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 8d13b39bd..c17bbb5cd 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1686,6 +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; if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { @@ -1695,7 +1696,6 @@ struct btSoftColliders const btVector3 ra = n.m_x - wtr.getOrigin(); c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra); - c.m_c5 = n.m_effectiveMass_inv; // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); c.m_c1 = ra; } @@ -1766,7 +1766,6 @@ struct btSoftColliders btVector3 bary; if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true)) { - f.m_pcontact[3] = 1; btScalar ima = n0->m_im + n1->m_im + n2->m_im; const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; // todo: collision between multibody and fixed deformable face will be missed. @@ -1839,10 +1838,9 @@ struct btSoftColliders psb->m_faceRigidContacts.push_back(c); } } - else - { - 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;