From b8fd4a106010b557eaa3a85ef116909567c32933 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Wed, 5 Aug 2020 15:44:26 -0700 Subject: added caching barycenters for static constraints, with lots of debugging lines --- examples/DeformableDemo/GraspDeformable.cpp | 113 +++++++++++++++++++-- .../NarrowPhaseCollision/btGjkEpa2.cpp | 3 +- src/BulletSoftBody/btConjugateResidual.h | 3 +- .../btDeformableBackwardEulerObjective.cpp | 2 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 30 +++++- src/BulletSoftBody/btDeformableContactConstraint.h | 2 +- .../btDeformableContactProjection.cpp | 22 +++- src/BulletSoftBody/btDeformableContactProjection.h | 6 +- .../btDeformableMultiBodyDynamicsWorld.cpp | 1 + src/BulletSoftBody/btSoftBody.cpp | 96 +++++++++-------- src/BulletSoftBody/btSoftBodyInternals.h | 4 +- 11 files changed, 222 insertions(+), 60 deletions(-) diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index f52f67fe9..1d692a60b 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -35,6 +35,9 @@ #include "../CommonInterfaces/CommonFileIOInterface.h" #include "Bullet3Common/b3FileUtils.h" +#include "../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" +#include "../src/LinearMath/btScalar.h" + ///The GraspDeformable shows grasping a volumetric deformable objects with multibody gripper with moter constraints. static btScalar sGripperVerticalVelocity = 0.f; static btScalar sGripperClosingTargetVelocity = 0.f; @@ -73,7 +76,7 @@ public: void resetCamera() { - float dist = 0.3; + float dist = 0.2; float pitch = -45; float yaw = 100; float targetPos[3] = {0, -0.1, 0}; @@ -256,12 +259,12 @@ void GraspDeformable::initPhysics() //create a ground { btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.))); - groundShape->setMargin(0.001); + groundShape->setMargin(0.003); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -5.1, 0)); + groundTransform.setOrigin(btVector3(0, -5, 0)); groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: btScalar mass(0.); @@ -338,12 +341,12 @@ void GraspDeformable::initPhysics() { bool onGround = false; const btScalar s = .05; - const btScalar h = -0.02; + const btScalar h = 0.01; btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), btVector3(+s, h, -s), btVector3(-s, h, +s), btVector3(+s, h, +s), - 10,10, + 2,2, 0, true); if (onGround) @@ -356,19 +359,109 @@ void GraspDeformable::initPhysics() 0, true); psb->getCollisionShape()->setMargin(0.001); + + psb->generateBendingConstraints(2); - psb->setTotalMass(0.01); - psb->setSpringStiffness(10); - psb->setDampingCoefficient(0.05); + psb->setTotalMass(0.02); + 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; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; + getDeformableDynamicsWorld()->addSoftBody(psb); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(0.05,0.005, true)); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity*0.1)); + + btScalar elastic_stiffness = 0.2; + btScalar damping_stiffness = 0.01; + bool not_damp_all_directions = true; + btScalar bending_stiffness = 0.2; + psb->setSpringStiffness(elastic_stiffness); + btDeformableLagrangianForce* springForce = + new btDeformableMassSpringForce(elastic_stiffness, + damping_stiffness, not_damp_all_directions,bending_stiffness); + getDeformableDynamicsWorld()->addForce(psb, springForce); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); + } + + if(0){ + + btSoftBody* psb = NULL; + + std::vector shapes; + tinyobj::attrib_t attribute; + char absolute_path[1024]; + b3BulletDefaultFileIO fileio; + fileio.findResourcePath("dang_sticky_rice_chips_full_sim.obj", absolute_path, 1024); + std::string err = tinyobj::LoadObj(attribute, shapes, absolute_path, "", &fileio); + if (!shapes.empty()) + { + const tinyobj::shape_t& shape = shapes[0]; + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + for (int i = 0; i < attribute.vertices.size(); i++) + { + vertices.push_back(attribute.vertices[i]); + } + for (int i = 0; i < shape.mesh.indices.size(); i++) + { + indices.push_back(shape.mesh.indices[i].vertex_index); + } + int numTris = shape.mesh.indices.size() / 3; + if (numTris > 0) + { + psb = btSoftBodyHelpers::CreateFromTriMesh( getDeformableDynamicsWorld()->getWorldInfo(), &vertices[0], &indices[0], numTris); + + btAssert(psb!=nullptr); + } + btScalar elastic_stiffness = 0.2; + btScalar damping_stiffness = 0.01; + bool not_damp_all_directions = true; + btScalar bending_stiffness = 0.2; + btDeformableLagrangianForce* springForce = + new btDeformableMassSpringForce(elastic_stiffness, + damping_stiffness, not_damp_all_directions,bending_stiffness); + psb->generateBendingConstraints(3); + getDeformableDynamicsWorld()->addForce(psb, springForce); + m_forces.push_back(springForce); + + btVector3 gravity =getDeformableDynamicsWorld()->getGravity(); + btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity *0.2); + getDeformableDynamicsWorld()->addForce(psb, gravityForce); + m_forces.push_back(gravityForce); + + btScalar friction = 1; + psb->m_cfg.kDF = friction; + + // turn on the collision flag for deformable + // collision between deformable and rigid + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + // turn on face contact for multibodies + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; + /// turn on face contact for rigid body + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; + // collion between deformable and deformable and self-collision + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + psb->setCollisionFlags(0); + + psb->setTotalMass(0.02); + psb->setSelfCollision(false); + btScalar repulsionStiffness = 0.2; + psb->setSpringStiffness(repulsionStiffness); + psb->initializeFaceTree(); + } + // psb->scale(btVector3(1, 1, 1)); + psb->rotate(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.5)); +// psb->translate(tr); + + psb->getCollisionShape()->setMargin(0.005); + psb->getCollisionShape()->setUserPointer(psb); + + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + getDeformableDynamicsWorld()->addSoftBody(psb); + } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index 45d181713..856b5d411 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..4e01bb2f5 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) { } @@ -39,6 +39,7 @@ public: // return the number of iterations taken int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) { + verbose = true; BT_PROFILE("CRSolve"); btAssert(x.size() == b.size()); reinitialize(b); diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 0c3e0b5eb..4a36e6617 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -18,7 +18,7 @@ #include "LinearMath/btQuickprof.h" btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) - : m_softBodies(softBodies), m_projection(softBodies), m_backupVelocity(backup_v), m_implicit(false) + : m_softBodies(softBodies), m_projection(softBodies, backup_v), m_backupVelocity(backup_v), m_implicit(false) { m_massPreconditioner = new MassPreconditioner(m_softBodies); m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 4916848fe..824e48331 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -32,6 +32,17 @@ btDeformableBodySolver::~btDeformableBodySolver() void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) { +// std::cout <<"before solve deformable constraints "<m_faces.size(); i++){ +// std::cout<<"face "<m_faces[i]; + btVector3 vel = btVector3(0,0,0); + for(int j = 0; j<3; j++){ + vel+= face.m_n[j]->m_v* face.m_pcontact[j]; + } +// std::cout<addLagrangeMultiplier(m_dv, x); m_objective->m_preconditioner->reinitialize(true); computeStep(x, rhs); + m_objective->m_projection.checkConstraints(x); for (int i = 0; i < m_dv.size(); ++i) { m_dv[i] = x[i]; } } updateVelocity(); + +// std::cout <<"after solve deformable constraints "<m_faces.size(); i++){ +// std::cout<<"face "<m_faces[i]; + btVector3 vel = btVector3(0,0,0); + for(int j = 0; j<3; j++){ + vel+=face.m_n[j]->m_v* face.m_pcontact[j]; + } +// std::cout<m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv; +// std::cout<<"node "<m_nodes[j].m_splitv[0]<<" "<m_nodes[j].m_splitv[1]<<" "<m_nodes[j].m_splitv[2]<<" "<m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2()); ++counter; @@ -349,7 +377,7 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit) { m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter]; } - psb->m_nodes[j].m_v = m_backupVelocity[counter]; +// psb->m_nodes[j].m_v = m_backupVelocity[counter]; ++counter; } } diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index dc7077377..3b559bbf7 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..1fd67daf5 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]); } } @@ -525,11 +526,22 @@ void btDeformableContactProjection::setLagrangeMultiplier() { 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; + lm.m_vals[0] = lm.m_vals[1] = lm.m_vals[2] = 0; + + btVector3 face_vel = btVector3(0,0,0); + btVector3 backup_face_vel = btVector3(0,0,0); + for(int kk = 0; kk<3; kk++){ + face_vel += (face->m_n[kk]->m_v + face->m_n[kk]->m_splitv)* bary[kk]; + backup_face_vel += m_backupVelocity[face->m_n[kk]->index] * bary[kk]; + } + + btVector3 dvel = face_vel - backup_face_vel; + for (int k = 0; k < 3; ++k) { face->m_n[k]->m_constrained = true; @@ -538,15 +550,21 @@ 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); lm.m_dirs[2] = btVector3(0, 0, 1); + lm.m_vals[0] = dvel[0]; + lm.m_vals[1] = dvel[1]; + lm.m_vals[2] = dvel[2]; } else { + face->m_pcontact[3] = 0; lm.m_num_constraints = 1; lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal; + lm.m_vals[0] = dvel.dot(m_faceRigidConstraints[i][j].m_normal); } m_lagrangeMultipliers.push_back(lm); } diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 4964eaf99..82ae19ae6 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -32,6 +32,7 @@ struct LagrangeMultiplier btScalar m_weights[3]; // weights of the nodes involved, same size as m_num_nodes btVector3 m_dirs[3]; // Constraint directions, same size of m_num_constraints; int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes; + btScalar m_vals[3]; }; class btDeformableContactProjection @@ -39,6 +40,7 @@ class btDeformableContactProjection public: typedef btAlignedObjectArray TVStack; btAlignedObjectArray& m_softBodies; + const TVStack& m_backupVelocity; // all constraints involving face btAlignedObjectArray m_allFaceConstraints; @@ -64,8 +66,8 @@ public: bool m_useStrainLimiting; - btDeformableContactProjection(btAlignedObjectArray& softBodies) - : m_softBodies(softBodies) + btDeformableContactProjection(btAlignedObjectArray& softBodies, const TVStack& backupVelocity) + : m_softBodies(softBodies), m_backupVelocity(backupVelocity) { } diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index dbde78944..257483d64 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -401,6 +401,7 @@ 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 427c14954..90f39db20 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,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO return (dst < 0); } #endif - btGjkEpaSolver2::sResults results; + btTransform triangle_transform; triangle_transform.setIdentity(); triangle_transform.setOrigin(f.m_n[0]->m_q); @@ -2906,13 +2870,65 @@ 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 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); + const btConvexShape* csh = static_cast(shp); + btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr,results); + cti.m_colObj = colObjWrap->getCollisionObject(); +// std::cout<<"****** distance1 "<m_x[0]<<" "<m_x[1]<<" "<m_x[2]<<" "<m_x[0]<<" "<m_x[1]<<" "<m_x[2]<<" "<m_x[0]<<" "<m_x[1]<<" "<m_x[2]<<" "<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; + +// std::cout<<"contact point 1 "<getMargin() <<" "<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; + +// std::cout<<"contact point 1 "<getMargin() <<" "<getCollisionObject(); cti.m_normal = results.normal; cti.m_offset = dst; diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 8d13b39bd..e626a8b75 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1766,7 +1766,7 @@ struct btSoftColliders btVector3 bary; if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true)) { - f.m_pcontact[3] = 1; +// 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. @@ -1843,6 +1843,8 @@ struct btSoftColliders { f.m_pcontact[3] = 0; } + //always mark caching contact as false + f.m_pcontact[3] = 0; } btSoftBody* psb; const btCollisionObjectWrapper* m_colObj1Wrap; -- cgit v1.2.1 From 086638d16e03cce058fc4249939617c7bfbe60bc Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Thu, 6 Aug 2020 20:54:15 -0700 Subject: fix bug in multibody point contact with deformable --- examples/DeformableDemo/GraspDeformable.cpp | 25 ++++++++++++++++++---- .../btDeformableContactProjection.cpp | 7 +++++- src/BulletSoftBody/btSoftBodyInternals.h | 2 +- 3 files changed, 28 insertions(+), 6 deletions(-) diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 1d692a60b..285f7c8c8 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -205,9 +205,9 @@ void GraspDeformable::initPhysics() getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0; getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 150; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - m_maxPickingForce = 0.001; + m_maxPickingForce =100; // build a gripper - if(1) + if(0) { bool damping = true; bool gyro = false; @@ -257,9 +257,10 @@ void GraspDeformable::initPhysics() } //create a ground + if(0) { btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.))); - groundShape->setMargin(0.003); + groundShape->setMargin(0.001); m_collisionShapes.push_back(groundShape); btTransform groundTransform; @@ -285,6 +286,22 @@ void GraspDeformable::initPhysics() //add the ground to the dynamics world m_dynamicsWorld->addRigidBody(body,1,1+2); } + + if(1) + { + bool gyro = false; + bool canSleep = false; + bool selfCollide = true; + btVector3 linkHalfExtents(10, 5, 10); + btVector3 baseHalfExtents(10, 5, 10); + btVector3 basepos(0, -5, 0); + btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), basepos, linkHalfExtents, baseHalfExtents, false); + + mbC->setCanSleep(canSleep); + mbC->setHasSelfCollision(selfCollide); + mbC->setUseGyroTerm(gyro); + addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents); + } // create a soft block if (0) @@ -453,7 +470,7 @@ void GraspDeformable::initPhysics() } // psb->scale(btVector3(1, 1, 1)); psb->rotate(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.5)); -// psb->translate(tr); + psb->translate(btVector3(0,1,0)); psb->getCollisionShape()->setMargin(0.005); psb->getCollisionShape()->setUserPointer(psb); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 1fd67daf5..6394eb9a2 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -494,6 +494,9 @@ void btDeformableContactProjection::setLagrangeMultiplier() lm.m_dirs[2] = btVector3(0, 0, 1); m_lagrangeMultipliers.push_back(lm); } + + + if( m_faceRigidConstraints[i].size()==0){ for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) { if (!m_nodeRigidConstraints[i][j].m_binding) @@ -519,7 +522,9 @@ 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) diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index e626a8b75..81fd9bbca 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; } -- cgit v1.2.1 From c5fc913293e6e3b9b0819d31d5ed6540d7fa575a Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Mon, 10 Aug 2020 13:29:24 -0700 Subject: fix softbody v --- src/BulletSoftBody/btDeformableBodySolver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 824e48331..2d5537f49 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -377,7 +377,7 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit) { m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter]; } -// psb->m_nodes[j].m_v = m_backupVelocity[counter]; + psb->m_nodes[j].m_v = m_backupVelocity[counter]; ++counter; } } -- cgit v1.2.1 From 25aa0f566920865d131b5eadbb22f0106499eb29 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Wed, 12 Aug 2020 10:13:49 -0700 Subject: clean up code --- examples/DeformableDemo/GraspDeformable.cpp | 132 ++------------------- src/BulletSoftBody/btConjugateResidual.h | 1 - .../btDeformableBackwardEulerObjective.cpp | 2 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 30 +---- .../btDeformableContactProjection.cpp | 17 --- src/BulletSoftBody/btDeformableContactProjection.h | 6 +- src/BulletSoftBody/btSoftBody.cpp | 32 +---- src/BulletSoftBody/btSoftBodyInternals.h | 8 +- 8 files changed, 23 insertions(+), 205 deletions(-) diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 285f7c8c8..f52f67fe9 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -35,9 +35,6 @@ #include "../CommonInterfaces/CommonFileIOInterface.h" #include "Bullet3Common/b3FileUtils.h" -#include "../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" -#include "../src/LinearMath/btScalar.h" - ///The GraspDeformable shows grasping a volumetric deformable objects with multibody gripper with moter constraints. static btScalar sGripperVerticalVelocity = 0.f; static btScalar sGripperClosingTargetVelocity = 0.f; @@ -76,7 +73,7 @@ public: void resetCamera() { - float dist = 0.2; + float dist = 0.3; float pitch = -45; float yaw = 100; float targetPos[3] = {0, -0.1, 0}; @@ -205,9 +202,9 @@ void GraspDeformable::initPhysics() getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0; getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 150; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - m_maxPickingForce =100; + m_maxPickingForce = 0.001; // build a gripper - if(0) + if(1) { bool damping = true; bool gyro = false; @@ -257,7 +254,6 @@ void GraspDeformable::initPhysics() } //create a ground - if(0) { btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.))); groundShape->setMargin(0.001); @@ -265,7 +261,7 @@ void GraspDeformable::initPhysics() btTransform groundTransform; groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -5, 0)); + groundTransform.setOrigin(btVector3(0, -5.1, 0)); groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: btScalar mass(0.); @@ -286,22 +282,6 @@ void GraspDeformable::initPhysics() //add the ground to the dynamics world m_dynamicsWorld->addRigidBody(body,1,1+2); } - - if(1) - { - bool gyro = false; - bool canSleep = false; - bool selfCollide = true; - btVector3 linkHalfExtents(10, 5, 10); - btVector3 baseHalfExtents(10, 5, 10); - btVector3 basepos(0, -5, 0); - btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), basepos, linkHalfExtents, baseHalfExtents, false); - - mbC->setCanSleep(canSleep); - mbC->setHasSelfCollision(selfCollide); - mbC->setUseGyroTerm(gyro); - addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents); - } // create a soft block if (0) @@ -358,12 +338,12 @@ void GraspDeformable::initPhysics() { bool onGround = false; const btScalar s = .05; - const btScalar h = 0.01; + const btScalar h = -0.02; btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), btVector3(+s, h, -s), btVector3(-s, h, +s), btVector3(+s, h, +s), - 2,2, + 10,10, 0, true); if (onGround) @@ -376,109 +356,19 @@ void GraspDeformable::initPhysics() 0, true); psb->getCollisionShape()->setMargin(0.001); - - psb->generateBendingConstraints(2); - psb->setTotalMass(0.02); - + psb->setTotalMass(0.01); + psb->setSpringStiffness(10); + psb->setDampingCoefficient(0.05); 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; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; - getDeformableDynamicsWorld()->addSoftBody(psb); - - btScalar elastic_stiffness = 0.2; - btScalar damping_stiffness = 0.01; - bool not_damp_all_directions = true; - btScalar bending_stiffness = 0.2; - psb->setSpringStiffness(elastic_stiffness); - btDeformableLagrangianForce* springForce = - new btDeformableMassSpringForce(elastic_stiffness, - damping_stiffness, not_damp_all_directions,bending_stiffness); - getDeformableDynamicsWorld()->addForce(psb, springForce); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); - } - - if(0){ - - btSoftBody* psb = NULL; - - std::vector shapes; - tinyobj::attrib_t attribute; - char absolute_path[1024]; - b3BulletDefaultFileIO fileio; - fileio.findResourcePath("dang_sticky_rice_chips_full_sim.obj", absolute_path, 1024); - std::string err = tinyobj::LoadObj(attribute, shapes, absolute_path, "", &fileio); - if (!shapes.empty()) - { - const tinyobj::shape_t& shape = shapes[0]; - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - for (int i = 0; i < attribute.vertices.size(); i++) - { - vertices.push_back(attribute.vertices[i]); - } - for (int i = 0; i < shape.mesh.indices.size(); i++) - { - indices.push_back(shape.mesh.indices[i].vertex_index); - } - int numTris = shape.mesh.indices.size() / 3; - if (numTris > 0) - { - psb = btSoftBodyHelpers::CreateFromTriMesh( getDeformableDynamicsWorld()->getWorldInfo(), &vertices[0], &indices[0], numTris); - - btAssert(psb!=nullptr); - } - btScalar elastic_stiffness = 0.2; - btScalar damping_stiffness = 0.01; - bool not_damp_all_directions = true; - btScalar bending_stiffness = 0.2; - btDeformableLagrangianForce* springForce = - new btDeformableMassSpringForce(elastic_stiffness, - damping_stiffness, not_damp_all_directions,bending_stiffness); - psb->generateBendingConstraints(3); - getDeformableDynamicsWorld()->addForce(psb, springForce); - m_forces.push_back(springForce); - - btVector3 gravity =getDeformableDynamicsWorld()->getGravity(); - btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity *0.2); - getDeformableDynamicsWorld()->addForce(psb, gravityForce); - m_forces.push_back(gravityForce); - - btScalar friction = 1; - psb->m_cfg.kDF = friction; - - // turn on the collision flag for deformable - // collision between deformable and rigid - psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - // turn on face contact for multibodies - psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; - /// turn on face contact for rigid body - psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; - // collion between deformable and deformable and self-collision - psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; - psb->setCollisionFlags(0); - - psb->setTotalMass(0.02); - psb->setSelfCollision(false); - btScalar repulsionStiffness = 0.2; - psb->setSpringStiffness(repulsionStiffness); - psb->initializeFaceTree(); - } - // psb->scale(btVector3(1, 1, 1)); - psb->rotate(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.5)); - psb->translate(btVector3(0,1,0)); - - psb->getCollisionShape()->setMargin(0.005); - psb->getCollisionShape()->setUserPointer(psb); - - psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects - psb->m_cfg.kCHR = 1; // collision hardness with rigid body - getDeformableDynamicsWorld()->addSoftBody(psb); - + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(0.05,0.005, true)); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity*0.1)); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); diff --git a/src/BulletSoftBody/btConjugateResidual.h b/src/BulletSoftBody/btConjugateResidual.h index 4e01bb2f5..614612036 100644 --- a/src/BulletSoftBody/btConjugateResidual.h +++ b/src/BulletSoftBody/btConjugateResidual.h @@ -39,7 +39,6 @@ public: // return the number of iterations taken int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) { - verbose = true; BT_PROFILE("CRSolve"); btAssert(x.size() == b.size()); reinitialize(b); diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 4a36e6617..0c3e0b5eb 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -18,7 +18,7 @@ #include "LinearMath/btQuickprof.h" btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) - : m_softBodies(softBodies), m_projection(softBodies, backup_v), m_backupVelocity(backup_v), m_implicit(false) + : m_softBodies(softBodies), m_projection(softBodies), m_backupVelocity(backup_v), m_implicit(false) { m_massPreconditioner = new MassPreconditioner(m_softBodies); m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 2d5537f49..139d69c7d 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -32,17 +32,6 @@ btDeformableBodySolver::~btDeformableBodySolver() void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) { -// std::cout <<"before solve deformable constraints "<m_faces.size(); i++){ -// std::cout<<"face "<m_faces[i]; - btVector3 vel = btVector3(0,0,0); - for(int j = 0; j<3; j++){ - vel+= face.m_n[j]->m_v* face.m_pcontact[j]; - } -// std::cout<addLagrangeMultiplier(m_dv, x); m_objective->m_preconditioner->reinitialize(true); computeStep(x, rhs); - m_objective->m_projection.checkConstraints(x); for (int i = 0; i < m_dv.size(); ++i) { m_dv[i] = x[i]; } } updateVelocity(); - -// std::cout <<"after solve deformable constraints "<m_faces.size(); i++){ -// std::cout<<"face "<m_faces[i]; - btVector3 vel = btVector3(0,0,0); - for(int j = 0; j<3; j++){ - vel+=face.m_n[j]->m_v* face.m_pcontact[j]; - } -// std::cout<m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv; -// std::cout<<"node "<m_nodes[j].m_splitv[0]<<" "<m_nodes[j].m_splitv[1]<<" "<m_nodes[j].m_splitv[2]<<" "<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/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 6394eb9a2..733e648c4 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -495,8 +495,6 @@ void btDeformableContactProjection::setLagrangeMultiplier() m_lagrangeMultipliers.push_back(lm); } - - if( m_faceRigidConstraints[i].size()==0){ for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) { if (!m_nodeRigidConstraints[i][j].m_binding) @@ -523,7 +521,6 @@ void btDeformableContactProjection::setLagrangeMultiplier() } m_lagrangeMultipliers.push_back(lm); } - } for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) { @@ -536,16 +533,6 @@ void btDeformableContactProjection::setLagrangeMultiplier() btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; LagrangeMultiplier lm; lm.m_num_nodes = 3; - lm.m_vals[0] = lm.m_vals[1] = lm.m_vals[2] = 0; - - btVector3 face_vel = btVector3(0,0,0); - btVector3 backup_face_vel = btVector3(0,0,0); - for(int kk = 0; kk<3; kk++){ - face_vel += (face->m_n[kk]->m_v + face->m_n[kk]->m_splitv)* bary[kk]; - backup_face_vel += m_backupVelocity[face->m_n[kk]->index] * bary[kk]; - } - - btVector3 dvel = face_vel - backup_face_vel; for (int k = 0; k < 3; ++k) { @@ -560,16 +547,12 @@ void btDeformableContactProjection::setLagrangeMultiplier() lm.m_dirs[0] = btVector3(1, 0, 0); lm.m_dirs[1] = btVector3(0, 1, 0); lm.m_dirs[2] = btVector3(0, 0, 1); - lm.m_vals[0] = dvel[0]; - lm.m_vals[1] = dvel[1]; - lm.m_vals[2] = dvel[2]; } else { face->m_pcontact[3] = 0; lm.m_num_constraints = 1; lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal; - lm.m_vals[0] = dvel.dot(m_faceRigidConstraints[i][j].m_normal); } m_lagrangeMultipliers.push_back(lm); } diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 82ae19ae6..4964eaf99 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -32,7 +32,6 @@ struct LagrangeMultiplier btScalar m_weights[3]; // weights of the nodes involved, same size as m_num_nodes btVector3 m_dirs[3]; // Constraint directions, same size of m_num_constraints; int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes; - btScalar m_vals[3]; }; class btDeformableContactProjection @@ -40,7 +39,6 @@ class btDeformableContactProjection public: typedef btAlignedObjectArray TVStack; btAlignedObjectArray& m_softBodies; - const TVStack& m_backupVelocity; // all constraints involving face btAlignedObjectArray m_allFaceConstraints; @@ -66,8 +64,8 @@ public: bool m_useStrainLimiting; - btDeformableContactProjection(btAlignedObjectArray& softBodies, const TVStack& backupVelocity) - : m_softBodies(softBodies), m_backupVelocity(backupVelocity) + btDeformableContactProjection(btAlignedObjectArray& softBodies) + : m_softBodies(softBodies) { } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 90f39db20..cca065afd 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2860,6 +2860,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO } #endif + // collision detection using x* btTransform triangle_transform; triangle_transform.setIdentity(); triangle_transform.setOrigin(f.m_n[0]->m_q); @@ -2871,7 +2872,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO if (dst >= 0) return false; - // use the contact position of the previous collision + // Use consistent barycenter to recalculate distance. #ifdef CACHE_PREV_COLLISION if (f.m_pcontact[3] != 0) { @@ -2881,37 +2882,22 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO const btConvexShape* csh = static_cast(shp); btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr,results); cti.m_colObj = colObjWrap->getCollisionObject(); -// std::cout<<"****** distance1 "<m_x[0]<<" "<m_x[1]<<" "<m_x[2]<<" "<m_x[0]<<" "<m_x[1]<<" "<m_x[2]<<" "<m_x[0]<<" "<m_x[1]<<" "<m_x[2]<<" "<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; - -// std::cout<<"contact point 1 "<getMargin() <<" "<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); @@ -2923,18 +2909,12 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO f.m_pcontact[i] = bary[i]; dst = results.distance - csh->getMargin() - margin; - -// std::cout<<"contact point 1 "<getMargin() <<" "<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 81fd9bbca..62ec76769 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -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,11 +1838,8 @@ struct btSoftColliders psb->m_faceRigidContacts.push_back(c); } } - else - { - f.m_pcontact[3] = 0; - } - //always mark caching contact as false + // Set caching barycenters to be false after collision detection. + // Only turn on when contact is static. f.m_pcontact[3] = 0; } btSoftBody* psb; -- cgit v1.2.1 From 07521869db69e82017419dd10bc6a0c4c6e120b5 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Wed, 12 Aug 2020 10:27:47 -0700 Subject: format file format format --- .../NarrowPhaseCollision/btGjkEpa2.cpp | 2 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 2 +- src/BulletSoftBody/btDeformableContactConstraint.h | 2 +- .../btDeformableContactProjection.cpp | 18 +++--- .../btDeformableMultiBodyDynamicsWorld.cpp | 1 - src/BulletSoftBody/btSoftBody.cpp | 64 +++++++++++----------- src/BulletSoftBody/btSoftBodyInternals.h | 8 +-- 7 files changed, 48 insertions(+), 49 deletions(-) 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; -- cgit v1.2.1 From 2d42dea27f2387c8e8922801cfee18b276aea0da Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Mon, 17 Aug 2020 10:52:07 -0700 Subject: turn off cache prev collision by default --- src/BulletSoftBody/btSoftBody.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index f71545215..98c6f5759 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2821,7 +2821,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO btGjkEpaSolver2::sResults results; // #define USE_QUADRATURE 1 -#define CACHE_PREV_COLLISION +//#define CACHE_PREV_COLLISION // use collision quadrature point #ifdef USE_QUADRATURE -- cgit v1.2.1