Collision check

Was merged with #132 and added generic collision check