From c6c753316782185cf7dbf33d7aaa52eeb710978a Mon Sep 17 00:00:00 2001 From: jingyuc Date: Wed, 28 Jul 2021 16:36:15 -0400 Subject: [PATCH] fixed constraints wip --- examples/ReducedDeformableDemo/BasicTest.cpp | 3 +- .../btReducedSoftBody.cpp | 26 ++++++++++ .../BulletReducedSoftBody/btReducedSoftBody.h | 6 +++ .../btReducedSoftBodySolver.cpp | 49 ++++++++++--------- 4 files changed, 60 insertions(+), 24 deletions(-) diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 624a93a35..4a5b89df8 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -133,7 +133,7 @@ void BasicTest::initPhysics() m_broadphase = new btDbvtBroadphase(); btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver(); reducedSoftBodySolver->setDamping(damping_alpha, damping_beta); - btVector3 gravity = btVector3(0, 0, 0); + btVector3 gravity = btVector3(0, -9.8, 0); reducedSoftBodySolver->setGravity(gravity); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); @@ -159,6 +159,7 @@ void BasicTest::initPhysics() // rsb->translate(btVector3(0, 2, 0)); //TODO: add back translate and scale // rsb->setTotalMass(0.5); rsb->setStiffnessScale(1); + 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; diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index 281544d70..1491f316f 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -83,6 +83,15 @@ void btReducedSoftBody::setMassScale(const btScalar rho) m_rhoScale = rho; } +void btReducedSoftBody::setFixedNodes() +{ + for (int i = 0; i < m_nFull; ++i) + { + if (abs(m_nodes[i].m_x[2] - (-2)) < 1e-3) + m_fixedNodes.push_back(i); + } +} + void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform) { btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform); @@ -221,7 +230,14 @@ void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n 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; + } } void btReducedSoftBody::applyReducedInternalForce(tDenseArray& reduced_force, const btScalar damping_alpha, const btScalar damping_beta) @@ -230,4 +246,14 @@ void btReducedSoftBody::applyReducedInternalForce(tDenseArray& reduced_force, co { reduced_force[r] += m_ksScale * m_Kr[r] * (m_reducedDofs[r] + damping_beta * m_reducedVelocity[r]); } +} + +void btReducedSoftBody::applyFixedContraints(btScalar dt, tDenseArray& reduced_force) +{ + 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); + // 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 729e05397..cf2c217d4 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -54,6 +54,7 @@ class btReducedSoftBody : public btSoftBody // full space TVStack m_x0; // Rest position tDenseArray m_nodalMass; // Mass on each node + btAlignedObjectArray m_fixedNodes; // index of the fixed nodes // // Api @@ -76,6 +77,8 @@ class btReducedSoftBody : public btSoftBody void setMassScale(const btScalar rho); + void setFixedNodes(); + virtual void translate(const btVector3& trs); void updateRestNodalPositions(); @@ -107,6 +110,9 @@ class btReducedSoftBody : public btSoftBody // apply impulse to nodes in the full space void applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt, tDenseArray& reduced_force); + // apply fixed contraints to the nodes + void applyFixedContraints(btScalar dt, tDenseArray& reduced_force); + // apply gravity to the rigid frame void applyRigidGravity(const btVector3& gravity, btScalar dt); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index ab4daa441..3a9bbb047 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -40,29 +40,32 @@ void btReducedSoftBodySolver::applyForce() // 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++; - } - } + // 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); // update reduced velocity for (int r = 0; r < rsb->m_reducedDofs.size(); ++r)