1 files changed, 20 insertions, 0 deletions
diff --git a/src/util.c b/src/util.c
index 63cc9ab..10d4c09 100644
--- a/src/util.c
+++ b/src/util.c
@@ -3,3 +3,23 @@
 AxisAngle AxisAngleIdentity() {
 	return (AxisAngle){Vector3Zero(), 0.0};
 }
+
+float signum(float n) {
+	if (n > 0.0)
+		return 1.0;
+	else if (n < 0.0)
+		return -1.0;
+	else
+		return 0.0;
+}
+
+float closestAngle(float a1, float a2) {
+	float a = fmodf(a1, PI);
+	float b = fmodf(a2, PI);
+	float dir = b - a;
+
+	if (fabsf(dir) > (PI/2))
+		dir = -(signum(dir) * PI) + dir;
+
+	return dir;
+}