add support for damping. the fixed constraint is working again with the deltaImpulse form

This commit is contained in:
jingyuc
2021-09-12 21:53:23 -04:00
parent 9d1aafc51c
commit b4efd91476
7 changed files with 51 additions and 64 deletions
+17 -19
View File
@@ -30,7 +30,7 @@
// static btScalar E = 50;
// static btScalar nu = 0.3;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static btScalar damping_beta = 0.01;
static btScalar COLLIDING_VELOCITY = 0;
static int start_mode = 6;
static int num_modes = 1;
@@ -134,16 +134,15 @@ public:
void stepSimulation(float deltaTime)
{
// TODO: remove this. very hacky way of adding initial deformation
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
if (first_step /* && !rsb->m_bUpdateRtCst*/)
{
getDeformedShape(rsb, 0, 1);
first_step = false;
// rsb->mapToReducedDofs();
}
// btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
// if (first_step /* && !rsb->m_bUpdateRtCst*/)
// {
// getDeformedShape(rsb, 0, 1);
// first_step = false;
// // rsb->mapToReducedDofs();
// }
float internalTimeStep = 1. / 60.f;
// float internalTimeStep = 1e-3;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
// sim_time += internalTimeStep;
@@ -198,7 +197,7 @@ void BasicTest::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();
@@ -224,18 +223,17 @@ void BasicTest::initPhysics()
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 4, 0));
init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
// rsb->setTotalMass(0.5);
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
// set fixed nodes
// rsb->setFixedNodes(0);
// rsb->setFixedNodes(1);
// rsb->setFixedNodes(2);
// rsb->setFixedNodes(3);
rsb->setFixedNodes(0);
rsb->setFixedNodes(1);
rsb->setFixedNodes(2);
rsb->setFixedNodes(3);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
@@ -247,7 +245,7 @@ void BasicTest::initPhysics()
// rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// rsb->setRigidVelocity(btVector3(0, 1, 0));
rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
// btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
// getDeformableDynamicsWorld()->addForce(rsb, gravity_force);
@@ -255,11 +253,11 @@ void BasicTest::initPhysics()
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(true);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
// add a few rigid bodies
// Ctor_RbUpStack(); // TODO: no rigid body for now
+5 -5
View File
@@ -30,7 +30,7 @@
// static btScalar E = 50;
// static btScalar nu = 0.3;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static btScalar damping_beta = 0.01;
static btScalar COLLIDING_VELOCITY = 0;
static int start_mode = 6;
static int num_modes = 10;
@@ -157,11 +157,11 @@ void FreeFall::initPhysics()
btTransform init_transform;
init_transform.setIdentity();
// init_transform.setOrigin(btVector3(0, 2.5, 0));
init_transform.setOrigin(btVector3(0, 10, 0));
// init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
// init_transform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
// init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, 0));
// init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
// rsb->setTotalMass(0.5);
@@ -275,7 +275,7 @@ void FreeFall::initPhysics()
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
@@ -30,10 +30,10 @@
// static btScalar E = 50;
// static btScalar nu = 0.3;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static btScalar damping_beta = 0.01;
static btScalar COLLIDING_VELOCITY = 4;
static int start_mode = 6;
static int num_modes = 1;
static int num_modes = 10;
class ReducedGrasp : public CommonDeformableBodyBase
{
@@ -287,8 +287,8 @@ void ReducedGrasp::initPhysics()
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 4, 0));
// init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(200);
@@ -11,33 +11,38 @@ btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint(
: m_rsb(rsb), m_ri(ri), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal)
{
// get impulse
m_impulseFactor = rsb->getImpulseFactor(m_node->index);
m_impulseFactorInv = rsb->getImpulseFactor(m_node->index).inverse();
}
btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
{
// target velocity of fixed constraint is 0
btVector3 impulse = -(m_impulseFactor.inverse() * m_node->m_v);
// apply full space impulse
std::cout << "node: " << m_node->index << " impulse: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
// std::cout << "impulse norm: " << impulse.norm() << "\n";
m_rsb->applyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
// get residual //TODO: only calculate the velocity of the given node
m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
btVector3 deltaVa = getDeltaVa();
btVector3 rel_vel = m_node->m_v + deltaVa;
btVector3 deltaImpulse = -(m_impulseFactorInv * rel_vel);
applyImpulse(deltaImpulse);
// calculate residual
btScalar residualSquare = btDot(m_node->m_v, m_node->m_v);
btScalar residualSquare = btDot(rel_vel, rel_vel);
return residualSquare;
}
// this calls reduced deformable body's applyFullSpaceImpulse
// this calls reduced deformable body's internalApplyFullSpaceImpulse
void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse)
{
m_rsb->applyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
// apply full space impulse
std::cout << "node: " << m_node->index << " impulse: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
m_rsb->internalApplyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
// get the new nodal velocity
// m_node->m_v = m_rsb->computeNodeFullVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index);
// m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
}
btVector3 btReducedDeformableStaticConstraint::getDeltaVa() const
{
return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index);
}
// ================= base contact constraints ===================
@@ -7,7 +7,7 @@ class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint
public:
btReducedSoftBody* m_rsb;
btScalar m_dt;
btMatrix3x3 m_impulseFactor;
btMatrix3x3 m_impulseFactorInv;
btVector3 m_ri;
btReducedDeformableStaticConstraint(btReducedSoftBody* rsb,
@@ -24,6 +24,8 @@ class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint
// this calls reduced deformable body's applyFullSpaceImpulse
virtual void applyImpulse(const btVector3& impulse);
btVector3 getDeltaVa() const;
// virtual void applySplitImpulse(const btVector3& impulse) {}
};
@@ -272,7 +272,7 @@ void btReducedSoftBody::updateReducedVelocity(btScalar solverdt, bool explicit_f
btScalar delta_v = 0;
if (explicit_force)
{
delta_v = solverdt * mass_inv * m_reducedForceElastic[r];
delta_v = solverdt * mass_inv * (m_reducedForceElastic[r] + m_reducedForceDamping[r]);
}
else
{
@@ -571,8 +571,8 @@ void btReducedSoftBody::internalApplyFullSpaceImpulse(const btVector3& impulse,
// apply impulse force
applyFullSpaceNodalForce(impulse / dt, n_node);
// update reduced internal force
applyReducedDampingForce(m_reducedVelocity); //TODO: this needs to be the current velocity
// update delta damping force
applyReducedDampingForce(m_internalDeltaReducedVelocity);
// delta reduced velocity
for (int r = 0; r < m_nReduced; ++r)
@@ -128,7 +128,6 @@ void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt)
void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt)
{
static bool applied = false;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
@@ -143,8 +142,6 @@ void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt)
rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer);
// get reduced velocity at time^*
// get reduced velocity at time^*
// get reduced velocity at time^*
rsb->updateReducedVelocity(solverdt, true);
}
@@ -273,27 +270,12 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de
{
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
btAlignedObjectArray<btScalar> residual;
residual.resize(m_staticConstraints[i].size(), 0);
for (int k = 0; k < m_staticConstraints[i].size(); ++k)
{
btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][k];
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
btVector3 error;
error.setZero();
std::cout << "fixed_nodes: ";
for (int p = 0; p < rsb->m_fixedNodes.size(); ++p)
{
std::cout << rsb->m_nodes[rsb->m_fixedNodes[p]].m_v.norm() << '\t';
error += rsb->m_nodes[rsb->m_fixedNodes[p]].m_v;
}
std::cout << '\n';
std::cout << "norm: " << error.norm() << "\n";
}
}
// handle contact constraint