diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 81bce3732..c912b8a47 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -48,7 +48,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->getWorldTransform()); + rsb->mapToFullPosition(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"; diff --git a/examples/ReducedDeformableDemo/FreeFall.cpp b/examples/ReducedDeformableDemo/FreeFall.cpp index c2deb2a68..e12a7c9c0 100644 --- a/examples/ReducedDeformableDemo/FreeFall.cpp +++ b/examples/ReducedDeformableDemo/FreeFall.cpp @@ -94,20 +94,26 @@ public: // btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), flag); 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; + // 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()->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)); for (int p = 0; p < rsb->m_fixedNodes.size(); ++p) { deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0)); // std::cout << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[0] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[1] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[2] << "\n"; } + for (int p = 0; p < rsb->m_contactNodesList.size(); ++p) + { + deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0)); + // std::cout << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[0] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[1] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[2] << "\n"; + } + deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1)); deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 5, 0), 0.1, btVector3(1, 1, 1)); deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 10, 0), 0.1, btVector3(1, 1, 1)); @@ -129,7 +135,7 @@ void FreeFall::initPhysics() m_broadphase = new btDbvtBroadphase(); btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver(); reducedSoftBodySolver->setDamping(damping_alpha, damping_beta); - btVector3 gravity = btVector3(0, -10, 0); + btVector3 gravity = btVector3(0, 0, 0); reducedSoftBodySolver->setGravity(gravity); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); @@ -152,7 +158,7 @@ void FreeFall::initPhysics() getDeformableDynamicsWorld()->addSoftBody(rsb); rsb->getCollisionShape()->setMargin(0.1); // rsb->scale(btVector3(1, 1, 1)); - rsb->translate(btVector3(0, 5, 0)); //TODO: add back translate and scale + rsb->translate(btVector3(0, 0.1, 0)); //TODO: add back translate and scale // rsb->setTotalMass(0.5); rsb->setStiffnessScale(0.5); @@ -168,7 +174,7 @@ void FreeFall::initPhysics() btSoftBodyHelpers::generateBoundaryFaces(rsb); // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); - // rsb->setRigidVelocity(btVector3(0, 1, 0)); + rsb->setRigidVelocity(btVector3(0, 0, 1)); // rsb->setRigidAngularVelocity(btVector3(1, 0, 0)); // btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); @@ -177,12 +183,12 @@ void FreeFall::initPhysics() } // create a static rigid box as the ground { - btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.), btScalar(50.), btScalar(50.))); + btBoxShape* groundShape = createBoxShape(btVector3(btScalar(5.), btScalar(5.), btScalar(0.5))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -50, 0)); + groundTransform.setOrigin(btVector3(0, 0, 5)); { btScalar mass(0.); createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0)); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index 1bea23882..445e0b479 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -122,6 +122,7 @@ void btReducedSoftBody::setMassScale(const btScalar rho) void btReducedSoftBody::setFixedNodes(const int n_node) { m_fixedNodes.push_back(n_node); + m_nodes[n_node].m_im = 0; // set inverse mass to be zero for the constraint solver. } void btReducedSoftBody::internalInitialization() @@ -235,7 +236,7 @@ void btReducedSoftBody::updateReducedDofs(btScalar solverdt) } } -void btReducedSoftBody::mapToFullDofs(const btTransform& ref_trans) +void btReducedSoftBody::mapToFullPosition(const btTransform& ref_trans) { btVector3 origin = ref_trans.getOrigin(); btMatrix3x3 rotation = ref_trans.getBasis(); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index f1e1252c6..2cb54041a 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -76,6 +76,8 @@ class btReducedSoftBody : public btSoftBody tDenseArray m_nodalMass; // Mass on each node btAlignedObjectArray m_fixedNodes; // index of the fixed nodes + btAlignedObjectArray m_contactNodesList; + // // Api // @@ -133,7 +135,7 @@ class btReducedSoftBody : public btSoftBody void updateReducedVelocity(btScalar solverdt); // map to full degree of freedoms - void mapToFullDofs(const btTransform& ref_trans); + void mapToFullPosition(const btTransform& ref_trans); // compute full space velocity from the reduced velocity void mapToFullVelocity(const btTransform& ref_trans); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index 79abc537e..74704455d 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -34,6 +34,23 @@ void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt) { btReducedSoftBody* rsb = static_cast(m_softBodies[i]); + // clear contacts variables + rsb->m_nodeRigidContacts.resize(0); + rsb->m_faceRigidContacts.resize(0); + rsb->m_faceNodeContacts.resize(0); + + // calculate inverse mass matrix for all nodes + if (rsb->isActive()) + { + for (int j = 0; j < rsb->m_nodes.size(); ++j) + { + if (rsb->m_nodes[j].m_im > 0) + { + rsb->m_nodes[j].m_effectiveMass_inv = rsb->m_nodes[j].m_effectiveMass.inverse(); + } + } + } + // apply damping rsb->applyDamping(solverdt); @@ -53,6 +70,17 @@ void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt) // predict full space velocity (needed for constraints) rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform()); + // update full space nodal position + rsb->mapToFullPosition(rsb->getInterpolationWorldTransform()); + + // update bounding box + rsb->updateBounds(); + + // std::cout << "bounds\n"; + // std::cout << rsb->m_bounds[0][0] << '\t' << rsb->m_bounds[0][1] << '\t' << rsb->m_bounds[0][2] << '\n'; + // std::cout << rsb->m_bounds[1][0] << '\t' << rsb->m_bounds[1][1] << '\t' << rsb->m_bounds[1][2] << '\n'; + + // apply fixed constraints // rsb->applyFixedContraints(solverdt); @@ -96,7 +124,7 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) rsb->proceedToTransform(timeStep, true); // update mesh nodal positions for the next time step - rsb->mapToFullDofs(rsb->getRigidTransform()); + rsb->mapToFullPosition(rsb->getRigidTransform()); // end of time step clean up and update rsb->endOfTimeStepZeroing(); @@ -104,7 +132,42 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) m_simTime += timeStep; } -void btReducedSoftBodySolver::solveConstraints(btScalar timeStep) +void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedSoftBody* rsb = static_cast(m_softBodies[i]); + + // set fixed constraints + for (int j = 0; j < rsb->m_nodes.size(); ++j) + { + // if (rsb->m_nodes[j].m_im == 0) + // { + // btDeformableStaticConstraint static_constraint(&rsb->m_nodes[j], infoGlobal); + // m_staticConstraints[i].push_back(static_constraint); + // } + } + + // set Deformable Node vs. Rigid constraint + rsb->m_contactNodesList.clear(); + for (int j = 0; j < rsb->m_nodeRigidContacts.size(); ++j) + { + rsb->m_contactNodesList.push_back(rsb->m_nodeRigidContacts[j].m_node->index); + // const btSoftBody::DeformableNodeRigidContact& contact = rsb->m_nodeRigidContacts[j]; + // // skip fixed points + // if (contact.m_node->m_im == 0) + // { + // continue; + // } + // btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal); + // m_nodeRigidConstraints[i].push_back(constraint); + } + std::cout << "#contact nodes: " << rsb->m_contactNodesList.size() << "\n"; + + } +} + +void btReducedSoftBodySolver::solveDeformableConstraints(btScalar timeStep) { for (int iter = 0; iter < 100; ++iter) // TODO: add a fllag for this. { diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h index c456c042a..efb6fa470 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h @@ -36,7 +36,11 @@ class btReducedSoftBodySolver : public btDeformableBodySolver virtual void applyTransforms(btScalar timeStep); - virtual void solveConstraints(btScalar timeStep); + // set up contact constraints + virtual void setConstraints(const btContactSolverInfo& infoGlobal); + + // solve all constraints (fixed and contact) + virtual void solveDeformableConstraints(btScalar solverdt); // virtual void setProjection() {} @@ -44,7 +48,6 @@ class btReducedSoftBodySolver : public btDeformableBodySolver // virtual void setupDeformableSolve(bool implicit); - // virtual void solveDeformableConstraints(btScalar solverdt); }; #endif // BT_REDUCED_SOFT_BODY_DYNAMICS_WORLD_H \ No newline at end of file diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index d8b47ea4e..bfae800ac 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -72,7 +72,7 @@ public: void reinitialize(const btAlignedObjectArray& softBodies, btScalar dt); // set up contact constraints - void setConstraints(const btContactSolverInfo& infoGlobal); + virtual void setConstraints(const btContactSolverInfo& infoGlobal); // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion virtual void predictMotion(btScalar solverdt); @@ -199,11 +199,9 @@ public: m_objective->m_projection.setLagrangeMultiplier(); } - virtual void solveConstraints(btScalar timeStep) {} - // unused functions virtual void optimize(btAlignedObjectArray& softBodies, bool forceUpdate = false) {} - // virtual void solveConstraints(btScalar dt) {} + virtual void solveConstraints(btScalar dt) {} virtual bool checkInitialized() { return true; } virtual void copyBackToSoftBodies(bool bMove = true) {} }; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index d6749d550..ce4d9a9da 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -326,9 +326,6 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar 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); } void btDeformableMultiBodyDynamicsWorld::setupConstraints()