Barraff's Collision Resolver: Contact point solve order

Please don't post Bullet support questions here, use the above forums instead.
Post Reply
SinisterMephisto
Posts: 20
Joined: Tue Sep 01, 2009 4:19 pm

Barraff's Collision Resolver: Contact point solve order

Post by SinisterMephisto »

Hi,
I recently implemented the collision resolution method presented by Barraff in his papers.
I am using a third party engine to acquire my contact points (Unity3d PhysX). Sometimes I get 2-3 contact points
and they are pretty far apart from Center Of Mass.

When i apply the solver this distance causes the amount of angularImpulse to be pretty high causing collisions that should have no rotations to rotate.

The baffling thing is that the impulse calculated by the other contact points do not cancel it out. I thought maybe accumulating the impulses and averaging them out will help but it is always the same result.
I even began sorting the contact points by depth. Min and Max depth. It only resulted in a change of rotation direction.

The linear components of collision resolution work perfectly only the angular suffer.

Code: Select all

//ra1 is the vector from body1's position to contact point (C - position)
//j is j (I don't know the word for it)
//nA1 is the collision normal

Vector3 torque = Vector3.Cross(ra1, j*nA1);
Vector3 temp = particle.InertiaTensorInv * torque;  //Angular velocity
angularVel += temp; //Updating

Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Re: Barraff's Collision Resolver: Contact point solve order

Post by Dirk Gregorius »

Look for "Sequential Impulse" and "Projected Gauss-Seidel (PGS)". Bullet has implementations of both!

Erin Catto has published a bunch of things about both topics. I suggest reading everything you find here: https://code.google.com/p/box2d/downloads/list
SinisterMephisto
Posts: 20
Joined: Tue Sep 01, 2009 4:19 pm

Re: Barraff's Collision Resolver: Contact point solve order

Post by SinisterMephisto »

I managed to get the unwanted rotation removed by not updating the velocities immediatley.
Basically solving all the contacts with the Rigidbody still at the same state.

I began reading the GDC papers on SI last night.

Are the 2d and 3d implementations the same?
Bullet seems like a large code base to sift through.

Thanks.
mikeshafer
Posts: 49
Joined: Fri Feb 24, 2012 10:45 am

Re: Barraff's Collision Resolver: Contact point solve order

Post by mikeshafer »

So I had the same problem when I first started implementing my own physics engine. I was using Chris Hecker's GDM article and it's a great start, but it produces jiggly results.

The more advanced (and more accurate) way to do it is to use a constraint solver. You're probably thinking "I don't need constraints, I'm not doing joints/rag-doll yet", but yeah it turns out you can have a constraint called a "contact constraint" that does collision response for you. You need to write a contact point refreshing engine and then you have to create constraints from your contacts. Then you solve your contact constraints. I will try to help you with what I wrote for my own engine:

Code: Select all

void MEHPhysicsRigidBody::UpdateContactPoints()
{
   scalar contactBreakingThreshold = BASE_CONTACT_BREAKING_THRESHOLD * MEHMAX(MINIMUM_CONTACT_RADIUS, GetMaxRadius());

   for (s32 i = 0; i < (s32)m_contactPoints.GetSize(); i++)
   {              
      if (m_contactPoints[i].penetrationDist >= contactBreakingThreshold)
      {
         RemoveManifoldPoint(i);
         i--;
      }
      else
      {       
         MEHVector3 projPt = m_contactPoints[i].transOtherNormal;
         projPt *= -m_contactPoints[i].penetrationDist;
         projPt += m_contactPoints[i].transPoint;
         
         MEHVector3 projDiff = m_contactPoints[i].transOtherPoint;
         projDiff -= projPt;
         
         if (projDiff.Magnitude2() >= contactBreakingThreshold*contactBreakingThreshold)
         {
            RemoveManifoldPoint(i);
            i--;
         }
      }
   }
}
This is my code for refreshing contact points. You need to call it once per frame for overlapping/non-sleeping objects. It is essentially derived from Bullet's methodology. Then you need a constraint solver. Something like:

Code: Select all


