From a37d23fa24a8e99e52872d2843b1619a5ad8a866 Mon Sep 17 00:00:00 2001 From: jingyuc Date: Fri, 30 Jul 2021 02:08:27 -0400 Subject: [PATCH] re-work the simulation time loop in order to support fixed constraints. still wip --- examples/ReducedDeformableDemo/BasicTest.cpp | 10 +- .../btReducedSoftBody.cpp | 107 +++++++++++++++--- .../BulletReducedSoftBody/btReducedSoftBody.h | 28 ++++- .../btReducedSoftBodySolver.cpp | 105 +++++++---------- .../btReducedSoftBodySolver.h | 15 ++- src/BulletSoftBody/btDeformableBodySolver.h | 6 +- .../btDeformableMultiBodyDynamicsWorld.cpp | 41 +++---- 7 files changed, 201 insertions(+), 111 deletions(-) diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 401b0fb63..420ee5925 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -95,9 +95,9 @@ public: btReducedSoftBody* rsb = static_cast(static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]); if (first_step /* && !rsb->m_bUpdateRtCst*/) { - // getDeformedShape(rsb, 0, 0.5); + getDeformedShape(rsb, 0, 0.5); first_step = false; - rsb->updateReducedDofs(); + rsb->mapToReducedDofs(); } float internalTimeStep = 1. / 60.f; @@ -138,7 +138,7 @@ void BasicTest::initPhysics() m_broadphase = new btDbvtBroadphase(); btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver(); reducedSoftBodySolver->setDamping(damping_alpha, damping_beta); - btVector3 gravity = btVector3(0, -9.8, 0); + btVector3 gravity = btVector3(0, 0, 0); reducedSoftBodySolver->setGravity(gravity); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); @@ -164,7 +164,7 @@ void BasicTest::initPhysics() rsb->translate(btVector3(0, 4, 0)); //TODO: add back translate and scale // rsb->setTotalMass(0.5); rsb->setStiffnessScale(1); - // rsb->setFixedNodes(); + rsb->setFixedNodes(); rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects rsb->m_cfg.kCHR = 1; // collision hardness with rigid body rsb->m_cfg.kDF = 0; @@ -175,7 +175,7 @@ void BasicTest::initPhysics() // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); // rsb->setRigidVelocity(btVector3(0, 1, 0)); - rsb->setRigidAngularVelocity(btVector3(10, 0, 0)); + // rsb->setRigidAngularVelocity(btVector3(1, 0, 0)); // btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); // getDeformableDynamicsWorld()->addForce(rsb, gravity_force); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index f9cb794c3..adaf0647c 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -34,7 +34,9 @@ void btReducedSoftBody::setReducedModes(int start_mode, int num_modes, int full_ m_nFull = full_size; m_reducedDofs.resize(m_nReduced, 0); m_reducedVelocity.resize(m_nReduced, 0); + m_reducedForce.resize(m_nReduced, 0); m_nodalMass.resize(full_size, 0); + endOfTimeStepZeroing(); } void btReducedSoftBody::setMassProps(const tDenseArray& mass_array) @@ -95,12 +97,20 @@ void btReducedSoftBody::setFixedNodes() m_fixedNodes.push_back(0); } +void btReducedSoftBody::endOfTimeStepZeroing() +{ + for (int i = 0; i < m_nReduced; ++i) + { + m_reducedForce[i] = 0; + } +} + void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform) { btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform); } -void btReducedSoftBody::updateReducedDofs() +void btReducedSoftBody::mapToReducedDofs() { btAssert(m_reducedDofs.size() == m_nReduced); for (int j = 0; j < m_nReduced; ++j) @@ -117,7 +127,23 @@ void btReducedSoftBody::updateReducedDofs() } } -void btReducedSoftBody::updateFullDofs() +void btReducedSoftBody::updateReducedDofs(btScalar solverdt) +{ + for (int r = 0; r < m_nReduced; ++r) + { + m_reducedDofs[r] += solverdt * m_reducedVelocity[r]; + } +} + +void btReducedSoftBody::updateFullDofs(btScalar solverdt) +{ + for (int i = 0; i < m_nFull; ++i) + { + m_nodes[i].m_x += solverdt * m_nodes[i].m_v; + } +} + +void btReducedSoftBody::mapToFullDofs() { btAssert(m_nodes.size() == m_nFull); TVStack delta_x; @@ -141,6 +167,51 @@ void btReducedSoftBody::updateFullDofs() } } +void btReducedSoftBody::updateReducedVelocity(btScalar solverdt) +{ + // 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_reducedForce[r]; + m_reducedVelocity[r] -= delta_v; //! check this + } +} + +void btReducedSoftBody::updateFullVelocity(btScalar solverdt) +{ + TVStack v_from_reduced; + v_from_reduced.resize(m_nFull); + // btVector3 current_com = m_rigidTransformWorld.getOrigin() - m_initialOrigin; + for (int i = 0; i < m_nFull; ++i) + { + btVector3 r = m_nodes[i].m_x - m_rigidTransformWorld.getOrigin(); + + // compute velocity contributed by the reduced velocity + for (int k = 0; k < 3; ++k) + { + v_from_reduced[i][k] = 0; + for (int r = 0; r < m_nReduced; ++r) + { + v_from_reduced[i][k] += m_modes[r][3 * i + k] * m_reducedVelocity[r]; + } + } + + // get new velocity + m_nodes[i].m_v = m_angularVelocity.cross(r) + + m_rigidTransformWorld.getBasis() * v_from_reduced[i] + + m_linearVelocity; + } +} + +void btReducedSoftBody::updateMeshNodePositions(btScalar solverdt) +{ + for (int i = 0; i < m_nFull; ++i) + { + m_nodes[i].m_x = solverdt * m_nodes[i].m_v; + } +} + void btReducedSoftBody::proceedToTransform(const btTransform& newTrans) { setCenterOfMassTransform(newTrans); @@ -215,19 +286,29 @@ void btReducedSoftBody::applyImpulse(const btVector3& impulse, const btVector3& } } -void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt, tDenseArray& reduced_force) +void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt) { + // zero the reduced force vector + endOfTimeStepZeroing(); + // impulse leads to the deformation in the reduced space btVector3 impulse = m_nodalMass[n_node] / dt * (target_vel - m_nodes[n_node].m_v); for (int i = 0; i < m_nReduced; ++i) { for (int k = 0; k < 3; ++k) { - reduced_force[i] += m_modes[i][3 * n_node + k] * impulse[k]; + m_reducedForce[i] = m_modes[i][3 * n_node + k] * impulse[k]; } } + // impulse causes rigid motion applyImpulse(impulse, m_nodes[n_node].m_x); + + // apply impulse to the reduced velocity + updateReducedVelocity(dt); + + // apply impulse force, update velocity + updateFullVelocity(dt); } void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt) @@ -235,27 +316,27 @@ void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt) // update rigid frame velocity m_linearVelocity += dt * gravity; - // update nodal velocity - for (int i = 0; i < m_nFull; ++i) - { - m_nodes[i].m_v = m_nodes[i].m_v + dt * gravity; - } + // // update nodal velocity + // for (int i = 0; i < m_nFull; ++i) + // { + // m_nodes[i].m_v += dt * gravity; + // } } -void btReducedSoftBody::applyReducedInternalForce(tDenseArray& reduced_force, const btScalar damping_alpha, const btScalar damping_beta) +void btReducedSoftBody::applyReducedInternalForce(const btScalar damping_alpha, const btScalar damping_beta) { for (int r = 0; r < m_nReduced; ++r) { - reduced_force[r] += m_ksScale * m_Kr[r] * (m_reducedDofs[r] + damping_beta * m_reducedVelocity[r]); + m_reducedForce[r] += m_ksScale * m_Kr[r] * (m_reducedDofs[r] + damping_beta * m_reducedVelocity[r]); } } -void btReducedSoftBody::applyFixedContraints(btScalar dt, tDenseArray& reduced_force) +void btReducedSoftBody::applyFixedContraints(btScalar dt) { for (int n = 0; n < m_fixedNodes.size(); ++n) { // std::cout << reduced_force[0] << "\t" << reduced_force[1] << "\n"; - applyFullSpaceImpulse(btVector3(0, 0, 0), m_fixedNodes[n], dt, reduced_force); + applyFullSpaceImpulse(btVector3(0, 0, 0), m_fixedNodes[n], dt); // std::cout << reduced_force[0] << "\t" << reduced_force[1] << "\n"; } } \ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index 59ae1bccf..08bc045eb 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -49,6 +49,7 @@ class btReducedSoftBody : public btSoftBody tDenseMatrix m_modes; // modes of the reduced deformable model. Each inner array is a mode, outer array size = n_modes tDenseArray m_reducedDofs; // Reduced degree of freedom tDenseArray m_reducedVelocity; // Reduced velocity array + tDenseArray m_reducedForce; // reduced 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? @@ -89,15 +90,32 @@ class btReducedSoftBody : public btSoftBody void predictIntegratedTransform(btScalar step, btTransform& predictedTransform); + void endOfTimeStepZeroing(); + // // reduced dof related // // compute reduced degree of freedoms - void updateReducedDofs(); + void updateReducedDofs(btScalar solverdt); + + // map to reduced degree of freedoms + void mapToReducedDofs(); // compute full degree of freedoms - void updateFullDofs(); + void updateFullDofs(btScalar solverdt); + + // map to full degree of freedoms + void mapToFullDofs(); + + // compute reduced velocity update + void updateReducedVelocity(btScalar solverdt); + + // compute full space velocity from the reduced velocity + void updateFullVelocity(btScalar solverdt); + + // update the full space mesh positions + void updateMeshNodePositions(btScalar solverdt); // // rigid motion related @@ -110,16 +128,16 @@ class btReducedSoftBody : public btSoftBody void applyImpulse(const btVector3& impulse, const btVector3& rel_pos); // apply impulse to nodes in the full space - void applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt, tDenseArray& reduced_force); + void applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt); // apply fixed contraints to the nodes - void applyFixedContraints(btScalar dt, tDenseArray& reduced_force); + void applyFixedContraints(btScalar dt); // apply gravity to the rigid frame void applyRigidGravity(const btVector3& gravity, btScalar dt); // apply reduced force - void applyReducedInternalForce(tDenseArray& reduced_force, const btScalar damping_alpha, const btScalar damping_beta); + void applyReducedInternalForce(const btScalar damping_alpha, const btScalar damping_beta); void proceedToTransform(const btTransform& newTrans); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index ec4a1d37b..feab9a938 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -21,93 +21,70 @@ void btReducedSoftBodySolver::setGravity(const btVector3& gravity) void btReducedSoftBodySolver::predictMotion(btScalar solverdt) { - applyExplicitForce(); + applyExplicitForce(solverdt); + + // predict new mesh location + predictReduceDeformableMotion(solverdt); } -void btReducedSoftBodySolver::applyForce() +void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt) { for (int i = 0; i < m_softBodies.size(); ++i) { btReducedSoftBody* rsb = static_cast(m_softBodies[i]); - // get reduced force - btAlignedObjectArray reduced_force; - reduced_force.resize(rsb->m_reducedDofs.size(), 0); + rsb->updateReducedVelocity(solverdt); + + // map reduced velocity to full space velocity + rsb->updateFullVelocity(solverdt); + + // update mesh nodal position + // rsb->updateMeshNodePositions(solverdt); + } +} + +void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt) +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedSoftBody* rsb = static_cast(m_softBodies[i]); + + // apply gravity to the rigid frame + rsb->applyRigidGravity(m_gravity, solverdt); // add internal force (elastic force & damping force) - rsb->applyReducedInternalForce(reduced_force, m_dampingAlpha, m_dampingBeta); - - // apply impulses to reduced deformable objects - static btScalar sim_time = 0; - static int apply_impulse = 0; - // if (rsb->m_reducedModel && apply_impulse < 4) - // { - // if (sim_time > 1 && apply_impulse == 0) - // { - // rsb->applyFullSpaceImpulse(btVector3(0, 1, 0), 0, 2.0 * m_dt, reduced_force); - // apply_impulse++; - // } - // if (sim_time > 2 && apply_impulse == 1) - // { - // rsb->applyFullSpaceImpulse(btVector3(0, -1.2, 0), 0, m_dt, reduced_force); - // apply_impulse++; - // } - // if (sim_time > 3 && apply_impulse == 2) - // { - // rsb->applyFullSpaceImpulse(btVector3(1.1, 0, 0), 0, m_dt, reduced_force); - // apply_impulse++; - // } - // if (sim_time > 4 && apply_impulse == 3) - // { - // rsb->applyFullSpaceImpulse(btVector3(-1, 0, 0), 0, 2.0 * m_dt, reduced_force); - // apply_impulse++; - // } - // } - - // apply fixed contraints - // rsb->applyFixedContraints(m_dt, reduced_force); //TODO: solver iteratively with other constraints - - // update reduced velocity - for (int r = 0; r < rsb->m_reducedDofs.size(); ++r) - { - btScalar mass_inv = (rsb->m_Mr[r] == 0) ? 0 : 1.0 / rsb->m_Mr[r]; - btScalar delta_v = m_dt * mass_inv * reduced_force[r]; - rsb->m_reducedVelocity[r] -= delta_v; - } + rsb->applyReducedInternalForce(m_dampingAlpha, m_dampingBeta); } } -void btReducedSoftBodySolver::applyExplicitForce() -{ - // apply gravity to the rigid frame - for (int i = 0; i < m_softBodies.size(); ++i) - { - btReducedSoftBody* rsb = static_cast(m_softBodies[i]); - rsb->applyRigidGravity(m_gravity, m_dt); - } - - // apply internal forces and impulses - applyForce(); -} - void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) { for (int i = 0; i < m_softBodies.size(); ++i) { btReducedSoftBody* rsb = static_cast(m_softBodies[i]); - for (int r = 0; r < rsb->m_reducedDofs.size(); ++r) - rsb->m_reducedDofs[r] += timeStep * rsb->m_reducedVelocity[r]; + // update reduced dofs for the next time step + rsb->updateReducedDofs(timeStep); // rigid motion btTransform predictedTrans; rsb->predictIntegratedTransform(timeStep, predictedTrans); - // std::cout << predictedTrans.getOrigin()[0] << '\t' << predictedTrans.getOrigin()[1] << '\t' << predictedTrans.getOrigin()[2] << '\n'; rsb->proceedToTransform(predictedTrans); - // std::cout << rsb->getWorldTransform().getOrigin()[0] << '\t' << rsb->getWorldTransform().getOrigin()[1] << '\t' << rsb->getWorldTransform().getOrigin()[2] << '\n'; - // std::cout << "----------\n"; - // map reduced dof back to full space - rsb->updateFullDofs(); + // update mesh nodal positions for the next time step + rsb->updateFullDofs(timeStep); + + // end of time step clean up + rsb->endOfTimeStepZeroing(); } +} + +void btReducedSoftBodySolver::solveConstraints(btScalar timeStep) +{ + // for (int i = 0; i < m_softBodies.size(); ++i) + // { + // btReducedSoftBody* rsb = static_cast(m_softBodies[i]); + + // rsb->applyFixedContraints(timeStep); + // } } \ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h index ea9e73e81..33fb28a5d 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h @@ -14,7 +14,9 @@ class btReducedSoftBodySolver : public btDeformableBodySolver btVector3 m_gravity; - void applyForce(); + void predictReduceDeformableMotion(btScalar solverdt); + + void applyExplicitForce(btScalar solverdt); public: btReducedSoftBodySolver(); @@ -31,10 +33,17 @@ class btReducedSoftBodySolver : public btDeformableBodySolver virtual void predictMotion(btScalar solverdt); - virtual void applyExplicitForce(); - virtual void applyTransforms(btScalar timeStep); + virtual void solveConstraints(btScalar timeStep); + + // virtual void setProjection() {} + + // virtual void setLagrangeMultiplier() {} + + // virtual void setupDeformableSolve(bool implicit); + + // virtual void solveDeformableConstraints(btScalar solverdt); }; #endif // BT_REDUCED_SOFT_BODY_DYNAMICS_WORLD_H \ No newline at end of file diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 7049f1992..d8b47ea4e 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -85,7 +85,7 @@ public: void backupVelocity(); // set m_dv and m_backupVelocity to desired value to prepare for momentum solve - void setupDeformableSolve(bool implicit); + virtual void setupDeformableSolve(bool implicit); // set the current velocity to that backed up in m_backupVelocity void revertVelocity(); @@ -199,9 +199,11 @@ public: m_objective->m_projection.setLagrangeMultiplier(); } + virtual void solveConstraints(btScalar timeStep) {} + // unused functions virtual void optimize(btAlignedObjectArray& softBodies, bool forceUpdate = false) {} - virtual void solveConstraints(btScalar dt) {} + // virtual void solveConstraints(btScalar dt) {} virtual bool checkInitialized() { return true; } virtual void copyBackToSoftBodies(bool bMove = true) {} }; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 1f3922a04..3fe69031b 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -302,30 +302,33 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) { - BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints"); - // save v_{n+1}^* velocity after explicit forces - m_deformableBodySolver->backupVelocity(); + // BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints"); + // // save v_{n+1}^* velocity after explicit forces + // m_deformableBodySolver->backupVelocity(); - // set up constraints among multibodies and between multibodies and deformable bodies - setupConstraints(); + // // set up constraints among multibodies and between multibodies and deformable bodies + // setupConstraints(); - // solve contact constraints - solveContactConstraints(); + // // solve contact constraints + // solveContactConstraints(); - // set up the directions in which the velocity does not change in the momentum solve - if (m_useProjection) - m_deformableBodySolver->setProjection(); - else - m_deformableBodySolver->setLagrangeMultiplier(); + // // set up the directions in which the velocity does not change in the momentum solve + // if (m_useProjection) + // m_deformableBodySolver->setProjection(); + // else + // m_deformableBodySolver->setLagrangeMultiplier(); - // for explicit scheme, m_backupVelocity = v_{n+1}^* - // for implicit scheme, m_backupVelocity = v_n - // Here, set dv = v_{n+1} - v_n for nodes in contact - m_deformableBodySolver->setupDeformableSolve(m_implicit); + // // for explicit scheme, m_backupVelocity = v_{n+1}^* + // // for implicit scheme, m_backupVelocity = v_n + // // Here, set dv = v_{n+1} - v_n for nodes in contact + // m_deformableBodySolver->setupDeformableSolve(m_implicit); - // At this point, dv should be golden for nodes in contact - // proceed to solve deformable momentum equation - m_deformableBodySolver->solveDeformableConstraints(timeStep); + // // At this point, dv should be golden for nodes in contact + // // proceed to solve deformable momentum equation + // m_deformableBodySolver->solveDeformableConstraints(timeStep); + + // TODO: need better design + m_deformableBodySolver->solveConstraints(timeStep); } void btDeformableMultiBodyDynamicsWorld::setupConstraints()