1 files changed, 46 insertions, 0 deletions
diff --git a/lib/angle_normalize.c b/lib/angle_normalize.c
new file mode 100644
index 0000000..8784f2c
--- /dev/null
+++ b/lib/angle_normalize.c
@@ -0,0 +1,46 @@
+// Angle Normalize
+//
+// angle_normalize - normalize angle to [-PI, PI]
+//
+// y[t] = fmod( x[t], 2*PI ), adjusted to [-PI, PI]
+//
+// X_n - 1
+// Y_n - 1
+// C_n - 0
+
+#include <math.h>
+#include "ts.h"
+
+
+
+ts_parameter_t const ts_angle_normalize_p = { .n_inputs = 1, .n_outputs = 1, .n_constants = 0, .params_size = 0 };
+
+
+uint_t ts_angle_normalize_offset( ts_constant_t const *c )
+{
+	( void )c;
+	return ( 0 );
+}
+
+int ts_angle_normalize(
+	fp_t * const *y, uint_t y_n,
+	fp_t const * const *x, uint_t x_n,
+	uint_t n, ts_constant_t const *c
+)
+{
+	TS_PROLOGUE( ts_angle_normalize_p );
+	(void)c;
+	for( uint_t i = 0; i < n; ++i )
+	{
+		fp_t r = fmod( x[0][i], 2.0 * PI );
+		if( r > PI )
+			r -= 2.0 * PI;
+		if( r < -PI )
+			r += 2.0 * PI;
+		y[0][i] = r;
+	}
+	return 0;
+}
+
+
+