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

delta impulse version is working

Browse Source
This commit is contained in:
jingyuc 2021-08-24 00:40:27 -04:00
parent d9152b9cf5
commit 840db372b9
9 changed files with 223 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/ReducedDeformableDemo/FreeFall.cpp
View File

@ -161,7 +161,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, 4.5, 0)); //TODO: add back translate and scale
// rsb->setTotalMass(0.5);
rsb->setStiffnessScale(1);
rsb->setDamping(damping_alpha, damping_beta);
@ -194,7 +194,7 @@ void FreeFall::initPhysics()
btTransform groundTransform;
groundTransform.setIdentity();
// groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.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));

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

@ -53,48 +53,65 @@ btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstr
m_appliedTangentImpulse = 0;
m_impulseFactorNormal = 0;
m_impulseFactorTangent = 0;
m_rhs = 0;
m_contactNormalA = c.m_cti.m_normal;
m_contactNormalB = -c.m_cti.m_normal;
m_impulseFactorInv = c.m_c0;
m_normalImpulseFactorInv = (m_impulseFactorInv * m_contactNormalA).dot(m_contactNormalA);
}
btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
{
const btSoftBody::sCti& cti = m_contact->m_cti;
btVector3 va = getVa();
btVector3 vb = getVb();
// btVector3 normal(0, 1, 0);
// for (int p = 0; p < m_rsb->m_nFull; ++p)
// {
// for (int k = 0; k < 3; ++k)
// {
// std::cout << m_rsb->m_nodes[p].m_x[k] << '\t';
// }
// std::cout << '\n';
// }
// const btSoftBody::sCti& cti = m_contact->m_cti;
btVector3 deltaVa = getVa() - m_bufferVelocityA;
btVector3 deltaVb = getDeltaVb();
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 relative velocity and magnitude
btVector3 v_rel = vb - va;
btScalar v_rel_normal = btDot(v_rel, cti.m_normal);
if (m_penetration > 0)
btVector3 deltaV_rel = deltaVa - deltaVb;
btScalar deltaV_rel_normal = -btDot(deltaV_rel, m_contactNormalA);
// btVector3 v_rel = va - vb;
// btScalar v_rel_normal = -btDot(v_rel, m_contactNormalA);
// if (m_penetration > 0)
// {
// std::cout << "penetrate!!!!\n";
// v_rel_normal += m_penetration / infoGlobal.m_timeStep; // add penetration correction vel
// }
btScalar deltaImpulse = m_rhs - deltaV_rel_normal * m_normalImpulseFactorInv;
// btScalar deltaImpulse = m_rhs - v_rel_normal * m_normalImpulseFactorInv;
// cumulative impulse that has been applied
btScalar sum = m_appliedNormalImpulse + deltaImpulse;
// if the cumulative impulse is pushing the object into the rigid body, set it zero
if (sum < 0)
{
std::cout << "penetrate!!!!\n";
v_rel_normal += m_penetration / infoGlobal.m_timeStep; // add penetration correction vel
std::cout <<"set zeroed!!!\n";
deltaImpulse = -m_appliedNormalImpulse;
m_appliedNormalImpulse = 0;
}
else
{
m_appliedNormalImpulse = sum;
}
std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n';
std::cout << "deltaImpulse: " << deltaImpulse << '\n';
// if (!infoGlobal.m_splitImpulse)
// {
// v_rel_normal += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
// }
// if it's separating, no need to do anything
if (v_rel_normal > 0)
{
return 0;
}
btScalar residualSquare = v_rel_normal * v_rel_normal; // get residual
// if (v_rel_normal > 0)
// {
// return 0;
// }
// btScalar residualSquare = v_rel_normal * v_rel_normal; // get residual
btScalar residualSquare = deltaImpulse / m_normalImpulseFactorInv; // get residual
residualSquare *= residualSquare;
// // compute the tangential relative vel
// btVector3 v_rel_tangent = v_rel - v_rel_normal * cti.m_normal;
@ -118,10 +135,14 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
// }
// get total impulse
btVector3 impulse_normal = m_contact->m_c0 * (-v_rel);
// btVector3 impulse_normal = m_impulseFactorInv * (-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;
// btVector3 impulse = impulse_normal.dot(m_contactNormalA) * m_contactNormalA;
// btVector3 impulse = -m_normalImpulseFactorInv * v_rel_normal * m_contactNormalA;
// btVector3 impulse = m_appliedNormalImpulse * m_contactNormalA;
btVector3 impulse = deltaImpulse * m_contactNormalA;
btVector3 impulse_dir = impulse;
impulse_dir.safeNormalize();
@ -176,7 +197,20 @@ btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidConta
void btReducedDeformableNodeRigidContactConstraint::warmStarting()
{
//
btVector3 va = getVa();
btVector3 vb = getVb();
m_bufferVelocityA = va;
m_bufferVelocityB = vb;
// get relative velocity and magnitude
btVector3 v_rel = va - vb;
btScalar v_rel_normal = -btDot(v_rel, m_contactNormalA);
if (m_penetration > 0)
{
v_rel_normal += m_penetration / m_dt; // add penetration correction vel
}
m_rhs = -v_rel_normal * m_normalImpulseFactorInv;
}
btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const
@ -184,6 +218,11 @@ btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const
return m_node->m_v;
}
btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVb() const
{
return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index);
}
btVector3 btReducedDeformableNodeRigidContactConstraint::getSplitVb() const
{
return m_node->m_splitv;
@ -197,8 +236,9 @@ 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_relPosB, m_node->index, m_dt);
m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
m_rsb->internalApplyFullSpaceImpulse(impulse, m_relPosB, 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';
}

