算GPS照片之间的距离

# -*- coding: cp936 -*-

#WriteTo:yilong594@hotmail.com
# http://download.csdn.net/detail/testemule/5666185
##提示 对于distance.exe,若算指定点的距离，需要有input.txt文件，若无input.txt则计算所有点之间距离输出为distance.csv
##若包含input.txt输出distance2.csv
##input.txt可按如下两种方式:
##
##格式1(使用照片名称,照片名称之间用英文逗号隔开)：
##照片1名称,照片2名称
##...
##格式2(使用经纬度，各值之间用1个或多个空格分割开)
##照片1纬度值 照片1经度值 照片2纬度值 照片2经度值 
###...
import math,os
def LL2UTM_USGS(lat, lon):
    a=6378137
    f=1/298.257223563
    Ln=int(lon/6)+1+30
    lonOrigin=6*(Ln-30)-3
    FN=0
    '''
    ** Input：(a, f, lat, lon, lonOrigin, FN)
    ** a 椭球体长半轴
    ** f 椭球体扁率 f=(a-b)/a 其中b代表椭球体的短半轴
    ** lat 经过UTM投影之前的纬度
    ** lon 经过UTM投影之前的经度
    ** lonOrigin 中央经度线
    ** FN 纬度起始点，北半球为0，南半球为10000000.0m
    ---------------------------------------------
    ** Output:(UTMNorthing, UTMEasting)
    ** UTMNorthing 经过UTM投影后的纬度方向的坐标
    ** UTMEasting 经过UTM投影后的经度方向的坐标
    ---------------------------------------------
    ** 功能描述：UTM投影
    ** 作者： Ace Strong
    ** 单位： CCA NUAA
    ** 创建日期：2008年7月19日
    ** 版本：1.0
    ** 本程序实现的公式请参考
    ** "Coordinate Conversions and Transformations including Formulas" p35.
    ** & http://www.uwgb.edu/dutchs/UsefulData/UTMFormulas.htm
    '''




    # e表示WGS84第一偏心率,eSquare表示e的平方
    eSquare = 2*f - f*f
    k0 = 0.9996




    # 确保longtitude位于-180.00----179.9之间
    lonTemp = (lon+180)-int((lon+180)/360)*360-180
    latRad = math.radians(lat)
    lonRad = math.radians(lonTemp)
    lonOriginRad = math.radians(lonOrigin)
    e2Square = (eSquare)/(1-eSquare)




    V = a/math.sqrt(1-eSquare*math.sin(latRad)**2)
    T = math.tan(latRad)**2
    C = e2Square*math.cos(latRad)**2
    A = math.cos(latRad)*(lonRad-lonOriginRad)
    M = a*((1-eSquare/4-3*eSquare**2/64-5*eSquare**3/256)*latRad
    -(3*eSquare/8+3*eSquare**2/32+45*eSquare**3/1024)*math.sin(2*latRad)
    +(15*eSquare**2/256+45*eSquare**3/1024)*math.sin(4*latRad)
    -(35*eSquare**3/3072)*math.sin(6*latRad))




    # x
    UTMEasting = k0*V*(A+(1-T+C)*A**3/6
    + (5-18*T+T**2+72*C-58*e2Square)*A**5/120)+ 500000.0
    # y
    UTMNorthing = k0*(M+V*math.tan(latRad)*(A**2/2+(5-T+9*C+4*C**2)*A**4/24
    +(61-58*T+T**2+600*C-330*e2Square)*A**6/720))
    # 南半球纬度起点为10000000.0m
    UTMNorthing += FN
    return (UTMEasting, UTMNorthing,Ln)
def dist((x1,y1),(x2,y2)):
    return round(((x1-x2)**2+(y1-y2)**2)**0.5,3)


def distlatlon((x1,y1),(x2,y2)):
    utm01=LL2UTM_USGS(x1,y1)
    utm02=LL2UTM_USGS(x2,y2)
    utm1=LL2UTM_USGS(x1,y1)[0:2]
    utm2=LL2UTM_USGS(x2,y2)[0:2]
    if utm01[2]==utm02[2]:
        ds=dist((utm1[0],utm1[1]),(utm2[0],utm2[1]))
    else:
        ds='error'
    return ds


