orocos/orocos_kinematics_dynamics — pull request #32 by rethink-kmaroney (2014-10-01 17:08:33+00:00)
Title: Fixes GetQuaternion non-zero matrix trace check

Currently, the matrix trace is adding 1.0 before zero (epsilon) comparison.

As noted on the referenced source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
And in related discussion: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ethan.htm

Adding the offset of 1 before epsilon comparison introduces instability as Tr + 1 approaches 0.