10
src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h
View File

@ -38,6 +38,8 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac
btScalar m_appliedTangentImpulse;
btScalar m_impulseFactorNormal;
btScalar m_impulseFactorTangent;
btScalar m_normalImpulseFactorInv;
btScalar m_rhs;
btVector3 m_contactNormalA; // for rigid body
btVector3 m_contactNormalB; // for reduced deformable body
@ -45,6 +47,9 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac
btVector3 m_relPosB; // relative position of the contact point for B
btMatrix3x3 m_impulseFactorInv; // total inverse impulse matrix
btVector3 m_bufferVelocityA; // velocity at the beginning of the iteration
btVector3 m_bufferVelocityB;
btReducedDeformableRigidContactConstraint(btReducedSoftBody* rsb,
const btSoftBody::DeformableRigidContact& c,
const btContactSolverInfo& infoGlobal,
@ -60,6 +65,8 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac
virtual void applyImpulse(const btVector3& impulse) {}
virtual void applySplitImpulse(const btVector3& impulse) {} // TODO: may need later
virtual btVector3 getDeltaVb() const = 0;
};
// ================= node vs rigid constraints ===================
@ -81,6 +88,9 @@ class btReducedDeformableNodeRigidContactConstraint : public btReducedDeformable
// get the velocity of the deformable node in contact
virtual btVector3 getVb() const;
// get velocity change of the node in contat
virtual btVector3 getDeltaVb() const;
// get the split impulse velocity of the deformable face at the contact point
virtual btVector3 getSplitVb() const;

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

