A flight path planning approach may be deterministic and guarantee a safe, quasi-optimal path. A plurality of three-dimensional voxels may be determined as cells of a rectangular grid. The cells may have a predetermined length and width. A shortest safe path through the grid graph may be calculated from a local start to a local goal defined as points on a nominal global path. Geometric smoothing may be performed on the basis line from the local start to the local goal to generate a smooth three-dimensional trajectory that can be followed by a given rotorcraft. Dynamic smoothing may be performed on the three-dimensional trajectory to provide a maximum possible speed profile over a path defined by the dynamic smoothing. The three dimensional path information may be provided to an autopilot, which may then control the rotorcraft to fly along the defined path.