void MEHConstraint::ToNormal(const MEHVector3 &radius1, const MEHVector3 &radius2, const MEHVector3 &normal,
                           MEHPhysicsRigidBody *bodyA, MEHPhysicsRigidBody *bodyB, scalar penetrationVelocity, 
                           scalar erp, scalar cfm)
{
   m_jacobiVectors[0] = normal;
   
   m_jacobiVectors[1] = radius1;
   m_jacobiVectors[1].ToCrossProductWith(normal);
      
   m_jacobiVectors[2] = normal;
   m_jacobiVectors[2] *= (scalar)-1.0;

   m_jacobiVectors[3] = normal;
   m_jacobiVectors[3].ToCrossProductWith(radius2);
      
   scalar num1 = 0.0;
   
   num1 = m_jacobiVectors[0].DotProductWith(bodyA->GetVelocity()) + m_jacobiVectors[1].DotProductWith(bodyA->GetAngularVelocity());

   scalar num2 = 0.0;
   
   if (bodyB && bodyB->GetMass() != MEHMAXSCALAR)
      num2 = m_jacobiVectors[2].DotProductWith(bodyB->GetVelocity()) + m_jacobiVectors[3].DotProductWith(bodyB->GetAngularVelocity());

   scalar numerator = (scalar)(-1.0 + 0.0)*(num1 + num2) - (penetrationVelocity > 0.0 ? penetrationVelocity : penetrationVelocity * erp);
      
   m_constraint = numerator;
   
   m_erp = 1.0;
   m_cfm = cfm;
   m_calcDamping = false;
   m_isContact = true;

   m_lowerBound = 0.0;
   m_upperBound = (scalar)1e10;
}
erp and cfm are old variables introduced into the physics community by the open source ODE engine. There's a way to express them in terms of spring constant and damp ratio, that is one of Erin's papers. But for contact constraints I use an "erp" of 0.2 and a cfm of 0.0. The m_erp = 1.0 that you see is to make sure I don't do ERP twice for contact constraints. The thing with ERP for contact constraints is that you want to affect "position error" but not "velocity error" (I think Bullet is like this too). By error we're talking about how much we need to move and how much we need to speed up to resolve a collision. The movement (the amount of overlap) is divided by the time-step to make it a velocity and then you can just add it to your velocity and make it all a velocity error. That's essentially what this function is doing.

Finally, you need a constraint solver. Something like:

Code: Select all


