The sinking problem is due to solving the penetration constraint on the velocity level. This is a linearization of whatever the actual non-linear equations represent the simulation. This linearization represent a loss of energy/information, so stuff sinks. This is sometimes called "constraint drift". All solutions to get rid of constraint drift involve taking an approximation of the error of linearization, and then attempt to move the rigid body in the opposite direction of the error. Usually it's something like MoveShape( Error( constraint ) * -FACTOR ) where the factor is some number less than 1.0 and greater than 0. Often times there's an iterative loop that takes small steps to approximate the error and evaluate positions after each step.

The modern solution is based on Cline's paper: https://pdfs.semanticscholar.org/476d/fce4be549655938c499663af246702cbc781.pdf. The idea is to take a typical velocity jacobian and use it on the position level. I believe Erin Catto referred to this style as Full-NGS and wrote down some notes on his b2Island.cpp in the comments: https://github.com/erincatto/Box2D/blob/master/Box2D/Box2D/Dynamics/b2Island.cpp. The velocity jacobian describes the gradient of the position constraint with respect to time -- this means it describes how to change velocity to achieve the constraint's solution. NGS is a sort of mathematically hacky way to use this velocity gradient on the position level, and if we take small steps it looks quite nice to the eyes.

The simpler solution is to implement baumgarte stabilization. There are plenty of resources both here and on the bullet physics forums on how to implement this. In either case there's quite a bit of math involved if you want to understand what is going on. However for now if you just want to get something working I would recommend copying the solver from Box2D Lite and porting it to 3D: http://box2d.org/files/GDC2006/Box2D_Lite.zip. The rough idea of baumgarte is to measure the position error caused by constraint drift and add in a little extra energy (extra velocity) to the velocity constraint solver.

Edit: I took a very brief look at the Kenwright page. I think he is using some outdated techniques. These techniques seems similar to the ones in the orange physics books (also not recommended): https://www.amazon.com/dp/B00AQNVXFW. These two resources are attempting to use aggressive sleep techniques to cover up instabilities in their naive velocity solvers. Each constraint solved is in isolation. The modern approach (like in Box2D and other popular 3D physics engines) is to use projected gauss seidel, or sequential impulses, to solve for solutions. If you take the solver from Box2D Lite you will have a sequential impulses implementation, and it should be quite similar to the solver you have right now.

Hi Randy,

Just so you know, you are my new favorite person on the internet! Thank you for the super helpful answer!

Glad you found your way! Be sure you implement the accumulated impulse and correct impulse clamping, otherwise baumgarte won't help too much!

Oh boy,

So this might be the reason why my code wasn't working as expected when i patched kenwrights code into it!

Sorry for the ignorance here, but what is accumulated impulse and impulse clamping?

For every contact in the collision manifest, i calculate j (impulse magnitude) and add the impulse to the velocity of my objects.

This sounds like accumulated impulse? Since impulse accumulates for every contact point....

I also divide j by the number of contact points, to even out the impulse being applied.

I have no idea what impulse should be clamped to tough....

Can you link me any resources?

I don't know if this is needed, but for context, this is how i calculate my impulse:

