def mat3T(*args):
    if len(args) == 3:
        return mat3(args[0], args[1], args[2]).transpose()
    elif len(args) == 1:
        return mat3(args[0][0], args[0][1], args[0][2]).transpose()
    else:
        raise Exception, "Number of argument should be 1 or 3!"

    def UBxds_to_dnz(self):
        """ Convert the XDS direct space Orientation Matrix to a mosflm OM
        
        Denzo CAMERA coordinate frame has orthonormal axes with:

          y // to the rotation (spindel) axis
          z // to the beam
          x perpendicular to z and to the beam
        
        For more details see the denzo documentation:
        http://www.ccp4.ac.uk/dist/x-windows/Mosflm/doc/mosflm_user_guide.html#a3
        """
            
        if "UB" not in self.dict.keys():
            self.debut()
            
        BEAM = vec3(self.dict["beam"])
        ROT = vec3(self.dict["rot"])
        UBxds = self.dict["UB"]
                
        CAMERA_y = ROT.normalize()
        CAMERA_x = CAMERA_y.cross(BEAM).normalize()
        CAMERA_z = CAMERA_x.cross(CAMERA_y)
        CAMERA = mat3(CAMERA_x,CAMERA_y,CAMERA_z).transpose()
                    
        return  CAMERA * UBxds

def getPermutUB(PGoperators, UB, _epsilonCell=1e-2):
    "Apply Point group permutations to UB, return a list of permutated UB"

    cell = reciprocal(UB_to_cellParam(UB))
    permutedList = []

    for equiv in PGoperators:
        # Note that equiv_mat is the transpose of the normal equiv matrix!
        # because of the way mat3 matrices are contructed.
        equiv_mat = mat3(equiv[0],equiv[1],equiv[2])
        new_UB = UB * equiv_mat

        # One simple way to verify that the permutation is correct is
        # to extract the cell parameters from the permuted UB matrix
        if _debug:
            new_cell = reciprocal(UB_to_cellParam(new_UB))
            assert abs(new_cell[0] - cell[0]) < _epsilonCell and \
                   abs(new_cell[1] - cell[1]) < _epsilonCell and \
                   abs(new_cell[2] - cell[2]) < _epsilonCell and \
                   abs(new_cell[3] - cell[3]) < _epsilonCell and \
                   abs(new_cell[4] - cell[4]) < _epsilonCell and \
                   abs(new_cell[5] - cell[5]) < _epsilonCell 

        permutedList.append(new_UB)
    return permutedList

    def debut(self):
        "Do simple cristallographic calculations from XDS initial parameters"

        A = vec3(self.dict["A"])
        B = vec3(self.dict["B"])
        C = vec3(self.dict["C"])

        volum = A.cross(B)*C
        Ar = B.cross(C).__div__(volum)
        Br = C.cross(A).__div__(volum)
        Cr = A.cross(B).__div__(volum)

        """
        Ar = B.cross(C)/volum
        Br = C.cross(A)/volum
        Cr = A.cross(B)/volum
        """
        UBxds = mat3(Ar,Br,Cr)

        BEAM = vec3(self.dict["beam"])
        wavelength = 1/BEAM.length()

        self.dict["cell_volum"] = volum
        self.dict["wavelength"] = wavelength
        self.dict["Ar"] = Ar
        self.dict["Br"] = Br
        self.dict["Cr"] = Cr
        self.dict["UB"] = UBxds

    def __init__(self, init, rotationAxes=(ex, ey, ez), inversAxesOrder=0):
        self.inversAxesOrder = inversAxesOrder
        self.e1 = Vector(rotationAxes[0]).normalize()
        self.e2 = Vector(rotationAxes[1]).normalize()
        self.e3 = Vector(rotationAxes[2]).normalize()
        if inversAxesOrder:
            self.e1 = Vector(rotationAxes[2]).normalize()
            self.e3 = Vector(rotationAxes[0]).normalize()
        self.rotationAxes = self.e1, self.e2, self.e3
	if hasattr(init,'is_tensor') == 1:
	    self.tensor = init
	elif hasattr(init,'is_rotation') == 1:
	    self.tensor = init.tensor
	elif len(init) == 3:
            a1, a2, a3 = init
            if inversAxesOrder: a3, a2, a1 = init
            R1 = Rotation2(self.e1, a1)
            R2 = Rotation2(self.e2, a2)
            R3 = Rotation2(self.e3, a3)
	    self.tensor = R1*R2*R3
	elif len(init) == 9 and \
            (type(init) == type([]) or type(init) == type(())):
            "Used for compatibility with other tensor types, like cgtypes.mat3"
            "In that case, use mat3.mlist."
            self.tensor = mat3(list(init))
	else:
	    raise TypeError, 'no valid arguments'

 def UBxds_to_mos(self):
     """ Convert the XDS direct space Orientation Matrix to a mosflm OM
     
     Mosflm CAMERA coordinate frame has orthonormal axes with:
     
       z // rotation axis
       y perpendicular to z and to the beam
       x perpendicular to y and z (along the beam)
     
     For more details see the mosflm documentation:
     http://www.ccp4.ac.uk/dist/x-windows/Mosflm/doc/mosflm_user_guide.html#a3
     """
     
     if "UB" not in self.dict.keys():
         self.debut()
         
     BEAM = vec3(self.dict["beam"])
     ROT = vec3(self.dict["rot"])
     UBxds = self.dict["UB"]
     
     CAMERA_z = ROT.normalize()
     CAMERA_y = CAMERA_z.cross(BEAM).normalize()
     CAMERA_x = CAMERA_y.cross(CAMERA_z)
     CAMERA = mat3(CAMERA_x,CAMERA_y,CAMERA_z).transpose()
     
     return  CAMERA * UBxds * self.dict["wavelength"]

