I finally found a solution, which does not involved the rather crude method of correcting positions.
This completely eliminated both the sinking and the jitter.
Sinking is caused by the resultant force being insufficient to push the objects apart, in the case of an object coming to "rest" on a stationary object. So we need an additional force to help. This is the normal force, and is proportional to the amount the two bodies are interpenetrating, and the density of the bodies: i.e. the more the objects interpenetrate, the harder they push back:
penetrationMultiplier = 1 + penetrationDepth * density;
Ft = F * penetrationMultiplier;
Density is a value for each body between 0.0 and 1.0. With very low penetration depths, the magnitude of the penetration value will not be much over 1.0, but will increase in magnitude as penetration depth increases.
Jitter is caused by a body never fully coming to rest, and keeps bouncing until the you reach the limits of floating point precision. To solve this, we must set a minimum limit on motion of bodies, below which, the body can be deemed to be "at rest". Let us call this value
epsilon. When a body is at rest, only a force of magnitude greater than
epsilon will cause the body to no longer be at rest.
The calculation to place a body at rest, or "asleep" applies to both linear and angular motion, and is applied at the point of updating positions by
velocity * deltaTime:
void update(float deltaTime)
if(m_awake == true)
if ( length(velocity) < epsilon )
velocity = vec3(0.0);
m_awake = false;
position += (velocity * deltaTime) * damping;
rotation += (angularvelocity * deltaTime) * damping;
Damping is value which will sap a tiny amount of energy from a bodies angular and linear velocities over time, which will allow a body to eventually lose energy to the point of epsilon coming into effect.
Then when a force is applied, to potentially wake it up:
void applyForce(vec3 force, vec3 contactPoint)
if (length(force) > epsilon)
m_awake = true;
// apply force as normal, calculating new linear and angular motion
Eventually, due to energy losses via restitution, damping and inertia/momentum calculations, the magnitude of the impulse will be insufficient to cause a noticeable change in linear or angular velocity, and thus the body will go to, and remain, asleep.