diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index c912b8a47..00d143c7f 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -157,7 +157,6 @@ void BasicTest::initPhysics() m_broadphase = new btDbvtBroadphase(); btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver(); - reducedSoftBodySolver->setDamping(damping_alpha, damping_beta); btVector3 gravity = btVector3(0, -10, 0); reducedSoftBodySolver->setGravity(gravity); @@ -183,7 +182,8 @@ 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(0.5); + rsb->setStiffnessScale(10); + rsb->setDamping(damping_alpha, damping_beta); // set fixed nodes rsb->setFixedNodes(0); diff --git a/examples/ReducedDeformableDemo/FreeFall.cpp b/examples/ReducedDeformableDemo/FreeFall.cpp index d44f750ba..2ddea4e98 100644 --- a/examples/ReducedDeformableDemo/FreeFall.cpp +++ b/examples/ReducedDeformableDemo/FreeFall.cpp @@ -134,7 +134,6 @@ void FreeFall::initPhysics() m_broadphase = new btDbvtBroadphase(); btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver(); - reducedSoftBodySolver->setDamping(damping_alpha, damping_beta); btVector3 gravity = btVector3(0, 0, 0); reducedSoftBodySolver->setGravity(gravity); @@ -161,6 +160,7 @@ void FreeFall::initPhysics() rsb->translate(btVector3(0, 0.1, 0)); //TODO: add back translate and scale // rsb->setTotalMass(0.5); rsb->setStiffnessScale(0.5); + rsb->setDamping(damping_alpha, damping_beta); // no fixed nodes // rsb->setFixedNodes(0); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp index 210557d01..292a4cf52 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp @@ -22,7 +22,13 @@ btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSol // apply full space impulse applyImpulse(impulse); - return 0; + // get residual //TODO: only calculate the velocity of the given node + m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform()); + + // calculate residual + btScalar residualSquare = btDot(m_node->m_v, m_node->m_v); + + return residualSquare; } // this calls reduced deformable body's applyFullSpaceImpulse diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index db944d2e5..4745f9aba 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -29,6 +29,10 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_co m_linearDamping = 0; m_angularDamping = 0; + // Rayleigh damping + m_dampingAlpha = 0; + m_dampingBeta = 0; + m_rigidTransformWorld.setIdentity(); } @@ -40,6 +44,7 @@ void btReducedSoftBody::setReducedModes(int start_mode, int num_modes, int full_ m_reducedDofs.resize(m_nReduced, 0); 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_reducedForceExternal.resize(m_nReduced, 0); m_nodalMass.resize(full_size, 0); @@ -125,6 +130,12 @@ void btReducedSoftBody::setFixedNodes(const int n_node) m_nodes[n_node].m_im = 0; // set inverse mass to be zero for the constraint solver. } +void btReducedSoftBody::setDamping(const btScalar alpha, const btScalar beta) +{ + m_dampingAlpha = alpha; + m_dampingBeta = beta; +} + void btReducedSoftBody::internalInitialization() { // zeroing @@ -219,6 +230,7 @@ void btReducedSoftBody::endOfTimeStepZeroing() m_reducedForceInternal[i] = 0; m_reducedForceExternal[i] = 0; m_reducedDofsBuffer[i] = m_reducedDofs[i]; + m_reducedVelocityBuffer[i] = m_reducedVelocity[i]; } // std::cout << "zeroed!\n"; } @@ -255,8 +267,7 @@ void btReducedSoftBody::updateReducedVelocity(btScalar solverdt) { 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]); - // std::cout << mass_inv << '\t' << delta_v << '\t' << m_reducedForce[r] << '\n'; - m_reducedVelocity[r] += delta_v; + m_reducedVelocity[r] = m_reducedVelocityBuffer[r] + delta_v; } } @@ -287,16 +298,16 @@ void btReducedSoftBody::mapToFullVelocity(const btTransform& ref_trans) 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; } - else - { - btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, m_interpolationWorldTransform); - m_interpolateInvInertiaTensorWorld = m_interpolationWorldTransform.getBasis().scaled(m_invInertiaLocal) * m_interpolationWorldTransform.getBasis().transpose(); - } } void btReducedSoftBody::translate(const btVector3& trs) @@ -460,18 +471,21 @@ void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& impulse, const bt // apply impulse force applyFullSpaceNodalForce(impulse / dt, n_node); + // update reduced internal force + applyReducedInternalForce(m_reducedDofs, m_reducedVelocity); + // update reduced velocity updateReducedVelocity(dt); // TODO: add back - // update reduced dofs - updateReducedDofs(dt); + // // update reduced dofs + // updateReducedDofs(dt); - // internal force - // applyReducedInternalForce(0, 0.01); // TODO: this should be necessary, but no obvious effects. Check again. + // // internal force + // // applyReducedInternalForce(0, 0.01); // TODO: this should be necessary, but no obvious effects. Check again. - // update local moment arm - updateLocalMomentArm(); - updateExternalForceProjectMatrix(true); + // // update local moment arm + // updateLocalMomentArm(); + // updateExternalForceProjectMatrix(true); // impulse causes rigid motion applyRigidImpulse(impulse, rel_pos); @@ -503,11 +517,11 @@ void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt) m_linearVelocity += dt * gravity; } -void btReducedSoftBody::applyReducedInternalForce(const btScalar damping_alpha, const btScalar damping_beta) +void btReducedSoftBody::applyReducedInternalForce(const tDenseArray& reduce_dofs, const tDenseArray& reduce_vel) { for (int r = 0; r < m_nReduced; ++r) { - m_reducedForceInternal[r] = - m_ksScale * m_Kr[r] * (m_reducedDofs[r] + damping_beta * m_reducedVelocity[r]); + m_reducedForceInternal[r] = - m_ksScale * m_Kr[r] * (reduce_dofs[r] + m_dampingBeta * reduce_vel[r]); } } diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index 681754920..40e309c46 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -49,6 +49,10 @@ class btReducedSoftBody : public btSoftBody btMatrix3x3 m_interpolateInvInertiaTensorWorld; btVector3 m_initialOrigin; // initial center of mass (original of the m_rigidTransformWorld) + // damping + btScalar m_dampingAlpha; + btScalar m_dampingBeta; + public: // @@ -63,6 +67,7 @@ class btReducedSoftBody : public btSoftBody tDenseArray m_reducedDofs; // Reduced degree of freedom tDenseArray m_reducedDofsBuffer; // Reduced degree of freedom at t^n 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_eigenvalues; // eigenvalues of the reduce deformable model @@ -104,6 +109,8 @@ class btReducedSoftBody : public btSoftBody void setFixedNodes(const int n_node); + void setDamping(const btScalar alpha, const btScalar beta); + // // various internal updates // @@ -169,7 +176,7 @@ class btReducedSoftBody : public btSoftBody void applyRigidGravity(const btVector3& gravity, btScalar dt); // apply reduced force - void applyReducedInternalForce(const btScalar damping_alpha, const btScalar damping_beta); + void applyReducedInternalForce(const tDenseArray& reduce_dofs, 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 aa21d4a47..1807c4652 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -5,16 +5,9 @@ btReducedSoftBodySolver::btReducedSoftBodySolver() { m_dampingAlpha = 0; m_dampingBeta = 0; - m_simTime = 0; m_gravity = btVector3(0, 0, 0); } -void btReducedSoftBodySolver::setDamping(btScalar alpha, btScalar beta) -{ - m_dampingAlpha = alpha; - m_dampingBeta = beta; -} - void btReducedSoftBodySolver::setGravity(const btVector3& gravity) { m_gravity = gravity; @@ -79,6 +72,10 @@ void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt) for (int i = 0; i < m_softBodies.size(); ++i) { btReducedSoftBody* rsb = static_cast(m_softBodies[i]); + if (!rsb->isActive()) + { + continue; + } // clear contacts variables rsb->m_nodeRigidContacts.resize(0); @@ -86,37 +83,28 @@ void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt) rsb->m_faceNodeContacts.resize(0); // calculate inverse mass matrix for all nodes - if (rsb->isActive()) + for (int j = 0; j < rsb->m_nodes.size(); ++j) { - for (int j = 0; j < rsb->m_nodes.size(); ++j) + if (rsb->m_nodes[j].m_im > 0) { - if (rsb->m_nodes[j].m_im > 0) - { - rsb->m_nodes[j].m_effectiveMass_inv = rsb->m_nodes[j].m_effectiveMass.inverse(); - } + rsb->m_nodes[j].m_effectiveMass_inv = rsb->m_nodes[j].m_effectiveMass.inverse(); } } - // apply damping - rsb->applyDamping(solverdt); - - // rigid motion + // rigid motion: t, R at time^* rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform()); - // update reduced velocity and dofs - rsb->updateReducedVelocity(solverdt); - - // update reduced dofs + // update reduced dofs at time^* rsb->updateReducedDofs(solverdt); - // update local moment arm + // update local moment arm at time^* rsb->updateLocalMomentArm(); rsb->updateExternalForceProjectMatrix(true); - // predict full space velocity (needed for constraints) + // predict full space velocity at time^* (needed for constraints) rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform()); - // update full space nodal position + // update full space nodal position at time^* rsb->mapToFullPosition(rsb->getInterpolationWorldTransform()); // update bounding box @@ -128,17 +116,6 @@ void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt) { rsb->updateFaceTree(true, true); } - - // std::cout << "bounds\n"; - // std::cout << rsb->m_bounds[0][0] << '\t' << rsb->m_bounds[0][1] << '\t' << rsb->m_bounds[0][2] << '\n'; - // std::cout << rsb->m_bounds[1][0] << '\t' << rsb->m_bounds[1][1] << '\t' << rsb->m_bounds[1][2] << '\n'; - - - // apply fixed constraints - // rsb->applyFixedContraints(solverdt); - - // TODO: update mesh nodal position. need it for collision - // rsb->updateMeshNodePositions(solverdt); } } @@ -149,18 +126,17 @@ void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt) { btReducedSoftBody* rsb = static_cast(m_softBodies[i]); - // apply gravity to the rigid frame + // apply gravity to the rigid frame, get m_linearVelocity at time^* rsb->applyRigidGravity(m_gravity, solverdt); // add internal force (elastic force & damping force) - rsb->applyReducedInternalForce(m_dampingAlpha, m_dampingBeta); + rsb->applyReducedInternalForce(rsb->m_reducedDofsBuffer, rsb->m_reducedVelocityBuffer); - // apply external force or impulses - // if (!applied && m_simTime > 2) - // { - // rsb->applyFullSpaceImpulse(btVector3(0, -5, 0), 0, solverdt); - // applied = true; - // } + // get reduced velocity at time^* + rsb->updateReducedVelocity(solverdt); + + // apply damping (no need at this point) + // rsb->applyDamping(solverdt); } } @@ -170,19 +146,25 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) { btReducedSoftBody* rsb = static_cast(m_softBodies[i]); - // update reduced dofs for the next time step - // rsb->updateReducedDofs(timeStep); // TODO: add back - // rigid motion rsb->proceedToTransform(timeStep, true); - // update mesh nodal positions for the next time step + // update reduced dofs for time^n+1 + rsb->updateReducedDofs(timeStep); + + // update local moment arm for time^n+1 + rsb->updateLocalMomentArm(); + rsb->updateExternalForceProjectMatrix(true); + + // update mesh nodal positions for time^n+1 rsb->mapToFullPosition(rsb->getRigidTransform()); + // update mesh nodal velocity + rsb->mapToFullVelocity(rsb->getRigidTransform()); + // end of time step clean up and update rsb->endOfTimeStepZeroing(); } - m_simTime += timeStep; } void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) @@ -198,28 +180,29 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob // set fixed constraints for (int j = 0; j < rsb->m_fixedNodes.size(); ++j) { - if (rsb->m_nodes[j].m_im == 0) + int i_node = rsb->m_fixedNodes[j]; + if (rsb->m_nodes[i_node].m_im == 0) { - btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[rsb->m_fixedNodes[j]], rsb->getRelativePos(rsb->m_fixedNodes[j]), infoGlobal, m_dt); + btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[i_node], rsb->getRelativePos(i_node), infoGlobal, m_dt); m_staticConstraints[i].push_back(static_constraint); } } btAssert(rsb->m_fixedNodes.size() == m_staticConstraints[i].size()); - // set Deformable Node vs. Rigid constraint - for (int j = 0; j < rsb->m_nodeRigidContacts.size(); ++j) - { - const btSoftBody::DeformableNodeRigidContact& contact = rsb->m_nodeRigidContacts[j]; - // skip fixed points - if (contact.m_node->m_im == 0) - { - continue; - } - btReducedDeformableNodeRigidContactConstraint constraint(rsb, contact, infoGlobal, m_dt); - m_nodeRigidConstraints[i].push_back(constraint); - rsb->m_contactNodesList.push_back(contact.m_node->index); - } - std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n"; + // set Deformable Node vs. Rigid constraint //TODO: add back contact + // for (int j = 0; j < rsb->m_nodeRigidContacts.size(); ++j) + // { + // const btSoftBody::DeformableNodeRigidContact& contact = rsb->m_nodeRigidContacts[j]; + // // skip fixed points + // if (contact.m_node->m_im == 0) + // { + // continue; + // } + // btReducedDeformableNodeRigidContactConstraint constraint(rsb, contact, infoGlobal, m_dt); + // m_nodeRigidConstraints[i].push_back(constraint); + // rsb->m_contactNodesList.push_back(contact.m_node->index); + // } + // std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n"; // set Deformable Face vs. Rigid constraint // for (int j = 0; j < rsb->m_faceRigidContacts.size(); ++j) diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h index 29f67b85f..64410c940 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h @@ -10,7 +10,6 @@ class btReducedSoftBody; class btReducedSoftBodySolver : public btDeformableBodySolver { protected: - btScalar m_simTime; btScalar m_dampingAlpha; btScalar m_dampingBeta; @@ -28,8 +27,6 @@ class btReducedSoftBodySolver : public btDeformableBodySolver btReducedSoftBodySolver(); ~btReducedSoftBodySolver() {} - void setDamping(btScalar alpha, btScalar beta); - void setGravity(const btVector3& gravity); virtual SolverTypes getSolverType() const