fix rotation and trajectory calculations. add testing