Calculate 3d True Position