source: trunk/GSASIImath.py @ 797

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

some fixes to TOF stuff
some fixes to SXC stuff

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