Problematisch ist, dass c_move und c_trace unnötig langsam sind, bzw. c_move eig. am Ziel vorbei geht, da du ja vermutlich keine Kollisionserkennung brauchst.

PS: von direkt überhalb (nur z achse verschoben) mit der kollisionsbox von dem gebäude zu tracen (wie kasimir gesagt hat) ist genauer als den weg den ich oben beschrieben habe, aber langsamer. Was du brauchst musst du wissen.