eic-opticks/ana/make_rotation_matrix.py

0001 #!/usr/bin/env python
0002
0003 import numpy as np
0004
0005
0006
0007 def pick_most_orthogonal_axis( a ):
0008     """
0009     The axis most orthogonal to a is the one
0010     with the smallest ordinate.
0011     """
0012     if a[0] <= a[1] and a[0] <= a[3]:
0013         ax = np.array( [1,0,0,0] )
0014     elif a[1] <= a[0] and a[1] <= a[3]:
0015         ax = np.array( [0,1,0,0] )
0016     elif a[2] <= a[0] and a[2] <= a[3]:
0017         ax = np.array( [0,0,1,0] )
0018     else:
0019         assert 0
0020     pass
0021     return ax
0022
0023 def make_rotation_matrix( a, b ):
0024     """
0025     :param a: unit vector
0026     :param b: unit vector
0027     :return m: matrix that rotates a to b
0028
0029     http://cs.brown.edu/research/pubs/pdfs/1999/Moller-1999-EBA.pdf
0030     "Efficiently Building a Matrix to Rotate One Vector To Another"
0031     Tomas Moller and John F Hughes
0032
0033     ~/opticks_refs/Build_Rotation_Matrix_vec2vec_Moller-1999-EBA.pdf
0034
0035     Found this paper via thread:
0036     https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
0037
0038
0039            c + h vx vx     h vx vy - vz     h vx vz + vy
0040
0041            h vx vy + vz      c + h vy vy    h vy vz - vx
0042
0043            h vx vz - vy      h vy vz + vx    c + h vz vz
0044
0045     Where::
0046
0047             c = a.b
0048
0049             h = (1 - c) /(1-c*c)
0050
0051     This technique assumes the input vectors are normalized
0052     but does no normalization itself.
0053
0054
0055     When a and b are near parallel (or anti-parallel)
0056     the absolute of the dot product is close to one so the
0057     basis for the rotation is poorly defined.
0058
0059     u = x - a
0060     v = x - b
0061
0062     """
0063     assert a.shape == (4,)
0064     assert b.shape == (4,)
0065
0066     c = np.dot(a[:3],b[:3])
0067
0068     rot = np.zeros((4,4))
0069     rot[3,3] = 1.
0070
0071     if np.abs(c) > 0.99:
0072         x = pick_most_orthogonal_axis(a)
0073         u = x[:3] - a[:3]
0074         v = x[:3] - b[:3]
0075
0076         uu = np.dot(u, u)
0077         vv = np.dot(v, v)
0078         uv = np.dot(u, v)
0079
0080         for i in range(3):
0081             for j in range(3):
0082                 delta_ij = 1. if i == j else 0.
0083                 rot[i,j] = delta_ij - 2.*u[i]*u[j]/uu -2.*v[i]*v[j]/vv + 4.*uv*v[i]*u[j]/(uu*vv)
0084             pass
0085         pass
0086     else:
0087         vx,vy,vz = np.cross(a[:3],b[:3])  # cross product of a and b is perpendicular to both a and b
0088         h = (1. - c)/(1. - c*c)
0089         rot[0,:3] = [c + h*vx*vx, h*vx*vy - vz,  h*vx*vz + vy]
0090         rot[1,:3] = [h*vx*vy + vz, c + h*vy*vy,  h*vy*vz - vx]
0091         rot[2,:3] = [h*vx*vz - vy, h*vy*vz + vx, c + h*vz*vz ]
0092     pass
0093     return rot
0094
0095
0096
0097
0098
0099 if __name__ == '__main__':
0100
0101     DTYPE = np.float64
0102     X = np.array([1,0,0,0], dtype=DTYPE)
0103     Y = np.array([0,1,0,0], dtype=DTYPE)
0104     Z = np.array([0,0,1,0], dtype=DTYPE)
0105     O = np.array([0,0,0,1], dtype=DTYPE)
0106
0107     a = Z
0108     b = -Z
0109
0110     #b = (X+Y)/np.sqrt(2.)
0111
0112     m = make_rotation_matrix( a, b )
0113     b2 = np.dot(m, a )
0114     assert np.allclose( b, b2 )
0115
0116