Instead of handling contact tests and collision response in the same function in collision.c, first generate contact points and return them as a list, then free at the end of the stepping function. This way the contact response can be integrated into the conjugate gradient method properly instead of using the hackish and unstable double evaluation that is currently used.