I have only looked through the code yet.
Looks very clean, good job! The only thing that might make it even cleaner is to avoid magic numbers in code, like penetration depth multiplication or offset with something like 0.004. This is better solved as a constant defined outside the function or at the beginning. Or the multiplication with 100 afterwards (all of that in the main function)