def test_2(axes_i):
    from ThreeAxisRotation import ThreeAxisRotation
    import Numeric, random
    
    r = random.uniform
    a1, a2, a3 = r(-pi,pi),r(-pi,pi),r(-pi,pi)
    angles_i = [a1, a2, a3]
    rot1 = ThreeAxisRotation (angles_i, rotationAxes=axes_i, inversAxesOrder=1)
    rot2 = ThreeAxisRotation2(angles_i, rotationAxes=axes_i, inversAxesOrder=1)
    rot1t = mat3(list(Numeric.ravel(rot1.tensor.array)))
    diffmat1 = math.sqrt(addReduceSq((rot1t - rot2.tensor).mlist))

    sol1A, sol1B = rot1.getAngles()
    sol2A, sol2B = rot2.getAngles()
    
    diff1I = math.sqrt(min(addReduceSq(map(diff, zip(sol1A,angles_i))), 
                           addReduceSq(map(diff, zip(sol1B,angles_i)))))
    diff2I = math.sqrt(min(addReduceSq(map(diff, zip(sol2A,angles_i))), 
                           addReduceSq(map(diff, zip(sol2B,angles_i)))))
    diffAA = math.sqrt(addReduceSq(map(diff, zip(sol2A,sol1A))))
    diffBB = math.sqrt(addReduceSq(map(diff, zip(sol2B,sol1B))))
    
    #if 1:
    ae = 1e-12
    if diffmat1 > 1e-12 or diffAA > ae or diffBB > ae or diff1I > ae or diff2I > ae:
        print "0 "+3*"%10.2f" % tuple(angles_i)
        print "1A"+3*"%10.2f" % tuple(sol1A)
        print "2A"+3*"%10.2f" % tuple(sol2A)
        print "1B"+3*"%10.2f" % tuple(sol1B)
        print "2B"+3*"%10.2f" % tuple(sol2B)
        print 'Matrix difference \t%.1e' % diffmat1
        print '1I Angle difference \t%.1e' % diff1I
        print '2I Angle difference \t%.1e' % diff1I
        print 'AA Angle difference \t%.1e' % diffAA
        print 'BB Angle difference \t%.1e' % diffBB

 def Adnz_to_rotxyz(self, Adnz, Bdnz, vertical, spindle):
     U2a = (Bdnz * spindle).normalize()
     U1a = Bdnz * vertical
     U1b = (U1a - (U1a * U2a) * U2a).normalize()
     Udnz0 = mat3(U1b, U2a, U1b.cross(U2a)).transpose()
     Adnz0 = Udnz0 * Bdnz
     RMAT = Adnz * Adnz0.inverse()
     dnzrmat = ThreeAxisRotation2(RMAT.mlist, self.DNZAxes)
     rotxyz = dnzrmat.getAngles()
     return rotxyz, Udnz0

