martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Explicitly formulating bias term.

master
Martin Felis 2020-11-15 21:46:43 +01:00
parent 4389dd19fc
commit 4a8d691334
1 changed files with 28 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/rbdlsim.cc
View File

@ -293,10 +293,13 @@ void CalcImpulseVariables(
const Vector3d& dir,
MatrixNd* MInv,
VectorNd* jac,
double* G_MInv_GT) {
double* G_MInv_GT,
double* bias_vel,
double restitution) {
if (body == nullptr || body->mIsStatic) {
jac->setZero();
*G_MInv_GT = 0.;
*bias_vel = 0.;
return;
}
@ -320,6 +323,8 @@ void CalcImpulseVariables(
(*jac) = dir.transpose() * G_constr;
*G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose();
*bias_vel = (*jac) * qdot * restitution;
}
void PrepareConstraintImpulse(
@ -333,7 +338,10 @@ void PrepareConstraintImpulse(
cinfo.dir,
&cinfo.MInvA,
&cinfo.jacA,
&cinfo.GMInvGTA);
&cinfo.GMInvGTA,
&cinfo.biasVelocityA,
cinfo.effectiveRestitution);
CalcImpulseVariables(
body_b,
cinfo.mBodyBIndex,
@ -341,7 +349,9 @@ void PrepareConstraintImpulse(
cinfo.dir,
&cinfo.MInvB,
&cinfo.jacB,
&cinfo.GMInvGTB);
&cinfo.GMInvGTB,
&cinfo.biasVelocityB,
cinfo.effectiveRestitution);
}
/// Calculates the impulse that we apply on body_b to resolve the contact.
@ -352,19 +362,30 @@ void CalcConstraintImpulse(
const double dt) {
// Todo: add nonlinear effects * dt
double ref_a = 0.;
double ref_b = 0.;
double vel_a = 0.;
double vel_b = 0.;
double ref = 0.;
double rhs = 0.;
if (body_a && !body_a->mIsStatic) {
rhs += cinfo.jacA * body_a->qdot * (1.0 + cinfo.effectiveRestitution);
vel_a = cinfo.jacA * body_a->qdot;
ref_a += cinfo.jacA * body_a->qdot * (1.0 + cinfo.effectiveRestitution);
rhs += cinfo.jacA * body_a->qdot + cinfo.biasVelocityA;
}
if (body_b && !body_b->mIsStatic) {
rhs += -cinfo.jacB * (body_b->qdot) * (1.0 + cinfo.effectiveRestitution);
vel_b = cinfo.jacB * body_b->qdot;
ref_b += -cinfo.jacB * (body_b->qdot) * (1.0 + cinfo.effectiveRestitution);
rhs += -cinfo.jacB * body_b->qdot - cinfo.biasVelocityB;
}
ref = ref_a + ref_b;
double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
double old_impulse = cinfo.accumImpulse;
// TODO: is this really needed here??
cinfo.deltaImpulse = std::max(0., rhs / denom);
cinfo.deltaImpulse = rhs / denom;
cinfo.accumImpulse = std::max(0., cinfo.accumImpulse + cinfo.deltaImpulse);
cinfo.deltaImpulse = cinfo.accumImpulse - old_impulse;
}
@ -446,6 +467,7 @@ void World::resolveCollisions(double dt) {
for (CollisionInfo& cinfo : mContactPoints) {
CalcConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo, dt);
ApplyConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
gLog ("Iter %d: Apply impulse: %f", i, cinfo.deltaImpulse);
}
}
}