Python Quaternion.from_matrix方法代码示例

# 需要导入模块: from SofaPython import Quaternion [as 别名]
# 或者: from SofaPython.Quaternion import from_matrix [as 别名]
def getImageTransform(filename, scaleFactor=1):
    """ Returns dim, voxelsize and rigid position of an image given an .mhd header image file
        a scaleFactor can be given to normalize image length units (usually in mm)
    """
    scale=[0,0,0]
    tr=[0,0,0]
    dim=[0,0,0]

    with open(filename,'r') as f:
        for line in f:
            splitted = line.split()
            if len(splitted)!=0:
                if 'ElementSpacing'==splitted[0] or 'spacing'==splitted[0] or 'scale3d'==splitted[0] or 'voxelSize'==splitted[0]:
                    scale = map(float,splitted[2:5])
                if 'Position'==splitted[0] or 'Offset'==splitted[0] or 'translation'==splitted[0] or 'origin'==splitted[0]:
                    tr = map(float,splitted[2:5])
                if 'Orientation'==splitted[0] or 'Rotation'==splitted[0] or 'TransformMatrix'==splitted[0] :
                    R = numpy.array([map(float,splitted[2:5]),map(float,splitted[5:8]),map(float,splitted[8:11])])
                if 'DimSize'==splitted[0] or 'dimensions'==splitted[0] or 'dim'==splitted[0]:
                    dim = map(int,splitted[2:5])
    q = quat.from_matrix(R)
    if scaleFactor!=1:
        scale = [s*scaleFactor for s in scale]
        tr = [t*scaleFactor for t in tr]
    offset=[tr[0],tr[1],tr[2],q[0],q[1],q[2],q[3]]
    return (dim,scale,offset)

# 需要导入模块: from SofaPython import Quaternion [as 别名]
# 或者: from SofaPython.Quaternion import from_matrix [as 别名]
def decomposeInertia(inertia):
    """ Decompose an inertia matrix into
    - a diagonal inertia
    - the rotation (quaternion) to get to the frame in wich the inertia is diagonal
    """
    U, diagonal_inertia, V = numpy.linalg.svd(inertia)
    # det should be 1->rotation or -1->reflexion
    if numpy.linalg.det(U) < 0 : # reflexion
        # made it a rotation by negating a column
        U[:,0] = -U[:,0]
    inertia_rotation = Quaternion.from_matrix( U )
    return diagonal_inertia, inertia_rotation

# 需要导入模块: from SofaPython import Quaternion [as 别名]
# 或者: from SofaPython.Quaternion import from_matrix [as 别名]
def read_rigid(rigidFileName):
    rigidFile = open( rigidFileName, "r" )
    line = list( rigidFile )
    rigidFile.close()
#        for i in xrange(len(line)):
#            print str(i) + str(line[i])
                    
    start = 1

    res = MassInfo()

    res.mass = float( line[start].split(' ')[1] )
    #volm = float( line[start + 1].split(' ')[1])
    res.com = map(float, line[start + 3].split(' ')[1:] )

    inertia = map(float, line[start + 2].split(' ')[1:] ) # pick inertia matrix from file
    res.inertia = array( [res.mass * x for x in inertia] ).reshape( 3, 3 ) # convert it in numpy 3x3 matrix

    # extracting principal axes basis and corresponding rotation and diagonal inertia

    if inertia[1]>1e-5 or inertia[2]>1e-5 or inertia[5]>1e-5 : # if !diagonal (1e-5 seems big but the precision from a mesh is poor)
#        print res.inertia
        U, res.diagonal_inertia, V = linalg.svd(res.inertia)
        # det should be 1->rotation or -1->reflexion
        if linalg.det(U) < 0 : # reflexion
            # made it a rotation by negating a column
#            print "REFLEXION"
            U[:,0] = -U[:,0]
        res.inertia_rotation = quat.from_matrix( U )
#       print "generate_rigid not diagonal U" +str(U)
#       print "generate_rigid not diagonal V" +str(V)
#       print "generate_rigid not diagonal d" +str(res.diagonal_inertia)
    else :
        res.diagonal_inertia = res.inertia.diagonal()
        res.inertia_rotation = [0,0,0,1]

#        print "generate_rigid " + str(res.mass) + " " + str( res.inertia ) + " " + str( res.diagonal_inertia )

    return res

# 需要导入模块: from SofaPython import Quaternion [as 别名]
# 或者: from SofaPython.Quaternion import from_matrix [as 别名]
def generate_rigid(filename, density = 1000.0, scale=[1,1,1], rotation=[0,0,0]):

        # TODO bind GenerateRigid
        # - faster than writing in a file
        # - more robust (if several processes try to work in the same file)


        tmpfilename = Tools.path( __file__ ) +"/tmp.rigid"

        cmd = [ Sofa.build_dir() + '/bin/GenerateRigid', filename, tmpfilename, str(density), str(scale[0]), str(scale[1]), str(scale[2]), str(rotation[0]), str(rotation[1]), str(rotation[2]) ]

#        print cmd
                         
        try:

            output = Popen(cmd, stdout=PIPE)

        except OSError:
            # try the debug version
            cmd[0] += 'd'

            try:
                    output = Popen(cmd, stdout=PIPE)
            except OSError:
                    print 'error when calling GenerateRigid, do you have GenerateRigid built in SOFA?'
                    raise

        output.communicate() # wait until Popen command is finished!!!

        # GenerateRigid output is stored in the file tmpfilename
        rigidFile = open( tmpfilename, "r" )
        line = list( rigidFile )
        rigidFile.close()
        
#        for i in xrange(len(line)):
#            print str(i) + str(line[i])
                    
        start = 1
        
        res = MassInfo()
        
        res.mass = float( line[start].split(' ')[1] )
        #volm = float( line[start + 1].split(' ')[1] )
        res.com = map(float, line[start + 3].split(' ')[1:] )


        inertia = map(float, line[start + 2].split(' ')[1:] ) # pick inertia matrix from file
        res.inertia = array( [res.mass * x for x in inertia] ).reshape( 3, 3 ) # convert it in numpy 3x3 matrix


        # extracting principal axes basis and corresponding rotation and diagonal inertia

        if inertia[1]>1e-5 or inertia[2]>1e-5 or inertia[5]>1e-5 : # if !diagonal (1e-5 seems big but the precision from a mesh is poor)
#            print res.inertia
            U, res.diagonal_inertia, V = linalg.svd(res.inertia)
            # det should be 1->rotation or -1->reflexion
            if linalg.det(U) < 0 : # reflexion
                # made it a rotation by negating a column
#                print "REFLEXION"
                U[:,0] = -U[:,0]
            res.inertia_rotation = quat.from_matrix( U )
#            print "generate_rigid not diagonal U" +str(U)
#            print "generate_rigid not diagonal V" +str(V)
#            print "generate_rigid not diagonal d" +str(res.diagonal_inertia)
        else :
            res.diagonal_inertia = res.inertia.diagonal()
            res.inertia_rotation = [0,0,0,1]

        

#        print "generate_rigid " + str(res.mass) + " " + str( res.inertia ) + " " + str( res.diagonal_inertia )

        return res