void MEHJacobian::SolveSingleIteration(scalar elapsedSecs)
{
   if (m_numConstraints == 0)
      return;

   if (elapsedSecs == 0.0)
      return;
     
   // the equation we want to solve is:
   // 1/dt * c - J*(v/dt + M^-1F) = JM^-1J^T * lambda
   // where:
   // dt = elapsedSecs
   // c = m_constraint from the constraint data structure (this is a motor or error values)
   // J = a Jacobian row (the recipe for the constraint dynamic)
   // v = either <v_{m_owner}, v_{bodyB}> or <w_{m_owner}, w_{bodyB}> depending on linear or angular constraint
   // M = the masses matrix
   // lambda = impulse vector
   
   // we need to compute everything but lambda and then get it into Ax = b form to solve via Projected Gauss-Seidel where x is lambda
   scalar b[MAX_CONSTRAINTS_PER_JACOBIAN];
   u32 j;
      
   scalar oneOverDT = (scalar)1.0 / elapsedSecs;
   
   MEHVector3 linearDeltaAccelA = MEHVector3::Zero;
   MEHVector3 angularDeltaAccelA = MEHVector3::Zero;
   
   if (!m_bodyA->IsSleeping())
   {
      linearDeltaAccelA = m_bodyA->GetDeltaLinearVelocityFromSolver();
      linearDeltaAccelA *= oneOverDT;

      angularDeltaAccelA = m_bodyA->GetDeltaAngularVelocityFromSolver();
      angularDeltaAccelA *= oneOverDT;
   }

   MEHVector3 linearDeltaAccelB = MEHVector3::Zero;
   MEHVector3 angularDeltaAccelB = MEHVector3::Zero;

   if (m_bodyB && !m_bodyB->IsSleeping())
   {
      linearDeltaAccelB = m_bodyB->GetDeltaLinearVelocityFromSolver();
      linearDeltaAccelB *= oneOverDT;

      angularDeltaAccelB = m_bodyB->GetDeltaAngularVelocityFromSolver();
      angularDeltaAccelB *= oneOverDT;
   }

   for (u32 i = 0; i < m_numConstraints; i++)
   {  
      scalar aDP, bDP;
            
      aDP = m_constraints[i][0].DotProductWith(linearDeltaAccelA) + m_constraints[i][1].DotProductWith(angularDeltaAccelA);

      if (m_bodyB)
         bDP = m_constraints[i][2].DotProductWith(linearDeltaAccelB) + m_constraints[i][3].DotProductWith(angularDeltaAccelB);
      else
         bDP = 0.0;
      
      b[i] = m_C[i] - (aDP + bDP);
   }
   
   // now we solve for x (lambda) ((impulses)) using Projected Gauss-Seidel (basically Gauss-Seidel with a clamp)
   
   for (u32 i = 0; i < m_numConstraints; i++)
   {
      scalar AxSum = 0.0;
      for (j = 0; j < 3; j++)
      {
         AxSum += m_constraints[i][0][j] * m_a[j];
         AxSum += m_constraints[i][1][j] * m_a[j+3];
         AxSum += m_constraints[i][2][j] * m_a[j+6];
         AxSum += m_constraints[i][3][j] * m_a[j+9];
      }
      
      scalar rowDiff = b[i] - AxSum;
      scalar deltaLambda;
         
      if (MEHISZERO(m_d[i]))
         deltaLambda = 0.0;
      else
         deltaLambda = rowDiff / m_d[i];
         
      m_lastImpulses[i] = m_lambda[i];
      m_lambda[i] = m_lastImpulses[i] + deltaLambda;
         
      if (m_lambda[i] < m_constraints[i].GetLowerBound())
		   m_lambda[i] = m_constraints[i].GetLowerBound();
      else if (m_lambda[i] > m_constraints[i].GetUpperBound())
         m_lambda[i] = m_constraints[i].GetUpperBound();       

      deltaLambda = m_lambda[i] - m_lastImpulses[i];
      //MEHASSERT(m_lastImpulses[i] == 0.0 || MEHFABS(deltaLambda) < 100.0);
         
      for (j = 0; j < 12; j++)
         m_a[j] += m_B[j][i] * deltaLambda;
   }
   
   // we need to solve for v2 in M(v2 - v1) = dt*(J^T*lambda + Fext)
   // we have v2 = dt*(M^-1J^T*lambda) + M^-1*Fext*dt + v1
   MEHVector3 newLinearVelocityA;
   MEHVector3 newLinearVelocityB;
   MEHVector3 newAngularVelocityA;
   MEHVector3 newAngularVelocityB;
   
   MEHVector3 linearJTlambdaA = MEHVector3::Zero;
   MEHVector3 linearJTlambdaB = MEHVector3::Zero;
   MEHVector3 angularJTlambdaA = MEHVector3::Zero;
   MEHVector3 angularJTlambdaB = MEHVector3::Zero;
            
   for (j = 0; j < m_numConstraints; j++)
   {
      for (u32 i = 0; i < 3; i++)
      {
         linearJTlambdaA[i] += m_constraints[j][0][i] * m_lambda[j];
         angularJTlambdaA[i] += m_constraints[j][1][i] * m_lambda[j];

         linearJTlambdaB[i] += m_constraints[j][2][i] * m_lambda[j];         
         angularJTlambdaB[i] += m_constraints[j][3][i] * m_lambda[j];
      }

      m_aggregateImpulses[j] = m_lambda[j];
   }
   
   newLinearVelocityA = linearJTlambdaA;
   newLinearVelocityA *= elapsedSecs;
   newLinearVelocityA /= m_bodyA->GetMass();
   
   m_bodyA->GetDeltaLinearVelocityFromSolver() += newLinearVelocityA;
         
   newAngularVelocityA = angularJTlambdaA;
   newAngularVelocityA *= elapsedSecs;
   newAngularVelocityA *= m_bodyA->GetInverseInertiaTensor();
   
   MEHASSERT(newAngularVelocityA.Magnitude() < 10000);

   m_bodyA->GetDeltaAngularVelocityFromSolver() += newAngularVelocityA;
        
   if (m_bodyB)
   {
      newLinearVelocityB = linearJTlambdaB;
      newLinearVelocityB *= elapsedSecs;
      newLinearVelocityB /= m_bodyB->GetMass();
              
      m_bodyB->GetDeltaLinearVelocityFromSolver() += newLinearVelocityB;
            
      newAngularVelocityB = angularJTlambdaB;
      newAngularVelocityB *= elapsedSecs;
      newAngularVelocityB *= m_bodyB->GetInverseInertiaTensor();

      MEHASSERT(newAngularVelocityB.Magnitude() < 10000);
      
      m_bodyB->GetDeltaAngularVelocityFromSolver() += newAngularVelocityB;
   }
}

