Merge pull request microsoft#2956 from cthoma20-arc/fix-rotation-rate

Added angle wrapping when calculating angular velocity

AirLib/include/common/VectorMath.hpp

@@ -4,8 +4,10 @@
 #ifndef air_VectorMath_hpp
 #define air_VectorMath_hpp
 
+#include <cmath>
 #include "common/common_utils/Utils.hpp"
 #include "common_utils/RandomGenerator.hpp"
+
 STRICT_MODE_OFF
 //if not using unaligned types then disable vectorization to avoid alignment issues all over the places
 //#define EIGEN_DONT_VECTORIZE
@@ -333,9 +335,9 @@ class VectorMathT {
 		RealT p_e, r_e, y_e;
 		toEulerianAngle(end, p_e, r_e, y_e);
 
-		RealT p_rate = (p_e - p_s) / dt;
-		RealT r_rate = (r_e - r_s) / dt;
-		RealT y_rate = (y_e - y_s) / dt;
+		RealT p_rate = normalizeAngle(p_e - p_s, (RealT) (2 * M_PI)) / dt;
+		RealT r_rate = normalizeAngle(r_e - r_s, (RealT) (2 * M_PI)) / dt;
+		RealT y_rate = normalizeAngle(y_e - y_s, (RealT) (2 * M_PI)) / dt;
 
 		//TODO: optimize below
 		//Sec 1.3, https://ocw.mit.edu/courses/mechanical-engineering/2-154-maneuvering-and-control-of-surface-and-underwater-vehicles-13-49-fall-2004/lecture-notes/lec1.pdf