improved collision finding and improved testing of path tracking
added some code to improve testing