def getPermutU(PGoperators, U, _epsilonCell=1e-4, debug=True):
    "Apply Point group permutations to U, return a list of permutated U"

    permutedList = []
    for equiv in PGoperators:
        # Note that equiv_mat is the transpose of the normal equiv matrix!
        # because of the way mat3 matrices are contructed.
        equiv_mat = mat3(equiv[0],equiv[1],equiv[2])
        new_U = equiv_mat.transpose() * U
        if is_orthogonal(new_U):
            permutedList.append(new_U)
        else:
            print "Internal Error: permuted U matrix is not orthogonal !"
            print new_U
    return permutedList

    def getOmega(self):
        """Calculate an Omega value (in radian) wich defines how the fast (X) and
        slow (Y) axis of detector files are  orientated toward the camera frame.
        The calculation of this omega value is supposed to reflect the Mosflm
        definition...

        But it seems that I get different values from the mosflm defaults... This
        may be due to: A) My missanderstanding of the mosflm documentation, B)
        Some tricks in the image reading routines.

        Nonetheless, this calculated value works for translating
        correctly the beam coordinates from XDS to mosflm [at least in the tested
        cases of MARCCD, MAR345 and ADSC detector images].

        Reference:
        http://www.ccp4.ac.uk/dist/x-windows/Mosflm/doc/mosflm_user_guide.html#a3
        """
        # Xd = CAMERA_y = beam
        # Yd = CAMERA_z = rot
        Xd =  vec3(self.dict["beam"]).normalize()
        Yd =  vec3(self.dict["rot"]).normalize()
        CAMERA_x = Xd.cross(Yd)
        CAMERA = mat3(CAMERA_x, Xd, Yd).transpose()
            
        # This is the definition of the fast:X and slow:Y axis for the detector files.
        XDSdetector_X = vec3(self.dict["detector_X"])
        XDSdetector_Y = vec3(self.dict["detector_Y"])
                
        # Now this axes are translated in the mosflm Camera frame
        Xs = XDSdetector_X*CAMERA
        Ys = XDSdetector_Y*CAMERA
        
        # Both angles should be identical.
        omegaX = Xd.angle(Xs)
        omegaY = Yd.angle(Ys)
        
        if _debug:
            print "DEBUG: X xds: fast =",XDSdetector_X
            print "DEBUG: Y xds: slow =",XDSdetector_Y
            print "DEBUG: Xs: fast =",XDSdetector_X,"->", Xs
            print "DEBUG: Ys: slow =",XDSdetector_Y,"->", Ys
            print "DEBUG: Xd: ", Xd
            print "DEBUG: Yd: ", Yd
            print "DEBUG: OmegaX:   %8.2f" % (omegaX*r2d)
            print "DEBUG: OmegaY:   %8.2f" % (omegaY*r2d)
            
        return omegaX

    def get_U0(self, rcell=None, vertical=None, spindle=None, clean=False):
        "Calculate denzo U0 from spindle, verctical"
        if not rcell: rcell = self.cell_r
        if not vertical: vertical = self.verticalAxis
        if not spindle: spindle = self.spindleAxis

        Bmat = self.get_B(rcell)
        vertical = vec3(vertical)
        spindle = vec3(spindle)

        U0y = (Bmat * spindle).normalize()
        U0xi = Bmat * vertical
        U0x = (U0xi - (U0xi * U0y) * U0y).normalize()

        U0 = mat3(U0x, U0y, U0x.cross(U0y)).transpose()

        # cleaning... Just cosmetic, not realy needed.
        if clean: U0 = cleanU0(U0)
        return U0

 def get_B(self, rcell=None):
     """ Denzo Orthogonalisation matrix
       b* is aligned with spindle axis (2-nd coordinate)
       a* is in the plane perpendicular to the beam (1-st,2-nd coords)
     """
     if not rcell:
         rcell = self.cell_r
     sr = map(sind, rcell[3:6])
     cr = map(cosd, rcell[3:6])
     B  = mat3()
     B13 = (cr[1]-cr[2]*cr[0])/sr[2]
     B[0,0] = rcell[0] * sr[2]
     B[1,0] = rcell[0] * cr[2]
     B[1,1] = rcell[1]
     B[0,2] = rcell[2] * B13
     B[1,2] = rcell[2] * cr[0]
     B[2,2] = rcell[2] * (sr[0]**2 - B13**2)**0.5
     
     return B

