[mr3263] PathFinder: Use raycast for calculate a straight line path; Add a config param to on/off path optimization (for debug purposes)