Removed code duplication (dimension parameter instead of X Y and Z at the method name).
Started adding a better collision handling. Next time it will be mathematically derived
and correctly implemented.