This paper presents an interesting technique for finding the trajectory of an outdoor robot. This technique applies Fast Marching to a 3D surface terrain represented by a triangular mesh in order to calculate a smooth trajectory between two points. The method uses a triangular mesh instead of a square one because this kind of grid adapts better to 3D surfaces. The novelty of this approach is that, in the first step of the method, the algorithm calculates a weight matrix W that can represents difficulty, viscosity, refraction index or incertitude based on the information extracted from the 3D surface characteristics and the sensor data of the robot. Within the bestowed experiments these features are the height, the spherical variance, the gradient of the surface and the incertitude in the position of other objects or robots and also the incertitude in the map because some portions of the map can’t be measured directly by the robot. This matrix 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 trajectory with least energy consumption, the fastest path, the most plain terrain or the safest path. The method also gives the robot’s maximum admisible speed, which depends on the wave front propagation velocity. The results presented in this paper show that it is possible to model the path characteristics as desired, by varying this matrix W. Moreover, as it is shown in the experimental part, this method is also useful for calculating paths for climbing robots in complex purely 3D environments. At the end of the paper, it is shown that this method can also be used for robot avoidance when two robots with opposite trajectories approach each other, knowing each others position.