def axis_and_angle(mat_3):
    """From a rotation matrix return a corresponding rotation as an
       axis (a normalized vector) and angle (in radians).
       The angle is in the interval (-pi, pi]
    """
    asym = -asymmetrical_part(mat_3)
    axis = vec3(asym[1, 2], asym[2, 0], asym[0, 1])
    sine = axis.length()
    if abs(sine) > 1.e-10:
        axis = axis/sine
        projector = dyadic_product(axis, axis)
        cosine = trace((mat_3-projector))/(3.-axis*axis)
        angle = angle_from_sine_and_cosine(sine, cosine)
    else:
        tsr = 0.5*(mat_3+mat3(1))
        diag = tsr[0, 0], tsr[1, 1], tsr[3, 3] 
        i = tsr.index(max(diag))
        axis = vec3(tsr.getRow(i)/(tsr[i, i])**0.5)
        angle = 0.
        if trace(tsr) < 2.:
            angle = math.pi
    return axis, angle

        diag = tsr[0, 0], tsr[1, 1], tsr[3, 3] 
        i = tsr.index(max(diag))
        axis = vec3(tsr.getRow(i)/(tsr[i, i])**0.5)
        angle = 0.
        if trace(tsr) < 2.:
            angle = math.pi
    return axis, angle

# Test code
if __name__ == '__main__':

    from Scientific.Geometry import Vector ##.Transformation import *
    from Scientific.Geometry.Transformation import Rotation
    from random import random
    #
    Q = mat3(0.36, 0.48, -0.8, -0.8, 0.6, 0, 0.48, 0.64, 0.60)
    axis_q, angle_q = axis_and_angle(Q)
    print "Axis_q:  %9.6f%9.6f%9.6f" % tuple(axis_q),
    print "Angle_q: %10.5f" % (angle_q*R2D)
    #
    for iii in range(1e6):
        axis_i = list(vec3([random(), random(), random()]).normalize())
        angle_i = 3*random()
        rme = mat3().rotation(angle_i, vec3(axis_i))
        axis_1, angle_1 = axis_and_angle(rme)

        v = Vector(axis_i)
        r = Rotation(v, angle_i)
        axis_2, angle_2 = r.axisAndAngle()
        axis_d = (axis_1 - vec3(tuple(axis_2))).length()
        angle_d = abs(angle_1 - angle_2)