void ApplyImpulse(RigidbodyVolume& A, RigidbodyVolume& B, const CollisionManifold& M, int c) {
	float invMass1 = A.InvMass();
	float invMass2 = B.InvMass();
	float invMassSum = invMass1 + invMass2;

	if (invMassSum == 0.0f) {
		return; // Both objects have infinate mass!
	}

	vec3 r1 = M.contacts[c] - A.position;
	vec3 r2 = M.contacts[c] - B.position;
	mat4 i1 = A.InvTensor();
	mat4 i2 = B.InvTensor();

	// Relative velocity
	vec3 relativeVel = (B.velocity + Cross(B.angVel, r2)) - (A.velocity + Cross(A.angVel, r1));
	// Relative collision normal
	vec3 relativeNorm = M.normal;
	Normalize(relativeNorm);

	// Moving away from each other? Do nothing!
	if (Dot(relativeVel, relativeNorm) > 0.0f) {
		return;
	}

	float e = fminf(A.cor, B.cor);

	float numerator = (-(1.0f + e) * Dot(relativeVel, relativeNorm));
	float d1 = invMassSum;
	vec3 d2 = Cross(MultiplyVector(Cross(r1, relativeNorm), i1), r1);
	vec3 d3 = Cross(MultiplyVector(Cross(r2, relativeNorm), i2), r2);
	float denominator = d1 + Dot(relativeNorm, d2 + d3);

	float j = (denominator == 0.0f) ? 0.0f : numerator / denominator;
	if (M.contacts.size() > 0.0f && j != 0.0f) {
		j /= (float)M.contacts.size();
	}

	vec3 impulse = relativeNorm * j;
	A.velocity = A.velocity - impulse *  invMass1;
	B.velocity = B.velocity + impulse *  invMass2;

	A.angVel = A.angVel - MultiplyVector(Cross(r1, impulse), i1);
	B.angVel = B.angVel + MultiplyVector(Cross(r2, impulse), i2);

	// Friction
	vec3 t = relativeVel - (relativeNorm * Dot(relativeVel, relativeNorm));
	if (CMP(MagnitudeSq(t), 0.0f)) {
		return;
	}
	Normalize(t);

	numerator = -Dot(relativeVel, t);
	d1 = invMassSum;
	d2 = Cross(MultiplyVector(Cross(r1, t), i1), r1);
	d3 = Cross(MultiplyVector(Cross(r2, t), i2), r2);
	denominator = d1 + Dot(t, d2 + d3);

	float jt = (denominator == 0.0f) ? 0.0f : numerator / denominator;
	if (M.contacts.size() > 0.0f && jt != 0.0f) {
		jt /= (float)M.contacts.size();
	}

	if (CMP(jt, 0.0f)) {
		return;
	}

	float friction = sqrtf(A.friction * B.friction);
	if (jt > j * friction) {
		jt = j * friction;
	}
	else if (jt < -j * friction) {
		jt = -j * friction;
	}
	vec3 tangentImpuse = t * jt;

	A.velocity = A.velocity - tangentImpuse *  invMass1;
	B.velocity = B.velocity + tangentImpuse *  invMass2;

	A.angVel = A.angVel - MultiplyVector(Cross(r1, tangentImpuse), i1);
	B.angVel = B.angVel + MultiplyVector(Cross(r2, tangentImpuse), i2);
}

The majority of that code will remain the same once you implement accumulated + clamped impulses. Line with comment about if shapes are moving away do nothing -- that's naive. With an accumulated impulse sometimes the impulses can be negative to correct overshooting. The code you have cannot do this. Instead we want to create an accumulated impulse and clamp the accumulated impulse at 0. Check out all of Erin Catto's downloaded GDC resources from box2d.org for slides on this topic. Check out Box2D Lite for example code (linked above). It's going to be a fair bit of reading, which is why I recommended earlier to just fiddle with the Box2D Lite code and port it into your own code. Then you can come back here and ask specific questions if you get stuck.

I couldn't find anything that really explained what accumulated impulses are, at first i tried to store the impulse of each contact inside the contact body its-self (without modifying the velocity of the object directly) and then applying this accumulated velocity in one big go. That made everything explode, spectacularly!

I think i'm onto something with my new iteration, i changed my physics loop from

• Find intersection
• Apply Impulses
• Integrate velocity & positoon

To

• Find intersections
• Integrate velocity
• Apply Impulses
• Integrate postiion

Which seems to have really helped. Is this change in update loop what's referred to as accumulated impulse, or am i doing something unrelated?

I also changed my ApplyImpulse function based on your feedback. I could not get the coefficient of restitution working with this new correction, so it's gone for now. If anyone is interested, the updated code looks like this:

