Hi, I'm trying to implement a PGS solver for collision response, but I'm currently stuck.

How the problem manifests is for example a box resting on the ground, if the box is given a small rotation then it might spin out of control after a while.

Also boxes in a stack are not stable but slide apart very quickly. (without PGS my box stacks are decently stable)

Much grateful for any advice! 🙂

int maxIterations = 10;
for (int i = 0; i < maxIterations; i++) {
   bool converged = true;
   for (int j = 0; j < contact.counter; j++) {
       ContactPoint& contactPoint = contact.points[j];
       // Calculate relative velocity
       glm::vec3 rA = contactPoint.coordinate - objA.position;
       glm::vec3 rB = contactPoint.coordinate - objB.position;
       glm::vec3 relativeVelocity = (objB.linearVelocity + glm::cross(objB.angularVelocity, rB)) - (objA.linearVelocity + glm::cross(objA.angularVelocity, rA));
                               
       // Continue if objects already are moving apart
       if (glm::dot(relativeVelocity, contact.normal) > 0)
           continue;
       // Calculate J
       float e = std::min(objA.restitution, objB.restitution);
       float J = -(1.0f + e) * glm::dot(relativeVelocity, contact.normal);
       J /= objA.invMass + objB.invMass +
           glm::dot(glm::cross(rA, contact.normal), objA.inverseInertia * glm::cross(rA, contact.normal)) +
           glm::dot(glm::cross(rB, contact.normal), objB.inverseInertia * glm::cross(rB, contact.normal));
       // Clamp
       float temp = contactPoint.accumulatedImpulse;
       contactPoint.accumulatedImpulse = glm::max(temp + J, 0.0f);
       float deltaImpulse = contactPoint.accumulatedImpulse - temp;
       // Add normal
       glm::vec3 deltaNormalImpulse = deltaImpulse * contact.normal;
       // Apply impulse
       if (glm::length(deltaNormalImpulse) > 1e-6f) {
           objA.linearVelocity -= deltaNormalImpulse * objA.invMass;
           objA.angularVelocity -= objA.inverseInertia * glm::cross(rA, deltaNormalImpulse);
           objB.linearVelocity += deltaNormalImpulse * objB.invMass;
           objB.angularVelocity += objB.inverseInertia * glm::cross(rB, deltaNormalImpulse);
           converged = false;
       }
   }
   if (converged)
       break;
}

// position correction
if (objA.isStatic)
   objB.position += (collisionNormal * depth);
else if (objB.isStatic)
   objA.position += (-collisionNormal * depth);
else
{
   float totalInverseMass = objA.invMass + objB.invMass;
   float displacementA = (objA.invMass / totalInverseMass) * depth * strength;
   float displacementB = (objB.invMass / totalInverseMass) * depth * strength;

   objA.position -= collisionNormal * displacementA;
   objB.position += collisionNormal * displacementB;
}

Some things I notice:

• You are missing the frictional part (i.e. tangent impulses). Without that the behavior for stacking will not be good.
• Your position correction is too fast and won't work well with multiple objects, it needs to be inside the iteration loop to converge toward a global solution. I handle corrections by using the “split impulse” method and it works well without adding energy. This works by keeping 2 velocities per object (usual and bias velocity). At each iteration, calculate the bias impulse that would correct for a certain percentage of the penetration (e.g. 20%), 100% correction is unstable. Once converged, sum the usual and bias velocities and integrate to position. Bias velocity is reset to 0 every frame.
• Are you using a fixed time step? That can make a big improvement in stability.
• How do you manage the contacts? That can also make a big difference. You want to maintain a contact manifold of at least 3 to 4 points, no more. Depending on how you do collision detection, you may have to build the contact manifold over multiple frames (adding a new point each frame, and throwing away the less useful points). Try to keep the points that maximize the area of the contact polygon.
• You handle restitution inside the iteration loop, which doesn't seem right. You instead want to calculate the “bounce speed” before the iterations, using the initial relative velocity, which should be the target separation speed. Then the iterations try to achieve the bounce speed by applying impulses. In your code, the bounce speed changes while iterating, which is strange.
• In my working code I apply the accumulated impulses from the previous frame in a pre-step, before the main iteration loop. This is called “warm starting” I believe, and helps converge to a good solution faster.
• Iterations should be applied across all objects in a simulation island together, i.e. you should apply impulses to all objects before moving onto the next iteration. It's not clear if that's what you are doing, doesn't seem like it from the code example. Stability and convergence can be improved by randomizing the order in which impulses are applied in each iteration. This randomization prevents the later object pairs from dominating the solution.
• Comparing to my code, it looks like you may have a sign error in the line: glm::max( temp + J, 0.0f). In my code I subtract J, not add.

@Aressera Thanks so much for the answer, it seems the problem was the restitution! I added this (code below) before the iteration begins and the stacks are as stable now as before I added PGS (even a bit more stable!). That's the problem with trying to implement physics without understanding it… sigh.

for (int j = 0; j < contact.counter; j++) {
    ContactPoint& contactPoint = contact.points[j];

    glm::vec3 rA = contactPoint.coordinate - objA.position;
    glm::vec3 rB = contactPoint.coordinate - objB.position;

    glm::vec3 initialRelativeVelocity = (objB.linearVelocity + glm::cross(objB.angularVelocity, rB)) - (objA.linearVelocity + glm::cross(objA.angularVelocity, rA));
    float initialNormalVel = glm::dot(initialRelativeVelocity, contact.normal);

    float e = std::min(objA.restitution, objB.restitution);
    
    // store targetBounceSpeed per contactPoint
    contactPoint.targetBounceSpeed = -(1.0f + e) * initialNormalVel;
}