def dyadic_product(vector1, vector2):
    "Dyadic product of two vectors."
    matr1, matr2 = mat3(), mat3()
    matr1.setColumn(0, vector1)
    matr2.setRow(0, vector2)
    return matr1*matr2

    def parse(self, filename):
        "Denzo x-file parser"
        try:
            xfile = open(filename,"r").read().splitlines()
            xhead = xfile[:7]
            xtail = xfile[-30:]

        except:
            raise ParserError, "Error, Can't read file: %s" % filename

        if xhead[0][:6] == "HEADER":
            xhead =  xhead[1:]
        self.title = xhead[0]
        mats = map(str2floats, xhead[1:4])
        self.UB = mat3(mats[0][:3], mats[1][:3], mats[2][:3]).transpose()
        self.U =  mat3(mats[0][3:], mats[1][3:], mats[2][3:]).transpose()

        if len(xhead[4].split()) == 4: line1, line2 = xhead[4], xhead[5][:40]
        else: line1, line2 = xhead[4][:48], xhead[4][48:88]
        self.phi0, self.phi1, self.xtod, self.wavel = str2floats(line1)
        self.rotz, self.roty, self.rotx, self.mosaic = str2floats(line2)
        self.crystal_setting = self.rotz, self.roty, self.rotx

        # Extract reciprocal unit cell vectors
        self.Ar = vec3(self.UB.getColumn(0))
        self.Br = vec3(self.UB.getColumn(1))
        self.Cr = vec3(self.UB.getColumn(2))

        # Extract reciprocal cell parameters
        self.cell_r = UB_to_cellParam(self.UB)
        self.cell = reciprocal(self.cell_r)

        # Calculate direct unit cell vectors
        self.volum_r = self.Ar.cross(self.Br)*self.Cr
        self.volum = 1/self.volum_r

        self.A = self.Br.cross(self.Cr)*self.volum
        self.B = self.Cr.cross(self.Ar)*self.volum
        self.C = self.Ar.cross(self.Br)*self.volum

        for line in xtail:
            lineSplit = line.split()
            if line.upper().count("SPACE GROUP"):
                self.spg = lineSplit[2]
            elif line.upper().count("SPINDLE AXIS"):
                self.spindleAxis = map(int,lineSplit[2:5])
                self.verticalAxis = map(int,lineSplit[7:])
            elif line.upper().count("MOTOR AXIS"):
                self.motorAxis = map(float,lineSplit[2:5])
            elif line.upper().count("DISTANCE"):
                self.distance = float(lineSplit[1])
            elif line.upper().count("X BEAM"):
                self.beam_x = float(lineSplit[2])
                self.beam_y = float(lineSplit[5])
            elif line.upper().count("SECTOR"):
                self.sector = int(lineSplit[1])
            elif line.upper().count("RAW DATA FILE"):
                self.template = str(lineSplit[-1]).replace("'","")
            elif line.upper().count("UNIT CELL"):
                self.cell2 = map(float,lineSplit[2:])
                # Verify that the cell extracted from UB correspond
                # to the cell read from the xfile tail
                assert abs(self.cell2[0] - self.cell[0]) < 1e-2 and \
                       abs(self.cell2[1] - self.cell[1]) < 1e-2 and \
                       abs(self.cell2[2] - self.cell[2]) < 1e-2 and \
                       abs(self.cell2[3] - self.cell[3]) < 2e-2 and \
                       abs(self.cell2[4] - self.cell[4]) < 2e-2 and \
                       abs(self.cell2[5] - self.cell[5]) < 2e-2 

        # Verify that the calculation method for UB_to_Rotxyz works correctly
        _rotx, _roty, _rotz = self.UB_to_Rotxyz()
        assert abs(_rotx - self.rotx) < 2e-2 and \
               abs(_roty - self.roty) < 2e-2 and \
               abs(_rotz - self.rotz) < 2e-2

        # Verify that the calculation method for Adnz_to_Udnz works correctly
        _U = self.Adnz_to_Udnz()
        print diffMAT(_U, self.U)
        assert diffMAT(_U, self.U) < 5e-6

