fixed dynamics bug, fixed simulation output that was missing rotation matrix multiplication for calculating global position and orientation