Fix crash caused by clear method not resetting internals correctly

Better flight trajectory point algorithm