diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index 9587d2638..0afde723b 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -140,4 +140,19 @@ void btReducedSoftBody::applyImpulse(const btVector3& impulse, const btVector3& applyTorqueImpulse(rel_pos.cross(impulse * m_linearFactor)); } } +} + +void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt, tDenseArray& reduced_force) +{ + // 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]; + } + } + // impulse causes rigid motion + applyImpulse(impulse, m_nodes[n_node].m_x); } \ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index 58a7df1c3..40b5b4385 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -83,7 +83,11 @@ class btReducedSoftBody : public btSoftBody void applyTorqueImpulse(const btVector3& torque); + // apply impulse to the rigid frame 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 proceedToTransform(const btTransform& newTrans); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index f9c1c3cf2..024c022e7 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -25,63 +25,41 @@ void btReducedSoftBodySolver::applyForce() reduced_force.resize(rsb->m_reducedDofs.size(), 0); // add internal force (elastic force & damping force) - for (int r = 0; r < rsb->m_reducedDofs.size(); ++r) { + for (int r = 0; r < rsb->m_reducedDofs.size(); ++r) + { // map all force to reduced // for (int i = 0; i < force.size(); ++i) // for (int k = 0; k < 3; ++k) // reduced_force[r] += scale * rsb->m_modes[r][3 * i + k] * force[i][k]; - // std::cout << reduced_force[r] << '\t'; - reduced_force[r] += rsb->m_Kr[r] * (rsb->m_reducedDofs[r] + 0.1 * rsb->m_reducedVelocity[r]); - // std::cout << reduced_force[r] << '\n'; - // std::cout << rsb->m_Kr[r] << "\t" << rsb->m_reducedDofs[r] << "\n"; } - // apply impulses to reduced deformable objects static btScalar sim_time = 0; - static btScalar target_vel = 1; static int apply_impulse = 0; if (rsb->m_reducedModel && apply_impulse < 4) { - btScalar f_imp = 0; - btVector3 imp(0, 0, 0); if (sim_time > 1 && apply_impulse == 0) { - f_imp = rsb->m_nodalMass[0] * (target_vel - rsb->m_nodes[0].m_v[1]) / m_dt; - imp[1] = f_imp; + rsb->applyFullSpaceImpulse(btVector3(0, 1, 0), 0, m_dt, reduced_force); apply_impulse++; - std::cout << "hit" << f_imp << "\n"; } if (sim_time > 2 && apply_impulse == 1) { - f_imp = - 1.2 * rsb->m_nodalMass[0] * (target_vel - rsb->m_nodes[0].m_v[1]) / m_dt; - imp[1] = f_imp; + rsb->applyFullSpaceImpulse(btVector3(0, -1, 0), 0, m_dt, reduced_force); apply_impulse++; - std::cout << "hit2" << f_imp << "\n"; } if (sim_time > 3 && apply_impulse == 2) { - f_imp = 1 * rsb->m_nodalMass[0] * (target_vel - rsb->m_nodes[0].m_v[0]) / m_dt; - imp[2] = f_imp; + rsb->applyFullSpaceImpulse(btVector3(1, 0, 0), 0, m_dt, reduced_force); apply_impulse++; - std::cout << "hit3" << f_imp << "\n"; } if (sim_time > 4 && apply_impulse == 3) { - f_imp = -1.1 * rsb->m_nodalMass[0] * (target_vel - rsb->m_nodes[0].m_v[0]) / m_dt; - imp[2] = f_imp; + rsb->applyFullSpaceImpulse(btVector3(-1, 0, 0), 0, m_dt, reduced_force); apply_impulse++; - std::cout << "hit4" << f_imp << "\n"; } - for (int i = 0; i < rsb->m_reducedDofs.size(); ++i) - { - reduced_force[i] += rsb->m_modes[i][0 * 3 + 1] * f_imp; - } - - - rsb->applyImpulse(imp, rsb->m_nodes[0].m_x); } // update reduced velocity