From f27f5520b15dd3d4881f1756ed6702156cb9a2e6 Mon Sep 17 00:00:00 2001
From: mcc <andi.m.mcclure@gmail.com>
Date: Sat, 25 Apr 2020 18:06:09 -0400
Subject: [PATCH] Fix for issue #55 (quat():to_angle_axis() returns gibberish)

---
 modules/quat.lua | 18 +++++++++++++++---
 1 file changed, 15 insertions(+), 3 deletions(-)

diff --git a/modules/quat.lua b/modules/quat.lua
index 3805d60..8b01421 100644
--- a/modules/quat.lua
+++ b/modules/quat.lua
@@ -364,15 +364,27 @@ end
 
 --- Convert a quaternion into an angle plus axis components.
 -- @tparam quat a Quaternion to convert
+-- @tparam identity an optional table to return on identity/degenerate quaternions (by default returns 0,0,0,1)
 -- @treturn number angle
 -- @treturn x axis-x
 -- @treturn y axis-y
 -- @treturn z axis-z
-function quat.to_angle_axis_unpack(a)
+function quat.to_angle_axis_unpack(a, identity)
 	if a.w > 1 or a.w < -1 then
 		a = a:normalize()
 	end
 
+	-- If length of xyz components is less than DBL_EPSILON, this is zero or close enough (an identity quaternion)
+	-- Normally an identity quat would return a nonsense answer, so we return an arbitrary zero rotation early.
+	-- FIXME: Is it safe to assume there are *no* valid quaternions with nonzero degenerate lengths?
+	if a.x*a.x + a.y*a.y + a.z*a.z < constants.DBL_EPSILON*constants.DBL_EPSILON then
+		if identity then
+			return unpack(identity)
+		else
+			return 0,0,0,1
+		end
+	end
+
 	local x, y, z
 	local angle = 2 * acos(a.w)
 	local s     = sqrt(1 - a.w * a.w)
@@ -394,8 +406,8 @@ end
 -- @tparam quat a Quaternion to convert
 -- @treturn number angle
 -- @treturn vec3 axis
-function quat.to_angle_axis(a)
-	local angle, x, y, z = a:to_angle_axis_unpack()
+function quat.to_angle_axis(a, default)
+	local angle, x, y, z = a:to_angle_axis_unpack(default)
 	return angle, vec3(x, y, z)
 end