Hey Y'all!

I've been trying to make my own (simple) physics engine. However I've run into (another) problem. I've been banging my head against it for 3+ weeks now, and just can't figure it out.

My problems is that my objects are ‘drifting’. If one box lands on top of another perfectly above it's center of mass, it stays nice and still:

However, if the top box is even a little offset in either X or Z, it starts building momentum in that direction until it slides off. Here the box is offset by 0.01 units in the positive X direction:

I know why this happens: when perfectly centered, the contact point is right underneath the top box's center of mass. So the entry in the jacobian that dictates the constraint torque for the top box, -(r1 x normal), has length 0. However, when off-center by any amount, the contact point is no longer underneath the center of mass, while the the normal remains the same (0, -1, 0), so that value suddenly becomes non-zero. This causes the box to rotate ever so slightly, which in turn causes the collision normal of the next time step to rotate with it, which causes the box to be pushed out along that normal and ‘drift’. I'm finding the collision normal, penetration depth and contact points using EPA (as described here).

I thought I could fix this by caching contacts for multiple time steps. I have implemented this in the video above. Each purple point represents an active contact. Each frame I loop over all active contact and apply their forces to the involved objects. After this is done, each object adds the accumulated forces to its velocities and updates it position and orientation:

Velocity += ForcesConstraints;
Transform.Translation += deltaTime * Velocity;


AngularVelocity += TorqueConstraints;
Transform.Rotation = quat.FromAxisAngle((deltaTime * AngularVelocity).Length, AngularVelocity.NormalizedSafe) * Parent.Transform.Rotation;

Because a contact might get pushed back by another contact, I loop over the contacts multiple times. Whenever a contact applies force to an object, it loops over all current active contacts that object is involved in (including itself) and calls their UpdateConstraint function to update their penetration depths:

private void ApplyForces(RigidBody M, Time deltaTime, vec3 deltaVel, vec3 deltaRot)
{
	M.ForcesConstraints += deltaVel;
    M.TorqueConstraints += deltaRot;

    foreach (Constraint c in M.InvolvedConstraints)
		c.UpdateConstraint(M, deltaTime, deltaVel, deltaRot);
}

public override void UpdateConstraint(RigidBody M, Time deltaTime, vec3 deltaVel, vec3 deltaRot)
{
	vec3 contact = M == M1 ? _contact.Item1 : _contact.Item2; //select the right contact point

	//I understand that this way of computing the actual positional delta the deltaRot represents is incredibly bad, but I coulnd't get anything else to work (I'm a linear algebra newbie).
	var rot = (quat.FromAxisAngle((deltaTime * deltaRot).Length, deltaRot.NormalizedSafe) * contact) - contact;

	var deltaPos = (deltaVel * deltaTime) + rot;
 
	_pendepth += (M == M1 ? 1 : -1) * vec3.Dot(deltaPos, _normal);
}

However, none of this helps at all. The problem remains the same. I don't know what else to do. I feel like I'm missing something huge.

The full code is an absolute mess right now, but it can be found over here.

Thanks for reading and have a great day!