Fixed multiple major problems with collision solver.