From fdfb08b72fcba5e970476e26f4f57f25031a4f3a Mon Sep 17 00:00:00 2001 From: jingyuc Date: Wed, 4 Aug 2021 22:44:53 -0400 Subject: [PATCH] single fixed constraint for the rigid frame is working --- examples/ReducedDeformableDemo/BasicTest.cpp | 29 ++++-- .../btReducedSoftBody.cpp | 92 +++++++++++++++---- .../BulletReducedSoftBody/btReducedSoftBody.h | 17 ++-- .../btReducedSoftBodySolver.cpp | 40 ++++---- .../btDeformableMultiBodyDynamicsWorld.cpp | 54 +++++------ 5 files changed, 152 insertions(+), 80 deletions(-) diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 19a81f16f..09907f9f9 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -35,7 +35,7 @@ static btScalar damping_alpha = 0.0; static btScalar damping_beta = 0.01; static btScalar COLLIDING_VELOCITY = 0; static int start_mode = 6; -static int num_modes = 1; +static int num_modes = 2; class BasicTest : public CommonDeformableBodyBase { @@ -50,7 +50,7 @@ class BasicTest : public CommonDeformableBodyBase // rsb->m_nodes[i].m_x[k] += rsb->m_modes[mode_n][3 * i + k] * scale; rsb->m_reducedDofs[mode_n] = scale; - rsb->mapToFullDofs(); + rsb->mapToFullDofs(rsb->getWorldTransform()); std::cout << "-----------\n"; std::cout << rsb->m_nodes[0].m_x[0] << '\t' << rsb->m_nodes[0].m_x[1] << '\t' << rsb->m_nodes[0].m_x[2] << '\n'; std::cout << "-----------\n"; @@ -118,14 +118,25 @@ public: for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) { - btSoftBody* rsb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + btSoftBody* rsb = static_cast(deformableWorld->getSoftBodyArray()[i]); { btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer()); // btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), flag); - btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); - deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.2, btVector3(1, 1, 1)); - deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 2, 0), 0.2, btVector3(1, 1, 1)); - deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 4, 0), 0.2, btVector3(1, 1, 1)); + btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + + btVector3 origin = rsb->getRigidTransform().getOrigin(); + btVector3 line_x = rsb->getRigidTransform().getBasis() * 2 * btVector3(1, 0, 0) + origin; + btVector3 line_y = rsb->getRigidTransform().getBasis() * 2 * btVector3(0, 1, 0) + origin; + btVector3 line_z = rsb->getRigidTransform().getBasis() * 2 * btVector3(0, 0, 1) + origin; + + deformableWorld->getDebugDrawer()->drawLine(origin, line_x, btVector3(1, 0, 0)); + deformableWorld->getDebugDrawer()->drawLine(origin, line_y, btVector3(0, 1, 0)); + deformableWorld->getDebugDrawer()->drawLine(origin, line_z, btVector3(0, 0, 1)); + deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[0].m_x, 0.2, btVector3(1, 0, 0)); + + 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)); + deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 4, 0), 0.1, btVector3(1, 1, 1)); } } } @@ -144,7 +155,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, -10, 0); reducedSoftBodySolver->setGravity(gravity); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); @@ -181,7 +192,7 @@ void BasicTest::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)); // 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 47f4cfd75..5dbc2f370 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -21,9 +21,13 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_co m_angularFactor.setValue(1, 1, 1); m_linearFactor.setValue(1, 1, 1); m_invInertiaLocal.setValue(1, 1, 1); + // m_invInertiaLocal.setZero(); m_mass = 0.0; m_inverseMass = 0.0; + m_linearDamping = 0; + m_angularDamping = 0; + m_rigidTransformWorld.setIdentity(); } @@ -62,6 +66,30 @@ void btReducedSoftBody::setInertiaProps(const btVector3& inertia) inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0), inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); + + // // CoM + // btVector3 x_com(0,0,0); + // for (int i = 0; i < m_nFull; ++i) + // { + // x_com += m_nodalMass[i] * m_nodes[i].m_x; + // } + // x_com /= m_mass; + + // btMatrix3x3 inertia_temp; + // inertia_temp.setZero(); + // for (int i = 0; i < m_nFull; ++i) + // { + // btVector3 ri = m_nodes[i].m_x - x_com; + // for (int a = 0; a < 3; ++a) + // { + // for (int b = 0; b < 3; ++b) + // { + // inertia_temp[a][b] += m_nodalMass[i] * ri[a] * ri[b]; + // } + // } + // } + // m_invInertiaLocal = inertia_temp.inverse(); + // update world inertia tensor updateInertiaTensor(); @@ -187,10 +215,15 @@ void btReducedSoftBody::updateReducedDofs(btScalar solverdt) } } -void btReducedSoftBody::mapToFullDofs() +void btReducedSoftBody::mapToFullDofs(const btTransform& ref_trans) { - btVector3 origin = m_rigidTransformWorld.getOrigin(); - btMatrix3x3 rotation = m_rigidTransformWorld.getBasis(); + // btVector3 origin = m_rigidTransformWorld.getOrigin(); + // btMatrix3x3 rotation = m_rigidTransformWorld.getBasis(); + // btVector3 origin = m_interpolationWorldTransform.getOrigin(); + // btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis(); + btVector3 origin = ref_trans.getOrigin(); + btMatrix3x3 rotation = ref_trans.getBasis(); + for (int i = 0; i < m_nFull; ++i) { @@ -209,14 +242,13 @@ void btReducedSoftBody::updateReducedVelocity(btScalar solverdt) } } -void btReducedSoftBody::mapToFullVelocity(btScalar solverdt) +void btReducedSoftBody::mapToFullVelocity(const btTransform& ref_trans) { 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_com = m_nodes[i].m_x - m_rigidTransformWorld.getOrigin(); + btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i]; // compute velocity contributed by the reduced velocity for (int k = 0; k < 3; ++k) @@ -230,10 +262,9 @@ void btReducedSoftBody::mapToFullVelocity(btScalar solverdt) // get new velocity m_nodes[i].m_v = m_angularVelocity.cross(r_com) + - m_rigidTransformWorld.getBasis() * v_from_reduced[i] + + ref_trans.getBasis() * v_from_reduced[i] + m_linearVelocity; } - std::cout << m_nodes[0].m_v[0] << '\t' << m_nodes[0].m_v[1] << '\t' << m_nodes[0].m_v[2] << '\n'; } void btReducedSoftBody::proceedToTransform(const btTransform& newTrans) @@ -251,7 +282,7 @@ void btReducedSoftBody::setCenterOfMassTransform(const btTransform& xform) { m_interpolationWorldTransform = xform; } - m_interpolationLinearVelocity = getLinearVelocity(); + m_interpolationLinearVelocity = getLinearVelocity(); // TODO: check where these are used? m_interpolationAngularVelocity = getAngularVelocity(); m_rigidTransformWorld = xform; updateInertiaTensor(); @@ -280,6 +311,13 @@ void btReducedSoftBody::updateRestNodalPositions() void btReducedSoftBody::updateInertiaTensor() { m_invInertiaTensorWorld = m_rigidTransformWorld.getBasis().scaled(m_invInertiaLocal) * m_rigidTransformWorld.getBasis().transpose(); + // m_invInertiaTensorWorld = m_rigidTransformWorld.getBasis() * m_invInertiaLocal * m_rigidTransformWorld.getBasis().transpose(); +} + +void btReducedSoftBody::applyDamping(btScalar timeStep) +{ + m_linearVelocity *= btScalar(1) - m_linearDamping; + m_angularDamping *= btScalar(1) - m_angularDamping; } void btReducedSoftBody::applyCentralImpulse(const btVector3& impulse) @@ -313,7 +351,10 @@ void btReducedSoftBody::applyRigidImpulse(const btVector3& impulse, const btVect void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt) { // TODO: get correct impulse using the impulse factor - btVector3 ri = m_localMomentArm[n_node]; + // btVector3 ri = m_rigidTransformWorld.getBasis() * m_localMomentArm[n_node]; + btVector3 ri = m_interpolationWorldTransform.getBasis() * m_localMomentArm[n_node]; + // std::cout << ri[0] << '\t' << ri[1] << '\t' << ri[2] << '\n'; + btMatrix3x3 ri_skew(0, -ri[2], ri[1], ri[2], 0, -ri[0], -ri[1], ri[0], 0); @@ -323,22 +364,35 @@ void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n btMatrix3x3 impulse_factor(inv_mass, 0, 0, 0, inv_mass, 0, 0, 0, inv_mass); - impulse_factor -= ri_skew.scaled(m_invInertiaLocal) * ri_skew; + // impulse_factor -= ri_skew.scaled(m_invInertiaLocal) * ri_skew; + btMatrix3x3 m_interpolateInvInertiaTensorWorld = m_interpolationWorldTransform.getBasis().scaled(m_invInertiaLocal) * m_interpolationWorldTransform.getBasis().transpose(); + impulse_factor -= ri_skew * m_interpolateInvInertiaTensorWorld * ri_skew; + + // impulse_factor -= ri_skew * m_invInertiaTensorWorld * ri_skew; // get impulse - btVector3 impulse = impulse_factor * (target_vel - m_nodes[n_node].m_v); + btVector3 dv = target_vel - m_nodes[n_node].m_v; + // std::cout << dv[0] << '\t' << dv[1] << '\t' << dv[2] << '\n'; + // std::cout << m_nodes[n_node].m_v[0] << '\t' << m_nodes[n_node].m_v[1] << '\t' << m_nodes[n_node].m_v[2] << '\n'; + btVector3 impulse = impulse_factor.inverse() * dv; + // std::cout << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n'; + // std::cout << "----------\n"; // apply impulse force - applyFullSpaceNodalForce(impulse / dt, n_node); + // applyFullSpaceNodalForce(impulse / dt, n_node); // impulse causes rigid motion - applyRigidImpulse(impulse, m_nodes[n_node].m_x); + // applyRigidImpulse(impulse, m_worldTransform.getOrigin()); + // std::cout << m_nodes[n_node].m_x[0] << '\t' << m_nodes[n_node].m_x[1] << '\t' << m_nodes[n_node].m_x[2] << '\n'; + + // applyRigidImpulse(impulse, m_nodes[n_node].m_x); + applyRigidImpulse(impulse, ri); } void btReducedSoftBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_node) { // f_local = R^-1 * f_ext - btVector3 f_local = m_worldTransform.getBasis().transpose() * f_ext; + btVector3 f_local = m_rigidTransformWorld.getBasis().transpose() * f_ext; // f_scaled = localInvInertia * (r_k x f_local) btVector3 rk_cross_f_local = m_localMomentArm[n_node].cross(f_local); @@ -347,6 +401,7 @@ void btReducedSoftBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_n { f_scaled[k] = m_invInertiaLocal[k] * rk_cross_f_local[k]; } + // f_scaled = m_invInertiaLocal * rk_cross_f_local; // f_ext_r = [S^T * P]_{n_node} * f_local for (int r = 0; r < m_nReduced; ++r) @@ -378,8 +433,11 @@ void btReducedSoftBody::applyFixedContraints(btScalar dt) { for (int n = 0; n < m_fixedNodes.size(); ++n) { - // std::cout << reduced_force[0] << "\t" << reduced_force[1] << "\n"; + // std::cout << m_reducedForce[0] << "\t" << m_reducedForce[1] << "\n"; applyFullSpaceImpulse(btVector3(0, 0, 0), m_fixedNodes[n], dt); - // std::cout << reduced_force[0] << "\t" << reduced_force[1] << "\n"; + + // mapToFullVelocity(getInterpolationWorldTransform()); + // std::cout << "after" << m_nodes[0].m_v[0] << '\t' << m_nodes[0].m_v[0] << '\t' << m_nodes[0].m_v[0] << '\n'; + // std::cout << m_reducedForce[0] << "\t" << m_reducedForce[1] << "\n"; } } \ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index 888681005..538caddea 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -33,7 +33,10 @@ class btReducedSoftBody : public btSoftBody // rigid frame btScalar m_mass; // total mass of the rigid frame btScalar m_inverseMass; // inverse of the total mass of the rigid frame + btVector3 m_linearVelocity; btVector3 m_angularVelocity; + btScalar m_linearDamping; // linear damping coefficient + btScalar m_angularDamping; // angular damping coefficient btVector3 m_linearFactor; btVector3 m_angularFactor; btVector3 m_invInertiaLocal; @@ -43,7 +46,6 @@ class btReducedSoftBody : public btSoftBody public: - btVector3 m_linearVelocity; // // Fields // @@ -124,14 +126,16 @@ class btReducedSoftBody : public btSoftBody void updateReducedVelocity(btScalar solverdt); // map to full degree of freedoms - void mapToFullDofs(); + void mapToFullDofs(const btTransform& ref_trans); // compute full space velocity from the reduced velocity - void mapToFullVelocity(btScalar solverdt); + void mapToFullVelocity(const btTransform& ref_trans); // // rigid motion related // + void applyDamping(btScalar timeStep); + void applyCentralImpulse(const btVector3& impulse); void applyTorqueImpulse(const btVector3& torque); @@ -170,7 +174,7 @@ class btReducedSoftBody : public btSoftBody return m_mass; } - btTransform& getWorldTransform() + btTransform& getRigidTransform() { return m_rigidTransformWorld; } @@ -184,11 +188,6 @@ class btReducedSoftBody : public btSoftBody return m_angularVelocity; } - const btVector3& getOrigin() const - { - return m_rigidTransformWorld.getOrigin(); - } - #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 void clampVelocity(btVector3& v) const { v.setX( diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index 90e8dd231..204ee769d 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -34,16 +34,20 @@ void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt) { btReducedSoftBody* rsb = static_cast(m_softBodies[i]); - // rigid motion - btTransform predictedTrans; - rsb->predictIntegratedTransform(solverdt, predictedTrans); - rsb->proceedToTransform(predictedTrans); + // apply damping + rsb->applyDamping(solverdt); - // apply fixed constraints - rsb->applyFixedContraints(solverdt); + // rigid motion + rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform()); // update reduced velocity and dofs - rsb->updateReducedVelocity(solverdt); + // rsb->updateReducedVelocity(solverdt); // TODO: add back + + // predict full space velocity (needed for constraints) + rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform()); + + // apply fixed constraints + // rsb->applyFixedContraints(solverdt); // TODO: update mesh nodal position. need it for collision // rsb->updateMeshNodePositions(solverdt); @@ -61,14 +65,14 @@ void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt) rsb->applyRigidGravity(m_gravity, solverdt); // add internal force (elastic force & damping force) - rsb->applyReducedInternalForce(m_dampingAlpha, m_dampingBeta); + // rsb->applyReducedInternalForce(m_dampingAlpha, m_dampingBeta); // TODO: add back // apply external force or impulses - if (!applied && m_simTime > 2) - { - rsb->applyFullSpaceImpulse(btVector3(0, -5, 0), 0, solverdt); - applied = true; - } + // if (!applied && m_simTime > 2) + // { + // rsb->applyFullSpaceImpulse(btVector3(0, -5, 0), 0, solverdt); + // applied = true; + // } } } @@ -79,15 +83,15 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) btReducedSoftBody* rsb = static_cast(m_softBodies[i]); // update reduced dofs for the next time step - rsb->updateReducedDofs(timeStep); + // rsb->updateReducedDofs(timeStep); // TODO: add back // rigid motion - btTransform predictedTrans; - rsb->predictIntegratedTransform(timeStep, predictedTrans); - rsb->proceedToTransform(predictedTrans); + // btTransform predictedTrans; + // rsb->predictIntegratedTransform(timeStep, predictedTrans); + rsb->proceedToTransform(rsb->getInterpolationWorldTransform()); // update mesh nodal positions for the next time step - rsb->mapToFullDofs(); + rsb->mapToFullDofs(rsb->getRigidTransform()); // end of time step clean up and update rsb->updateExternalForceProjectMatrix(true); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 46e886cd8..d6749d550 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -87,23 +87,23 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t ///apply gravity and explicit force to velocity, predict motion predictUnconstraintMotion(timeStep); - // ///perform collision detection that involves rigid/multi bodies - // btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); + ///perform collision detection that involves rigid/multi bodies + btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); - // btMultiBodyDynamicsWorld::calculateSimulationIslands(); + btMultiBodyDynamicsWorld::calculateSimulationIslands(); - // beforeSolverCallbacks(timeStep); + beforeSolverCallbacks(timeStep); // ///solve contact constraints and then deformable bodies momemtum equation solveConstraints(timeStep); - // afterSolverCallbacks(timeStep); + afterSolverCallbacks(timeStep); - // performDeformableCollisionDetection(); + performDeformableCollisionDetection(); - // applyRepulsionForce(timeStep); + applyRepulsionForce(timeStep); - // performGeometricCollisions(timeStep); + performGeometricCollisions(timeStep); integrateTransforms(timeStep); @@ -302,30 +302,30 @@ 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);