Updated vector class.

test/test_math.py

@@ -0,0 +1,66 @@
+from __future__ import print_function
+from pylab import *
+from pprint import pprint
+
+#pylint: disable=all
+
+phi = 0.1
+theta = 0.2
+psi = 0.3
+
+cosPhi = cos(phi)
+cosPhi_2 = cos(phi/2)
+sinPhi = sin(phi)
+sinPhi_2 = sin(phi/2)
+
+cosTheta = cos(theta)
+cosTheta_2 = cos(theta/2)
+sinTheta = sin(theta)
+sinTheta_2 = sin(theta/2)
+
+cosPsi = cos(psi)
+cosPsi_2 = cos(psi/2)
+sinPsi = sin(psi)
+sinPsi_2 = sin(psi/2)
+
+C_nb = array([
+    [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi],
+    [cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi],
+    [-sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]])
+    
+print('\nC_nb')
+pprint(C_nb)
+
+theta = arcsin(-C_nb[2,0])
+phi = arctan(C_nb[2,1]/ C_nb[2,2])
+psi = arctan(C_nb[1,0]/ C_nb[0,0])
+print('\nphi {:f}, theta {:f}, psi {:f}\n'.format(phi, theta, psi))
+
+q = array([
+    cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2,
+    sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2,
+    cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2,
+    cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2])
+     
+a = q[0]
+b = q[1]
+c = q[2]
+d = q[3]
+
+print('\nq')
+pprint(q.T)
+
+a2 = a*a
+b2 = b*b
+c2 = c*c
+d2 = d*d
+
+C2_nb = array([
+    [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c)],
+    [2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b)],
+    [2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]])
+
+print('\nC2_nb')
+pprint(C2_nb)
+
+# vim: set et ft=python fenc=utf-8 ff=unix sts=4 sw=4 ts=8 :