Today’s experiment was to solve of unfortunate side effect of the sampling method I chose—namely initial collision. If two agents are overlapping, the sampling will always return blocked and the two agent will get stuck. I previously side stepped this problem by just ignoring the initial collision.