Update Waypoint Collision to Avoid Based on a Radius

[MattsAttack]

Currently collision avoidance checks if paths intersect but doesn't take into consideration radius around the path. So drones can still collide if they're waypoints are close to each other but parallel.

[MattsAttack]

@HarperReinhardt could you review/test this when you have a chance? Should be good to go