@ -22,6 +22,8 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_co
// rigid motion
m_linearVelocity.setZero();
m_angularVelocity.setZero();
m_internalDeltaLinearVelocity.setZero();
m_internalDeltaAngularVelocity.setZero();
m_angularFactor.setValue(1, 1, 1);
m_linearFactor.setValue(1, 1, 1);
m_invInertiaLocal.setValue(1, 1, 1);
@ -51,6 +53,7 @@ void btReducedSoftBody::setReducedModes(int start_mode, int num_modes, int full_
m_reducedForceElastic.resize(m_nReduced, 0);
m_reducedForceDamping.resize(m_nReduced, 0);
m_reducedForceExternal.resize(m_nReduced, 0);
m_internalDeltaReducedVelocity.resize(m_nReduced, 0);
m_nodalMass.resize(full_size, 0);
m_localMomentArm.resize(m_nFull);
}
@ -234,12 +237,26 @@ void btReducedSoftBody::endOfTimeStepZeroing()
m_reducedForceElastic[i] = 0;
m_reducedForceDamping[i] = 0;
m_reducedForceExternal[i] = 0;
m_internalDeltaReducedVelocity[i] = 0;
m_reducedDofsBuffer[i] = m_reducedDofs[i];
m_reducedVelocityBuffer[i] = m_reducedVelocity[i];
}
// std::cout << "zeroed!\n";
}
void btReducedSoftBody::applyInternalVelocityChanges()
{
m_linearVelocity += m_internalDeltaLinearVelocity;
m_angularVelocity += m_internalDeltaAngularVelocity;
m_internalDeltaLinearVelocity.setZero();
m_internalDeltaAngularVelocity.setZero();
for (int r = 0; r < m_nReduced; ++r)
{
m_reducedVelocity[r] += m_internalDeltaReducedVelocity[r];
m_internalDeltaReducedVelocity[r] = 0;
}
}
void btReducedSoftBody::predictIntegratedTransform(btScalar dt, btTransform& predictedTransform)
{
btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, predictedTransform);
@ -292,33 +309,50 @@ void btReducedSoftBody::updateReducedVelocity(btScalar solverdt, bool explicit_f
void btReducedSoftBody::mapToFullVelocity(const btTransform& ref_trans)
{
TVStack v_from_reduced;
v_from_reduced.resize(m_nFull);
for (int i = 0; i < m_nFull; ++i)
{
btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i];
m_nodes[i].m_v = computeNodeFullVelocity(ref_trans, i);
}
}
// compute velocity contributed by the reduced velocity
for (int k = 0; k < 3; ++k)
const btVector3 btReducedSoftBody::computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const
{
btVector3 v_from_reduced(0, 0, 0);
btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node];
// compute velocity contributed by the reduced velocity
for (int k = 0; k < 3; ++k)
{
for (int r = 0; r < m_nReduced; ++r)
{
v_from_reduced[i][k] = 0;
for (int r = 0; r < m_nReduced; ++r)
{
v_from_reduced[i][k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
}
v_from_reduced[k] += m_modes[r][3 * n_node + k] * m_reducedVelocity[r];
}
}
// get new velocity
btVector3 vel = m_angularVelocity.cross(r_com) +
ref_trans.getBasis() * v_from_reduced +
m_linearVelocity;
return vel;
}
// get new velocity
m_nodes[i].m_v = m_angularVelocity.cross(r_com) +
ref_trans.getBasis() * v_from_reduced[i] +
m_linearVelocity;
const btVector3 btReducedSoftBody::internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const
{
btVector3 deltaV_from_reduced(0, 0, 0);
btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node];
// compute velocity contributed by the reduced velocity
for (int k = 0; k < 3; ++k)
{
for (int r = 0; r < m_nReduced; ++r)
{
deltaV_from_reduced[k] += m_modes[r][3 * n_node + k] * m_internalDeltaReducedVelocity[r];
}
}
// std::cout << "full space vel: \n";
// for (int i = 0; i < 4; ++i)
// {
// std::cout << m_nodes[i].m_v[0] << '\t' << m_nodes[i].m_v[1] << '\t' << m_nodes[i].m_v[2] << '\n';
// }
// get delta velocity
btVector3 deltaV = m_internalDeltaAngularVelocity.cross(r_com) +
ref_trans.getBasis() * deltaV_from_reduced +
m_internalDeltaLinearVelocity;
return deltaV;
}
void btReducedSoftBody::proceedToTransform(btScalar dt, bool end_of_time_step)
@ -377,6 +411,20 @@ void btReducedSoftBody::applyTorqueImpulse(const btVector3& torque)
#endif
}
void btReducedSoftBody::internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos)
{
if (m_inverseMass == btScalar(0.))
{
std::cout << "something went wrong...probably didn't initialize?\n";
btAssert(false);
}
// delta linear velocity
m_internalDeltaLinearVelocity += impulse * m_linearFactor * m_inverseMass;
// delta angular velocity
btVector3 torque = rel_pos.cross(impulse * m_linearFactor);
m_internalDeltaAngularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor;
}
void btReducedSoftBody::applyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos)
{
if (m_inverseMass != btScalar(0.))
@ -485,6 +533,27 @@ void btReducedSoftBody::applyPositionConstraint(const btVector3& target_pos, int
// applyFullSpaceImpulse(impulse, n_node, dt);
}
void btReducedSoftBody::internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt)
{
if (!m_rigidOnly)
{
// apply impulse force
applyFullSpaceNodalForce(impulse / dt, n_node);
// update reduced internal force
applyReducedDampingForce(m_reducedVelocity);
// delta reduced velocity
for (int r = 0; r < m_nReduced; ++r)
{
btScalar mass_inv = (m_Mr[r] == 0) ? 0 : 1.0 / m_Mr[r]; // TODO: this might be redundant, because Mr is identity
m_internalDeltaReducedVelocity[r] = dt * mass_inv * (m_reducedForceDamping[r] + m_reducedForceExternal[r]);
}
}
internalApplyRigidImpulse(impulse, rel_pos);
}
void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt)
{
if (!m_rigidOnly)
@ -572,4 +641,24 @@ void btReducedSoftBody::applyFixedContraints(btScalar dt)
// apply impulse again, position constraint
// applyPositionConstraint(m_x0[m_fixedNodes[n]], m_fixedNodes[n], dt);
}
}
btScalar btReducedSoftBody::getTotalMass() const
{
return m_mass;
}
btTransform& btReducedSoftBody::getRigidTransform()
{
return m_rigidTransformWorld;
}
const btVector3& btReducedSoftBody::getLinearVelocity() const
{
return m_linearVelocity;
}
const btVector3& btReducedSoftBody::getAngularVelocity() const
{
return m_angularVelocity;
}

