Added collision solver to implementation, starting to write tests