In this paper, a new path planning method for robots used in outdoor environments is presented. The proposed method applies Fast Marching to a 3D surface represented by a triangular mesh to calculate a smooth trajectory from one point to another. The method uses a triangular mesh instead of a square one since this kind of grid adapts better to 3D surfaces. The novelty of this approach is that, before running the algorithm, the method calculates a weight matrix WW based on the information extracted from the 3D surface characteristics. In the presented experiments these features are the height, the spherical variance, and the gradient of the surface. This matrix can be viewed as a difficulty map situated over the 3D surface and is used to limit the propagation speed of the Fast Marching wave in order to find the best path depending on the task requirements, e.g., the least energy consumption path, the fastest path, or the most plain terrain. The algorithm also gives the speed for the robot, which depends on the wave front propagation speed. The results presented in this paper show how, by varying this matrix WW, the paths obtained are different. Moreover, as it is shown in the experimental part, this algorithm is also useful for calculating paths for climbing robots in much more complex environments. Finally, at the end of the paper, it is shown that this algorithm can also be used for robot avoidance when two robots approach each other, and they know each other’s position.