def distlat((x1,y1),(x2,y2)):
    utm01=LL2UTM_USGS(x1,y1)
    utm02=LL2UTM_USGS(x2,y2)
    utm1=LL2UTM_USGS(x1,y1)[0:2]
    utm2=LL2UTM_USGS(x2,y2)[0:2]
    if utm01[2]==utm02[2]:
        ds=abs(round(utm1[0]-utm2[0],3))
    else:
        ds='error'
    return ds
def distlon((x1,y1),(x2,y2)):
    utm01=LL2UTM_USGS(x1,y1)
    utm02=LL2UTM_USGS(x2,y2)
    utm1=LL2UTM_USGS(x1,y1)[0:2]
    utm2=LL2UTM_USGS(x2,y2)[0:2]
    if utm01[2]==utm02[2]:
        ds=abs(round(utm1[1]-utm2[1],3))
    else:
        ds='error'
    return ds
def Comb(listx, n):
    for i in range(len(listx)):
        v = listx[i:i+1]
        if n == 1:
            yield v
        else:
            rest = listx[i+1:]
            for c in Comb(rest, n-1):
                yield v + c
def dsall(utm):
    d=[]
    for x in range(len(utm)):
        for y in range(len(utm)):
            if utm[x][1][2]==utm[y][1][2]:
                d.append( (utm[x][0],utm[y][0],dist((utm[x][1][0],utm[x][1][1]),(utm[y][1][0],utm[y][1][1]))))
            else:
                d.append( (utm[x][0],utm[y][0],'error'))
    f1=open('distance_all.csv','w')
    f1.close()
    for x in d:
        xx=x[0]+','+str(x[1])+','+str(x[2])+'\n'
        open('distance_all.csv','ab').write(xx)
def ds(ref1,ref2):
    num1=None;num2=None
    d=[]
    for x in range(len(utm)):
        if utm[x][0]==ref1:
            num1= x
    for y in range(len(utm)):
        if utm[y][0]==ref2:
            num2= y
##    for x in range(len(utm)):
##        for y in range(len(utm)):
##            if utm[x][1][2]==utm[y][1][2]:
##                d.append( (utm[x][0],utm[y][0],dist((utm[x][1][0],utm[x][1][1]),(utm[y][1][0],utm[y][1][1]))))
##            else:
##                d.append( (utm[x][0],utm[y][0],'error'))
##    for x in d:
##        xx=x[0]+','+str(x[1])+','+str(x[2])+'\n'
##        open('distance.csv','ab').write(xx)


    if num1!=None and num2!=None:
        if utm[num1][1][2]==utm[num2][1][2]:
            dista=dist((utm[num1][1][0],utm[num1][1][1]),(utm[num2][1][0],utm[num2][1][1]))
        else:
            dista='error'               
        f5=open('distance.csv','ab')
        xx=utm[num1][0]+','+utm[num2][0]+','+str(dista)+'\n'
        f5.write( xx)
        f5.close()              
f=open('result.txt','r')
utm=[]
names=[]
for x in f.readlines():
    a=x.split()
    name=a[0]
    names.append(name)
    lat=float(a[2])
    lon=float(a[3])
    newlatlon=LL2UTM_USGS(lat,lon)
    utm.append((name,newlatlon))
f.close()


ff=open('input2.txt','w').write('')
for x in Comb(names,2):
    open('input2.txt','ab').write( x[0]+','+x[1]+'\n')
###组合1
dsall(utm)
###组合2
f1=open('distance.csv','w')
f1.close()
f3=open('input2.txt','r')
for x in f3.readlines():
    x=x.strip()
    if x!='':
        ref1=x.split(',')[0].strip()
        ref2=x.split(',')[1].strip()
        ds(ref1,ref2)
        f3.close()
os.remove('input2.txt')
###自定义组合
if os.path.exists('input.txt'):
    f3=open('input.txt','r')
    f2=open('distance.csv','w')
    f2.close()
    for x in f3.readlines():
        x=x.strip()
        if x!='':
            try:
                ref1=x.split(',')[0].strip()
                ref2=x.split(',')[1].strip()
                ds(ref1,ref2)
            except:
                ref1=float(x.split()[0].strip())
                ref2=float(x.split()[1].strip())
                ref3=float(x.split()[2].strip())
                ref4=float(x.split()[3].strip())
                diss=distlatlon((ref1,ref2),(ref3,ref4))
                open('distance.csv','ab').write(str(ref1)+','+str(ref2)+","+str(ref3)+','+str(ref4)+','+str(diss)+'\n')


    f3.close()