Fixed position calculation

Added rudimentary protection against objects entering each other on collision.