36
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h
View File

@ -32,6 +32,10 @@ class btReducedSoftBody : public btSoftBody
TVStack m_localMomentArm; // Sq + x0
btVector3 m_internalDeltaLinearVelocity;
btVector3 m_internalDeltaAngularVelocity;
tDenseArray m_internalDeltaReducedVelocity;
protected:
// rigid frame
btScalar m_mass; // total mass of the rigid frame
@ -132,6 +136,8 @@ class btReducedSoftBody : public btSoftBody
void endOfTimeStepZeroing();
void applyInternalVelocityChanges();
//
// position and velocity update related
//
@ -148,6 +154,12 @@ class btReducedSoftBody : public btSoftBody
// compute full space velocity from the reduced velocity
void mapToFullVelocity(const btTransform& ref_trans);
// get a single node's full space velocity from the reduced velocity
const btVector3 computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const;
// get a single node's all delta velocity
const btVector3 internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const;
//
// rigid motion related
//
@ -164,9 +176,11 @@ class btReducedSoftBody : public btSoftBody
//
// apply impulse to the rigid frame
void internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos);
void applyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos);
// apply impulse to nodes in the full space
void internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt);
void applyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt);
// apply nodal external force in the full space
@ -199,24 +213,10 @@ class btReducedSoftBody : public btSoftBody
//
// accessors
//
btScalar getTotalMass() const
{
return m_mass;
}
btTransform& getRigidTransform()
{
return m_rigidTransformWorld;
}
const btVector3& getLinearVelocity() const
{
return m_linearVelocity;
}
const btVector3& getAngularVelocity() const
{
return m_angularVelocity;
}
btScalar getTotalMass() const;
btTransform& getRigidTransform();
const btVector3& getLinearVelocity() const;
const btVector3& getAngularVelocity() const;
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
void clampVelocity(btVector3& v) const {

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

@ -302,4 +302,13 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de
}
}
return residualSquare;
}
void btReducedSoftBodySolver::deformableBodyInternalWriteBack()
{
for (int i = 0; i < m_softBodies.size(); ++i)
{
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
rsb->applyInternalVelocityChanges();
}
}

3
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h
View File

@ -47,6 +47,9 @@ class btReducedSoftBodySolver : public btDeformableBodySolver
// solve all constraints (fixed and contact)
virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
// apply all the delta velocities
virtual void deformableBodyInternalWriteBack();
// virtual void setProjection() {}
// virtual void setLagrangeMultiplier() {}

2
src/BulletSoftBody/btDeformableBodySolver.h
View File

@ -199,6 +199,8 @@ public:
m_objective->m_projection.setLagrangeMultiplier();
}
virtual void deformableBodyInternalWriteBack() {}
// unused functions
virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {}
virtual void solveConstraints(btScalar dt) {}

2
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@ -56,6 +56,8 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
m_analyticsData.m_numBodies = numBodies;
m_analyticsData.m_numContactManifolds = numManifolds;
m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
m_deformableSolver->deformableBodyInternalWriteBack();
break;
}
}