From eacdbe67a043e5171fcd7b1ee60b9309c7bd063a Mon Sep 17 00:00:00 2001 From: jingyuc Date: Mon, 16 Aug 2021 14:36:35 -0400 Subject: [PATCH] reduced velocity should be updated from i-th iteration instead of from the predicted state, because thexternal force has no memory of the previous iteration --- examples/ReducedDeformableDemo/BasicTest.cpp | 6 +- .../btReducedDeformableContactConstraint.cpp | 5 +- .../btReducedSoftBody.cpp | 63 +++++++++++++------ .../BulletReducedSoftBody/btReducedSoftBody.h | 12 ++-- .../btReducedSoftBodySolver.cpp | 55 ++++++++-------- .../btDeformableMultiBodyConstraintSolver.cpp | 5 ++ 6 files changed, 92 insertions(+), 54 deletions(-) diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 00d143c7f..1b925d356 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -33,7 +33,7 @@ static btScalar damping_alpha = 0.0; static btScalar damping_beta = 0.01; static btScalar COLLIDING_VELOCITY = 0; static int start_mode = 6; -static int num_modes = 4; +static int num_modes = 1; class BasicTest : public CommonDeformableBodyBase { @@ -182,7 +182,7 @@ void BasicTest::initPhysics() // rsb->scale(btVector3(1, 1, 1)); //TODO: add back scale rsb->translate(btVector3(0, 4, 0)); // rsb->setTotalMass(0.5); - rsb->setStiffnessScale(10); + rsb->setStiffnessScale(100); rsb->setDamping(damping_alpha, damping_beta); // set fixed nodes @@ -212,7 +212,7 @@ void BasicTest::initPhysics() getDeformableDynamicsWorld()->setUseProjection(true); getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3; getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200); - getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; + getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-6; getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true; getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; // add a few rigid bodies diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp index 292a4cf52..17d49a63a 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp @@ -20,7 +20,10 @@ btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSol btVector3 impulse = -(m_impulseFactor.inverse() * m_node->m_v); // apply full space impulse - applyImpulse(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()); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index 4745f9aba..d6718c77c 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -45,7 +45,8 @@ void btReducedSoftBody::setReducedModes(int start_mode, int num_modes, int full_ m_reducedDofsBuffer.resize(m_nReduced, 0); m_reducedVelocity.resize(m_nReduced, 0); m_reducedVelocityBuffer.resize(m_nReduced, 0); - m_reducedForceInternal.resize(m_nReduced, 0); + m_reducedForceElastic.resize(m_nReduced, 0); + m_reducedForceDamping.resize(m_nReduced, 0); m_reducedForceExternal.resize(m_nReduced, 0); m_nodalMass.resize(full_size, 0); m_localMomentArm.resize(m_nFull); @@ -227,7 +228,8 @@ void btReducedSoftBody::endOfTimeStepZeroing() { for (int i = 0; i < m_nReduced; ++i) { - m_reducedForceInternal[i] = 0; + m_reducedForceElastic[i] = 0; + m_reducedForceDamping[i] = 0; m_reducedForceExternal[i] = 0; m_reducedDofsBuffer[i] = m_reducedDofs[i]; m_reducedVelocityBuffer[i] = m_reducedVelocity[i]; @@ -260,15 +262,27 @@ void btReducedSoftBody::mapToFullPosition(const btTransform& ref_trans) } } -void btReducedSoftBody::updateReducedVelocity(btScalar solverdt) +void btReducedSoftBody::updateReducedVelocity(btScalar solverdt, bool explicit_force) { // update reduced velocity for (int r = 0; r < m_nReduced; ++r) { btScalar mass_inv = (m_Mr[r] == 0) ? 0 : 1.0 / m_Mr[r]; // TODO: this might be redundant, because Mr is identity - btScalar delta_v = solverdt * mass_inv * (m_reducedForceInternal[r] + m_reducedForceExternal[r]); - m_reducedVelocity[r] = m_reducedVelocityBuffer[r] + delta_v; + btScalar delta_v = 0; + if (explicit_force) + { + delta_v = solverdt * mass_inv * m_reducedForceElastic[r]; + } + else + { + delta_v = solverdt * mass_inv * (m_reducedForceDamping[r] + m_reducedForceExternal[r]); + } + // delta_v = solverdt * mass_inv * (m_reducedForceElastic[r] + m_reducedForceDamping[r] + m_reducedForceExternal[r]); + std::cout << "delta_v: " << delta_v << '\n'; + // m_reducedVelocity[r] = m_reducedVelocityBuffer[r] + delta_v; + m_reducedVelocity[r] += delta_v; } + std::cout << "force: " << m_reducedForceElastic[0] << '\t' << m_reducedForceExternal[0] << '\n'; } void btReducedSoftBody::mapToFullVelocity(const btTransform& ref_trans) @@ -294,20 +308,20 @@ void btReducedSoftBody::mapToFullVelocity(const btTransform& ref_trans) ref_trans.getBasis() * v_from_reduced[i] + m_linearVelocity; } + + // std::cout << "full space vel: \n"; + // for (int i = 0; i < 4; ++i) + // { + // std::cout << m_nodes[i].m_v[0] << '\t' << m_nodes[i].m_v[1] << '\t' << m_nodes[i].m_v[2] << '\n'; + // } } void btReducedSoftBody::proceedToTransform(btScalar dt, bool end_of_time_step) { - // else - // { - btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, m_interpolationWorldTransform); - m_interpolateInvInertiaTensorWorld = m_interpolationWorldTransform.getBasis().scaled(m_invInertiaLocal) * m_interpolationWorldTransform.getBasis().transpose(); - // } - if (end_of_time_step) - { - m_rigidTransformWorld = m_interpolationWorldTransform; - m_invInertiaTensorWorld = m_interpolateInvInertiaTensorWorld; - } + btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, m_interpolationWorldTransform); + m_interpolateInvInertiaTensorWorld = m_interpolationWorldTransform.getBasis().scaled(m_invInertiaLocal) * m_interpolationWorldTransform.getBasis().transpose(); + m_rigidTransformWorld = m_interpolationWorldTransform; + m_invInertiaTensorWorld = m_interpolateInvInertiaTensorWorld; } void btReducedSoftBody::translate(const btVector3& trs) @@ -472,7 +486,7 @@ void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& impulse, const bt applyFullSpaceNodalForce(impulse / dt, n_node); // update reduced internal force - applyReducedInternalForce(m_reducedDofs, m_reducedVelocity); + applyReducedDampingForce(m_reducedVelocity); // update reduced velocity updateReducedVelocity(dt); // TODO: add back @@ -493,8 +507,9 @@ void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& impulse, const bt void btReducedSoftBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_node) { - // f_local = R^-1 * f_ext - btVector3 f_local = m_rigidTransformWorld.getBasis().transpose() * f_ext; + // f_local = R^-1 * f_ext //TODO: interpoalted transfrom + // btVector3 f_local = m_rigidTransformWorld.getBasis().transpose() * f_ext; + btVector3 f_local = m_interpolationWorldTransform.getBasis().transpose() * f_ext; // f_ext_r = [S^T * P]_{n_node} * f_local tDenseArray f_ext_r; @@ -517,11 +532,19 @@ void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt) m_linearVelocity += dt * gravity; } -void btReducedSoftBody::applyReducedInternalForce(const tDenseArray& reduce_dofs, const tDenseArray& reduce_vel) +void btReducedSoftBody::applyReducedElasticForce(const tDenseArray& reduce_dofs) { for (int r = 0; r < m_nReduced; ++r) { - m_reducedForceInternal[r] = - m_ksScale * m_Kr[r] * (reduce_dofs[r] + m_dampingBeta * reduce_vel[r]); + m_reducedForceElastic[r] = - m_ksScale * m_Kr[r] * reduce_dofs[r]; + } +} + +void btReducedSoftBody::applyReducedDampingForce(const tDenseArray& reduce_vel) +{ + for (int r = 0; r < m_nReduced; ++r) + { + m_reducedForceDamping[r] = - m_dampingBeta * m_Kr[r] * reduce_vel[r]; } } diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index 40e309c46..a40725b8b 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -69,7 +69,8 @@ class btReducedSoftBody : public btSoftBody tDenseArray m_reducedVelocity; // Reduced velocity array tDenseArray m_reducedVelocityBuffer; // Reduced velocity array at t^n tDenseArray m_reducedForceExternal; // reduced external force - tDenseArray m_reducedForceInternal; // reduced internal force + tDenseArray m_reducedForceElastic; // reduced internal elastic force + tDenseArray m_reducedForceDamping; // reduced internal damping force tDenseArray m_eigenvalues; // eigenvalues of the reduce deformable model tDenseArray m_Kr; // reduced stiffness matrix tDenseArray m_Mr; // reduced mass matrix //TODO: do we need this? @@ -137,7 +138,7 @@ class btReducedSoftBody : public btSoftBody void updateReducedDofs(btScalar solverdt); // compute reduced velocity update - void updateReducedVelocity(btScalar solverdt); + void updateReducedVelocity(btScalar solverdt, bool explicit_force = false); // map to full degree of freedoms void mapToFullPosition(const btTransform& ref_trans); @@ -175,8 +176,11 @@ class btReducedSoftBody : public btSoftBody // apply gravity to the rigid frame void applyRigidGravity(const btVector3& gravity, btScalar dt); - // apply reduced force - void applyReducedInternalForce(const tDenseArray& reduce_dofs, const tDenseArray& reduce_vel); + // apply reduced elastic force + void applyReducedElasticForce(const tDenseArray& reduce_dofs); + + // apply reduced damping force + void applyReducedDampingForce(const tDenseArray& reduce_vel); // calculate the impulse factor virtual btMatrix3x3 getImpulseFactor(int n_node); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index 1807c4652..efc05f8e5 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -130,10 +130,11 @@ void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt) rsb->applyRigidGravity(m_gravity, solverdt); // add internal force (elastic force & damping force) - rsb->applyReducedInternalForce(rsb->m_reducedDofsBuffer, rsb->m_reducedVelocityBuffer); + rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer); + // rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer); // get reduced velocity at time^* - rsb->updateReducedVelocity(solverdt); + rsb->updateReducedVelocity(solverdt, true); // apply damping (no need at this point) // rsb->applyDamping(solverdt); @@ -165,6 +166,8 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) // end of time step clean up and update rsb->endOfTimeStepZeroing(); } + + // exit(100); } void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) @@ -236,30 +239,30 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de } // handle contact constraint - for (int i = 0; i < numDeformableBodies; ++i) - { - for (int j = 0; j < m_softBodies.size(); ++j) - { - btReducedSoftBody* rsb = static_cast(m_softBodies[i]); - if (rsb != deformableBodies[i]) - { - continue; - } + // for (int i = 0; i < numDeformableBodies; ++i) + // { + // for (int j = 0; j < m_softBodies.size(); ++j) + // { + // btReducedSoftBody* rsb = static_cast(m_softBodies[i]); + // if (rsb != deformableBodies[i]) + // { + // continue; + // } - // node vs rigid contact - for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) - { - btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; - btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); - residualSquare = btMax(residualSquare, localResidualSquare); - } - // for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) - // { - // btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; - // btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); - // residualSquare = btMax(residualSquare, localResidualSquare); - // } - } - } + // // node vs rigid contact + // for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) + // { + // btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; + // btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); + // residualSquare = btMax(residualSquare, localResidualSquare); + // } + // // for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) + // // { + // // btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; + // // btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); + // // residualSquare = btMax(residualSquare, localResidualSquare); + // // } + // } + // } return residualSquare; } \ No newline at end of file diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 631fd5fbe..851cab761 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -37,6 +37,11 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b // solver body velocity <- rigid body velocity writeToSolverBody(bodies, numBodies, infoGlobal); + std::cout << "iter: " << iteration << "\tres: " << m_leastSquaresResidual << '\n'; + std::cout << "------------------\n"; + // std::cout << infoGlobal.m_leastSquaresResidualThreshold << '\n'; + // std::cout << maxIterations << '\n'; + if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) { #ifdef VERBOSE_RESIDUAL_PRINTF