Merge pull request #56 from mcclure/identity-quat

Fix for issue #55 (quat():to_angle_axis() returns gibberish)

modules/quat.lua

@@ -364,15 +364,27 @@ end
 
 --- Convert a quaternion into an angle plus axis components.
 -- @tparam quat a Quaternion to convert
+-- @tparam identityAxis vec3 of axis to use on identity/degenerate quaternions (optional, 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, identityAxis)
 	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 identityAxis then
+			return 0,identityAxis:unpack()
+		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)
@@ -392,10 +404,11 @@ end
 
 --- Convert a quaternion into an angle/axis pair.
 -- @tparam quat a Quaternion to convert
+-- @tparam identityAxis vec3 of axis to use on identity/degenerate quaternions (optional, default returns 0,vec3(0,0,1))
 -- @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, identityAxis)
+	local angle, x, y, z = a:to_angle_axis_unpack(identityAxis)
 	return angle, vec3(x, y, z)
 end
 

spec/quat_spec.lua

@@ -310,6 +310,22 @@ describe("quat:", function()
 		assert.is.equal(3, axis.z)
 	end)
 
+	it("converts between a quaternion and angle/axis (identity quaternion) (by component)", function()
+		local angle, x,y,z = quat():to_angle_axis_unpack()
+		assert.is.equal(0, angle)
+		assert.is.equal(0, x)
+		assert.is.equal(0, y)
+		assert.is.equal(1, z)
+	end)
+
+	it("converts between a quaternion and angle/axis (identity quaternion with fallback)", function()
+		local angle, axis = quat():to_angle_axis(vec3(2,3,4))
+		assert.is.equal(0, angle)
+		assert.is.equal(2, axis.x)
+		assert.is.equal(3, axis.y)
+		assert.is.equal(4, axis.z)
+	end)
+
 	it("gets a string representation of a quaternion", function()
 		local a = quat():to_string()
 		assert.is.equal("(+0.000,+0.000,+0.000,+1.000)", a)