コード例 #1
0
ファイル: rotationm.py プロジェクト: veteman/thepython2blob
 def getangax(self):
     #http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
     tolerance = 1e-12
     trace = 0
     for i in xrange(3):
         trace += self.matrix.getelement(i,i)
     arg = (trace - 1)/2
     angle = math2.acos(arg)
     d1 = self.matrix.getelement(2,1) - self.matrix.getelement(1,2)
     d2 = self.matrix.getelement(0,2) - self.matrix.getelement(2,0)
     d3 = self.matrix.getelement(1,0) - self.matrix.getelement(0,1)
     s = d1*d1 + d2*d2 + d3*d3
     if s > tolerance:
         isqrt = 1/math.sqrt(s)
         x = d1*isqrt
         y = d2*isqrt
         z = d3*isqrt
         axis = vector3d.Vector3D(x, y, z)
     elif arg + 1 < tolerance:
         #Half revolution
         xx = (self.matrix.getelement(0,0)+1)/2
         yy = (self.matrix.getelement(1,1)+1)/2
         zz = (self.matrix.getelement(2,2)+1)/2
         xy = (self.matrix.getelement(0,1)+self.matrix.getelement(1,0))/4
         xz = (self.matrix.getelement(0,2)+self.matrix.getelement(2,0))/4
         yz = (self.matrix.getelement(1,2)+self.matrix.getelement(2,1))/4
         if xx > yy and xx > zz:
             if xx < tolerance:
                 x = 0
                 y = SQRTHALF
                 z = SQRTHALF
             else:
                 x = math.sqrt(xx)
                 y = xy/x
                 z = xz/x
         elif yy > zz:
             if yy < tolerance:
                 x = SQRTHALF
                 y = 0
                 z = SQRTHALF
             else:
                 y = math.sqrt(yy)
                 x = xy/y
                 z = yz/y
         else:
             if zz < tolerance:
                 x = SQRTHALF
                 y = SQRTHALF
                 z = 0
             else:
                 z = math.sqrt(zz)
                 x = xz/z
                 y = yz/z
         axis = vector3d.Vector3D(x, y, z)
     else:
         #No revolution
         axis = vector3d.xbasev()  #Dummy vector
     return angle, axis
コード例 #2
0
ファイル: vector3d.py プロジェクト: veteman/thepython2blob
def anglebetween(v1, v2):
    #Should be valid for all vectors v1, v2.
    #v1 or v2 = (0, 0, 0) will return 0
    normprod = v1.norm() * v2.norm();
    if normprod == 0:
        # Correct way to handle zero vectors
        return 0
    dp = dotproduct(v1, v2)
    return math2.acos(dp / normprod)