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] 
283      sa=sin(angle*degtorad/2.0) 
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