It's hard to tell that it's working well enough before I add friction which I will try now. Thanks for the tip about warm starting and bias impulse! Simulation islands is something I've wanted to implement as well.

The bias impulse is the same as the one Erin Catto speaks about here I assume?
https://box2d.org/files/ErinCatto_SequentialImpulses_GDC2006.pdf

Question about warm starting: How do you map the new contact to a previously cached contact without iterating over all possible object combinations?

ContactPoints: I calculate all the intersection using Sutherland-Hodgman clipping. I currently have no method of simplifying it so 1-8 contactpoints.

Zoler1337 said:
The bias impulse is the same as the one Erin Catto speaks about here I assume? https://box2d.org/files/ErinCatto_SequentialImpulses_GDC2006.pdf

Yes. I used a combination of older Box2D and Bullet to figure out how to implement my physics engine.

Zoler1337 said:
Question about warm starting: How do you map the new contact to a previously cached contact without iterating over all possible object combinations?

I compare the new contact position in the local coordinate system of the object to the previous contact(s), and use a distance threshold to determine if it is the same. If found, I update all of the contact data (keeping the accumulated impulses). If not found, I add the new point to the manifold. If the number of points in the manifold would exceed 4, I find the 3 points which define the triangle with the largest area and keep those, and then add the new point.

If you have more than 4 contacts I suppose it can still work, but ideally you can reduce those to just a few using a similar approach to the above. I use GJK+EPA to find one contact per frame, though it would be more stable to do one-shot contact generation on the first frame there is a collision.

This is wrong:

// Continue if objects already are moving apart

if (glm::dot(relativeVelocity, contact.normal) > 0)

continue;

The relative velocity direction is handled by the accumulated impulse and clamping.

Hmm something is seriously wrong then because if I remove that check then impulses goes out of control (objects fly everywhere in random directions with massive speeds).

But it's fairly stable if I keep that check.

Maybe, but this is wrong for sure. This is the main improvement that comes from PGS over the impulse solver (as by Brian Mirtich) that lead to Sequential Impulses. The key realization is that you can apply contracting delta impulse as long as the accumulated impulse is larger zero. And you only can get a contracting impulse if you have a separating relative velocity.

I am referring to this part in your code:

float temp = contactPoint.accumulatedImpulse;

contactPoint.accumulatedImpulse = glm::max(temp + J, 0.0f);

float deltaImpulse = contactPoint.accumulatedImpulse - temp;

Also, you cannot just translate to resolve penetration:

if (objA.isStatic) objB.position += (collisionNormal * depth);

else if (objB.isStatic)

objA.position += (-collisionNormal * depth);

else { f

loat totalInverseMass = objA.invMass + objB.invMass;

float displacementA = (objA.invMass / totalInverseMass) * depth * strength;

float displacementB = (objB.invMass / totalInverseMass) * depth * strength;

objA.position -= collisionNormal * displacementA;

objB.position += collisionNormal * displacementB;

I would try without restitution and use Baumgarte stablization first (instead of position projection).

Thank you for the reply!

I have tried to implement the split impulse method @aressera recommended earlier and it works pretty well! Objects are now not penetrating too much in a stack of 10 objects and stability is way up when settling after a rotated collision. There's still some jitter and penetration though but maybe warm starting is what's needed.

// run after PGS iteration instead of correcting positions
for (int j = 0; j < contact.counter; j++) {
ContactPoint& cp = contact.points[j];

   float slop = 0.1f;
   float Beta = 0.2f;

   float penetrationError = glm::max(contact.depth - slop, 0.0f);
   float J_bias = cp.m_eff * (Beta / deltaTime) * penetrationError;
   glm::vec3 J_biasNormal = J_bias * contact.normal;

   objA.biasLinearVelocity -= J_biasNormal * objA.invMass;
   objA.biasAngularVelocity -= objA.inverseInertia * glm::cross(cp.rA, J_biasNormal);

   objB.biasLinearVelocity += J_biasNormal * objB.invMass;
   objB.biasAngularVelocity += objB.inverseInertia * glm::cross(cp.rB, J_biasNormal);
}

I then tried to set restitution = 0 (e) and it actually works! Much more stable and I could now remove this check:

if (glm::dot(relativeVelocity, contact.normal) > 0)
	continue;

How would I go about re-inserting restitution though? My physics eduction is sadly lagging way behind my programming.

Also if you got any specific tips for how a CS student could improve their physics skills it would be much appreciated. I saw you are working as a physics architect at Valve and that sounds like so much fun!

Edit: After a few seconds (5-10) a stack of objects start to slide apart.. no idea why but maybe it's something which is natural without friction and warm starting?

Yes, you will need warm starting for a stack of 10. The warm starting will improve friction and stacking dramatically. Splitting impulses works good as well.

The restitution is solved the way you suggest. But I did not verify if your formula is correct. One important gotcha is that you uses the relative velocity along the contact normal before applying the warm starting.

If I am asked this I usually recommend porting Box2D Lite to 3D. A lot of students I know have followed this advice and learned a bunch this way. Also, read everything from Erin Catto on Box2D.org. This is a good start…