diff --git a/examples/ReducedDeformableDemo/FreeFall.cpp b/examples/ReducedDeformableDemo/FreeFall.cpp index 7995cb95b..5c23a4f2d 100644 --- a/examples/ReducedDeformableDemo/FreeFall.cpp +++ b/examples/ReducedDeformableDemo/FreeFall.cpp @@ -108,11 +108,15 @@ public: 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"; } + // static int num = 0; for (int p = 0; p < rsb->m_nodeRigidContacts.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"; + // std::cout << rsb->m_nodes[rsb->m_contactNodesList[p]].m_x[0] << "\t" << rsb->m_nodes[rsb->m_contactNodesList[p]].m_x[1] << "\t" << rsb->m_nodes[rsb->m_contactNodesList[p]].m_x[2] << "\n"; + // std::cout << rsb->m_contactNodesList[p] << "\n"; + // num++; } + // if(num > 0) exit(100); 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)); @@ -134,7 +138,7 @@ void FreeFall::initPhysics() m_broadphase = new btDbvtBroadphase(); btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver(); - btVector3 gravity = btVector3(0, 0, 0); + btVector3 gravity = btVector3(0, -10, 0); reducedSoftBodySolver->setGravity(gravity); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); @@ -157,10 +161,11 @@ void FreeFall::initPhysics() getDeformableDynamicsWorld()->addSoftBody(rsb); rsb->getCollisionShape()->setMargin(0.1); // rsb->scale(btVector3(1, 1, 1)); - rsb->translate(btVector3(0, 0, 0)); //TODO: add back translate and scale + rsb->translate(btVector3(0, 10, 0)); //TODO: add back translate and scale // rsb->setTotalMass(0.5); rsb->setStiffnessScale(0.5); rsb->setDamping(damping_alpha, damping_beta); + // rsb->setFriction(200); // no fixed nodes // rsb->setFixedNodes(0); @@ -174,7 +179,7 @@ void FreeFall::initPhysics() btSoftBodyHelpers::generateBoundaryFaces(rsb); // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); - rsb->setRigidVelocity(btVector3(0, 0, 1)); + // rsb->setRigidVelocity(btVector3(0, 0, 0)); // rsb->setRigidAngularVelocity(btVector3(1, 0, 0)); // btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); @@ -183,14 +188,14 @@ void FreeFall::initPhysics() } // create a static rigid box as the ground { - 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(50), btScalar(50), btScalar(50))); + // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(3), btScalar(3), btScalar(3))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); - // groundTransform.setOrigin(btVector3(0, -50, 0)); - groundTransform.setOrigin(btVector3(0, 0, 6)); + groundTransform.setOrigin(btVector3(0, -50, 0)); + // groundTransform.setOrigin(btVector3(0, 0, 6)); { 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 a87f111d2..ec414f8d4 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp @@ -55,86 +55,66 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont const btSoftBody::sCti& cti = m_contact->m_cti; btVector3 va = getVa(); btVector3 vb = getVb(); - btVector3 vr = vb - va; - btScalar dn = btDot(vr, cti.m_normal); - // btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.m_deformable_cfm; - // if (m_penetration > 0) + + // btVector3 normal(0, 1, 0); + + // for (int p = 0; p < m_rsb->m_nFull; ++p) // { - // dn += m_penetration / infoGlobal.m_timeStep; + // for (int k = 0; k < 3; ++k) + // { + // std::cout << m_rsb->m_nodes[p].m_x[k] << '\t'; + // } + // std::cout << '\n'; // } + + // get relative velocity and magnitude + btVector3 v_rel = vb - va; + btScalar v_rel_normal = btDot(v_rel, cti.m_normal); + if (m_penetration > 0) + { + std::cout << "penetrate!!!!\n"; + v_rel_normal += m_penetration / infoGlobal.m_timeStep; // add penetration correction vel + } // if (!infoGlobal.m_splitImpulse) // { // dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; // } - // dn is the normal component of velocity diffrerence. Approximates the residual. - btVector3 vr_tangent = vr - dn * cti.m_normal; - btVector3 delta_vr = vr_tangent - vr; - - - btScalar residualSquare = dn * dn; - btVector3 impulse = (m_contact->m_c0 * delta_vr); - // btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); - // btVector3 impulse_tangent = impulse - impulse_normal; - std::cout << "impulse normal: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n'; - if (dn > 0) + + // if it's separating, no need to do anything + if (v_rel_normal > 0) { - std::cout << "skip\n"; return 0; } - // btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0))); - // if (!infoGlobal.m_splitImpulse) - // { - // impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal); - // } - // btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); - // btVector3 impulse_tangent = impulse - impulse_normal; - // if (dn > 0) - // { - // return 0; - // } - // m_binding = true; - // btScalar residualSquare = dn * dn; - // btVector3 old_total_tangent_dv = m_total_tangent_dv; - // // m_c5 is the inverse mass of the deformable node/face - // m_total_normal_dv -= m_contact->m_c5 * impulse_normal; - // m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent; + 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; + + // 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"; + } + + // get total impulse + 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'; - // if (m_total_normal_dv.dot(cti.m_normal) < 0) - // { - // // separating in the normal direction - // m_binding = false; - // m_static = false; - // impulse_tangent.setZero(); - // } - // else - // { - // if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm()) - // { - // // dynamic friction - // // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. - // m_static = false; - // if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) - // { - // m_total_tangent_dv = btVector3(0, 0, 0); - // } - // else - // { - // m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3; - // } - // // impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv); - // impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv); - // } - // else - // { - // // static friction - // m_static = true; - // } - // } - // impulse = impulse_normal + impulse_tangent; - // apply impulse to deformable nodes involved and change their velocities - // impulse = btVector3(0, 10, 0); - // impulse_normal = btVector3(0, 0, 0); - // impulse_tangent = btVector3(0, 0, 0); applyImpulse(impulse); // applyImpulse(impulse); // TODO: apply impulse? // apply impulse to the rigid/multibodies involved and change their velocities @@ -199,6 +179,7 @@ void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3 m_rsb->applyFullSpaceImpulse(impulse, m_contact->m_c1, 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'; } // ================= face vs rigid constraints =================== diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index f3e6cd4ff..1e374517d 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -17,8 +17,8 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_co m_rhoScale = 1.0; // rigid motion - m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); - m_angularVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); + m_linearVelocity.setZero(); + m_angularVelocity.setZero(); m_angularFactor.setValue(1, 1, 1); m_linearFactor.setValue(1, 1, 1); m_invInertiaLocal.setValue(1, 1, 1); @@ -259,6 +259,7 @@ void btReducedSoftBody::mapToFullPosition(const btTransform& ref_trans) for (int i = 0; i < m_nFull; ++i) { m_nodes[i].m_x = rotation * m_localMomentArm[i] + origin; + m_nodes[i].m_q = m_nodes[i].m_x; } } @@ -449,8 +450,8 @@ 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 K1; //TODO: change back + // return K1 + K2; } void btReducedSoftBody::applyVelocityConstraint(const btVector3& target_vel, int n_node, btScalar dt) @@ -484,14 +485,14 @@ 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); + // // 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/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index f88e68a7c..5c1660db2 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -130,8 +130,8 @@ void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt) rsb->applyRigidGravity(m_gravity, solverdt); // add internal force (elastic force & damping force) - rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer); - rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer); + // rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer); + // rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer); // get reduced velocity at time^* rsb->updateReducedVelocity(solverdt, true); @@ -151,7 +151,7 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) rsb->proceedToTransform(timeStep, true); // update reduced dofs for time^n+1 - rsb->updateReducedDofs(timeStep); + // rsb->updateReducedDofs(timeStep); // update local moment arm for time^n+1 rsb->updateLocalMomentArm(); @@ -209,7 +209,7 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob } std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n"; - // // set Deformable Face vs. Rigid constraint + // set Deformable Face vs. Rigid constraint // for (int j = 0; j < rsb->m_faceRigidContacts.size(); ++j) // { // const btSoftBody::DeformableFaceRigidContact& contact = rsb->m_faceRigidContacts[j]; @@ -218,8 +218,8 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob // { // continue; // } - // btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting); - // m_faceRigidConstraints[i].push_back(constraint); + // // btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting); + // // m_faceRigidConstraints[i].push_back(constraint); // } } @@ -269,12 +269,15 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de } // node vs rigid contact + std::cout << "!!#contact_nodes: " << m_nodeRigidConstraints[j].size() << '\n'; for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) { btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); } + + // face vs rigid contact // for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) // { // btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];