source: trunk/GSASIImath.py @ 755

Last change on this file since 755 was 755, checked in by vondreele, 11 years ago

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