Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-08-19 01:29:59 +00:00
Code Issues Packages Projects Releases Wiki Activity

normal impulse for multibody vs reduced deformable contact is working

Browse Source
This commit is contained in:
jingyuc
2021-09-15 16:45:20 -04:00
parent 19375d6569
commit bfd3d1cd8d
3 changed files with 65 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp
View File

@@ -217,9 +217,8 @@ void ReducedMotorGrasp::initPhysics()
m_broadphase = new btDbvtBroadphase();
btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
// btVector3 gravity = btVector3(0, 0, 0);
btVector3 gravity = btVector3(0, 0, 0);
// btVector3 gravity = btVector3(0, -9.81, 0);
btVector3 gravity = btVector3(0, 1, 0);
reducedSoftBodySolver->setGravity(gravity);
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
@@ -331,6 +330,8 @@ void ReducedMotorGrasp::initPhysics()
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
rsb->setRigidVelocity(btVector3(0, 1, 0));
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body

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

@@ -66,14 +66,17 @@ btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstr
void btReducedDeformableRigidContactConstraint::setSolverBody(btSolverBody& solver_body)
{
m_solverBody = &solver_body;
m_linearComponentNormal = -m_contactNormalA * m_solverBody->internalGetInvMass();
btVector3 torqueAxis = -m_relPosA.cross(m_contactNormalA);
m_angularComponentNormal = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis;
m_linearComponentTangent = m_contactTangent * m_solverBody->internalGetInvMass();
btVector3 torqueAxisTangent = m_relPosA.cross(m_contactTangent);
m_angularComponentTangent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxisTangent;
if (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
m_solverBody = &solver_body;
m_linearComponentNormal = -m_contactNormalA * m_solverBody->internalGetInvMass();
btVector3 torqueAxis = -m_relPosA.cross(m_contactNormalA);
m_angularComponentNormal = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis;
m_linearComponentTangent = m_contactTangent * m_solverBody->internalGetInvMass();
btVector3 torqueAxisTangent = m_relPosA.cross(m_contactTangent);
m_angularComponentTangent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxisTangent;
}
}
btVector3 btReducedDeformableRigidContactConstraint::getVa() const
@@ -88,7 +91,7 @@ btVector3 btReducedDeformableRigidContactConstraint::getVa() const
btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
{
btVector3 Va = getVa();
// btVector3 Va = getVa();
// btVector3 deltaVa = Va - m_bufferVelocityA;
// if (!m_collideStatic)
// {
@@ -103,8 +106,8 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
// if (!m_collideStatic)
// {
// std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n';
// std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n';
std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n';
std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n';
// }
// get delta relative velocity and magnitude (i.e., how much impulse has been applied?)
@@ -113,17 +116,17 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
// if (!m_collideStatic)
// {
// std::cout << "deltaV_rel: " << deltaV_rel[0] << '\t' << deltaV_rel[1] << '\t' << deltaV_rel[2] << "\n";
// std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n";
// std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n';
std::cout << "deltaV_rel: " << deltaV_rel[0] << '\t' << deltaV_rel[1] << '\t' << deltaV_rel[2] << "\n";
std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n";
std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n';
// }
// get the normal impulse to be applied
btScalar deltaImpulse = m_rhs - deltaV_rel_normal / m_normalImpulseFactor;
// if (!m_collideStatic)
// {
// std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << "\n";
// std::cout << "m_normalImpulseFactor: " << m_normalImpulseFactor << '\n';
std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << "\n";
std::cout << "m_normalImpulseFactor: " << m_normalImpulseFactor << '\n';
// }
{
@@ -143,8 +146,8 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
// if (!m_collideStatic)
// {
// std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n';
// std::cout << "deltaImpulse: " << deltaImpulse << '\n';
std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n';
std::cout << "deltaImpulse: " << deltaImpulse << '\n';
// }
// residual is the nodal normal velocity change in current iteration
@@ -232,7 +235,7 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
{
const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, deltaImpulse);
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, -deltaImpulse);
// if (impulse_tangent.norm() > SIMD_EPSILON)
// {
// // apply tangential component of the impulse
@@ -354,7 +357,45 @@ btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVa() const
btVector3 deltaVa(0, 0, 0);
if (!m_collideStatic)
{
deltaVa = m_solverBody->internalGetDeltaLinearVelocity() + m_solverBody->internalGetDeltaAngularVelocity().cross(m_relPosA);
if (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
deltaVa = m_solverBody->internalGetDeltaLinearVelocity() + m_solverBody->internalGetDeltaAngularVelocity().cross(m_relPosA);
}
else if (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = 0;
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
// add in the normal component of the va
btScalar vel = 0;
for (int k = 0; k < ndof; ++k)
{
vel += local_dv[k] * J_n[k];
}
deltaVa = m_contact->m_cti.m_normal * vel;
// add in the tangential components of the va
vel = 0;
for (int k = 0; k < ndof; ++k)
{
vel += local_dv[k] * J_t1[k];
}
deltaVa += m_contact->t1 * vel;
vel = 0;
for (int k = 0; k < ndof; ++k)
{
vel += local_dv[k] * J_t2[k];
}
deltaVa += m_contact->t2 * vel;
}
}
}
return deltaVa;
}

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

@@ -8,7 +8,7 @@
btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
: btSoftBody(worldInfo, node_count, x, m)
{
m_rigidOnly = false; //! only use rigid frame to debug
m_rigidOnly = true; //! only use rigid frame to debug
// reduced deformable
m_reducedModel = true;