source: trunk/GSASIImath.py @ 811

Last change on this file since 811 was 811, checked in by vondreele, 10 years ago

split GSASIIgrid.py into 3 parts; GSASIIconstrGUI.py & GSASIIrestrGUI.py are new
  • Property svn:eol-style set to native
  • Property svn:keywords set to Date Author Revision URL Id
File size: 38.1 KB
Line 
1# -*- coding: utf-8 -*-
2#GSASIImath - major mathematics routines
3########### SVN repository information ###################
4# $Date: 2012-12-08 16:31:48 +0000 (Sat, 08 Dec 2012) $
5# $Author: vondreele $
6# $Revision: 811 $
7# $URL: trunk/GSASIImath.py $
8# $Id: GSASIImath.py 811 2012-12-08 16:31:48Z vondreele $
9########### SVN repository information ###################
10import sys
11import os
12import os.path as ospath
13import random as rn
14import numpy as np
15import numpy.linalg as nl
16import numpy.ma as ma
17import cPickle
18import time
19import math
20import copy
21import GSASIIpath
22GSASIIpath.SetVersionNumber("$Revision: 811 $")
23import GSASIIElem as G2el
24import GSASIIlattice as G2lat
25import GSASIIspc as G2spc
26import numpy.fft as fft
27
28sind = lambda x: np.sin(x*np.pi/180.)
29cosd = lambda x: np.cos(x*np.pi/180.)
30tand = lambda x: np.tan(x*np.pi/180.)
31asind = lambda x: 180.*np.arcsin(x)/np.pi
32acosd = lambda x: 180.*np.arccos(x)/np.pi
33atan2d = lambda y,x: 180.*np.arctan2(y,x)/np.pi
34
35def HessianLSQ(func,x0,Hess,args=(),ftol=1.49012e-8,xtol=1.49012e-8, maxcyc=0):
36   
37    """
38    Minimize the sum of squares of a set of equations.
39
40    ::
41   
42                    Nobs
43        x = arg min(sum(func(y)**2,axis=0))
44                    y=0
45
46    Parameters
47    ----------
48    func : callable
49        should take at least one (possibly length N vector) argument and
50        returns M floating point numbers.
51    x0 : ndarray
52        The starting estimate for the minimization of length N
53    Hess : callable
54        A required function or method to compute the weighted vector and Hessian for func.
55        It must be a symmetric NxN array
56    args : tuple
57        Any extra arguments to func are placed in this tuple.
58    ftol : float
59        Relative error desired in the sum of squares.
60    xtol : float
61        Relative error desired in the approximate solution.
62    maxcyc : int
63        The maximum number of cycles of refinement to execute, if -1 refine
64        until other limits are met (ftol, xtol)
65
66    Returns
67    -------
68    x : ndarray
69        The solution (or the result of the last iteration for an unsuccessful
70        call).
71    cov_x : ndarray
72        Uses the fjac and ipvt optional outputs to construct an
73        estimate of the jacobian around the solution.  ``None`` if a
74        singular matrix encountered (indicates very flat curvature in
75        some direction).  This matrix must be multiplied by the
76        residual standard deviation to get the covariance of the
77        parameter estimates -- see curve_fit.
78    infodict : dict
79        a dictionary of optional outputs with the key s::
80
81            - 'fvec' : the function evaluated at the output
82
83
84    Notes
85    -----
86
87    """
88               
89    x0 = np.array(x0, ndmin=1)      #might be redundant?
90    n = len(x0)
91    if type(args) != type(()):
92        args = (args,)
93       
94    icycle = 0
95    One = np.ones((n,n))
96    lam = 0.001
97    lamMax = lam
98    nfev = 0
99    while icycle < maxcyc:
100        lamMax = max(lamMax,lam)
101        M = func(x0,*args)
102        nfev += 1
103        chisq0 = np.sum(M**2)
104        Yvec,Amat = Hess(x0,*args)
105        Adiag = np.sqrt(np.diag(Amat))
106        psing = np.where(np.abs(Adiag) < 1.e-14,True,False)
107        if np.any(psing):                #hard singularity in matrix
108            return [x0,None,{'num cyc':icycle,'fvec':M,'nfev':nfev,'lamMax':lamMax,'psing':psing}]
109        Anorm = np.outer(Adiag,Adiag)
110        Yvec /= Adiag
111        Amat /= Anorm       
112        while True:
113            Lam = np.eye(Amat.shape[0])*lam
114            Amatlam = Amat*(One+Lam)
115            try:
116                Xvec = nl.solve(Amatlam,Yvec)
117            except nl.LinAlgError:
118                print 'ouch #1'
119                psing = list(np.where(np.diag(nl.qr(Amatlam)[1]) < 1.e-14)[0])
120                return [x0,None,{'num cyc':icycle,'fvec':M,'nfev':nfev,'lamMax':lamMax,'psing':psing}]
121            Xvec /= Adiag
122            M2 = func(x0+Xvec,*args)
123            nfev += 1
124            chisq1 = np.sum(M2**2)
125            if chisq1 > chisq0:
126                lam *= 10.
127            else:
128                x0 += Xvec
129                lam /= 10.
130                break
131        if (chisq0-chisq1)/chisq0 < ftol:
132            break
133        icycle += 1
134    M = func(x0,*args)
135    nfev += 1
136    Yvec,Amat = Hess(x0,*args)
137    try:
138        Bmat = nl.inv(Amat)
139        return [x0,Bmat,{'num cyc':icycle,'fvec':M,'nfev':nfev,'lamMax':lamMax,'psing':[]}]
140    except nl.LinAlgError:
141        print 'ouch #2 linear algebra error in LS'
142        psing = []
143        if maxcyc:
144            psing = list(np.where(np.diag(nl.qr(Amat)[1]) < 1.e-14)[0])
145        return [x0,None,{'num cyc':icycle,'fvec':M,'nfev':nfev,'lamMax':lamMax,'psing':psing}]
146   
147def getVCov(varyNames,varyList,covMatrix):
148    vcov = np.zeros((len(varyNames),len(varyNames)))
149    for i1,name1 in enumerate(varyNames):
150        for i2,name2 in enumerate(varyNames):
151            try:
152                vcov[i1][i2] = covMatrix[varyList.index(name1)][varyList.index(name2)]
153            except ValueError:
154                vcov[i1][i2] = 0.0
155    return vcov
156
157def FindAtomIndexByIDs(atomData,IDs,Draw=True):
158    indx = []
159    for i,atom in enumerate(atomData):
160        if Draw and atom[-3] in IDs:
161            indx.append(i)
162        elif atom[-1] in IDs:
163            indx.append(i)
164    return indx
165
166def FillAtomLookUp(atomData):
167    atomLookUp = {}
168    for iatm,atom in enumerate(atomData):
169        atomLookUp[atom[-1]] = iatm
170    return atomLookUp
171
172def GetAtomsById(atomData,atomLookUp,IdList):
173    atoms = []
174    for id in IdList:
175        atoms.append(atomData[atomLookUp[id]])
176    return atoms
177   
178def GetAtomItemsById(atomData,atomLookUp,IdList,itemLoc,numItems=1):
179    Items = []
180    if not isinstance(IdList,list):
181        IdList = [IdList,]
182    for id in IdList:
183        if numItems == 1:
184            Items.append(atomData[atomLookUp[id]][itemLoc])
185        else:
186            Items.append(atomData[atomLookUp[id]][itemLoc:itemLoc+numItems])
187    return Items
188       
189def getMass(generalData):
190    mass = 0.
191    for i,elem in enumerate(generalData['AtomTypes']):
192        mass += generalData['NoAtoms'][elem]*generalData['AtomMass'][i]
193    return mass   
194
195def getDensity(generalData):
196   
197    mass = getMass(generalData)
198    Volume = generalData['Cell'][7]
199    density = mass/(0.6022137*Volume)
200    return density,Volume/mass
201   
202def getRestDist(XYZ,Amat):
203    return np.sqrt(np.sum(np.inner(Amat,(XYZ[1]-XYZ[0]))**2))
204
205def getRestAngle(XYZ,Amat):
206   
207    def calcVec(Ox,Tx,Amat):
208        return np.inner(Amat,(Tx-Ox))
209
210    VecA = calcVec(XYZ[1],XYZ[0],Amat)
211    VecA /= np.sqrt(np.sum(VecA**2))
212    VecB = calcVec(XYZ[1],XYZ[2],Amat)
213    VecB /= np.sqrt(np.sum(VecB**2))
214    edge = VecB-VecA
215    edge = np.sum(edge**2)
216    angle = (2.-edge)/2.
217    angle = max(angle,-1.)
218    return acosd(angle)
219   
220def getRestPlane(XYZ,Amat):
221    sumXYZ = np.zeros(3)
222    for xyz in XYZ:
223        sumXYZ += xyz
224    sumXYZ /= len(XYZ)
225    XYZ = np.array(XYZ)-sumXYZ
226    XYZ = np.inner(Amat,XYZ).T
227    Zmat = np.zeros((3,3))
228    for i,xyz in enumerate(XYZ):
229        Zmat += np.outer(xyz.T,xyz)
230    Evec,Emat = nl.eig(Zmat)
231    Evec = np.sqrt(Evec)/(len(XYZ)-3)
232    Order = np.argsort(Evec)
233    return Evec[Order[0]]
234   
235def getRestChiral(XYZ,Amat):
236   
237    VecA = np.empty((3,3))   
238    VecA[0] = np.inner(XYZ[1]-XYZ[0],Amat)
239    VecA[1] = np.inner(XYZ[2]-XYZ[0],Amat)
240    VecA[2] = np.inner(XYZ[3]-XYZ[0],Amat)
241    return nl.det(VecA)
242       
243def getDistDerv(Oxyz,Txyz,Amat,Tunit,Top,SGData):
244   
245    def calcDist(Ox,Tx,U,inv,C,M,T,Amat):
246        TxT = inv*(np.inner(M,Tx)+T)+C+U
247        return np.sqrt(np.sum(np.inner(Amat,(TxT-Ox))**2))
248       
249    inv = Top/abs(Top)
250    cent = abs(Top)/100
251    op = abs(Top)%100-1
252    M,T = SGData['SGOps'][op]
253    C = SGData['SGCen'][cent]
254    dx = .00001
255    deriv = np.zeros(6)
256    for i in [0,1,2]:
257        Oxyz[i] += dx
258        d0 = calcDist(Oxyz,Txyz,Tunit,inv,C,M,T,Amat)
259        Oxyz[i] -= 2*dx
260        deriv[i] = (calcDist(Oxyz,Txyz,Tunit,inv,C,M,T,Amat)-d0)/(2.*dx)
261        Oxyz[i] += dx
262        Txyz[i] += dx
263        d0 = calcDist(Oxyz,Txyz,Tunit,inv,C,M,T,Amat)
264        Txyz[i] -= 2*dx
265        deriv[i+3] = (calcDist(Oxyz,Txyz,Tunit,inv,C,M,T,Amat)-d0)/(2.*dx)
266        Txyz[i] += dx
267    return deriv
268   
269def getAngSig(VA,VB,Amat,SGData,covData={}):
270   
271    def calcVec(Ox,Tx,U,inv,C,M,T,Amat):
272        TxT = inv*(np.inner(M,Tx)+T)+C
273        TxT = G2spc.MoveToUnitCell(TxT)+U
274        return np.inner(Amat,(TxT-Ox))
275       
276    def calcAngle(Ox,TxA,TxB,unitA,unitB,invA,CA,MA,TA,invB,CB,MB,TB,Amat):
277        VecA = calcVec(Ox,TxA,unitA,invA,CA,MA,TA,Amat)
278        VecA /= np.sqrt(np.sum(VecA**2))
279        VecB = calcVec(Ox,TxB,unitB,invB,CB,MB,TB,Amat)
280        VecB /= np.sqrt(np.sum(VecB**2))
281        edge = VecB-VecA
282        edge = np.sum(edge**2)
283        angle = (2.-edge)/2.
284        angle = max(angle,-1.)
285        return acosd(angle)
286       
287    OxAN,OxA,TxAN,TxA,unitA,TopA = VA
288    OxBN,OxB,TxBN,TxB,unitB,TopB = VB
289    invA = invB = 1
290    invA = TopA/abs(TopA)
291    invB = TopB/abs(TopB)
292    centA = abs(TopA)/100
293    centB = abs(TopB)/100
294    opA = abs(TopA)%100-1
295    opB = abs(TopB)%100-1
296    MA,TA = SGData['SGOps'][opA]
297    MB,TB = SGData['SGOps'][opB]
298    CA = SGData['SGCen'][centA]
299    CB = SGData['SGCen'][centB]
300    if 'covMatrix' in covData:
301        covMatrix = covData['covMatrix']
302        varyList = covData['varyList']
303        AngVcov = getVCov(OxAN+TxAN+TxBN,varyList,covMatrix)
304        dx = .00001
305        dadx = np.zeros(9)
306        Ang = calcAngle(OxA,TxA,TxB,unitA,unitB,invA,CA,MA,TA,invB,CB,MB,TB,Amat)
307        for i in [0,1,2]:
308            OxA[i] += dx
309            a0 = calcAngle(OxA,TxA,TxB,unitA,unitB,invA,CA,MA,TA,invB,CB,MB,TB,Amat)
310            OxA[i] -= 2*dx
311            dadx[i] = (calcAngle(OxA,TxA,TxB,unitA,unitB,invA,CA,MA,TA,invB,CB,MB,TB,Amat)-a0)/dx
312            OxA[i] += dx
313           
314            TxA[i] += dx
315            a0 = calcAngle(OxA,TxA,TxB,unitA,unitB,invA,CA,MA,TA,invB,CB,MB,TB,Amat)
316            TxA[i] -= 2*dx
317            dadx[i+3] = (calcAngle(OxA,TxA,TxB,unitA,unitB,invA,CA,MA,TA,invB,CB,MB,TB,Amat)-a0)/dx
318            TxA[i] += dx
319           
320            TxB[i] += dx
321            a0 = calcAngle(OxA,TxA,TxB,unitA,unitB,invA,CA,MA,TA,invB,CB,MB,TB,Amat)
322            TxB[i] -= 2*dx
323            dadx[i+6] = (calcAngle(OxA,TxA,TxB,unitA,unitB,invA,CA,MA,TA,invB,CB,MB,TB,Amat)-a0)/dx
324            TxB[i] += dx
325           
326        sigAng = np.sqrt(np.inner(dadx,np.inner(AngVcov,dadx)))
327        if sigAng < 0.01:
328            sigAng = 0.0
329        return Ang,sigAng
330    else:
331        return calcAngle(OxA,TxA,TxB,unitA,unitB,invA,CA,MA,TA,invB,CB,MB,TB,Amat),0.0
332
333def GetDistSig(Oatoms,Atoms,Amat,SGData,covData={}):
334
335    def calcDist(Atoms,SyOps,Amat):
336        XYZ = []
337        for i,atom in enumerate(Atoms):
338            Inv,M,T,C,U = SyOps[i]
339            XYZ.append(np.array(atom[1:4]))
340            XYZ[-1] = Inv*(np.inner(M,np.array(XYZ[-1]))+T)+C+U
341            XYZ[-1] = np.inner(Amat,XYZ[-1]).T
342        V1 = XYZ[1]-XYZ[0]
343        return np.sqrt(np.sum(V1**2))
344       
345    Inv = []
346    SyOps = []
347    names = []
348    for i,atom in enumerate(Oatoms):
349        names += atom[-1]
350        Op,unit = Atoms[i][-1]
351        inv = Op/abs(Op)
352        m,t = SGData['SGOps'][abs(Op)%100-1]
353        c = SGData['SGCen'][abs(Op)/100]
354        SyOps.append([inv,m,t,c,unit])
355    Dist = calcDist(Oatoms,SyOps,Amat)
356   
357    sig = -0.001
358    if 'covMatrix' in covData:
359        parmNames = []
360        dx = .00001
361        dadx = np.zeros(6)
362        for i in range(6):
363            ia = i/3
364            ix = i%3
365            Oatoms[ia][ix+1] += dx
366            a0 = calcDist(Oatoms,SyOps,Amat)
367            Oatoms[ia][ix+1] -= 2*dx
368            dadx[i] = (calcDist(Oatoms,SyOps,Amat)-a0)/(2.*dx)
369        covMatrix = covData['covMatrix']
370        varyList = covData['varyList']
371        DistVcov = getVCov(names,varyList,covMatrix)
372        sig = np.sqrt(np.inner(dadx,np.inner(DistVcov,dadx)))
373        if sig < 0.001:
374            sig = -0.001
375   
376    return Dist,sig
377
378def GetAngleSig(Oatoms,Atoms,Amat,SGData,covData={}):
379       
380    def calcAngle(Atoms,SyOps,Amat):
381        XYZ = []
382        for i,atom in enumerate(Atoms):
383            Inv,M,T,C,U = SyOps[i]
384            XYZ.append(np.array(atom[1:4]))
385            XYZ[-1] = Inv*(np.inner(M,np.array(XYZ[-1]))+T)+C+U
386            XYZ[-1] = np.inner(Amat,XYZ[-1]).T
387        V1 = XYZ[1]-XYZ[0]
388        V1 /= np.sqrt(np.sum(V1**2))
389        V2 = XYZ[1]-XYZ[2]
390        V2 /= np.sqrt(np.sum(V2**2))
391        V3 = V2-V1
392        cang = min(1.,max((2.-np.sum(V3**2))/2.,-1.))
393        return acosd(cang)
394
395    Inv = []
396    SyOps = []
397    names = []
398    for i,atom in enumerate(Oatoms):
399        names += atom[-1]
400        Op,unit = Atoms[i][-1]
401        inv = Op/abs(Op)
402        m,t = SGData['SGOps'][abs(Op)%100-1]
403        c = SGData['SGCen'][abs(Op)/100]
404        SyOps.append([inv,m,t,c,unit])
405    Angle = calcAngle(Oatoms,SyOps,Amat)
406   
407    sig = -0.01
408    if 'covMatrix' in covData:
409        parmNames = []
410        dx = .00001
411        dadx = np.zeros(9)
412        for i in range(9):
413            ia = i/3
414            ix = i%3
415            Oatoms[ia][ix+1] += dx
416            a0 = calcAngle(Oatoms,SyOps,Amat)
417            Oatoms[ia][ix+1] -= 2*dx
418            dadx[i] = (calcAngle(Oatoms,SyOps,Amat)-a0)/(2.*dx)
419        covMatrix = covData['covMatrix']
420        varyList = covData['varyList']
421        AngVcov = getVCov(names,varyList,covMatrix)
422        sig = np.sqrt(np.inner(dadx,np.inner(AngVcov,dadx)))
423        if sig < 0.01:
424            sig = -0.01
425   
426    return Angle,sig
427
428def GetTorsionSig(Oatoms,Atoms,Amat,SGData,covData={}):
429   
430    def calcTorsion(Atoms,SyOps,Amat):
431       
432        XYZ = []
433        for i,atom in enumerate(Atoms):
434            Inv,M,T,C,U = SyOps[i]
435            XYZ.append(np.array(atom[1:4]))
436            XYZ[-1] = Inv*(np.inner(M,np.array(XYZ[-1]))+T)+C+U
437            XYZ[-1] = np.inner(Amat,XYZ[-1]).T
438        V1 = XYZ[1]-XYZ[0]
439        V2 = XYZ[2]-XYZ[1]
440        V3 = XYZ[3]-XYZ[2]
441        V1 /= np.sqrt(np.sum(V1**2))
442        V2 /= np.sqrt(np.sum(V2**2))
443        V3 /= np.sqrt(np.sum(V3**2))
444        M = np.array([V1,V2,V3])
445        D = nl.det(M)
446        Ang = 1.0
447        P12 = np.dot(V1,V2)
448        P13 = np.dot(V1,V3)
449        P23 = np.dot(V2,V3)
450        Tors = acosd((P12*P23-P13)/(np.sqrt(1.-P12**2)*np.sqrt(1.-P23**2)))*D/abs(D)
451        return Tors
452           
453    Inv = []
454    SyOps = []
455    names = []
456    for i,atom in enumerate(Oatoms):
457        names += atom[-1]
458        Op,unit = Atoms[i][-1]
459        inv = Op/abs(Op)
460        m,t = SGData['SGOps'][abs(Op)%100-1]
461        c = SGData['SGCen'][abs(Op)/100]
462        SyOps.append([inv,m,t,c,unit])
463    Tors = calcTorsion(Oatoms,SyOps,Amat)
464   
465    sig = -0.01
466    if 'covMatrix' in covData:
467        parmNames = []
468        dx = .00001
469        dadx = np.zeros(12)
470        for i in range(12):
471            ia = i/3
472            ix = i%3
473            Oatoms[ia][ix+1] += dx
474            a0 = calcTorsion(Oatoms,SyOps,Amat)
475            Oatoms[ia][ix+1] -= 2*dx
476            dadx[i] = (calcTorsion(Oatoms,SyOps,Amat)-a0)/(2.*dx)
477        covMatrix = covData['covMatrix']
478        varyList = covData['varyList']
479        TorVcov = getVCov(names,varyList,covMatrix)
480        sig = np.sqrt(np.inner(dadx,np.inner(TorVcov,dadx)))
481        if sig < 0.01:
482            sig = -0.01
483   
484    return Tors,sig
485       
486def GetDATSig(Oatoms,Atoms,Amat,SGData,covData={}):
487   
488    def calcDist(Atoms,SyOps,Amat):
489        XYZ = []
490        for i,atom in enumerate(Atoms):
491            Inv,M,T,C,U = SyOps[i]
492            XYZ.append(np.array(atom[1:4]))
493            XYZ[-1] = Inv*(np.inner(M,np.array(XYZ[-1]))+T)+C+U
494            XYZ[-1] = np.inner(Amat,XYZ[-1]).T
495        V1 = XYZ[1]-XYZ[0]
496        return np.sqrt(np.sum(V1**2))
497       
498    def calcAngle(Atoms,SyOps,Amat):
499        XYZ = []
500        for i,atom in enumerate(Atoms):
501            Inv,M,T,C,U = SyOps[i]
502            XYZ.append(np.array(atom[1:4]))
503            XYZ[-1] = Inv*(np.inner(M,np.array(XYZ[-1]))+T)+C+U
504            XYZ[-1] = np.inner(Amat,XYZ[-1]).T
505        V1 = XYZ[1]-XYZ[0]
506        V1 /= np.sqrt(np.sum(V1**2))
507        V2 = XYZ[1]-XYZ[2]
508        V2 /= np.sqrt(np.sum(V2**2))
509        V3 = V2-V1
510        cang = min(1.,max((2.-np.sum(V3**2))/2.,-1.))
511        return acosd(cang)
512
513    def calcTorsion(Atoms,SyOps,Amat):
514       
515        XYZ = []
516        for i,atom in enumerate(Atoms):
517            Inv,M,T,C,U = SyOps[i]
518            XYZ.append(np.array(atom[1:4]))
519            XYZ[-1] = Inv*(np.inner(M,np.array(XYZ[-1]))+T)+C+U
520            XYZ[-1] = np.inner(Amat,XYZ[-1]).T
521        V1 = XYZ[1]-XYZ[0]
522        V2 = XYZ[2]-XYZ[1]
523        V3 = XYZ[3]-XYZ[2]
524        V1 /= np.sqrt(np.sum(V1**2))
525        V2 /= np.sqrt(np.sum(V2**2))
526        V3 /= np.sqrt(np.sum(V3**2))
527        M = np.array([V1,V2,V3])
528        D = nl.det(M)
529        Ang = 1.0
530        P12 = np.dot(V1,V2)
531        P13 = np.dot(V1,V3)
532        P23 = np.dot(V2,V3)
533        Tors = acosd((P12*P23-P13)/(np.sqrt(1.-P12**2)*np.sqrt(1.-P23**2)))*D/abs(D)
534        return Tors
535           
536    Inv = []
537    SyOps = []
538    names = []
539    for i,atom in enumerate(Oatoms):
540        names += atom[-1]
541        Op,unit = Atoms[i][-1]
542        inv = Op/abs(Op)
543        m,t = SGData['SGOps'][abs(Op)%100-1]
544        c = SGData['SGCen'][abs(Op)/100]
545        SyOps.append([inv,m,t,c,unit])
546    M = len(Oatoms)
547    if M == 2:
548        Val = calcDist(Oatoms,SyOps,Amat)
549    elif M == 3:
550        Val = calcAngle(Oatoms,SyOps,Amat)
551    else:
552        Val = calcTorsion(Oatoms,SyOps,Amat)
553   
554    sigVals = [-0.001,-0.01,-0.01]
555    sig = sigVals[M-3]
556    if 'covMatrix' in covData:
557        parmNames = []
558        dx = .00001
559        N = M*3
560        dadx = np.zeros(N)
561        for i in range(N):
562            ia = i/3
563            ix = i%3
564            Oatoms[ia][ix+1] += dx
565            if M == 2:
566                a0 = calcDist(Oatoms,SyOps,Amat)
567            elif M == 3:
568                a0 = calcAngle(Oatoms,SyOps,Amat)
569            else:
570                a0 = calcTorsion(Oatoms,SyOps,Amat)
571            Oatoms[ia][ix+1] -= 2*dx
572            if M == 2:
573                dadx[i] = (calcDist(Oatoms,SyOps,Amat)-a0)/(2.*dx)               
574            elif M == 3:
575                dadx[i] = (calcAngle(Oatoms,SyOps,Amat)-a0)/(2.*dx)               
576            else:
577                dadx[i] = (calcTorsion(Oatoms,SyOps,Amat)-a0)/(2.*dx)
578        covMatrix = covData['covMatrix']
579        varyList = covData['varyList']
580        Vcov = getVCov(names,varyList,covMatrix)
581        sig = np.sqrt(np.inner(dadx,np.inner(Vcov,dadx)))
582        if sig < sigVals[M-3]:
583            sig = sigVals[M-3]
584   
585    return Val,sig
586       
587   
588def ValEsd(value,esd=0,nTZ=False):                  #NOT complete - don't use
589    # returns value(esd) string; nTZ=True for no trailing zeros
590    # use esd < 0 for level of precision shown e.g. esd=-0.01 gives 2 places beyond decimal
591    #get the 2 significant digits in the esd
592    edig = lambda esd: int(round(10**(math.log10(esd) % 1+1)))
593    #get the number of digits to represent them
594    epl = lambda esd: 2+int(1.545-math.log10(10*edig(esd)))
595   
596    mdec = lambda esd: -int(round(math.log10(abs(esd))))+1
597    ndec = lambda esd: int(1.545-math.log10(abs(esd)))
598    if esd > 0:
599        fmt = '"%.'+str(ndec(esd))+'f(%d)"'
600        return str(fmt%(value,int(round(esd*10**(mdec(esd)))))).strip('"')
601    elif esd < 0:
602         return str(round(value,mdec(esd)-1))
603    else:
604        text = str("%f"%(value))
605        if nTZ:
606            return text.rstrip('0')
607        else:
608            return text
609           
610def adjHKLmax(SGData,Hmax):
611    if SGData['SGLaue'] in ['3','3m1','31m','6/m','6/mmm']:
612        Hmax[0] = ((Hmax[0]+3)/6)*6
613        Hmax[1] = ((Hmax[1]+3)/6)*6
614        Hmax[2] = ((Hmax[2]+1)/4)*4
615    else:
616        Hmax[0] = ((Hmax[0]+2)/4)*4
617        Hmax[1] = ((Hmax[1]+2)/4)*4
618        Hmax[2] = ((Hmax[2]+1)/4)*4
619
620def FourierMap(data,reflData):
621   
622    generalData = data['General']
623    if not generalData['Map']['MapType']:
624        print '**** ERROR - Fourier map not defined'
625        return
626    mapData = generalData['Map']
627    dmin = mapData['Resolution']
628    SGData = generalData['SGData']
629    cell = generalData['Cell'][1:8]       
630    A = G2lat.cell2A(cell[:6])
631    Hmax = np.asarray(G2lat.getHKLmax(dmin,SGData,A),dtype='i')+1
632    adjHKLmax(SGData,Hmax)
633    Fhkl = np.zeros(shape=2*Hmax,dtype='c16')
634#    Fhkl[0,0,0] = generalData['F000X']
635    time0 = time.time()
636    for ref in reflData:
637        if ref[4] >= dmin:
638            Fosq,Fcsq,ph = ref[8:11]
639            for i,hkl in enumerate(ref[11]):
640                hkl = np.asarray(hkl,dtype='i')
641                dp = 360.*ref[12][i]
642                a = cosd(ph+dp)
643                b = sind(ph+dp)
644                phasep = complex(a,b)
645                phasem = complex(a,-b)
646                if 'Fobs' in mapData['MapType']:
647                    F = np.sqrt(Fosq)
648                    h,k,l = hkl+Hmax
649                    Fhkl[h,k,l] = F*phasep
650                    h,k,l = -hkl+Hmax
651                    Fhkl[h,k,l] = F*phasem
652                elif 'Fcalc' in mapData['MapType']:
653                    F = np.sqrt(Fcsq)
654                    h,k,l = hkl+Hmax
655                    Fhkl[h,k,l] = F*phasep
656                    h,k,l = -hkl+Hmax
657                    Fhkl[h,k,l] = F*phasem
658                elif 'delt-F' in mapData['MapType']:
659                    dF = np.sqrt(Fosq)-np.sqrt(Fcsq)
660                    h,k,l = hkl+Hmax
661                    Fhkl[h,k,l] = dF*phasep
662                    h,k,l = -hkl+Hmax
663                    Fhkl[h,k,l] = dF*phasem
664                elif '2*Fo-Fc' in mapData['MapType']:
665                    F = 2.*np.sqrt(Fosq)-np.sqrt(Fcsq)
666                    h,k,l = hkl+Hmax
667                    Fhkl[h,k,l] = F*phasep
668                    h,k,l = -hkl+Hmax
669                    Fhkl[h,k,l] = F*phasem
670                elif 'Patterson' in mapData['MapType']:
671                    h,k,l = hkl+Hmax
672                    Fhkl[h,k,l] = complex(Fosq,0.)
673                    h,k,l = -hkl+Hmax
674                    Fhkl[h,k,l] = complex(Fosq,0.)
675    rho = fft.fftn(fft.fftshift(Fhkl))/cell[6]
676    print 'Fourier map time: %.4f'%(time.time()-time0),'no. elements: %d'%(Fhkl.size)
677    mapData['rho'] = np.real(rho)
678    mapData['rhoMax'] = max(np.max(mapData['rho']),-np.min(mapData['rho']))
679    return mapData
680   
681# map printing for testing purposes
682def printRho(SGLaue,rho,rhoMax):                         
683    dim = len(rho.shape)
684    if dim == 2:
685        ix,jy = rho.shape
686        for j in range(jy):
687            line = ''
688            if SGLaue in ['3','3m1','31m','6/m','6/mmm']:
689                line += (jy-j)*'  '
690            for i in range(ix):
691                r = int(100*rho[i,j]/rhoMax)
692                line += '%4d'%(r)
693            print line+'\n'
694    else:
695        ix,jy,kz = rho.shape
696        for k in range(kz):
697            print 'k = ',k
698            for j in range(jy):
699                line = ''
700                if SGLaue in ['3','3m1','31m','6/m','6/mmm']:
701                    line += (jy-j)*'  '
702                for i in range(ix):
703                    r = int(100*rho[i,j,k]/rhoMax)
704                    line += '%4d'%(r)
705                print line+'\n'
706## keep this
707               
708def findOffset(SGData,A,Fhkl):   
709    if SGData['SpGrp'] == 'P 1':
710        return [0,0,0]   
711    hklShape = Fhkl.shape
712    hklHalf = np.array(hklShape)/2
713    sortHKL = np.argsort(Fhkl.flatten())
714    Fdict = {}
715    for hkl in sortHKL:
716        HKL = np.unravel_index(hkl,hklShape)
717        F = Fhkl[HKL[0]][HKL[1]][HKL[2]]
718        if F == 0.:
719            break
720        Fdict['%.6f'%(np.absolute(F))] = hkl
721    Flist = np.flipud(np.sort(Fdict.keys()))
722    F = str(1.e6)
723    i = 0
724    DH = []
725    Dphi = []
726    Hmax = 2*np.asarray(G2lat.getHKLmax(3.5,SGData,A),dtype='i')
727    for F in Flist:
728        hkl = np.unravel_index(Fdict[F],hklShape)
729        iabsnt,mulp,Uniq,Phi = G2spc.GenHKLf(list(hkl-hklHalf),SGData)
730        Uniq = np.array(Uniq,dtype='i')
731        Phi = np.array(Phi)
732        Uniq = np.concatenate((Uniq,-Uniq))+hklHalf         # put in Friedel pairs & make as index to Farray
733        Phi = np.concatenate((Phi,-Phi))                      # and their phase shifts
734        Fh0 = Fhkl[hkl[0],hkl[1],hkl[2]]
735        ang0 = np.angle(Fh0,deg=True)/360.
736        for H,phi in zip(Uniq,Phi)[1:]:
737            ang = (np.angle(Fhkl[H[0],H[1],H[2]],deg=True)/360.-phi)
738            dH = H-hkl
739            dang = ang-ang0
740            if np.any(np.abs(dH)-Hmax > 0):    #keep low order DHs
741                continue
742            DH.append(dH)
743            Dphi.append((dang+.5) % 1.0)
744        if i > 20 or len(DH) > 30:
745            break
746        i += 1
747    DH = np.array(DH)
748    print ' map offset no.of terms: %d from %d reflections'%(len(DH),len(Flist))
749    Dphi = np.array(Dphi)
750    steps = np.array(hklShape)
751    X,Y,Z = np.mgrid[0:1:1./steps[0],0:1:1./steps[1],0:1:1./steps[2]]
752    XYZ = np.array(zip(X.flatten(),Y.flatten(),Z.flatten()))
753    Dang = (np.dot(XYZ,DH.T)+.5)%1.-Dphi
754    Mmap = np.reshape(np.sum((Dang)**2,axis=1),newshape=steps)/len(DH)
755    hist,bins = np.histogram(Mmap,bins=1000)
756#    for i,item in enumerate(hist[:10]):
757#        print item,bins[i]
758    chisq = np.min(Mmap)
759    DX = -np.array(np.unravel_index(np.argmin(Mmap),Mmap.shape))
760    print ' map offset chi**2: %.3f, map offset: %d %d %d'%(chisq,DX[0],DX[1],DX[2])
761#    print (np.dot(DX,DH.T)+.5)%1.-Dphi
762    return DX
763   
764def ChargeFlip(data,reflData,pgbar):
765    generalData = data['General']
766    mapData = generalData['Map']
767    flipData = generalData['Flip']
768    FFtable = {}
769    if 'None' not in flipData['Norm element']:
770        normElem = flipData['Norm element'].upper()
771        FFs = G2el.GetFormFactorCoeff(normElem.split('+')[0].split('-')[0])
772        for ff in FFs:
773            if ff['Symbol'] == normElem:
774                FFtable.update(ff)
775    dmin = flipData['Resolution']
776    SGData = generalData['SGData']
777    cell = generalData['Cell'][1:8]       
778    A = G2lat.cell2A(cell[:6])
779    Vol = cell[6]
780    Hmax = np.asarray(G2lat.getHKLmax(dmin,SGData,A),dtype='i')+1
781    adjHKLmax(SGData,Hmax)
782    Ehkl = np.zeros(shape=2*Hmax,dtype='c16')       #2X64bits per complex no.
783    time0 = time.time()
784    for ref in reflData:
785        dsp = ref[4]
786        if dsp >= dmin:
787            ff = 0.1*Vol    #est. no. atoms for ~10A**3/atom
788            if FFtable:
789                SQ = 0.25/dsp**2
790                ff *= G2el.ScatFac(FFtable,SQ)[0]
791            if ref[8] > 0.:         #use only +ve Fobs**2
792                E = np.sqrt(ref[8])/ff
793            else:
794                E = 0.
795            ph = ref[10]
796            ph = rn.uniform(0.,360.)
797            for i,hkl in enumerate(ref[11]):
798                hkl = np.asarray(hkl,dtype='i')
799                dp = 360.*ref[12][i]
800                a = cosd(ph+dp)
801                b = sind(ph+dp)
802                phasep = complex(a,b)
803                phasem = complex(a,-b)
804                h,k,l = hkl+Hmax
805                Ehkl[h,k,l] = E*phasep
806                h,k,l = -hkl+Hmax       #Friedel pair refl.
807                Ehkl[h,k,l] = E*phasem
808#    Ehkl[Hmax] = 0.00001           #this to preserve F[0,0,0]
809    CEhkl = copy.copy(Ehkl)
810    MEhkl = ma.array(Ehkl,mask=(Ehkl==0.0))
811    Emask = ma.getmask(MEhkl)
812    sumE = np.sum(ma.array(np.absolute(CEhkl),mask=Emask))
813    Ncyc = 0
814    old = np.seterr(all='raise')
815    while True:       
816        CErho = np.real(fft.fftn(fft.fftshift(CEhkl)))*(1.+0j)
817        CEsig = np.std(CErho)
818        CFrho = np.where(np.real(CErho) >= flipData['k-factor']*CEsig,CErho,-CErho)
819        CFrho = np.where(np.real(CErho) <= flipData['k-Max']*CEsig,CFrho,-CFrho)      #solves U atom problem! make 20. adjustible
820        CFhkl = fft.ifftshift(fft.ifftn(CFrho))
821        CFhkl = np.where(CFhkl,CFhkl,1.0)           #avoid divide by zero
822        phase = CFhkl/np.absolute(CFhkl)
823        CEhkl = np.absolute(Ehkl)*phase
824        Ncyc += 1
825        sumCF = np.sum(ma.array(np.absolute(CFhkl),mask=Emask))
826        DEhkl = np.absolute(np.absolute(Ehkl)/sumE-np.absolute(CFhkl)/sumCF)
827        Rcf = min(100.,np.sum(ma.array(DEhkl,mask=Emask)*100.))
828        if Rcf < 5.:
829            break
830        GoOn = pgbar.Update(Rcf,newmsg='%s%8.3f%s\n%s %d'%('Residual Rcf =',Rcf,'%','No.cycles = ',Ncyc))[0]
831        if not GoOn or Ncyc > 10000:
832            break
833    np.seterr(**old)
834    print ' Charge flip time: %.4f'%(time.time()-time0),'no. elements: %d'%(Ehkl.size)
835    CErho = np.real(fft.fftn(fft.fftshift(CEhkl)))
836    print ' No.cycles = ',Ncyc,'Residual Rcf =%8.3f%s'%(Rcf,'%')+' Map size:',CErho.shape
837    roll = findOffset(SGData,A,CEhkl)
838       
839    mapData['Rcf'] = Rcf
840    mapData['rho'] = np.roll(np.roll(np.roll(CErho,roll[0],axis=0),roll[1],axis=1),roll[2],axis=2)
841    mapData['rhoMax'] = max(np.max(mapData['rho']),-np.min(mapData['rho']))
842    mapData['rollMap'] = [0,0,0]
843    return mapData
844   
845def SearchMap(data):
846    rollMap = lambda rho,roll: np.roll(np.roll(np.roll(rho,roll[0],axis=0),roll[1],axis=1),roll[2],axis=2)
847   
848    norm = 1./(np.sqrt(3.)*np.sqrt(2.*np.pi)**3)
849   
850    def noDuplicate(xyz,peaks,Amat):
851        XYZ = np.inner(Amat,xyz)
852        if True in [np.allclose(XYZ,np.inner(Amat,peak),atol=0.5) for peak in peaks]:
853            print ' Peak',xyz,' <0.5A from another peak'
854            return False
855        return True
856                           
857    def fixSpecialPos(xyz,SGData,Amat):
858        equivs = G2spc.GenAtom(xyz,SGData,Move=True)
859        X = []
860        xyzs = [equiv[0] for equiv in equivs]
861        for x in xyzs:
862            if np.sqrt(np.sum(np.inner(Amat,xyz-x)**2,axis=0))<0.5:
863                X.append(x)
864        if len(X) > 1:
865            return np.average(X,axis=0)
866        else:
867            return xyz
868       
869    def rhoCalc(parms,rX,rY,rZ,res,SGLaue):
870        Mag,x0,y0,z0,sig = parms
871        z = -((x0-rX)**2+(y0-rY)**2+(z0-rZ)**2)/(2.*sig**2)
872#        return norm*Mag*np.exp(z)/(sig*res**3)     #not slower but some faults in LS
873        return norm*Mag*(1.+z+z**2/2.)/(sig*res**3)
874       
875    def peakFunc(parms,rX,rY,rZ,rho,res,SGLaue):
876        Mag,x0,y0,z0,sig = parms
877        M = rho-rhoCalc(parms,rX,rY,rZ,res,SGLaue)
878        return M
879       
880    def peakHess(parms,rX,rY,rZ,rho,res,SGLaue):
881        Mag,x0,y0,z0,sig = parms
882        dMdv = np.zeros(([5,]+list(rX.shape)))
883        delt = .01
884        for i in range(5):
885            parms[i] -= delt
886            rhoCm = rhoCalc(parms,rX,rY,rZ,res,SGLaue)
887            parms[i] += 2.*delt
888            rhoCp = rhoCalc(parms,rX,rY,rZ,res,SGLaue)
889            parms[i] -= delt
890            dMdv[i] = (rhoCp-rhoCm)/(2.*delt)
891        rhoC = rhoCalc(parms,rX,rY,rZ,res,SGLaue)
892        Vec = np.sum(np.sum(np.sum(dMdv*(rho-rhoC),axis=3),axis=2),axis=1)
893        dMdv = np.reshape(dMdv,(5,rX.size))
894        Hess = np.inner(dMdv,dMdv)
895       
896        return Vec,Hess
897       
898    generalData = data['General']
899    phaseName = generalData['Name']
900    SGData = generalData['SGData']
901    Amat,Bmat = G2lat.cell2AB(generalData['Cell'][1:7])
902    drawingData = data['Drawing']
903    peaks = []
904    mags = []
905    dzeros = []
906    try:
907        mapData = generalData['Map']
908        contLevel = mapData['cutOff']*mapData['rhoMax']/100.
909        rho = copy.copy(mapData['rho'])     #don't mess up original
910        mapHalf = np.array(rho.shape)/2
911        res = mapData['Resolution']
912        incre = np.array(rho.shape,dtype=np.float)
913        step = max(1.0,1./res)+1
914        steps = np.array(3*[step,])
915    except KeyError:
916        print '**** ERROR - Fourier map not defined'
917        return peaks,mags
918    rhoMask = ma.array(rho,mask=(rho<contLevel))
919    indices = (-1,0,1)
920    rolls = np.array([[h,k,l] for h in indices for k in indices for l in indices])
921    for roll in rolls:
922        if np.any(roll):
923            rhoMask = ma.array(rhoMask,mask=(rhoMask-rollMap(rho,roll)<=0.))
924    indx = np.transpose(rhoMask.nonzero())
925    peaks = indx/incre
926    mags = rhoMask[rhoMask.nonzero()]
927    for i,[ind,peak,mag] in enumerate(zip(indx,peaks,mags)):
928        rho = rollMap(rho,ind)
929        rMM = mapHalf-steps
930        rMP = mapHalf+steps+1
931        rhoPeak = rho[rMM[0]:rMP[0],rMM[1]:rMP[1],rMM[2]:rMP[2]]
932        peakInt = np.sum(rhoPeak)*res**3
933        rX,rY,rZ = np.mgrid[rMM[0]:rMP[0],rMM[1]:rMP[1],rMM[2]:rMP[2]]
934        x0 = [peakInt,mapHalf[0],mapHalf[1],mapHalf[2],2.0]          #magnitude, position & width(sig)
935        result = HessianLSQ(peakFunc,x0,Hess=peakHess,
936            args=(rX,rY,rZ,rhoPeak,res,SGData['SGLaue']),ftol=.01,maxcyc=10)
937        x1 = result[0]
938        if not np.any(x1 < 0):
939            mag = x1[0]
940            peak = (np.array(x1[1:4])-ind)/incre
941        peak = fixSpecialPos(peak,SGData,Amat)
942        rho = rollMap(rho,-ind)       
943    dzeros = np.sqrt(np.sum(np.inner(Amat,peaks)**2,axis=0))
944    return np.array(peaks),np.array([mags,]).T,np.array([dzeros,]).T
945   
946def sortArray(data,pos,reverse=False):
947    #data is a list of items
948    #sort by pos in list; reverse if True
949    T = []
950    for i,M in enumerate(data):
951        T.append((M[pos],i))
952    D = dict(zip(T,data))
953    T.sort()
954    if reverse:
955        T.reverse()
956    X = []
957    for key in T:
958        X.append(D[key])
959    return X
960
961def PeaksEquiv(data,Ind):
962
963    def Duplicate(xyz,peaks,Amat):
964        if True in [np.allclose(np.inner(Amat,xyz),np.inner(Amat,peak),atol=0.5) for peak in peaks]:
965            return True
966        return False
967                           
968    generalData = data['General']
969    cell = generalData['Cell'][1:7]
970    Amat,Bmat = G2lat.cell2AB(generalData['Cell'][1:7])
971    A = G2lat.cell2A(cell)
972    SGData = generalData['SGData']
973    mapPeaks = data['Map Peaks']
974    XYZ = np.array([xyz[1:4] for xyz in mapPeaks])
975    Indx = {}
976    for ind in Ind:
977        xyz = np.array(mapPeaks[ind][1:4])
978        xyzs = np.array([equiv[0] for equiv in G2spc.GenAtom(xyz,SGData,Move=True)])
979#        for x in xyzs: print x
980        for jnd,xyz in enumerate(XYZ):       
981            Indx[jnd] = Duplicate(xyz,xyzs,Amat)
982    Ind = []
983    for ind in Indx:
984        if Indx[ind]:
985            Ind.append(ind)
986    return Ind
987               
988def PeaksUnique(data,Ind):
989#    XYZE = np.array([[equiv[0] for equiv in G2spc.GenAtom(xyz[1:4],SGData,Move=True)] for xyz in mapPeaks]) #keep this!!
990
991    def noDuplicate(xyz,peaks,Amat):
992        if True in [np.allclose(np.inner(Amat,xyz),np.inner(Amat,peak),atol=0.5) for peak in peaks]:
993            return False
994        return True
995                           
996    generalData = data['General']
997    cell = generalData['Cell'][1:7]
998    Amat,Bmat = G2lat.cell2AB(generalData['Cell'][1:7])
999    A = G2lat.cell2A(cell)
1000    SGData = generalData['SGData']
1001    mapPeaks = data['Map Peaks']
1002    Indx = {}
1003    XYZ = {}
1004    for ind in Ind:
1005        XYZ[ind] = np.array(mapPeaks[ind][1:4])
1006        Indx[ind] = True
1007    for ind in Ind:
1008        if Indx[ind]:
1009            xyz = XYZ[ind]
1010            for jnd in Ind:
1011                if ind != jnd and Indx[jnd]:                       
1012                    Equiv = G2spc.GenAtom(XYZ[jnd],SGData,Move=True)
1013                    xyzs = np.array([equiv[0] for equiv in Equiv])
1014                    Indx[jnd] = noDuplicate(xyz,xyzs,Amat)
1015    Ind = []
1016    for ind in Indx:
1017        if Indx[ind]:
1018            Ind.append(ind)
1019    return Ind
1020   
1021def setPeakparms(Parms,Parms2,pos,mag,ifQ=False,useFit=False):
1022    ind = 0
1023    if useFit:
1024        ind = 1
1025    ins = {}
1026    if 'C' in Parms['Type'][0]:                            #CW data - TOF later in an elif
1027        for x in ['U','V','W','X','Y']:
1028            ins[x] = Parms[x][ind]
1029        if ifQ:                              #qplot - convert back to 2-theta
1030            pos = 2.0*asind(pos*wave/(4*math.pi))
1031        sig = ins['U']*tand(pos/2.0)**2+ins['V']*tand(pos/2.0)+ins['W']
1032        gam = ins['X']/cosd(pos/2.0)+ins['Y']*tand(pos/2.0)           
1033        XY = [pos,0, mag,1, sig,0, gam,0]       #default refine intensity 1st
1034    else:
1035        if ifQ:
1036            dsp = 2.*np.pi/pos
1037            pos = Parms['difC']*dsp
1038        else:
1039            dsp = pos/Parms['difC'][1]
1040        if 'Pdabc' in Parms2:
1041            for x in ['sig-0','sig-1','X','Y']:
1042                ins[x] = Parms[x][ind]
1043            Pdabc = Parms2['Pdabc'].T
1044            alp = np.interp(dsp,Pdabc[0],Pdabc[1])
1045            bet = np.interp(dsp,Pdabc[0],Pdabc[2])
1046        else:
1047            for x in ['alpha','beta-0','beta-1','sig-0','sig-1','X','Y']:
1048                ins[x] = Parms[x][ind]
1049            alp = ins['alpha']/dsp
1050            bet = ins['beta-0']+ins['beta-1']/dsp**4
1051        sig = ins['sig-0']+ins['sig-1']*dsp**2
1052        gam = ins['X']*dsp+ins['Y']*dsp**2
1053        XY = [pos,0,mag,1,alp,0,bet,0,sig,0,gam,0]
1054    return XY
1055       
1056def getWave(Parms):
1057    try:
1058        return Parms['Lam'][1]
1059    except KeyError:
1060        return Parms['Lam1'][1]
1061   
1062def prodQQ(QA,QB):
1063    ''' Grassman quaternion product
1064        QA,QB quaternions; q=r+ai+bj+ck
1065    '''
1066    D = np.zeros(4)
1067    D[0] = QA[0]*QB[0]-QA[1]*QB[1]-QA[2]*QB[2]-QA[3]*QB[3]
1068    D[1] = QA[0]*QB[1]+QA[1]*QB[0]+QA[2]*QB[3]-QA[3]*QB[2]
1069    D[2] = QA[0]*QB[2]-QA[1]*QB[3]+QA[2]*QB[0]+QA[3]*QB[1]
1070    D[3] = QA[0]*QB[3]+QA[1]*QB[2]-QA[2]*QB[1]+QA[3]*QB[0]
1071    return D
1072   
1073def normQ(QA):
1074    ''' get length of quaternion & normalize it
1075        q=r+ai+bj+ck
1076    '''
1077    n = np.sqrt(np.sum(np.array(QA)**2))
1078    return QA/n
1079   
1080def invQ(Q):
1081    '''
1082        get inverse of quaternion
1083        q=r+ai+bj+ck; q* = r-ai-bj-ck
1084    '''
1085    return Q*np.array([1,-1,-1,-1])
1086   
1087def prodQVQ(Q,V):
1088    ''' compute the quaternion vector rotation qvq-1 = v'
1089        q=r+ai+bj+ck
1090    '''
1091    VP = np.zeros(3)
1092    T2 = Q[0]*Q[1]
1093    T3 = Q[0]*Q[2]
1094    T4 = Q[0]*Q[3]
1095    T5 = -Q[1]*Q[1]
1096    T6 = Q[1]*Q[2]
1097    T7 = Q[1]*Q[3]
1098    T8 = -Q[2]*Q[2]
1099    T9 = Q[2]*Q[3]
1100    T10 = -Q[3]*Q[3]
1101    VP[0] = 2.*((T8+T10)*V[0]+(T6-T4)*V[1]+(T3+T7)*V[2])+V[0]
1102    VP[1] = 2.*((T4+T6)*V[0]+(T5+T10)*V[1]+(T9-T2)*V[2])+V[1]
1103    VP[2] = 2.*((T7-T3)*V[0]+(T2+T9)*V[1]+(T5+T8)*V[2])+V[2] 
1104    return VP   
1105   
1106def Q2Mat(Q):
1107    ''' make rotation matrix from quaternion
1108        q=r+ai+bj+ck
1109    '''
1110    aa = Q[0]**2
1111    ab = Q[0]*Q[1]
1112    ac = Q[0]*Q[2]
1113    ad = Q[0]*Q[3]
1114    bb = Q[1]**2
1115    bc = Q[1]*Q[2]
1116    bd = Q[1]*Q[3]
1117    cc = Q[2]**2
1118    cd = Q[2]*Q[3]
1119    dd = Q[3]**2
1120    M = [[aa+bb-cc-dd, 2.*(bc-ad),  2.*(ac+bd)],
1121        [2*(ad+bc),   aa-bb+cc-dd,  2.*(cd-ab)],
1122        [2*(bd-ac),    2.*(ab+cd), aa-bb-cc+dd]]
1123    return np.array(M)
1124   
1125def AV2Q(A,V):
1126    ''' convert angle (radians -pi to pi) & vector to quaternion
1127        q=r+ai+bj+ck
1128    '''
1129    Q = np.zeros(4)
1130    d = np.sqrt(np.sum(np.array(V)**2))
1131    if d:
1132        V /= d
1133    else:
1134        return [1.,0.,0.,0.]    #identity
1135    p = A/2.
1136    Q[0] = np.cos(p)
1137    s = np.sin(p)
1138    Q[1:4] = V*s
1139    return Q
1140   
1141def AVdeg2Q(A,V):
1142    ''' convert angle (degrees -180 to 180) & vector to quaternion
1143        q=r+ai+bj+ck
1144    '''
1145    Q = np.zeros(4)
1146    d = np.sqrt(np.sum(np.array(V)**2))
1147    if d:
1148        V /= d
1149    else:
1150        return [1.,0.,0.,0.]    #identity
1151    p = A/2.
1152    Q[0] = cosd(p)
1153    S = sind(p)
1154    Q[1:4] = V*S
1155    return Q
1156   
Note: See TracBrowser for help on using the repository browser.