Die vec_for_vertex-Funktion hat keinen Bug.
Ich kann aber nicht wirklich schlau aus Deinem Code werden.
Das einzige, was mir jetzt auffällt, ist, dass antrieb_pos nur als Variable, mit nur einem Wert, definiert wird und nicht als Vektor, mit 3 Werten.