A method of finding a path from a start point to a target point, in multi-dimensional space, including: (a) determining a plurality of points in a physical space, including a start point and an target point; (b) computing, using a cost function, for said points an accumulated path cost from the start point to a point; representing a minimal cost path from the start point to the point with respect to an optimization criteria; (c) computing for at least some of said points an estimated-cost-to-target from a point to the target point; and (d) after computing said costs, determining at least one of a minimal path or a minimal path cost of a path from the start point to the target point in the physical space, wherein the determination is based on said accumulated path costs, and is minimal with respect to the optimization criteria.