From ec564d70daa8c1a66018f9606b02b873ae792c84 Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Thu, 28 Dec 2017 08:03:37 -0700
Subject: Start work on determining rotation using quaternions only

Rotation was previously approximated using axis/angle
This change starts down the path of using quaternions exclusively.
This change appears to give at least as good as answers as the axis/angle
model in basic cases (also only tested with 1 lighthouse), but it is
currently much slower and runs in unpredictable time.
---
 redist/linmath.c | 36 ++++++++++++++++++++++++++++++++++++
 redist/linmath.h |  2 ++
 2 files changed, 38 insertions(+)

(limited to 'redist')

diff --git a/redist/linmath.c b/redist/linmath.c
index 5d51708..1d70ee1 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -143,6 +143,42 @@ void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest)
 }
 
 
+void axisanglefromquat(FLT *angle, FLT *axis, FLT *q)
+{
+	// this way might be fine, too.
+	//FLT dist = FLT_SQRT((q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3]));
+	//
+	//*angle = 2 * FLT_ATAN2(dist, q[0]);
+
+	//axis[0] = q[1] / dist;
+	//axis[1] = q[2] / dist;
+	//axis[2] = q[3] / dist;
+	
+
+	// Good mathematical foundation for this algorithm found here:
+	// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm
+
+	FLT tmp[4] = { q[0], q[1], q[2], q[3] };
+
+	quatnormalize(tmp, q);
+
+	if (FLT_FABS(q[0] - 1) < FLT_EPSILON)
+	{
+		// we have a degenerate case where we're rotating approx. 0 degrees
+		*angle = 0;
+		axis[0] = 1;
+		axis[1] = 0;
+		axis[2] = 0;
+		return;
+	}
+
+	axis[0] = tmp[1] / sqrt(1 - (tmp[0] * tmp[0]));
+	axis[1] = tmp[2] / sqrt(1 - (tmp[0] * tmp[0]));
+	axis[2] = tmp[3] / sqrt(1 - (tmp[0] * tmp[0]));
+
+	*angle = 2 * FLT_ACOS(tmp[0]);
+}
+
 /////////////////////////////////////QUATERNIONS//////////////////////////////////////////
 //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman)
 //Under the mit/X11 license.
diff --git a/redist/linmath.h b/redist/linmath.h
index 8d6cf05..1876b34 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -72,6 +72,8 @@ FLT anglebetween3d( FLT * a, FLT * b );
 
 void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle);
 void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest);
+void axisanglefromquat(FLT *angle, FLT *axis, FLT *quat);
+
 
 //Quaternion things...
 
-- 
cgit v1.2.3