[frames] | no frames]

# Source Code for Module mglutil.math.rotax

```  1  """ This file contains the following functions,
2  rotax
3  mat_to_quat
4  mat_to_axis_angle
5  inverse4X4
6  rotVectToVect
7  interpolate3DTransform
8  """
9
10
11  from math import pi, sin, cos, sqrt
12  import Numeric as N
13  degtorad = pi/180.
14
15
16 -def rotax( a, b, tau, transpose=1 ):
17      """
18      Build 4x4 matrix of clockwise rotation about axis a-->b
19      by angle tau (radians).
20      a and b are sequences of 3 floats each
21      Result is a homogenous 4x4 transformation matrix.
22      NOTE: This has been changed by Brian, 8/30/01: rotax now returns
23      the rotation matrix, _not_ the transpose. This is to get
24      consistency across rotax, mat_to_quat and the classes in
25      transformation.py
26      when transpose is 1 (default) a C-style rotation matrix is returned
27      i.e. to be used is the following way Mx (opposite of OpenGL style which
28      is using the FORTRAN style)
29      """
30
31      assert len(a) == 3
32      assert len(b) == 3
33      if tau <= -2*pi or tau >= 2*pi:
34          tau = tau%(2*pi)
35
36      ct = cos(tau)
37      ct1 = 1.0 - ct
38      st = sin(tau)
39
40      # Compute unit vector v in the direction of a-->b. If a-->b has length
41      # zero, assume v = (1,1,1)/sqrt(3).
42
43      v = [b[0]-a[0], b[1]-a[1], b[2]-a[2]]
44      s = v[0]*v[0]+v[1]*v[1]+v[2]*v[2]
45      if s > 0.0:
46          s = sqrt(s)
47          v = [v[0]/s, v[1]/s, v[2]/s]
48      else:
49          val = sqrt(1.0/3.0)
50          v = (val, val, val)
51
52      rot = N.zeros( (4,4), 'f' )
53      # Compute 3x3 rotation matrix
54
55      v2 = [v[0]*v[0], v[1]*v[1], v[2]*v[2]]
56      v3 = [(1.0-v2[0])*ct, (1.0-v2[1])*ct, (1.0-v2[2])*ct]
57      rot[0][0]=v2[0]+v3[0]
58      rot[1][1]=v2[1]+v3[1]
59      rot[2][2]=v2[2]+v3[2]
60      rot[3][3] = 1.0;
61
62      v2 = [v[0]*st, v[1]*st, v[2]*st]
63      rot[1][0]=v[0]*v[1] * ct1-v2[2]
64      rot[2][1]=v[1]*v[2] * ct1-v2[0]
65      rot[0][2]=v[2]*v[0] * ct1-v2[1]
66      rot[0][1]=v[0]*v[1] * ct1+v2[2]
67      rot[1][2]=v[1]*v[2] * ct1+v2[0]
68      rot[2][0]=v[2]*v[0] * ct1+v2[1]
69
70      # add translation
71      for i in (0,1,2):
72          rot[3][i] = a[i]
73          for j in (0,1,2):
74              rot[3][i] = rot[3][i]-rot[j][i]*a[j]
75          rot[i][3]=0.0
76
77      if transpose:
78          return rot
79      else:
80          return N.transpose(rot)
81
82
83  from math import asin
84
85 -def rotVectToVect(vect1, vect2, i=None):
86      """returns a 4x4 transformation that will align vect1 with vect2
87  vect1 and vect2 can be any vector (non-normalized)
88  """
89      v1x, v1y, v1z = vect1
90      v2x, v2y, v2z = vect2
91
92      # normalize input vectors
93      norm = 1.0/sqrt(v1x*v1x + v1y*v1y + v1z*v1z )
94      v1x *= norm
95      v1y *= norm
96      v1z *= norm
97      norm = 1.0/sqrt(v2x*v2x + v2y*v2y + v2z*v2z )
98      v2x *= norm
99      v2y *= norm
100      v2z *= norm
101
102      # compute cross product and rotation axis
103      cx = v1y*v2z - v1z*v2y
104      cy = v1z*v2x - v1x*v2z
105      cz = v1x*v2y - v1y*v2x
106
107      # normalize
108      nc = sqrt(cx*cx + cy*cy + cz*cz)
109      if nc==0.0:
110          return [ [1., 0., 0., 0.],
111                   [0., 1., 0., 0.],
112                   [0., 0., 1., 0.],
113                   [0., 0., 0., 1.] ]
114
115      cx /= nc
116      cy /= nc
117      cz /= nc
118
119      # compute angle of rotation
120      if nc<0.0:
121          if i is not None:
122              print 'truncating nc on step:', i, nc
123          nc=0.0
124      elif nc>1.0:
125          if i is not None:
126              print 'truncating nc on step:', i, nc
127          nc=1.0
128
129      alpha = asin(nc)
130      if (v1x*v2x + v1y*v2y + v1z*v2z) < 0.0:
131          alpha = pi - alpha
132
133      # rotate about nc by alpha
134      # Compute 3x3 rotation matrix
135
136      ct = cos(alpha)
137      ct1 = 1.0 - ct
138      st = sin(alpha)
139
140      rot = [ [0., 0., 0., 0.],
141              [0., 0., 0., 0.],
142              [0., 0., 0., 0.],
143              [0., 0., 0., 0.] ]
144
145
146      rv2x, rv2y, rv2z = cx*cx, cy*cy, cz*cz
147      rv3x, rv3y, rv3z = (1.0-rv2x)*ct, (1.0-rv2y)*ct, (1.0-rv2z)*ct
148      rot[0][0] = rv2x + rv3x
149      rot[1][1] = rv2y + rv3y
150      rot[2][2] = rv2z + rv3z
151      rot[3][3] = 1.0;
152
153      rv4x, rv4y, rv4z = cx*st, cy*st, cz*st
154      rot[0][1] = cx * cy * ct1 - rv4z
155      rot[1][2] = cy * cz * ct1 - rv4x
156      rot[2][0] = cz * cx * ct1 - rv4y
157      rot[1][0] = cx * cy * ct1 + rv4z
158      rot[2][1] = cy * cz * ct1 + rv4x
159      rot[0][2] = cz * cx * ct1 + rv4y
160
161      return rot
162
163
164 -def mat_to_quat(matrix,transpose=1):
165      """ takes a four by four matrix (optionally with shape (16,) and
166      converts it into the axis of rotation and angle to rotate by
167      (x,y,z,theta). It does not expect an OpenGL style, transposed
168      matrix, so is consistent with rotax
169      """
170
171      if N.shape(matrix) not in ((16,),(4,4)):
172          raise ValueError("Argument must Numeric array of shape (4,4) or (16,)")
173
174      if N.shape(matrix) == (4, 4):
175          matrix = N.reshape(matrix,(16,))
176
177      cofactor1 = matrix[5]*matrix[10] - matrix[9]*matrix[6]
178      cofactor2 = matrix[8]*matrix[6] - matrix[4]*matrix[10]
179      cofactor3 = matrix[4]*matrix[9] - matrix[8]*matrix[5]
180
181      det = matrix[0]*cofactor1 + matrix[1]*cofactor2 + matrix[2]*cofactor3
182      if not (0.999 < det < 1.001):
183          print "Not a unit matrix: so not a pure rotation"
184          print 'Value of Determinant is: ',det
185      trace = matrix[0] + matrix[5] + matrix[10] + matrix[15]
186      if trace > 0.0000001:# rotation other than 180deg
187          S = 0.5/sqrt(trace)
188          Qw = 0.25/S
189          Qx = (matrix[9]-matrix[6])*S
190          Qy = (matrix[2]-matrix[8])*S
191          Qz = (matrix[4]-matrix[1])*S
192      else: #180deg rotation, just need to figure out the axis
193          Qw = 0.
194          diagonal = ((matrix[0],0),
195                      (matrix[5],5),
196                      (matrix[10],10))
197          idx = max(diagonal)[1]
198          if idx == 0:
199              S  = sqrt(1.0 + matrix[0] - matrix[5] - matrix[10])*2
200              Qy = (matrix[1] + matrix[4] ) / S
201              Qz = (matrix[2] + matrix[8] ) / S
202              Qx = N.sqrt(1-Qy*Qy-Qz*Qz)
203          elif idx==5:
204              S  = sqrt( 1.0 + matrix[5] - matrix[0] - matrix[10] )*2
205              Qx = (matrix[1] + matrix[4] ) / S
206              Qz = (matrix[6] + matrix[9] ) / S
207              Qy = N.sqrt(1-Qx*Qx-Qz*Qz)
208          elif idx==10:
209              S  = sqrt( 1.0 + matrix[10] - matrix[0] - matrix[5] )*2
210              Qx = (matrix[2] + matrix[8] ) / S
211              Qy = (matrix[6] + matrix[9] ) / S
212              Qz = N.sqrt(1-Qx*Qx-Qy*Qy)
213      # check if identity or not
214      if Qw != 1.:
215          angle = N.arccos(Qw)
216          theta = angle*360./N.pi
217          Z = sqrt(Qx*Qx + Qy*Qy + Qz*Qz)
218          if transpose:
219              Qx = -Qx/Z
220              Qy = -Qy/Z
221              Qz = -Qz/Z
222          else:
223              Qx = Qx/Z
224              Qy = Qy/Z
225              Qz = Qz/Z
226          Qw = theta
227          return [Qx,Qy,Qz,Qw]
228      else:
229          return [0.,0.,0.,0.]
230
231
232
233 -def inverse4X4(matrix):
234      """ returns the inverse of the given 4x4 transformation matrix
235  t_1: the negetive of Translation vector
236  r_1: the inverse of rotation matrix
237
238  inversed transformation is
239  1) t_1 applied first
240  2) then r_1 is applied
241
242  to validate the result, N.matrixmultiply(matrix, mat_inverse)==N.identity(4,'f')
243  """
244      # check the shape
245      if matrix.shape !=(4,4) and matrix.shape !=(16,) :
246          raise ValueError("Argument must Numeric array of shape (4,4) or (16,)")
247          return None
248      if matrix.shape ==(16,):
249          matrix=N.array(matrix,'f')
250          matrix=N.reshape(matrix,(4,4))  # force the matrix to be (4,4)
251      t_1=N.identity(4,'f')
252      t_1[:2,3]= - matrix[:2, 3]
253      r_1=N.identity(4,'f')
254      r_1[:3,:3] = N.transpose(matrix[:3,:3])
255      mat_inverse=N.matrixmultiply(r_1, t_1)
256      #asert N.matrixmultiply(matrix, mat_inverse) is N.identity(4,'f')
257      return mat_inverse
258
259
260 -def mat_to_axis_angle( matrix ):
261      """
262      NOTE: This function is added by Yong 2/01/04: given a 4x4 transformation
263  matrix of hinge motion, now returns the rotation angle and axis (defined by
264  vector and a point) Please be noticed that if the motion is not hinge, the
265  function will complain and return none
266  """
267      if matrix.shape != (16,) and matrix.shape != (4,4):
268          raise ValueError("matrix should be of shape (4,4) or (16,)")
269          return None
270
271      if matrix.shape == (16,):
272          matrix = N.reshape(matrix, (4,4))
273
274      from math import sin, cos, pi, sqrt, fabs, acos
275      degtorad = pi/180.
276
277      transf = matrix
278      from mglutil.math.rotax import mat_to_quat
279      rotMat =  N.identity(4, 'f')
280      rotMat[:3,:3] = transf[:3,:3]
281      qB = mat_to_quat(matrix=N.array(rotMat).flat)
282      angle = qB[3]
284      if(fabs(angle) < 0.0005):
285          sa = 1
286      if angle > 180.:
287          vector=[-qB[0]/sa, -qB[1]/sa, -qB[2]/sa]
288      else:
289          vector=[qB[0]/sa, qB[1]/sa, qB[2]/sa]
290      tranMat = transf[3,:3]
291
292      # check if the transformation is a hinge motion
293      a=vector
294      b=tranMat
295      c =[a[0]-b[0], a[1]-b[1], a[2]-b[2]]
296      a2= a[0]*a[0] + a[1]*a[1] + a[2]*a[2]
297      b2= b[0]*b[0] + b[1]*b[1] + b[2]*b[2]
298      c2= c[0]*c[0] + c[1]*c[1] + c[2]*c[2]
299      theta = acos((c2-a2-b2)/(2* sqrt(a2*b2))) / pi * 180
300      if fabs(theta -90) > 1e-4:
301          raise ValueError("The given transformation is not a hinge motion")
302          return None, None, None
303
304      ratio = sqrt( 1. / (2. * (1.- cos(degtorad * angle ))))
305      p = [tranMat[0]*ratio, tranMat[1]*ratio, tranMat[2]*ratio]
306
307      ang = 90. - angle/2.0
308      rot = rotax([0.,0.,0.], vector, ang*degtorad, transpose=1)
309      rot = rot[:3,:3]
310      point = N.matrixmultiply(p, rot)
311
312      return vector, point, angle
313
314
315 -def interpolate3DTransform(matrixList, indexList, percent):
316      """ This function gets input of two list and a percent value.
317  Return value is a 4x4 matrix corresponding to percent% of the transformation.
318
319  matrixList: a list of 4x4 transformation matrix
320  indexList : a list of sorted index (positive float number)
321  percent   : a positive float number.
322  if only one matrix in the matrix list:
323  percent =   0.0  means no transformation (identity)
324              1.0  means 100% of the transformation (returns mat)
325              0.58 means 58% of translation and rotatetion 58% of rotation angle
326              along the same rotation axis
327  percent can go above 1.0
328
329  If matrixList has more than one matrix:
330  matrixList=[M1,  M2,  M3]     #Attention: All M uses the same reference frame
331  indexList =[0.2, 0.5, 1.0]    #Attention: assume the list sorted ascendingly
332  p = 0.5 means apply M2
333  p = 0.8 means apply M3
334  p = 0.9 means apply M2 first, then apply 50% of M'.  M' is the transformation
335                      from M2 to M3.   50% = (0.9-0.8) / (1.0-0.8)
336                      M2 x M' = M3
337                      -->  M2.inverse x M2 x M'= M2.inverse x M3
338                      -->  M'= M2.inverse x M
339  """
340      listLen = len(matrixList)
341      if listLen != len(indexList):
342          raise ValueError("matrix list should have same length of index list")
343      if listLen == 0:
344          raise ValueError("no matrix found in the matrix list")
345
346      offset = -1
347      for i in range(listLen):
348          if indexList[i] >= percent:
349              offset = i
350              break
351
352      prevMat = nextMat = N.identity(4,'f')
353      if offset == -1:
354          prevMat = matrixList[-1]
355          p = percent/indexList[-1]
356          return _interpolateMat(matrixList[-1], p)
357      elif offset == 0:
358          nextMat = matrixList[0]
359          p = percent/indexList[0]
360          return _interpolateMat(N.array(matrixList[0]), p)
361      else:
362          prevMat = matrixList[offset-1]
363          nextMat = matrixList[offset]
364          p = (percent-indexList[offset-1])/(
365                                      indexList[offset]-indexList[offset-1])
366          from LinearAlgebra import inverse
367          M = N.matrixmultiply(inverse(prevMat), nextMat)
368          Mat = _interpolateMat(M, p)
369          return N.matrixmultiply(prevMat, Mat)
370
371
372 -def interpolate3DTransform1(matrixList, indexList, percent):
373      # MS version that does not assume identity as fist matrix and does
374      # not wrap around
375
376      if percent <= indexList[0]:
377          return matrixList[0]
378
379      if percent >=indexList[-1]:
380          return matrixList[-1]
381
382      listLen = len(indexList)
383      for i in range(listLen):
384          if indexList[i] > percent:
385              break
386
387      prevMat = matrixList[i-1]
388      nextMat = matrixList[i]
389      from LinearAlgebra import inverse
390      M = N.matrixmultiply(inverse(prevMat), nextMat)
391      p = (percent-indexList[i-1]) / (indexList[i]-indexList[i-1])
392      Mat = _interpolateMat(M, p)
393      return N.matrixmultiply(prevMat, Mat)
394
395
396
397 -def _interpolateMat(mat, percent):
398      """ called only by interpolate3DTransform()
399      """
400      if mat.shape != (16,) and mat.shape != (4,4):
401          raise ValueError("matrix should be of shape (4,4) or (16,)")
402          return None
403
404      if mat.shape == (16,):
405          mat = N.reshape(mat, (4,4))
406
407    #  if percent <0.0:
408    #      raise ValueError('The parameter percent should be a positive float"')
409    #      return
410
411      p = percent
412      transf = mat[:,:]
413      rotMat =  N.identity(4, 'f')
414      rotMat[:3,:3]=transf.astype(N.Float32)[:3,:3]
415
416      from mglutil.math.rotax import mat_to_quat
417      quat = mat_to_quat(matrix=N.array(rotMat).flat)
418      angle = quat[3] * p
419
420      newRotMat = rotax([0.,0.,0.], quat[:3], angle*degtorad, transpose = 1)
421      newTranMat =  N.identity(4, 'f')
422      newTranMat[3][0] = transf[3][0]*p
423      newTranMat[3][1] = transf[3][1]*p
424      newTranMat[3][2] = transf[3][2]*p
425
426      transform = newRotMat
427      transform[3][0] = newTranMat[3][0]
428      transform[3][1] = newTranMat[3][1]
429      transform[3][2] = newTranMat[3][2]
430      # That's it.. the self.transform is now updated.
431
432      return transform
433
<!--
expandto(location.href);
// -->

```

 Generated by Epydoc 3.0alpha3 on Fri Nov 2 14:06:25 2007 http://epydoc.sourceforge.net