finished a*, need to fix it for body intersections and direction validation