From b4efd914762984a76b348d76e996907e43e5d88c Mon Sep 17 00:00:00 2001 From: jingyuc Date: Sun, 12 Sep 2021 21:53:23 -0400 Subject: [PATCH] add support for damping. the fixed constraint is working again with the deltaImpulse form --- examples/ReducedDeformableDemo/BasicTest.cpp | 36 +++++++++---------- examples/ReducedDeformableDemo/FreeFall.cpp | 10 +++--- .../ReducedDeformableDemo/ReducedGrasp.cpp | 8 ++--- .../btReducedDeformableContactConstraint.cpp | 33 +++++++++-------- .../btReducedDeformableContactConstraint.h | 4 ++- .../btReducedSoftBody.cpp | 6 ++-- .../btReducedSoftBodySolver.cpp | 18 ---------- 7 files changed, 51 insertions(+), 64 deletions(-) diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 9da1de86b..fcdad9930 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -30,7 +30,7 @@ // static btScalar E = 50; // static btScalar nu = 0.3; static btScalar damping_alpha = 0.0; -static btScalar damping_beta = 0.0; +static btScalar damping_beta = 0.01; static btScalar COLLIDING_VELOCITY = 0; static int start_mode = 6; static int num_modes = 1; @@ -134,16 +134,15 @@ public: void stepSimulation(float deltaTime) { // TODO: remove this. very hacky way of adding initial deformation - btReducedSoftBody* rsb = static_cast(static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]); - if (first_step /* && !rsb->m_bUpdateRtCst*/) - { - getDeformedShape(rsb, 0, 1); - first_step = false; - // rsb->mapToReducedDofs(); - } + // btReducedSoftBody* rsb = static_cast(static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]); + // if (first_step /* && !rsb->m_bUpdateRtCst*/) + // { + // getDeformedShape(rsb, 0, 1); + // first_step = false; + // // rsb->mapToReducedDofs(); + // } float internalTimeStep = 1. / 60.f; - // float internalTimeStep = 1e-3; m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); // sim_time += internalTimeStep; @@ -198,7 +197,7 @@ void BasicTest::initPhysics() m_broadphase = new btDbvtBroadphase(); btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver(); - btVector3 gravity = btVector3(0, 0, 0); + btVector3 gravity = btVector3(0, -10, 0); reducedSoftBodySolver->setGravity(gravity); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); @@ -224,18 +223,17 @@ void BasicTest::initPhysics() btTransform init_transform; init_transform.setIdentity(); init_transform.setOrigin(btVector3(0, 4, 0)); - init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0)); + // init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0)); rsb->transform(init_transform); - // rsb->setTotalMass(0.5); rsb->setStiffnessScale(100); rsb->setDamping(damping_alpha, damping_beta); // set fixed nodes - // rsb->setFixedNodes(0); - // rsb->setFixedNodes(1); - // rsb->setFixedNodes(2); - // rsb->setFixedNodes(3); + rsb->setFixedNodes(0); + rsb->setFixedNodes(1); + rsb->setFixedNodes(2); + rsb->setFixedNodes(3); rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects rsb->m_cfg.kCHR = 1; // collision hardness with rigid body @@ -247,7 +245,7 @@ void BasicTest::initPhysics() // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); // rsb->setRigidVelocity(btVector3(0, 1, 0)); - rsb->setRigidAngularVelocity(btVector3(1, 0, 0)); + // rsb->setRigidAngularVelocity(btVector3(1, 0, 0)); // btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); // getDeformableDynamicsWorld()->addForce(rsb, gravity_force); @@ -255,11 +253,11 @@ void BasicTest::initPhysics() } getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); - getDeformableDynamicsWorld()->setUseProjection(true); + getDeformableDynamicsWorld()->setUseProjection(false); getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3; getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200); getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; - getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true; + getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false; getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; // add a few rigid bodies // Ctor_RbUpStack(); // TODO: no rigid body for now diff --git a/examples/ReducedDeformableDemo/FreeFall.cpp b/examples/ReducedDeformableDemo/FreeFall.cpp index 4d2173768..ee6ae089a 100644 --- a/examples/ReducedDeformableDemo/FreeFall.cpp +++ b/examples/ReducedDeformableDemo/FreeFall.cpp @@ -30,7 +30,7 @@ // static btScalar E = 50; // static btScalar nu = 0.3; static btScalar damping_alpha = 0.0; -static btScalar damping_beta = 0.0; +static btScalar damping_beta = 0.01; static btScalar COLLIDING_VELOCITY = 0; static int start_mode = 6; static int num_modes = 10; @@ -157,11 +157,11 @@ void FreeFall::initPhysics() btTransform init_transform; init_transform.setIdentity(); + // init_transform.setOrigin(btVector3(0, 2.5, 0)); init_transform.setOrigin(btVector3(0, 10, 0)); // init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0)); - // init_transform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0)); - // init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0)); - // init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, 0)); + // init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0)); + init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 2.0)); rsb->transform(init_transform); // rsb->setTotalMass(0.5); @@ -275,7 +275,7 @@ void FreeFall::initPhysics() getDeformableDynamicsWorld()->setLineSearch(false); getDeformableDynamicsWorld()->setUseProjection(false); getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2; - getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0; + getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.3; getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200); getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false; diff --git a/examples/ReducedDeformableDemo/ReducedGrasp.cpp b/examples/ReducedDeformableDemo/ReducedGrasp.cpp index 5c6461457..39780f835 100644 --- a/examples/ReducedDeformableDemo/ReducedGrasp.cpp +++ b/examples/ReducedDeformableDemo/ReducedGrasp.cpp @@ -30,10 +30,10 @@ // static btScalar E = 50; // static btScalar nu = 0.3; static btScalar damping_alpha = 0.0; -static btScalar damping_beta = 0.0; +static btScalar damping_beta = 0.01; static btScalar COLLIDING_VELOCITY = 4; static int start_mode = 6; -static int num_modes = 1; +static int num_modes = 10; class ReducedGrasp : public CommonDeformableBodyBase { @@ -287,8 +287,8 @@ void ReducedGrasp::initPhysics() btTransform init_transform; init_transform.setIdentity(); init_transform.setOrigin(btVector3(0, 4, 0)); - // init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0)); - init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0)); + init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0)); + // init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0)); rsb->transform(init_transform); rsb->setStiffnessScale(200); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp index f2ea2d343..85dece64c 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp @@ -11,33 +11,38 @@ btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint( : m_rsb(rsb), m_ri(ri), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal) { // get impulse - m_impulseFactor = rsb->getImpulseFactor(m_node->index); + m_impulseFactorInv = rsb->getImpulseFactor(m_node->index).inverse(); } btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { // target velocity of fixed constraint is 0 - btVector3 impulse = -(m_impulseFactor.inverse() * m_node->m_v); - - // apply full space impulse - std::cout << "node: " << m_node->index << " impulse: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n'; - // std::cout << "impulse norm: " << impulse.norm() << "\n"; - - m_rsb->applyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt); - - // get residual //TODO: only calculate the velocity of the given node - m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform()); + btVector3 deltaVa = getDeltaVa(); + btVector3 rel_vel = m_node->m_v + deltaVa; + btVector3 deltaImpulse = -(m_impulseFactorInv * rel_vel); + applyImpulse(deltaImpulse); // calculate residual - btScalar residualSquare = btDot(m_node->m_v, m_node->m_v); + btScalar residualSquare = btDot(rel_vel, rel_vel); return residualSquare; } -// this calls reduced deformable body's applyFullSpaceImpulse +// this calls reduced deformable body's internalApplyFullSpaceImpulse void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse) { - m_rsb->applyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt); + // apply full space impulse + std::cout << "node: " << m_node->index << " impulse: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n'; + m_rsb->internalApplyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt); + + // get the new nodal velocity + // m_node->m_v = m_rsb->computeNodeFullVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index); + // m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform()); +} + +btVector3 btReducedDeformableStaticConstraint::getDeltaVa() const +{ + return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index); } // ================= base contact constraints =================== diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h index fa672d789..aae716e99 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h @@ -7,7 +7,7 @@ class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint public: btReducedSoftBody* m_rsb; btScalar m_dt; - btMatrix3x3 m_impulseFactor; + btMatrix3x3 m_impulseFactorInv; btVector3 m_ri; btReducedDeformableStaticConstraint(btReducedSoftBody* rsb, @@ -24,6 +24,8 @@ class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint // this calls reduced deformable body's applyFullSpaceImpulse virtual void applyImpulse(const btVector3& impulse); + btVector3 getDeltaVa() const; + // virtual void applySplitImpulse(const btVector3& impulse) {} }; diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index ade7bc2cf..b14ade6dd 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -272,7 +272,7 @@ void btReducedSoftBody::updateReducedVelocity(btScalar solverdt, bool explicit_f btScalar delta_v = 0; if (explicit_force) { - delta_v = solverdt * mass_inv * m_reducedForceElastic[r]; + delta_v = solverdt * mass_inv * (m_reducedForceElastic[r] + m_reducedForceDamping[r]); } else { @@ -571,8 +571,8 @@ void btReducedSoftBody::internalApplyFullSpaceImpulse(const btVector3& impulse, // apply impulse force applyFullSpaceNodalForce(impulse / dt, n_node); - // update reduced internal force - applyReducedDampingForce(m_reducedVelocity); //TODO: this needs to be the current velocity + // update delta damping force + applyReducedDampingForce(m_internalDeltaReducedVelocity); // delta reduced velocity for (int r = 0; r < m_nReduced; ++r) diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index 701f9981b..92c7eca56 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -128,7 +128,6 @@ void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt) void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt) { - static bool applied = false; for (int i = 0; i < m_softBodies.size(); ++i) { btReducedSoftBody* rsb = static_cast(m_softBodies[i]); @@ -143,8 +142,6 @@ void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt) rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer); // get reduced velocity at time^* - // get reduced velocity at time^* - // get reduced velocity at time^* rsb->updateReducedVelocity(solverdt, true); } @@ -273,27 +270,12 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de { btReducedSoftBody* rsb = static_cast(m_softBodies[i]); - btAlignedObjectArray residual; - residual.resize(m_staticConstraints[i].size(), 0); - for (int k = 0; k < m_staticConstraints[i].size(); ++k) { btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][k]; btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); - - btVector3 error; - error.setZero(); - std::cout << "fixed_nodes: "; - for (int p = 0; p < rsb->m_fixedNodes.size(); ++p) - { - std::cout << rsb->m_nodes[rsb->m_fixedNodes[p]].m_v.norm() << '\t'; - error += rsb->m_nodes[rsb->m_fixedNodes[p]].m_v; - } - std::cout << '\n'; - std::cout << "norm: " << error.norm() << "\n"; } - } // handle contact constraint