The naming is not entirely accurate because a Jacobian is not for solving--an equation is, lol. But anyway, it's tempting to do something in a for loop for solving a positive definite matrix using PGS. But I quickly found out that's not optimal either. You want ACCUMULATE the deltas and iterate through ALL your physics objects once each for each iteration of solving PGS. This is the true strength of an iterative method to find "happy ground" between a large amount of collisions. So it's still a for loop, but it's a for loop that goes through each physics object once for each iteration of the algorithm.

If you do something like that, it should look a lot better, given that your collision engine is good.

Let me know if you have any questions.

Mike
SinisterMephisto
Posts: 20
Joined: Tue Sep 01, 2009 4:19 pm

Re: Barraff's Collision Resolver: Contact point solve order

Post by SinisterMephisto »

I think I cried a little. Thanks a lot man. I will implement this tonight
SinisterMephisto
Posts: 20
Joined: Tue Sep 01, 2009 4:19 pm

Re: Barraff's Collision Resolver: Contact point solve order

Post by SinisterMephisto »

The last function was pretty hard to follow due to all the arrays. Where in the Bullet SDK did you get inspiration for this part?
mikeshafer
Posts: 49
Joined: Fri Feb 24, 2012 10:45 am

Re: Barraff's Collision Resolver: Contact point solve order

Post by mikeshafer »

I got inspiration from Erin's paper, Iterative Dynamics in his GDC2005 zip on his box2D site. Bullet does it too though. Specifically, my inspiration for going through all the objects in the scene once with each iteration of the solver came from Bullet's methodology. I'll post more code here for everyone:

Code: Select all

