From 5d173532698c0e9cc8f564adc0e4d9c4e98a9ebf Mon Sep 17 00:00:00 2001 From: jingyuc Date: Thu, 29 Jul 2021 11:45:44 -0400 Subject: [PATCH] add rigidTransformWorld to make the rendering works after applying initial translation. Center of mass looks right. Tested with rotating free fall --- examples/ReducedDeformableDemo/BasicTest.cpp | 11 ++++-- .../btReducedSoftBody.cpp | 36 ++++++++++--------- .../BulletReducedSoftBody/btReducedSoftBody.h | 9 ++++- .../btReducedSoftBodySolver.cpp | 13 +++---- 4 files changed, 42 insertions(+), 27 deletions(-) diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 4a5b89df8..401b0fb63 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -108,13 +108,18 @@ public: { CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + // int flag = 0; for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) { btSoftBody* rsb = (btSoftBody*)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)); } } } @@ -156,10 +161,10 @@ void BasicTest::initPhysics() getDeformableDynamicsWorld()->addSoftBody(rsb); rsb->getCollisionShape()->setMargin(0.1); // rsb->scale(btVector3(1, 1, 1)); - // rsb->translate(btVector3(0, 2, 0)); //TODO: add back translate and scale + rsb->translate(btVector3(0, 4, 0)); //TODO: add back translate and scale // rsb->setTotalMass(0.5); rsb->setStiffnessScale(1); - rsb->setFixedNodes(); + // 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; @@ -170,7 +175,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(10, 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 1491f316f..f9cb794c3 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -23,6 +23,8 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_co m_invInertiaLocal.setValue(1, 1, 1); m_mass = 0.0; m_inverseMass = 0.0; + + m_rigidTransformWorld.setIdentity(); } void btReducedSoftBody::setReducedModes(int start_mode, int num_modes, int full_size) @@ -85,16 +87,17 @@ void btReducedSoftBody::setMassScale(const btScalar 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); - } + // for (int i = 0; i < m_nFull; ++i) + // { + // if (abs(m_nodes[i].m_x[2] - (-2)) < 1e-3) + // m_fixedNodes.push_back(i); + // } + m_fixedNodes.push_back(0); } void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform) { - btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform); + btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform); } void btReducedSoftBody::updateReducedDofs() @@ -117,10 +120,10 @@ void btReducedSoftBody::updateReducedDofs() void btReducedSoftBody::updateFullDofs() { btAssert(m_nodes.size() == m_nFull); - btAlignedObjectArray delta_x; + TVStack delta_x; delta_x.resize(m_nFull); - btVector3 origin = getWorldTransform().getOrigin(); - btMatrix3x3 rotation = getWorldTransform().getBasis(); + btVector3 origin = m_rigidTransformWorld.getOrigin(); + btMatrix3x3 rotation = m_rigidTransformWorld.getBasis(); for (int i = 0; i < m_nFull; ++i) { @@ -134,7 +137,7 @@ void btReducedSoftBody::updateFullDofs() } } // get new coordinates - m_nodes[i].m_x = rotation * (m_x0[i] + delta_x[i]) + origin; //TODO: assume the initial origin is at (0,0,0) + m_nodes[i].m_x = rotation * (m_x0[i] - m_initialOrigin + delta_x[i]) + origin; //TODO: assume the initial origin is at (0,0,0) } } @@ -147,7 +150,7 @@ void btReducedSoftBody::setCenterOfMassTransform(const btTransform& xform) { if (isKinematicObject()) { - m_interpolationWorldTransform = m_worldTransform; + m_interpolationWorldTransform = m_rigidTransformWorld; } else { @@ -155,7 +158,7 @@ void btReducedSoftBody::setCenterOfMassTransform(const btTransform& xform) } m_interpolationLinearVelocity = getLinearVelocity(); m_interpolationAngularVelocity = getAngularVelocity(); - m_worldTransform = xform; + m_rigidTransformWorld = xform; updateInertiaTensor(); } @@ -164,12 +167,11 @@ void btReducedSoftBody::translate(const btVector3& trs) // translate mesh btSoftBody::translate(trs); updateRestNodalPositions(); - // for (int i = 0; i < m_nFull; ++i) - // for (int k = 0; k < 3; ++k) - // std::cout << m_nodes[i].m_x[k] << "\t" << m_x0[i][k] << "\n"; // update rigid frame - m_worldTransform.setOrigin(trs); + m_rigidTransformWorld.setOrigin(trs); + m_interpolationWorldTransform = m_rigidTransformWorld; + m_initialOrigin = m_rigidTransformWorld.getOrigin(); updateInertiaTensor(); } @@ -182,7 +184,7 @@ void btReducedSoftBody::updateRestNodalPositions() void btReducedSoftBody::updateInertiaTensor() { - m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); + m_invInertiaTensorWorld = m_rigidTransformWorld.getBasis().scaled(m_invInertiaLocal) * m_rigidTransformWorld.getBasis().transpose(); } void btReducedSoftBody::applyCentralImpulse(const btVector3& impulse) diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index cf2c217d4..59ae1bccf 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -23,7 +23,9 @@ class btReducedSoftBody : public btSoftBody btVector3 m_linearFactor; btVector3 m_angularFactor; btVector3 m_invInertiaLocal; + btTransform m_rigidTransformWorld; btMatrix3x3 m_invInertiaTensorWorld; + btVector3 m_initialOrigin; // initial center of mass (original of the m_rigidTransformWorld) public: // @@ -128,6 +130,11 @@ class btReducedSoftBody : public btSoftBody return m_mass; } + btTransform& getWorldTransform() + { + return m_rigidTransformWorld; + } + const btVector3& getLinearVelocity() const { return m_linearVelocity; @@ -139,7 +146,7 @@ class btReducedSoftBody : public btSoftBody const btVector3& getOrigin() const { - return m_worldTransform.getOrigin(); + return m_rigidTransformWorld.getOrigin(); } #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index 3a9bbb047..ec4a1d37b 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -65,15 +65,13 @@ void btReducedSoftBodySolver::applyForce() // } // apply fixed contraints - rsb->applyFixedContraints(m_dt, reduced_force); + // rsb->applyFixedContraints(m_dt, reduced_force); //TODO: solver iteratively with other constraints // update reduced velocity for (int r = 0; r < rsb->m_reducedDofs.size(); ++r) { btScalar mass_inv = (rsb->m_Mr[r] == 0) ? 0 : 1.0 / rsb->m_Mr[r]; btScalar delta_v = m_dt * mass_inv * reduced_force[r]; - - sim_time += m_dt; rsb->m_reducedVelocity[r] -= delta_v; } } @@ -102,9 +100,12 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) rsb->m_reducedDofs[r] += timeStep * rsb->m_reducedVelocity[r]; // rigid motion - rsb->predictIntegratedTransform(timeStep, rsb->getInterpolationWorldTransform()); - - rsb->proceedToTransform(rsb->getInterpolationWorldTransform()); + btTransform predictedTrans; + rsb->predictIntegratedTransform(timeStep, predictedTrans); + // std::cout << predictedTrans.getOrigin()[0] << '\t' << predictedTrans.getOrigin()[1] << '\t' << predictedTrans.getOrigin()[2] << '\n'; + rsb->proceedToTransform(predictedTrans); + // std::cout << rsb->getWorldTransform().getOrigin()[0] << '\t' << rsb->getWorldTransform().getOrigin()[1] << '\t' << rsb->getWorldTransform().getOrigin()[2] << '\n'; + // std::cout << "----------\n"; // map reduced dof back to full space rsb->updateFullDofs();