diff options
author | Chuyuan Fu <fuchuyuan.kelly@gmail.com> | 2020-08-12 10:13:49 -0700 |
---|---|---|
committer | Chuyuan Fu <fuchuyuan.kelly@gmail.com> | 2020-08-12 10:13:49 -0700 |
commit | 25aa0f566920865d131b5eadbb22f0106499eb29 (patch) | |
tree | 83a07475f4e0390f8ef77f956522c93590fe0214 | |
parent | c5fc913293e6e3b9b0819d31d5ed6540d7fa575a (diff) | |
download | bullet3-25aa0f566920865d131b5eadbb22f0106499eb29.tar.gz |
clean up code
-rw-r--r-- | examples/DeformableDemo/GraspDeformable.cpp | 132 | ||||
-rw-r--r-- | src/BulletSoftBody/btConjugateResidual.h | 1 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp | 2 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableBodySolver.cpp | 30 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableContactProjection.cpp | 17 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableContactProjection.h | 6 | ||||
-rw-r--r-- | src/BulletSoftBody/btSoftBody.cpp | 32 | ||||
-rw-r--r-- | 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<tinyobj::shape_t> 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<btScalar> vertices; - btAlignedObjectArray<int> 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<btSoftBody*>& 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 "<<std::endl; - btSoftBody* psb = m_softBodies[0]; - for(int i = 0; i<psb->m_faces.size(); i++){ -// std::cout<<"face "<<i<<": "; - const btSoftBody::Face& face =psb->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<<vel[0]<<" "<<vel[1]<<" "<<vel[2]<<std::endl; - } BT_PROFILE("solveDeformableConstraints"); if (!m_implicit) { @@ -59,25 +48,12 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) m_objective->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 "<<std::endl; - btSoftBody* psb = m_softBodies[0]; - for(int i = 0; i<psb->m_faces.size(); i++){ -// std::cout<<"face "<<i<<": "; - const btSoftBody::Face& face =psb->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<<vel[0]<<" "<<vel[1]<<" "<<vel[2]<<std::endl; - } } else { @@ -306,11 +282,7 @@ void btDeformableBodySolver::updateVelocity() } else { - psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv; -// std::cout<<"node "<<j<<", back up vel "<< m_backupVelocity[counter][0] <<" "<< m_backupVelocity[counter][1]<<" "<<m_backupVelocity[counter][2]<<std::endl; -// std::cout<<" , dv: "<< m_dv[counter][0]<<" "<< m_dv[counter][1]<<" "<< m_dv[counter][2]<<" "<<std::endl; -// std::cout<<", split v: "<<psb->m_nodes[j].m_splitv[0]<<" "<<psb->m_nodes[j].m_splitv[1]<<" "<<psb->m_nodes[j].m_splitv[2]<<" "<<std::endl; - + 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/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<btVector3> TVStack; btAlignedObjectArray<btSoftBody*>& m_softBodies; - const TVStack& m_backupVelocity; // all constraints involving face btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints; @@ -66,8 +64,8 @@ public: bool m_useStrainLimiting; - btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backupVelocity) - : m_softBodies(softBodies), m_backupVelocity(backupVelocity) + btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& 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<const btConvexShape*>(shp); btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr,results); cti.m_colObj = colObjWrap->getCollisionObject(); -// std::cout<<"****** distance1 "<<results.distance<<std::endl; -// std::cout<<"normal "<<results.normal[0]<<" "<<results.normal[1]<<" "<<results.normal[2]<<std::endl; -// std::cout<<"point is at "<<contact_point[0]<<" "<<contact_point[1]<<" "<<contact_point[2]<<std::endl; -// std::cout<<"three vertices: "<<std::endl; -// std::cout<<f.m_n[0]->m_x[0]<<" "<<f.m_n[0]->m_x[1]<<" "<<f.m_n[0]->m_x[2]<<" "<<std::endl; -// std::cout<<f.m_n[1]->m_x[0]<<" "<<f.m_n[1]->m_x[1]<<" "<<f.m_n[1]->m_x[2]<<" "<<std::endl; -// std::cout<<f.m_n[2]->m_x[0]<<" "<<f.m_n[2]->m_x[1]<<" "<<f.m_n[2]->m_x[2]<<" "<<std::endl; dst = results.distance; cti.m_normal = results.normal; cti.m_offset = dst; - - - // use face contact to redo collision detection + + //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; - -// std::cout<<"contact point 1 "<<results.witnesses[0][0]<< " "<<results.witnesses[0][1]<< " "<<results.witnesses[0][2]<< " "<<std::endl; -// std::cout<<"contact point 2 "<<results.witnesses[1][0]<< " "<<results.witnesses[1][1]<< " "<<results.witnesses[1][2]<< " "<<std::endl; -// std::cout<<"result distance "<<results.distance<<std::endl; -// std::cout<< csh->getMargin() <<" "<<margin<<std::endl; -// std::cout<<"****** distance 2"<<dst<<std::endl; -// std::cout<<"normal "<< results.normal[0]<< " "<<results.normal[1]<< " "<<results.normal[2]<<std::endl; 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); @@ -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 "<<contact_point[0]<< " "<<contact_point[1]<< " "<<contact_point[2]<< " "<<std::endl; -// std::cout<<"contact point 2 "<<results.witnesses[1][0]<< " "<<results.witnesses[1][1]<< " "<<results.witnesses[1][2]<< " "<<std::endl; -// std::cout<<"result distance "<<results.distance<<std::endl; -// std::cout<< csh->getMargin() <<" "<<margin<<std::endl; -// std::cout<<"distance "<<dst<<std::endl; 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 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; |