bugfix calculating heading
added filter for tri
normalise vector in all cases