From 4f66d8bb87d1d23140b29c5da54aa81832fdd2fc Mon Sep 17 00:00:00 2001 From: jingyuc Date: Fri, 6 Aug 2021 17:10:58 -0400 Subject: [PATCH] update reduced variables and full space position every time applied the force --- examples/ReducedDeformableDemo/BasicTest.cpp | 6 ++-- .../btReducedSoftBody.cpp | 33 +++++++++++++++---- .../BulletReducedSoftBody/btReducedSoftBody.h | 2 +- .../btReducedSoftBodySolver.cpp | 16 ++++++--- 4 files changed, 43 insertions(+), 14 deletions(-) diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 2e61186fe..3ea820a26 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -32,10 +32,10 @@ // static btScalar damping_alpha = 0.1; // static btScalar damping_beta = 0.01; static btScalar damping_alpha = 0.0; -static btScalar damping_beta = 0.01; +static btScalar damping_beta = 0.0; static btScalar COLLIDING_VELOCITY = 0; static int start_mode = 6; -static int num_modes = 2; +static int num_modes = 1; class BasicTest : public CommonDeformableBodyBase { @@ -107,6 +107,7 @@ public: // } float internalTimeStep = 1. / 60.f; + // float internalTimeStep = 1e-4; m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); } @@ -136,6 +137,7 @@ public: for (int p = 0; p < rsb->m_fixedNodes.size(); ++p) { deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0)); + // std::cout << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[0] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[1] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[2] << "\n"; } deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1)); deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 2, 0), 0.1, btVector3(1, 1, 1)); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index fa0b66b65..0a1634017 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -126,6 +126,9 @@ void btReducedSoftBody::setFixedNodes() // m_fixedNodes.push_back(1); // m_fixedNodes.push_back(2); // m_fixedNodes.push_back(3); + + // m_fixedNodes.push_back(15); + // m_fixedNodes.push_back(18); } void btReducedSoftBody::internalInitialization() @@ -426,8 +429,10 @@ void btReducedSoftBody::applyVelocityConstraint(const btVector3& target_vel, int btMatrix3x3 K2 = RSARinv + ri_skew * m_interpolateInvInertiaTensorWorld * sum_multiply_A * rotation.transpose(); // get impulse - btMatrix3x3 impulse_factor = K1 + K2; + btMatrix3x3 impulse_factor = K1 + K2; //TODO: add back + // btMatrix3x3 impulse_factor = K1; btVector3 impulse = impulse_factor.inverse() * (target_vel - m_nodes[n_node].m_v); + std::cout << "impulse: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n'; // apply full space impulse applyFullSpaceImpulse(impulse, ri, n_node, dt); @@ -449,6 +454,16 @@ void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& impulse, const bt // update reduced velocity updateReducedVelocity(dt); // TODO: add back + // update reduced dofs + updateReducedDofs(dt); + + // update local moment arm + updateLocalMomentArm(); + updateExternalForceProjectMatrix(true); + + // std::cout << "vel: " << m_reducedVelocity[0] << "\t" << m_reducedVelocity[1] << "\n"; + // std::cout << "dofs:" << m_reducedDofs[0] << "\t" << m_reducedDofs[1] << "\n"; + // impulse causes rigid motion applyRigidImpulse(impulse, rel_pos); } @@ -457,6 +472,7 @@ void btReducedSoftBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_n { // f_local = R^-1 * f_ext btVector3 f_local = m_rigidTransformWorld.getBasis().transpose() * f_ext; + std::cout << "f_ext: " << f_ext[0] << '\t' << f_ext[1] << '\t' << f_ext[2] << '\n'; // f_scaled = localInvInertia * (r_k x f_local) // btVector3 rk_cross_f_local = m_localMomentArm[n_node].cross(f_local); @@ -471,18 +487,21 @@ void btReducedSoftBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_n f_ext_r.resize(m_nReduced, 0); for (int r = 0; r < m_nReduced; ++r) { + std::cout << "reduced_f before: " << m_reducedForce[r] << '\n'; for (int k = 0; k < 3; ++k) { f_ext_r[r] += (m_projPA[r][3 * n_node + k] + m_projCq[r][3 * n_node + k]) * f_local[k]; + // f_ext_r[r] += m_modes[r][3 * n_node + k] * f_local[k]; } - std::cout << "projPA: " << m_projPA[r][0] << '\t' << m_projPA[r][1] << '\t' << m_projPA[r][2] << '\n'; - std::cout << "projCq: " << m_projCq[r][0] << '\t' << m_projCq[r][1] << '\t' << m_projCq[r][2] << '\n'; + // std::cout << "projPA: " << m_projPA[r][0] << '\t' << m_projPA[r][1] << '\t' << m_projPA[r][2] << '\n'; + // std::cout << "projCq: " << m_projCq[r][0] << '\t' << m_projCq[r][1] << '\t' << m_projCq[r][2] << '\n'; - std::cout << "f_ext_r: " << r << '\t' << f_ext_r[r] << '\n'; - std::cout << "reduced_f: " << r << '\t' << m_reducedForce[r] << '\n'; m_reducedForce[r] += f_ext_r[r]; + std::cout << "f_ext_r: " << f_ext_r[r] << '\n'; + std::cout << "reduced_f after: " << m_reducedForce[r] << '\n'; } - // std::cout << "reduced_f: " << m_reducedForce[0] << '\t' << m_reducedForce[0] << '\n'; + // std::cout << "reduced_f: " << m_reducedForce[0] << '\n'; + std::cout << "gravity: " << m_mass * 10 << '\n'; } void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt) @@ -501,7 +520,7 @@ void btReducedSoftBody::applyReducedInternalForce(const btScalar damping_alpha, void btReducedSoftBody::applyFixedContraints(btScalar dt) { - for (int iter = 0; iter < 1; ++iter) + for (int iter = 0; iter < 50; ++iter) { // btVector3 vel_error(0, 0, 0); for (int n = 0; n < m_fixedNodes.size(); ++n) diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index e243f8870..c1750dc93 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -16,7 +16,7 @@ class btReducedSoftBody : public btSoftBody // Typedefs // typedef btAlignedObjectArray TVStack; - typedef btAlignedObjectArray tBlockDiagMatrix; + // typedef btAlignedObjectArray tBlockDiagMatrix; typedef btAlignedObjectArray tDenseArray; typedef btAlignedObjectArray > tDenseMatrix; diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index da3b22415..1f6ca2551 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -40,12 +40,19 @@ void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt) // rigid motion rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform()); - std::cout << "reduced_dofs: " << rsb->m_reducedDofs[0] << '\t' << rsb->m_reducedDofs[1] << '\n'; - std::cout << "reduced_vels: " << rsb->m_reducedVelocity[0] << '\t' << rsb->m_reducedVelocity[1] << '\n'; + // std::cout << "reduced_dofs: " << rsb->m_reducedDofs[0] << '\t' << rsb->m_reducedDofs[1] << '\n'; + // std::cout << "reduced_vels: " << rsb->m_reducedVelocity[0] << '\t' << rsb->m_reducedVelocity[1] << '\n'; // update reduced velocity and dofs rsb->updateReducedVelocity(solverdt); // TODO: add back + // update reduced dofs + rsb->updateReducedDofs(solverdt); + + // update local moment arm + rsb->updateLocalMomentArm(); + rsb->updateExternalForceProjectMatrix(true); + // predict full space velocity (needed for constraints) rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform()); @@ -86,7 +93,7 @@ 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 + // rsb->updateReducedDofs(timeStep); // TODO: add back // rigid motion // btTransform predictedTrans; @@ -97,7 +104,8 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) rsb->mapToFullDofs(rsb->getRigidTransform()); // end of time step clean up and update - rsb->updateExternalForceProjectMatrix(true); + // rsb->updateLocalMomentArm(); + // rsb->updateExternalForceProjectMatrix(true); rsb->endOfTimeStepZeroing(); } m_simTime += timeStep;