diff --git a/Extras/BulletRobotics/CMakeLists.txt b/Extras/BulletRobotics/CMakeLists.txt index 2fa6c4935..38e96775d 100644 --- a/Extras/BulletRobotics/CMakeLists.txt +++ b/Extras/BulletRobotics/CMakeLists.txt @@ -290,7 +290,6 @@ IF (INSTALL_EXTRA_LIBS) SET_TARGET_PROPERTIES(BulletRobotics PROPERTIES FRAMEWORK true) SET_TARGET_PROPERTIES(BulletRobotics PROPERTIES PUBLIC_HEADER "PhysicsClientC_API.h" ) ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) -ENDIF (INSTALL_EXTRA_LIBS) IF(NOT MSVC) CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/bullet_robotics.pc.cmake @@ -301,4 +300,6 @@ IF(NOT MSVC) DESTINATION ${PKGCONFIG_INSTALL_PREFIX} ) -ENDIF(NOT MSVC) \ No newline at end of file +ENDIF(NOT MSVC) +ENDIF (INSTALL_EXTRA_LIBS) + diff --git a/examples/DeformableDemo/DeformableContact.cpp b/examples/DeformableDemo/DeformableContact.cpp index 2432d3e27..8712d6de8 100644 --- a/examples/DeformableDemo/DeformableContact.cpp +++ b/examples/DeformableDemo/DeformableContact.cpp @@ -105,7 +105,8 @@ void DeformableContact::initPhysics() btVector3 gravity = btVector3(0, -10, 0); m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; - + getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25); + getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.Reset(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); { @@ -182,7 +183,7 @@ void DeformableContact::initPhysics() psb2->setTotalMass(1); psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb2->m_cfg.kCHR = 1; // collision hardness with rigid body - psb2->m_cfg.kDF = 0; + psb2->m_cfg.kDF = 0.5; psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; psb->translate(btVector3(3.5,0,0)); diff --git a/src/.DS_Store b/src/.DS_Store index ee616d543..26f6e2211 100644 Binary files a/src/.DS_Store and b/src/.DS_Store differ diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 9f8e13e20..03a884139 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -17,14 +17,19 @@ /* A single step of the deformable body simulation contains the following main components: -1. Update velocity to a temporary state v_{n+1}^* = v_n + explicit_force * dt / mass, where explicit forces include gravity and elastic forces. -2. Detect collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*. -3. Then velocities of deformable bodies v_{n+1} are solved in +Call internalStepSimulation multiple times, to achieve 240Hz (4 steps of 60Hz). +1. Deformable maintaintenance of rest lengths and volume preservation. Forces only depend on position: Update velocity to a temporary state v_{n+1}^* = v_n + explicit_force * dt / mass, where explicit forces include gravity and elastic forces. +2. Detect discrete collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*. + +3a. Solve all constraints, including LCP. Contact, position correction due to numerical drift, friction, and anchors for deformable. + TODO: add option for positional drift correction (using vel_target += erp * pos_error/dt + +3b. 5 Newton steps (multiple step). Conjugent Gradient solves linear system. Deformable Damping: Then velocities of deformable bodies v_{n+1} are solved in M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass, by a conjugate gradient solver, where the damping force is implicit and depends on v_{n+1}. -4. Contact constraints are solved as projections as in the paper by Baraff and Witkin https://www.cs.cmu.edu/~baraff/papers/sig98.pdf. Dynamic frictions are treated as a force and added to the rhs of the CG solve, whereas static frictions are treated as constraints similar to contact. -5. Position is updated via x_{n+1} = x_n + dt * v_{n+1}. -6. Apply position correction to prevent numerical drift. + Make sure contact constraints are not violated in step b by performing velocity projections as in the paper by Baraff and Witkin https://www.cs.cmu.edu/~baraff/papers/sig98.pdf. Dynamic frictions are treated as a force and added to the rhs of the CG solve, whereas static frictions are treated as constraints similar to contact. +4. Position is updated via x_{n+1} = x_n + dt * v_{n+1}. + The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf */ diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index b113b4546..cbe959bdc 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -175,7 +175,7 @@ public: btSoftBody::Tetra& tetra = psb->m_tetras[j]; btMatrix3x3 P; firstPiola(psb->m_tetraScratches[j],P); - +#if USE_SVD btMatrix3x3 U, V; btVector3 sigma; singularValueDecomposition(P, U, sigma, V); @@ -194,6 +194,7 @@ public: Sigma[1][1] = sigma[1]; Sigma[2][2] = sigma[2]; P = U * Sigma * V.transpose(); +#endif // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 6e20b3222..93aa1d3c0 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1288,7 +1288,7 @@ struct btSoftColliders c.m_node = node; c.m_face = face; c.m_weights = w; - c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + c.m_friction = btMax (psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR; c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR; psb[0]->m_scontacts.push_back(c); @@ -1346,7 +1346,7 @@ struct btSoftColliders c.m_bary = w; // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w; - c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im; c.m_c0 = btScalar(1)/(ma + c.m_imf); @@ -1408,7 +1408,7 @@ struct btSoftColliders c.m_bary = w; // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w; - c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im; c.m_c0 = btScalar(1)/(ma + c.m_imf); @@ -1462,7 +1462,7 @@ struct btSoftColliders c.m_bary = w; // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w; - c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im; c.m_c0 = btScalar(1)/(ma + c.m_imf); diff --git a/src/LinearMath/btImplicitQRSVD.h b/src/LinearMath/btImplicitQRSVD.h index cf96d4f72..54378b8a8 100644 --- a/src/LinearMath/btImplicitQRSVD.h +++ b/src/LinearMath/btImplicitQRSVD.h @@ -150,10 +150,14 @@ public: btScalar d = a * a + b * b; c = 1; s = 0; - if (d != 0) { - btScalar t = btScalar(1.0)/btSqrt(d); - c = a * t; - s = -b * t; + if (d > SIMD_EPSILON) { + btScalar sqrtd = btSqrt(d); + if (sqrtd>SIMD_EPSILON) + { + btScalar t = btScalar(1.0)/sqrtd; + c = a * t; + s = -b * t; + } } } @@ -167,7 +171,7 @@ public: btScalar d = a * a + b * b; c = 0; s = 1; - if (d != 0) { + if (d > SIMD_EPSILON) { btScalar t = btScalar(1.0)/btSqrt(d); s = a * t; c = b * t; @@ -432,10 +436,11 @@ inline void polarDecomposition(const btMatrix2x2& A, btScalar denominator = btSqrt(a*a+b*b); R.c = (btScalar)1; R.s = (btScalar)0; - if (denominator != 0) { + if (denominator > SIMD_EPSILON) { /* No need to use a tolerance here because x(0) and x(1) always have smaller magnitude then denominator, therefore overflow never happens. + In Bullet, we use a tolerance anyway. */ R.c = a / denominator; R.s = -b / denominator; @@ -485,7 +490,10 @@ inline void singularValueDecomposition( } else { btScalar tau = 0.5 * (x - z); - btScalar w = btSqrt(tau * tau + y * y); + btScalar val = tau * tau + y * y; + if (val > SIMD_EPSILON) + { + btScalar w = btSqrt(val); // w > y > 0 btScalar t; if (tau > 0) { @@ -508,6 +516,13 @@ inline void singularValueDecomposition( btScalar s2 = sine * sine; sigma(0,0) = c2 * x - csy + s2 * z; sigma(1,1) = s2 * x + csy + c2 * z; + } else + { + cosine = 1; + sine = 0; + sigma(0,0) = x; + sigma(1,1) = z; + } } // Sorting @@ -558,9 +573,15 @@ inline btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btSca { btScalar d = (btScalar)0.5 * (a1 - a2); btScalar bs = b1 * b1; - btScalar mu = a2 - copysign(bs / (btFabs(d) + btSqrt(d * d + bs)), d); + btScalar val = d * d + bs; + if (val>SIMD_EPSILON) + { + btScalar denom = btFabs(d) + btSqrt(val); + + btScalar mu = a2 - copySign(bs / (denom), d); // T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs)); return mu; + } } /** @@ -749,15 +770,20 @@ inline int singularValueDecomposition(const btMatrix3x3& A, btScalar beta_2 = B[1][2]; btScalar gamma_1 = alpha_1 * beta_1; btScalar gamma_2 = alpha_2 * beta_2; - tol *= btMax((btScalar)0.5 * btSqrt(alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2), (btScalar)1); - + btScalar val = alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2; + if (val > SIMD_EPSILON) + { + tol *= btMax((btScalar)0.5 * btSqrt(val), (btScalar)1); + } /** Do implicit shift QR until A^T A is block diagonal */ + int max_count = 100; while (btFabs(beta_2) > tol && btFabs(beta_1) > tol && btFabs(alpha_1) > tol && btFabs(alpha_2) > tol - && btFabs(alpha_3) > tol) { + && btFabs(alpha_3) > tol + && count < max_count) { mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2); r.compute(alpha_1 * alpha_1 - mu, gamma_1);