void MEHJacobian::Setup(scalar elapsedSecs)
{
   for (u32 j = 0; j < m_numConstraints; j++)
   {
      m_lastImpulses[j] = m_aggregateImpulses[j];
      m_aggregateImpulses[j] = 0.0;
   }

   scalar oneOverDT = (scalar)1.0 / elapsedSecs;
   
   u32 i, j;

   for (i = 0; i < m_numConstraints; i++)
   {                                           
      // calculate B = M^-1J^T
      for (j = 0; j < 3; j++)
      {
         m_B[j][i] = m_constraints[i][0][j];
         m_B[j][i] /= m_bodyA->GetMass();
         m_B[j+3][i] = m_constraints[i][1][j];
         
         if (m_bodyB)
         {
            m_B[j+6][i] = m_constraints[i][2][j];
            m_B[j+6][i] /= m_bodyB->GetMass();
            m_B[j+9][i] = m_constraints[i][3][j];
         }
         else
         {
            m_B[j+6][i] = 0.0;
            m_B[j+9][i] = 0.0;
         }         
      }
      
      MEHVector3 angCrossTimesInvTensorA(m_B[3][i], m_B[4][i], m_B[5][i]);
      angCrossTimesInvTensorA *= m_bodyA->GetInverseInertiaTensor();

      for (j = 0; j < 3; j++)
         m_B[j+3][i] = angCrossTimesInvTensorA[j];

      if (m_bodyB)
      {
         MEHVector3 angCrossTimesInvTensorB(m_B[9][i], m_B[10][i], m_B[11][i]);
         angCrossTimesInvTensorB *= m_bodyB->GetInverseInertiaTensor();

         for (j = 0; j < 3; j++)
            m_B[j+9][i] = angCrossTimesInvTensorB[j];
      }
            
      // the right hand side of the equation is for motors and damping.
      // damping is as follows:
      // k = m_{eff}*w^2
      // c = 2*m_{eff}*dampingRatio*w
      // gamma = 1/(c + dt*k)
      // beta = dt*k/(c + dt*k)
      // beta0 = k/(c + dt*k)
      // then we have Jv = C where:
      // C = beta0 * m_constraint + gamma * m_impulse
      
      if (m_constraints[i].NeedsDampingCalculation())
      {
         scalar totalEffectiveMass = 0.0;
      
         for (j = 0; j < 3; j++)
         {
            totalEffectiveMass += m_constraints[i][0][j] * m_B[j][i];
            totalEffectiveMass += m_constraints[i][1][j] * m_B[j+3][i];
            totalEffectiveMass += m_constraints[i][2][j] * m_B[j+6][i];
            totalEffectiveMass += m_constraints[i][3][j] * m_B[j+9][i];
         }
      
         totalEffectiveMass = (scalar)1.0 / totalEffectiveMass;

         scalar w = (scalar)2.0 * MEHMathToolbox::PI * m_constraints[i].GetDampingFrequency();
         scalar w2 = w*w;
         scalar k = totalEffectiveMass * w2;
         scalar c = (scalar)2.0 * totalEffectiveMass * m_constraints[i].GetDampingRatio() * w;
         scalar gamma = c + elapsedSecs * k;
         if (gamma != 0.0)
            gamma = (scalar)1.0 / gamma;
         scalar beta0 = elapsedSecs * k / (c + elapsedSecs * k);
         m_C[i] = beta0 * oneOverDT * m_constraints[i].GetConstraintValue() + gamma * m_lastImpulses[i];
      }
      else
      {
         m_C[i] = m_constraints[i].GetERP() * oneOverDT * m_constraints[i].GetConstraintValue() +
                  m_constraints[i].GetCFM() * m_lastImpulses[i];
      }
   }

   for (i = 0; i < m_numConstraints; i++)
   {
      m_d[i] = 0.0;
      //if (!m_constraints[i].NeedsDampingCalculation() && !m_constraints[i].IsContact())
      //   m_lastImpulses[i] = 0.0;

      for (j = 0; j < 3; j++)
      {
         m_d[i] += m_constraints[i][0][j] * m_B[j][i];
         m_d[i] += m_constraints[i][1][j] * m_B[j+3][i];
         m_d[i] += m_constraints[i][2][j] * m_B[j+6][i];
         m_d[i] += m_constraints[i][3][j] * m_B[j+9][i];
      }

      MEHASSERT(MEHFABS(m_d[i]) < 10000.0);
      
      m_lambda[i] = m_lastImpulses[i];
   }
   
   for (i = 0; i < 12; i++)
   { 
      m_a[i] = 0.0;

      for (j = 0; j < m_numConstraints; j++)
         m_a[i] += m_B[i][j] * m_lastImpulses[j];
   }
}
Someone needs to verify the if (m_constraints.NeedsDampingCalculation()) part though, I'm not sure I'm doing that correctly. It's based on the Erin's soft constraint presentation.

HTH,
Mike
SinisterMephisto
Posts: 20
Joined: Tue Sep 01, 2009 4:19 pm

Re: Barraff's Collision Resolver: Contact point solve order

Post by SinisterMephisto »

Might be a bit late to say this but if the only collisions i wish to resolve are ragdoll to ragdoll and ragdoll to floor what methods would be considered overkill. I have 0 need for stacking and physics engine is only meant for motion synthesis with no real desire to solve balancing.
Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Re: Barraff's Collision Resolver: Contact point solve order

Post by Dirk Gregorius »

Sequential impulses is the easiest you can you. If you have no need for stacking you can skip warmstarting.
Post Reply