#6 #8 #14 Update algorithm

Fix corridor collisions