Author: bugman
Date: Tue Sep 8 20:20:34 2009
New Revision: 9486
URL: http://svn.gna.org/viewcvs/relax?rev=9486&view=rev
Log:
Created the maths_fns.rotation_matrix.R_to_quaternion() function.
This is accompanied by 2 unit tests.
Modified:
1.3/maths_fns/rotation_matrix.py
1.3/test_suite/unit_tests/_maths_fns/test_rotation_matrix.py
Modified: 1.3/maths_fns/rotation_matrix.py
URL:
http://svn.gna.org/viewcvs/relax/1.3/maths_fns/rotation_matrix.py?rev=9486&r1=9485&r2=9486&view=diff
==============================================================================
--- 1.3/maths_fns/rotation_matrix.py (original)
+++ 1.3/maths_fns/rotation_matrix.py Tue Sep 8 20:20:34 2009
@@ -21,7 +21,7 @@
###############################################################################
# Python module imports.
-from math import acos, asin, atan2, cos, pi, sin
+from math import acos, asin, atan2, cos, pi, sin, sqrt
from numpy import array, cross, dot, float64, hypot, zeros
from numpy.linalg import norm
from random import gauss, uniform
@@ -302,6 +302,61 @@
quaternion_to_R(quat, R)
+def R_to_quaternion(R, quat):
+ """Convert a rotation matrix into quaternion form.
+
+ This is from Wikipedia
(http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion), where::
+
+ w = 0.5*sqrt(1+Qxx+Qyy+Qzz),
+ x = copysign(0.5*sqrt(1+Qxx-Qyy-Qzz),Qzy-Qyz),
+ y = copysign(0.5*sqrt(1-Qxx+Qyy-Qzz),Qxz-Qzx),
+ z = copysign(0.5*sqrt(1-Qxx-Qyy+Qzz),Qyx-Qxy),
+
+ where the quaternion is defined as q = (w, x, y, z), and the copysign
function is x with the
+ sign of y::
+
+ copysign(x, y) = abs(x) / abs(y) * y
+
+
+ @param R: The 3D rotation matrix.
+ @type R: numpy 3D, rank-2 array
+ @param quat: The quaternion.
+ @type quat: numpy 4D, rank-1 array
+ """
+
+ # Elements.
+ quat[0] = 0.5 * sqrt(1.0 + R[0, 0] + R[1, 1] + R[2, 2])
+ quat[1] = R[2, 1] - R[1, 2]
+ if quat[1]:
+ quat[1] = copysign(0.5*sqrt(1 + R[0, 0] - R[1, 1] - R[2, 2]),
quat[1])
+ quat[2] = R[0, 2] - R[2, 0]
+ if quat[2]:
+ quat[2] = copysign(0.5*sqrt(1 - R[0, 0] + R[1, 1] - R[2, 2]),
quat[2])
+ quat[3] = R[1, 0] - R[0, 1]
+ if quat[3]:
+ quat[3] = copysign(0.5*sqrt(1 - R[0, 0] - R[1, 1] + R[2, 2]),
quat[3])
+
+
+def copysign(x, y):
+ """Return x with the sign of y.
+
+ This is defined as::
+
+ copysign(x, y) = abs(x) / abs(y) * y
+
+
+ @param x: The value.
+ @type x: float
+ @param y: The value.
+ @type y: float
+ @return: x with the sign of y.
+ @rtype: float
+ """
+
+ # Return the value.
+ return abs(x) / abs(y) * y
+
+
def random_rot_axis(axis):
"""Generate a random rotation axis.
Modified: 1.3/test_suite/unit_tests/_maths_fns/test_rotation_matrix.py
URL:
http://svn.gna.org/viewcvs/relax/1.3/test_suite/unit_tests/_maths_fns/test_rotation_matrix.py?rev=9486&r1=9485&r2=9486&view=diff
==============================================================================
--- 1.3/test_suite/unit_tests/_maths_fns/test_rotation_matrix.py (original)
+++ 1.3/test_suite/unit_tests/_maths_fns/test_rotation_matrix.py Tue Sep 8
20:20:34 2009
@@ -353,6 +353,46 @@
self.assertAlmostEqual(gamma, gamma_new)
+ def test_R_to_quaternion_no_rot(self):
+ """Test the rotation matrix to quaternion conversion for a zero
angle rotation."""
+
+ # Generate the rotation matrix.
+ R = array([[1, 0, 0], [0, 1, 0], [0, 0, 1]], float64)
+
+ # The quaternion.
+ quat = zeros(4, float64)
+ R_to_quaternion(R, quat)
+ print("Quaternion:\n%s" % quat)
+
+ # The correct result.
+ quat_true = array([1, 0, 0, 0], float64)
+
+ # Checks.
+ self.assertEqual(norm(quat), 1)
+ for i in range(4):
+ self.assertAlmostEqual(quat[i], quat_true[i])
+
+
+ def test_R_to_quaternion_180_complex(self):
+ """Test the rotation matrix to quaternion conversion for a 180
degree rotation about [1, 1, 1]."""
+
+ # Generate the rotation matrix.
+ R = array([[0, 0, 1], [1, 0, 0], [0, 1, 0]], float64)
+
+ # The quaternion.
+ quat = zeros(4, float64)
+ R_to_quaternion(R, quat)
+ print("Quaternion:\n%s" % quat)
+
+ # The correct result.
+ quat_true = array([1, 1, 1, 1], float64) / 2
+
+ # Checks.
+ self.assertEqual(norm(quat), 1)
+ for i in range(4):
+ self.assertAlmostEqual(quat[i], quat_true[i])
+
+
def test_quaternion_to_axis_angle_no_rot(self):
"""Test the quaternion to rotation matrix conversion for a zero
angle rotation."""
r9486 - in /1.3: maths_fns/rotation_matrix.py test_suite/unit_tests/_maths_fns/test_rotation_matrix.py