diff --git a/examples/ReducedDeformableDemo/ConservationTest.cpp b/examples/ReducedDeformableDemo/ConservationTest.cpp index 2ad2b6838..3f345ed2e 100644 --- a/examples/ReducedDeformableDemo/ConservationTest.cpp +++ b/examples/ReducedDeformableDemo/ConservationTest.cpp @@ -30,7 +30,7 @@ static btScalar damping_alpha = 0.0; static btScalar damping_beta = 0.0; static int start_mode = 6; -static int num_modes = 20; +static int num_modes = 78; class ConservationTest : public CommonDeformableBodyBase { @@ -44,15 +44,15 @@ class ConservationTest : public CommonDeformableBodyBase // for (int k = 0; k < 3; ++k) // rsb->m_nodes[i].m_x[k] += rsb->m_modes[mode_n][3 * i + k] * scale; - rsb->m_reducedDofs[mode_n] = scale; - rsb->m_reducedDofsBuffer[mode_n] = scale; + // rsb->m_reducedDofs[mode_n] = scale; + // rsb->m_reducedDofsBuffer[mode_n] = scale; - // srand(1); - // for (int r = 0; r < rsb->m_nReduced; r++) - // { - // rsb->m_reducedDofs[r] = btScalar(rand()) / btScalar(RAND_MAX) - 0.5; - // rsb->m_reducedDofsBuffer[r] = rsb->m_reducedDofs[r]; - // } + srand(1); + for (int r = 0; r < rsb->m_nReduced; r++) + { + rsb->m_reducedDofs[r] = (btScalar(rand()) / btScalar(RAND_MAX) - 0.5); + rsb->m_reducedDofsBuffer[r] = rsb->m_reducedDofs[r]; + } rsb->mapToFullPosition(rsb->getRigidTransform()); // std::cout << "-----------\n"; @@ -114,12 +114,13 @@ public: } { std::ofstream myfile("angular_momentum.txt", std::ios_base::app); - btVector3 ri(0, 0, 0); - for (int i = 0; i < rsb->m_nFull; ++i) - { - ri = rsb->m_nodes[i].m_x - x_com; - total_angular += rsb->m_nodalMass[i] * ri.cross(rsb->m_nodes[i].m_v); - } + // btVector3 ri(0, 0, 0); + // for (int i = 0; i < rsb->m_nFull; ++i) + // { + // ri = rsb->m_nodes[i].m_x - x_com; + // total_angular += rsb->m_nodalMass[i] * ri.cross(rsb->m_nodes[i].m_v); + // } + total_angular = rsb->computeTotalAngularMomentum(); myfile << sim_time << "\t" << total_angular[0] << "\t" << total_angular[1] << "\t" << total_angular[2] << "\n"; myfile.close(); } @@ -154,8 +155,8 @@ public: first_step = false; } - float internalTimeStep = 1. / 60.f; - m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); sim_time += internalTimeStep; checkMomentum(rsb); @@ -239,7 +240,7 @@ void ConservationTest::initPhysics() // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); // rsb->setRigidVelocity(btVector3(0, 1, 0)); - rsb->setRigidAngularVelocity(btVector3(1, 0, 0)); + // rsb->setRigidAngularVelocity(btVector3(1, 0, 0)); } getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); diff --git a/examples/ReducedDeformableDemo/ModeVisualizer.cpp b/examples/ReducedDeformableDemo/ModeVisualizer.cpp index fe66f5736..b26d804af 100644 --- a/examples/ReducedDeformableDemo/ModeVisualizer.cpp +++ b/examples/ReducedDeformableDemo/ModeVisualizer.cpp @@ -28,7 +28,7 @@ static int start_mode = 6; -static int num_modes = 40; +static int num_modes = 78; static btScalar visualize_mode = 0; static btScalar frequency_scale = 1; @@ -92,10 +92,10 @@ public: int n_mode = floor(visualize_mode); btScalar scale = sin(sqrt(rsb->m_eigenvalues[n_mode]) * sim_time / frequency_scale); getDeformedShape(rsb, n_mode, scale); - btVector3 mass_weighted_column_sum = computeMassWeightedColumnSum(rsb, visualize_mode); - std::cout << "mode=" << int(visualize_mode) << "\t" << mass_weighted_column_sum[0] << "\t" - << mass_weighted_column_sum[1] << "\t" - << mass_weighted_column_sum[2] << "\n"; + // btVector3 mass_weighted_column_sum = computeMassWeightedColumnSum(rsb, visualize_mode); + // std::cout << "mode=" << int(visualize_mode) << "\t" << mass_weighted_column_sum[0] << "\t" + // << mass_weighted_column_sum[1] << "\t" + // << mass_weighted_column_sum[2] << "\n"; } virtual void renderScene() @@ -136,7 +136,7 @@ void ModeVisualizer::initPhysics() // create volumetric soft body { - btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes); + btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedBeam(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes); getDeformableDynamicsWorld()->addSoftBody(rsb); rsb->getCollisionShape()->setMargin(0.1); diff --git a/examples/ReducedDeformableDemo/Springboard.cpp b/examples/ReducedDeformableDemo/Springboard.cpp index 26c3d361e..e082abd44 100644 --- a/examples/ReducedDeformableDemo/Springboard.cpp +++ b/examples/ReducedDeformableDemo/Springboard.cpp @@ -84,11 +84,11 @@ public: sim_time += internalTimeStep; btReducedSoftBody* rsb = static_cast(getDeformableDynamicsWorld()->getSoftBodyArray()[0]); - std::ofstream myfile("fixed_node.txt", std::ios_base::app); - myfile << sim_time << "\t" << rsb->m_nodes[0].m_x[0] - rsb->m_x0[0][0] << "\t" - << rsb->m_nodes[0].m_x[1] - rsb->m_x0[0][1] << "\t" - << rsb->m_nodes[0].m_x[2] - rsb->m_x0[0][2] << "\n"; - myfile.close(); + // std::ofstream myfile("fixed_node.txt", std::ios_base::app); + // myfile << sim_time << "\t" << rsb->m_nodes[0].m_x[0] - rsb->m_x0[0][0] << "\t" + // << rsb->m_nodes[0].m_x[1] - rsb->m_x0[0][1] << "\t" + // << rsb->m_nodes[0].m_x[2] - rsb->m_x0[0][2] << "\n"; + // myfile.close(); } } @@ -187,19 +187,19 @@ void Springboard::initPhysics() Ctor_RbUpStack(); // create a static rigid box as the ground - // { - // // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50))); - // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10))); - // m_collisionShapes.push_back(groundShape); + { + // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50))); + btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10))); + m_collisionShapes.push_back(groundShape); - // btTransform groundTransform; - // groundTransform.setIdentity(); - // groundTransform.setOrigin(btVector3(0, 0, 0)); - // { - // btScalar mass(0.); - // createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0)); - // } - // } + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, 0, 0)); + { + btScalar mass(0.); + createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0)); + } + } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index 70cd5a3f7..a0f878bf9 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -282,13 +282,55 @@ void btReducedSoftBody::updateReducedVelocity(btScalar solverdt) void btReducedSoftBody::mapToFullVelocity(const btTransform& ref_trans) { // compute the reduced contribution to the angular velocity - btVector3 sum_linear(0, 0, 0); - btVector3 sum_angular(0, 0, 0); - m_linearVelocityFromReduced.setZero(); - m_angularVelocityFromReduced.setZero(); + // btVector3 sum_linear(0, 0, 0); + // btVector3 sum_angular(0, 0, 0); + // m_linearVelocityFromReduced.setZero(); + // m_angularVelocityFromReduced.setZero(); + // for (int i = 0; i < m_nFull; ++i) + // { + // btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i]; + // btMatrix3x3 r_star = Cross(r_com); + + // btVector3 v_from_reduced(0, 0, 0); + // for (int k = 0; k < 3; ++k) + // { + // for (int r = 0; r < m_nReduced; ++r) + // { + // v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r]; + // } + // } + + // btVector3 delta_linear = m_nodalMass[i] * v_from_reduced; + // btVector3 delta_angular = m_nodalMass[i] * (r_star * ref_trans.getBasis() * v_from_reduced); + // sum_linear += delta_linear; + // sum_angular += delta_angular; + // // std::cout << "delta_linear: " << delta_linear[0] << "\t" << delta_linear[1] << "\t" << delta_linear[2] << "\n"; + // // std::cout << "delta_angular: " << delta_angular[0] << "\t" << delta_angular[1] << "\t" << delta_angular[2] << "\n"; + // // std::cout << "sum_linear: " << sum_linear[0] << "\t" << sum_linear[1] << "\t" << sum_linear[2] << "\n"; + // // std::cout << "sum_angular: " << sum_angular[0] << "\t" << sum_angular[1] << "\t" << sum_angular[2] << "\n"; + // } + // m_linearVelocityFromReduced = 1.0 / m_mass * (ref_trans.getBasis() * sum_linear); + // m_angularVelocityFromReduced = m_interpolateInvInertiaTensorWorld * sum_angular; + // std::cout << "-----end here------\n"; + + // m_linearVelocity -= m_linearVelocityFromReduced; + // m_angularVelocity -= m_angularVelocityFromReduced; + for (int i = 0; i < m_nFull; ++i) { - btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i]; + m_nodes[i].m_v = computeNodeFullVelocity(ref_trans, i); + } +} + +const btVector3 btReducedSoftBody::computeTotalAngularMomentum() const +{ + btVector3 L_rigid = m_invInertiaTensorWorld.inverse() * m_angularVelocity; + btVector3 L_reduced(0, 0, 0); + btMatrix3x3 omega_prime_star = Cross(m_angularVelocityFromReduced); + + for (int i = 0; i < m_nFull; ++i) + { + btVector3 r_com = m_rigidTransformWorld.getBasis() * m_localMomentArm[i]; btMatrix3x3 r_star = Cross(r_com); btVector3 v_from_reduced(0, 0, 0); @@ -300,26 +342,10 @@ void btReducedSoftBody::mapToFullVelocity(const btTransform& ref_trans) } } - btVector3 delta_linear = m_nodalMass[i] * v_from_reduced; - btVector3 delta_angular = m_nodalMass[i] * (r_star * ref_trans.getBasis() * v_from_reduced); - sum_linear += delta_linear; - sum_angular += delta_angular; - // std::cout << "delta_linear: " << delta_linear[0] << "\t" << delta_linear[1] << "\t" << delta_linear[2] << "\n"; - // std::cout << "delta_angular: " << delta_angular[0] << "\t" << delta_angular[1] << "\t" << delta_angular[2] << "\n"; - // std::cout << "sum_linear: " << sum_linear[0] << "\t" << sum_linear[1] << "\t" << sum_linear[2] << "\n"; - // std::cout << "sum_angular: " << sum_angular[0] << "\t" << sum_angular[1] << "\t" << sum_angular[2] << "\n"; - } - m_linearVelocityFromReduced = 1.0 / m_mass * (ref_trans.getBasis() * sum_linear); - m_angularVelocityFromReduced = m_interpolateInvInertiaTensorWorld * sum_angular; - // std::cout << "-----end here------\n"; - - // m_linearVelocity -= m_linearVelocityFromReduced; - // m_angularVelocity -= m_angularVelocityFromReduced; - - for (int i = 0; i < m_nFull; ++i) - { - m_nodes[i].m_v = computeNodeFullVelocity(ref_trans, i); + L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced - omega_prime_star * r_com)); + // L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced)); } + return L_rigid + L_reduced; } const btVector3 btReducedSoftBody::computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index 026f14bc8..8c7c57632 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -184,6 +184,9 @@ class btReducedSoftBody : public btSoftBody // compute full space velocity from the reduced velocity void mapToFullVelocity(const btTransform& ref_trans); + // compute total angular momentum + const btVector3 computeTotalAngularMomentum() const; + // get a single node's full space velocity from the reduced velocity const btVector3 computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const;