diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index df3b0b549..dc094b1ce 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -183,7 +183,7 @@ void BasicTest::initPhysics() // rsb->scale(btVector3(1, 1, 1)); //TODO: add back scale rsb->translate(btVector3(0, 4, 0)); // rsb->setTotalMass(0.5); - rsb->setStiffnessScale(20); + rsb->setStiffnessScale(100); rsb->setDamping(damping_alpha, damping_beta); // set fixed nodes @@ -213,7 +213,7 @@ void BasicTest::initPhysics() getDeformableDynamicsWorld()->setUseProjection(true); getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3; getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200); - getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 0; + getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true; getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; // add a few rigid bodies diff --git a/examples/ReducedDeformableDemo/FreeFall.cpp b/examples/ReducedDeformableDemo/FreeFall.cpp index 5c23a4f2d..e76a97025 100644 --- a/examples/ReducedDeformableDemo/FreeFall.cpp +++ b/examples/ReducedDeformableDemo/FreeFall.cpp @@ -30,7 +30,7 @@ // static btScalar E = 50; // static btScalar nu = 0.3; static btScalar damping_alpha = 0.0; -static btScalar damping_beta = 0.01; +static btScalar damping_beta = 0.0; static btScalar COLLIDING_VELOCITY = 0; static int start_mode = 6; static int num_modes = 1; @@ -161,9 +161,9 @@ void FreeFall::initPhysics() getDeformableDynamicsWorld()->addSoftBody(rsb); rsb->getCollisionShape()->setMargin(0.1); // rsb->scale(btVector3(1, 1, 1)); - rsb->translate(btVector3(0, 10, 0)); //TODO: add back translate and scale + rsb->translate(btVector3(0, 5, 0)); //TODO: add back translate and scale // rsb->setTotalMass(0.5); - rsb->setStiffnessScale(0.5); + rsb->setStiffnessScale(1); rsb->setDamping(damping_alpha, damping_beta); // rsb->setFriction(200); @@ -179,7 +179,7 @@ void FreeFall::initPhysics() btSoftBodyHelpers::generateBoundaryFaces(rsb); // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); - // rsb->setRigidVelocity(btVector3(0, 0, 0)); + // rsb->setRigidVelocity(btVector3(0, 0, 1)); // rsb->setRigidAngularVelocity(btVector3(1, 0, 0)); // btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); @@ -188,14 +188,16 @@ 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(3), btScalar(3), btScalar(3))); + // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50))); + btBoxShape* groundShape = createBoxShape(btVector3(btScalar(5), btScalar(5), btScalar(5))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -50, 0)); + // groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0)); + groundTransform.setOrigin(btVector3(0, -2, -2)); // groundTransform.setOrigin(btVector3(0, 0, 6)); + // groundTransform.setOrigin(btVector3(0, -50, 0)); { btScalar mass(0.); createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0)); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp index ec414f8d4..18e88124f 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp @@ -48,7 +48,16 @@ btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstr const btContactSolverInfo& infoGlobal, btScalar dt) : m_rsb(rsb), m_dt(dt), btDeformableRigidContactConstraint(c, infoGlobal) -{} +{ + m_appliedNormalImpulse = 0; + m_appliedTangentImpulse = 0; + m_impulseFactorNormal = 0; + m_impulseFactorTangent = 0; + + m_contactNormalA = c.m_cti.m_normal; + m_contactNormalB = -c.m_cti.m_normal; + m_impulseFactorInv = c.m_c0; +} btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { @@ -77,7 +86,7 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont } // if (!infoGlobal.m_splitImpulse) // { - // dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + // v_rel_normal += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; // } // if it's separating, no need to do anything @@ -87,33 +96,36 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont } btScalar residualSquare = v_rel_normal * v_rel_normal; // get residual - // compute the tangential relative vel - btVector3 v_rel_tangent = v_rel - v_rel_normal * cti.m_normal; + // // compute the tangential relative vel + // btVector3 v_rel_tangent = v_rel - v_rel_normal * cti.m_normal; - // friction correction - btScalar delta_v_rel_normal = v_rel_normal; - btScalar delta_v_rel_tangent = m_contact->m_c3 * v_rel_normal; - // btScalar delta_v_rel_tangent = 0.3 * v_rel_normal; + // // friction correction + // btScalar delta_v_rel_normal = v_rel_normal; + // btScalar delta_v_rel_tangent = m_contact->m_c3 * v_rel_normal; + // // btScalar delta_v_rel_tangent = 0.3 * v_rel_normal; - btVector3 impulse_tangent(0, 0, 0); - if (v_rel_tangent.norm() < delta_v_rel_tangent) - { - // the object should be static - impulse_tangent = m_contact->m_c0 * (-v_rel_tangent); - } - else - { - // apply friction - impulse_tangent = m_contact->m_c0 * (-v_rel_tangent.safeNormalize() * delta_v_rel_tangent); - std::cout << "friction called\n"; - } + // btVector3 impulse_tangent(0, 0, 0); + // if (v_rel_tangent.norm() < delta_v_rel_tangent) + // { + // // the object should be static + // impulse_tangent = m_contact->m_c0 * (-v_rel_tangent); + // } + // else + // { + // // apply friction + // impulse_tangent = m_contact->m_c0 * (-v_rel_tangent.safeNormalize() * delta_v_rel_tangent); + // std::cout << "friction called\n"; + // } // get total impulse - btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * (-v_rel_normal)); + btVector3 impulse_normal = m_contact->m_c0 * (-v_rel); + // btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * (-v_rel_normal)); // btVector3 impulse = impulse_normal + impulse_tangent; btVector3 impulse = impulse_normal.dot(cti.m_normal) * cti.m_normal; - std::cout << "impulse direct: " << impulse[0] / cti.m_normal[0] << '\t' << impulse[1] / cti.m_normal[1]<< '\t' << impulse[2] / cti.m_normal[2] << '\n'; + btVector3 impulse_dir = impulse; + impulse_dir.safeNormalize(); + std::cout << "impulse direct: " << impulse_dir[0] << '\t' << impulse_dir[1] << '\t' << impulse_dir[2] << '\n'; applyImpulse(impulse); // applyImpulse(impulse); // TODO: apply impulse? @@ -156,7 +168,16 @@ btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidConta const btContactSolverInfo& infoGlobal, btScalar dt) : m_node(contact.m_node), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt) -{} +{ + m_relPosA = contact.m_c1; + m_relPosB = m_node->m_x - m_rsb->getRigidTransform().getOrigin(); + warmStarting(); +} + +void btReducedDeformableNodeRigidContactConstraint::warmStarting() +{ + // +} btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const { @@ -176,7 +197,7 @@ btVector3 btReducedDeformableNodeRigidContactConstraint::getDv(const btSoftBody: void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse) { std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n'; - m_rsb->applyFullSpaceImpulse(impulse, m_contact->m_c1, m_node->index, m_dt); + m_rsb->applyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt); m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform()); std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n'; // std::cout << "node: " << m_node->index << " m_x: " << m_node->m_x[0] << '\t' << m_node->m_x[1] << '\t' << m_node->m_x[2] << '\n'; diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h index 920e36544..a481589b7 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h @@ -34,6 +34,17 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac btReducedSoftBody* m_rsb; btScalar m_dt; + btScalar m_appliedNormalImpulse; + btScalar m_appliedTangentImpulse; + btScalar m_impulseFactorNormal; + btScalar m_impulseFactorTangent; + + btVector3 m_contactNormalA; // for rigid body + btVector3 m_contactNormalB; // for reduced deformable body + btVector3 m_relPosA; // relative position of the contact point for A + btVector3 m_relPosB; // relative position of the contact point for B + btMatrix3x3 m_impulseFactorInv; // total inverse impulse matrix + btReducedDeformableRigidContactConstraint(btReducedSoftBody* rsb, const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal, @@ -42,8 +53,12 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac btReducedDeformableRigidContactConstraint() {} virtual ~btReducedDeformableRigidContactConstraint() {} + virtual void warmStarting() {} + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + virtual void applyImpulse(const btVector3& impulse) {} + virtual void applySplitImpulse(const btVector3& impulse) {} // TODO: may need later }; @@ -61,6 +76,8 @@ class btReducedDeformableNodeRigidContactConstraint : public btReducedDeformable btReducedDeformableNodeRigidContactConstraint() {} virtual ~btReducedDeformableNodeRigidContactConstraint() {} + virtual void warmStarting(); + // get the velocity of the deformable node in contact virtual btVector3 getVb() const; diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index 1e374517d..87760ac72 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -1,4 +1,5 @@ #include "btReducedSoftBody.h" +#include "../btSoftBodyInternals.h" #include "btReducedSoftBodyHelpers.h" #include "LinearMath/btTransformUtil.h" #include @@ -7,6 +8,8 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m) : btSoftBody(worldInfo, node_count, x, m) { + m_rigidOnly = true; //! only use rigid frame to debug + // reduced deformable m_reducedModel = true; m_startMode = 0; @@ -450,8 +453,7 @@ btMatrix3x3 btReducedSoftBody::getImpulseFactor(int n_node) btMatrix3x3 K2 = RSARinv + ri_skew * m_interpolateInvInertiaTensorWorld * sum_multiply_A * rotation.transpose(); - return K1; //TODO: change back - // return K1 + K2; + return m_rigidOnly ? K1 : K1 + K2; } void btReducedSoftBody::applyVelocityConstraint(const btVector3& target_vel, int n_node, btScalar dt) @@ -485,14 +487,17 @@ void btReducedSoftBody::applyPositionConstraint(const btVector3& target_pos, int void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt) { - // // apply impulse force - // applyFullSpaceNodalForce(impulse / dt, n_node); + if (!m_rigidOnly) + { + // apply impulse force + applyFullSpaceNodalForce(impulse / dt, n_node); - // // update reduced internal force - // applyReducedDampingForce(m_reducedVelocity); + // update reduced internal force + applyReducedDampingForce(m_reducedVelocity); - // // update reduced velocity - // updateReducedVelocity(dt); // TODO: add back + // update reduced velocity + updateReducedVelocity(dt); // TODO: add back + } // // update reduced dofs // updateReducedDofs(dt); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index a40725b8b..649267604 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -2,7 +2,6 @@ #define BT_REDUCED_SOFT_BODY_H #include "../btSoftBody.h" -#include "../btSoftBodyInternals.h" #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" @@ -55,6 +54,8 @@ class btReducedSoftBody : public btSoftBody public: + bool m_rigidOnly; + // // Fields // @@ -80,6 +81,7 @@ class btReducedSoftBody : public btSoftBody tDenseArray m_nodalMass; // Mass on each node btAlignedObjectArray m_fixedNodes; // index of the fixed nodes + // contacts btAlignedObjectArray m_contactNodesList; // diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index 5c1660db2..3190877d2 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -54,6 +54,12 @@ void btReducedSoftBodySolver::reinitialize(const btAlignedObjectArray(m_softBodies[i]); + rsb->m_contactNodesList.clear(); + } + btDeformableBodySolver::updateSoftBodies(); } @@ -129,12 +135,17 @@ void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt) // apply gravity to the rigid frame, get m_linearVelocity at time^* rsb->applyRigidGravity(m_gravity, solverdt); - // add internal force (elastic force & damping force) - // rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer); - // rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer); + if (!rsb->m_rigidOnly) + { + // add internal force (elastic force & damping force) + rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer); + rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer); + // get reduced velocity at time^* // get reduced velocity at time^* - rsb->updateReducedVelocity(solverdt, true); + // get reduced velocity at time^* + rsb->updateReducedVelocity(solverdt, true); + } // apply damping (no need at this point) // rsb->applyDamping(solverdt); @@ -150,12 +161,15 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) // rigid motion rsb->proceedToTransform(timeStep, true); - // update reduced dofs for time^n+1 - // rsb->updateReducedDofs(timeStep); + if (!rsb->m_rigidOnly) + { + // update reduced dofs for time^n+1 + rsb->updateReducedDofs(timeStep); - // update local moment arm for time^n+1 - rsb->updateLocalMomentArm(); - rsb->updateExternalForceProjectMatrix(true); + // update local moment arm for time^n+1 + rsb->updateLocalMomentArm(); + rsb->updateExternalForceProjectMatrix(true); + } // update mesh nodal positions for time^n+1 rsb->mapToFullPosition(rsb->getRigidTransform()); @@ -207,6 +221,7 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob m_nodeRigidConstraints[i].push_back(constraint); rsb->m_contactNodesList.push_back(contact.m_node->index); } + std::cout << "contact list size: " << rsb->m_contactNodesList.size() << "\n"; std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n"; // set Deformable Face vs. Rigid constraint diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 851cab761..24c47fa5f 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -37,7 +37,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b // solver body velocity <- rigid body velocity writeToSolverBody(bodies, numBodies, infoGlobal); - std::cout << "iter: " << iteration << "\tres: " << m_leastSquaresResidual << '\n'; + std::cout << "iter: " << iteration << "\tres: " << m_leastSquaresResidual << '\t'; std::cout << "------------------\n"; // std::cout << infoGlobal.m_leastSquaresResidualThreshold << '\n'; // std::cout << maxIterations << '\n'; @@ -59,6 +59,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b break; } } + std::cout << "======next step=========\n"; } return 0.f; } diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 2bf4b3262..b6ab39721 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1696,7 +1696,7 @@ struct btSoftColliders if (psb->m_reducedModel) { c.m_c0 = (psb->getImpulseFactor(n.index)).inverse(); - c.m_c1 = n.m_x - psb->getRigidTransform().getOrigin(); + // c.m_c1 = n.m_x - psb->getRigidTransform().getOrigin(); } else { @@ -1705,8 +1705,8 @@ struct btSoftColliders c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra); // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); - c.m_c1 = ra; } + c.m_c1 = ra; } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) {