def solve(desiredSetting, VL, orthogMatrices, U0, Goniometer,
                              PG_permutions=[[X,Y,Z]]):
    """ Solve for datum position
    Uses:
        VL              Laboratory vectors to match
        orthogMatrices  Dict containing {'rec':Bm1t, 'dir':B}
        desiredSetting  List of the two vectors defining the desired setting
        U0              Zero-angle setting matrix
        PG_permutions   Point group permutations

    Return:
        datumSolutions (list of datum solutions in degrees)

    Local variables
        U0m1    [U0]**-1
        C       base vectors in crystal frame
        Cm1     [C]**-1
        L       base vectors in laboratory frame
        SL      L matrix with permuted signs
        CU      [C]**-1 [U0]**-1
    """

    # Orthogonalize crystal vectors h -> yv
    # YV crystal frame vectors defining desired setting (orthogonalized)

    YV = []
    for vi in 0, 1:
        # if reciproc: B*vi.vector elif direct: B1mt*vi.vector
        B = orthogMatrices[desiredSetting[vi].space]
        YV.append(B*desiredSetting[vi])

    #  referential_permutations sign permutations for four permutations of
    #        parallel/antiparallel (rotation axis & beam)
    #    y1 // e1, y2 // beamVector;  y1 anti// e1, y2 // beamVector
    #    y1 // e1, y2 anti// beamVector;  y1 anti// e1, y2 anti// beamVector

    referential_permutations = [ ex,  ey,  ez], \
                               [-ex, -ey,  ez], \
                               [ ex, -ey, -ez], \
                               [-ex,  ey, -ez]

    # Construct orthogonal frame in crystal space defined by vectors
    # y1 = yv(,1) and y2 = yv(,2). The base vectors form the COLUMNS
    # of matrix C
    #  - 1st vector along YV1
    #  - 3rd vector perpendicular to YV1 & YV2
    #  - 2nd vector completes the RH set

    C1 = YV[0].normalize()
    C3 = YV[0].cross(YV[1]).normalize()
    C2 = C3.cross(C1).normalize()
    C = mat3(C1, C2, C3)

    # Similarly, construct laboratory frame matrix defined by VL1, VL2
    #  1st vector along VL1
    #  3rd vector perpendicular to VL1 & VL2
    #  2nd vector completes the RH set

    L1 = VL[0].normalize()
    L3 = VL[0].cross(VL[1]).normalize()
    L2 = L3.cross(L1).normalize()
    L = mat3(L1, L2, L3)

    # The matrix which superimposes the two frames is L C**-1. This
    # is then the orientation matrix DU, including the required datum
    # position D.  The datum matrix is then  D = L C**-1 U**-1
    # We need to consider four possible alignments & values of D,
    # corresponding to y1 parallel & antiparallel to vl1, and y2
    # parallel & antiparallel to vl2. To get these, we change
    # the signs of l1 & l2, or l2 & l3, or both, as set out in array LS.

    datumSolutions = []
    independentSolution = []

    # Loop four solutions for D, each with possibly two sets of goniostat
    # angles

    Cm1 = C.inverse()
    B = orthogMatrices['rec']
    UB0 = U0 * B

    # Loop for introducing Point Group permutations:
    for PG_operator in PG_permutions:
        # We use the operator in reciprocal space so it need a transposition.
        PG_operator = mat3T(PG_operator)
        # printmat(B.dot(PG_operator),'B.dot(PG_operator)')

        # The PG operator is applied on the UB0 matrix
        U0perm = UB0 * PG_operator * B.inverse()
        if _debug:
            print
            printmat(PG_operator, 'PG_operator', "%12.6f")
            printmat(U0, 'U0', "%12.6f")
            printmat(U0perm, 'U0perm', "%12.6f")

        if not is_orthogonal(U0perm):
            print "!!!!!!!!!!!!!!! ERROR: Improper U0perm matrice!!"

        #  CU = C**-1 . U**-1
        CU = Cm1 * U0perm.inverse()
#.........这里部分代码省略.........

def det_axis_y(twotheta, dettype):
    "Return the Y axis vector of the Dector surface."
    detori = XDS_DETECTOR_DICT["orient"][dettype]
    return V3FMT % tuple(detori[1]*mat3().rotation(twotheta*D2R, detori[4]))

import math

from pycgtypes import vec3
from pycgtypes import mat3
from ThreeAxisRotation2 import *


r2d = 180/math.pi
cosd = lambda a: math.cos(a/r2d)
sind = lambda a: math.sin(a/r2d)
map_r2d = lambda l: map(lambda x: x*r2d, l)
map_d2r = lambda l: map(lambda x: x/r2d, l)
str2floats = lambda s: map(float, s.split())
ex, ey, ez = vec3(1,0,0), vec3(0,1,0), vec3(0,0,1)

Qdnz2mos = mat3( ez, ex, ey).transpose()
Qdnz2xds = mat3(-ey, ex,-ez).transpose()
Qmos2xds = mat3( ez,-ey, ex).transpose()
Qmos2dnz = mat3( ey, ez, ex).transpose()

DNZAxes = ey, -ex, -ez

if sys.version_info[:3] < (2,2,0):
    True = 1
    False = 0

_debug = False
#_debug = True

def mat3T(*args):
    if len(args) == 3:

def Rotation2(axis, angle):
    return mat3().rotation(angle, axis)