Efforts related to minimizing the environmental burden caused by agricultural activities and increasing economic efficiency are key contemporary drivers in the precision agriculture domain. Controlled Traffic Farming (CTF) techniques are being applied against soil compaction creation, using the on-line optimization of trajectory planning for soil-sensitive field operations. The research presented in this paper aims at a proof-of-concept solution with respect to optimizing farm machinery trajectories in order to minimize the environmental burden and increase economic efficiency. As such, it further advances existing CTF solutions by including (1) efficient plot divisions in 3D, (2) the optimization of entry and exit points of both plot and plot segments, (3) the employment of more machines in parallel and (4) obstacles in a farm machinery trajectory. The developed algorithm is expressed in terms of unified modeling language (UML) activity diagrams as well as pseudo-code. Results were visualized in 2D and 3D to demonstrate terrain impact. Verifications were conducted at a fully operational commercial farm (Rostěnice, the Czech Republic) against second-by-second sensor measurements of real farm machinery trajectories.