Corrects bug in Heun's method, now detects collisions.

Signed-off-by: Raphael Catolino <raphael.catolino@gmail.com>