Added small parts of the new collision solver, currently broken