void ApplyImpulse(RigidbodyVolume& A, RigidbodyVolume& B, const CollisionManifold& M, int c, float deltaTime) {
	float invMass1 = A.InvMass();
	float invMass2 = B.InvMass();
	float invMassSum = invMass1 + invMass2;

	if (invMassSum == 0.0f) {
		return; // Both objects have infinate mass!
	}

	vec3 r1 = M.contacts[c] - A.position;
	vec3 r2 = M.contacts[c] - B.position;
	mat4 i1 = A.InvTensor();
	mat4 i2 = B.InvTensor();

	vec3 relativeVel = (B.velocity + Cross(B.angVel, r2)) - (A.velocity + Cross(A.angVel, r1));
	vec3 relativeNorm = M.normal;
	Normalize(relativeNorm);

	// Moving away from each other? Do nothing!
	/*if (Dot(relativeVel, relativeNorm) > 0.0f) {
		return;
	}*/

	float bias = 0.2f;
	float slop = 0.01f;
	float correction = (bias / deltaTime) * fmaxf(M.depth - slop, 0.0f);

	float e = fminf(A.cor, B.cor);
	float numerator = (/*-(1.0f + e) **/ (-Dot(relativeVel, relativeNorm) + correction));
	float d1 = invMassSum;
	vec3 d2 = Cross(MultiplyVector(Cross(r1, relativeNorm), i1), r1);
	vec3 d3 = Cross(MultiplyVector(Cross(r2, relativeNorm), i2), r2);
	float denominator = d1 + Dot(relativeNorm, d2 + d3);

	float j = (denominator == 0.0f) ? 0.0f : numerator / denominator;
	/*if (M.contacts.size() > 0.0f && j != 0.0f) {
		j /= (float)M.contacts.size();
	}*/

	//j = fmaxf(j, 0.0f);

	vec3 impulse = relativeNorm * j;
	A.velocity = A.velocity - impulse *  invMass1;
	B.velocity = B.velocity + impulse *  invMass2;

	A.angVel = A.angVel - MultiplyVector(Cross(r1, impulse), i1);
	B.angVel = B.angVel + MultiplyVector(Cross(r2, impulse), i2);

	// Friction
	vec3 t = relativeVel - (relativeNorm * Dot(relativeVel, relativeNorm));
	if (CMP(MagnitudeSq(t), 0.0f)) {
		return;
	}
	Normalize(t);

	numerator = -Dot(relativeVel, t);
	d1 = invMassSum;
	d2 = Cross(MultiplyVector(Cross(r1, t), i1), r1);
	d3 = Cross(MultiplyVector(Cross(r2, t), i2), r2);
	denominator = d1 + Dot(t, d2 + d3);

	float jt = (denominator == 0.0f) ? 0.0f : numerator / denominator;
	if (M.contacts.size() > 0.0f && jt != 0.0f) {
		jt /= (float)M.contacts.size();
	}

	if (CMP(jt, 0.0f)) {
		return;
	}

	float friction = sqrtf(A.friction * B.friction);
	if (jt > j * friction) {
		jt = j * friction;
	}
	else if (jt < -j * friction) {
		jt = -j * friction;
	}
	vec3 tangentImpuse = t * jt;

	A.velocity = A.velocity - tangentImpuse *  invMass1;
	B.velocity = B.velocity + tangentImpuse *  invMass2;

	A.angVel = A.angVel - MultiplyVector(Cross(r1, tangentImpuse), i1);
	B.angVel = B.angVel + MultiplyVector(Cross(r2, tangentImpuse), i2);
}

At this point i've taken out all linear projection, and everything is running with the changes mentioned above. The simulation is pretty close. For reference,

This is a gif of a scene i build in unity: http://giphy.com/gifs/d1E2BSEJUEEz0dQ4

And this is my simulation: http://giphy.com/gifs/l0Ex6twOL4qhbX4Ig

I have a feeling the "crawling" i'm getting is due to either the removal of the coefficient restitution or maybe i'm doing friction wrong?

Thank you for taking the time to help!

The crawling is because you currently do not have an accumulated impulse setup. What happens is each vertex presses the OBB's corner out of the ground, but the opposite corner is now pressed downward due to rotation and penetrates the plane. This happens in a cyclic pattern and results in instability like you see.

Check out this PPT: http://box2d.org/files/GDC2006/GDC2006_Catto_Erin_PhysicsTutorial.ppt. Please read all slides but especially slides 26 till the end. Erin Catto describes accumulating impulses to allow each constraint to see its impulse history and fix overshoot introduced by the different constraints fighting each other (like the rotation example I just mentioned). Example implementation is in Box2D Lite: http://box2d.org/files/GDC2006/Box2D_Lite.zip.

Congrats! Yes the rest does seem to be geometry issues (collision detection and generating the contact manifold). Commonly there's a joke that 90% of the work is generating good contact points, 10% of the work is the constraint solver :)