AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity

need to project the impulse on the normal to prevent sliding

Browse Source
This commit is contained in:
jingyuc 2021-08-21 15:54:34 -04:00
parent 70ec2f9d46
commit 368385d5ad
4 changed files with 84 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
examples/ReducedDeformableDemo/FreeFall.cpp
View File

@ -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));

121
src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp
View File

@ -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 ===================

21
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp
View File

@ -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);

15
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp
View File

@ -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];