diff --git a/Server_CoAP/triangulation.py b/Server_CoAP/triangulation.py index 49321781aabf6965d34270dcd60ee0f4ebc7b8b6..1cc80b2c8c6b07c390feab9223b4f76064404421 100644 --- a/Server_CoAP/triangulation.py +++ b/Server_CoAP/triangulation.py @@ -1,92 +1,230 @@ import pymongo +import math from pymongo import MongoClient -distance = [[0] for i in range(3)] -rbp_ID = [[0] for i in range(3)] -rbp_position = [[0,0],[0,0],[0,0]] -line_position = [[0,0],[0,0],[0,0]] -meet = [[0,0],[0,0],[0,0]] -x=0 -y=0 -tmp=0 -try: - client = MongoClient('mongodb://iotuser:iotsystem@shmd01.iptime.org/IoT_System', 27017) - db=client.IoT_System - collection1=db.users - docs1=collection1.find() - docs1_counter=collection1.count() - print(docs1_counter) -except: - pass +def cal_distance(a1,b1,a2,b2): + return math.sqrt((a2-a1)*(a2-a1)+(b2-b1)*(b2-b1)) -try: - client = MongoClient('mongodb://iotuser:iotsystem@shmd01.iptime.org/IoT_System', 27017) - db=client.IoT_System - collection2=db.TrakingData - docs2=collection2.find().sort("distance",1) - docs2_counter=collection2.count() - print(docs2_counter) -except: - pass +def first_situation(a1,b1,r1,a2,b2,r2,a3,b3,r3):#첫번째 경우로 원 3개가 서로에게 모두 겹치는 경우가 있는 경우이다 + line_position = [[0,0,0],[0,0,0],[0,0,0]]#Ax+By=c의 형태[A,B,C] + meet1 = [[0,0],[0,0],[0,0]] + meet2 = [[0,0],[0,0],[0,0]] + meet_result = [[0,0],[0,0],[0,0]] + #첫번째 직선(1,2) + line_position[0][0]=-2*a1+2*a2 + line_position[0][1]=-2*b1+2*b2 + line_position[0][2]=(r1*r1-r2*r2)+(a2*a2-a1*a1)+(b2*b2-b1*b1) + #두번째 직선(1,3) + line_position[1][0]=-2*a1+2*a3 + line_position[1][1]=-2*b1+2*b3 + line_position[1][2]=(r1*r1-r3*r3)+(a3*a3-a1*a1)+(b3*b3-b1*b1) + #세번째 직선(2,3) + line_position[2][0]=-2*a2+2*a3 + line_position[2][1]=-2*b2+2*b3 + line_position[2][2]=(r2*r2-r3*r3)+(a3*a3-a2*a2)+(b3*b3-b2*b2) -try: - client = MongoClient('mongodb://iotuser:iotsystem@shmd01.iptime.org/IoT_System', 27017) - db=client.IoT_System - collection3=db.RasbpNodeData - docs3=collection3.find() - docs3_counter=collection3.count() - print(docs3_counter) -except: - pass + #첫번째 직선의 교점 + if(line_position[0][0]==0 and line_position[0][1]!=0): + meet1[0][1]=line_position[0][2]/line_position[0][1] + meet2[0][1]=meet1[0][1] + meet1[0][0]=math.sqrt(r1*r1-(meet1[0][1]-b1)*(meet1[0][1]-b1))+a1 + meet2[0][0]=a1-math.sqrt(r1*r1-(meet1[0][1]-b1)*(meet1[0][1]-b1)) + if(cal_distance(meet1[0][0],meet1[0][1],a3,b3)>=cal_distance(meet2[0][0],meet2[0][1],a3,b3)): + meet_result[0][0]=meet2[0][0] + meet_result[0][1]=meet2[0][1] + else: + meet_result[0][0]=meet1[0][0] + meet_result[0][1]=meet1[0][1] + elif(line_position[0][0]!=0 and line_position[0][1]==0): + meet1[0][0]=line_position[0][2]/line_position[0][0] + meet2[0][0]=meet1[0][0] + meet1[0][1]=math.sqrt(r1*r1-(meet1[0][0]-a1)*(meet1[0][0]-a1))+b1 + meet2[0][1]=b1-math.sqrt(r1*r1-(meet1[0][0]-a1)*(meet1[0][0]-a1)) + if(cal_distance(meet1[0][0],meet1[0][1],a3,b3)>=cal_distance(meet2[0][0],meet2[0][1],a3,b3)): + meet_result[0][0]=meet2[0][0] + meet_result[0][1]=meet2[0][1] + else: + meet_result[0][0]=meet1[0][0] + meet_result[0][1]=meet1[0][1] + else: + a=1+(line_position[0][0]/line_position[0][1])*(line_position[0][0]/line_position[0][1]) + b=(-2*a1)+((-2*line_position[0][0]*line_position[0][2]) / (line_position[0][1]*line_position[0][1]))+(2*line_position[0][0]*b1/line_position[0][1]) + c=(line_position[0][2]/line_position[0][1])*(line_position[0][2]/line_position[0][1]) + (-2*line_position[0][2]*b1/line_position[0][1]) + b1*b1 - r1*r1 + a1*a1 + meet1[0][0]=(-b+math.sqrt(b*b-4*a*c))/(2*a) + meet2[0][0]=(-b-math.sqrt(b*b-4*a*c))/(2*a) + meet1[0][1]=(-1*line_position[0][0]*meet1[0][0]/line_position[0][1])+(line_position[0][2]/line_position[0][1]) + meet2[0][1]=(-1*line_position[0][0]*meet2[0][0]/line_position[0][1])+(line_position[0][2]/line_position[0][1]) + if(cal_distance(meet1[0][0],meet1[0][1],a3,b3)>=cal_distance(meet2[0][0],meet2[0][1],a3,b3)): + meet_result[0][0]=meet2[0][0] + meet_result[0][1]=meet2[0][1] + else: + meet_result[0][0]=meet1[0][0] + meet_result[0][1]=meet1[0][1] + print("x1:",meet_result[0][0],"y1:",meet_result[0][1]) -for i in range(0,docs1_counter): - print("<",docs1[i]["uuid"],">") - for j in range(0,docs2_counter): - if(docs1[i]["uuid"]==docs2[j]["uuid"]): - if(tmp>=3): - break - else: - distance[tmp]=docs2[j]["distance"] - rbp_ID[tmp]=docs2[j]["rasbpID"] - print(distance[tmp]) - tmp+=1 - tmp=0 - print("-----------") - for h in range(0,3): - for k in range(0,docs3_counter): - if(docs3[k]["rasbpMAC"]==rbp_ID[h]): - rbp_position[h][0]=docs3[k]["x"] - rbp_position[h][1]=docs3[k]["y"] - print(rbp_ID[h],rbp_position[h][0],rbp_position[h][1]) - break - tmp=0 - print("-----------") + #두번째 직선의 교점 + if(line_position[1][0]==0 and line_position[1][1]!=0): + meet1[1][1]=line_position[1][2]/line_position[1][1] + meet2[1][1]=meet1[0][1] + meet1[1][0]=math.sqrt(r1*r1-(meet1[1][1]-b1)*(meet1[1][1]-b1))+a1 + meet2[1][0]=a1-math.sqrt(r1*r1-(meet1[1][1]-b1)*(meet1[0][1]-b1)) + if(cal_distance(meet1[1][0],meet1[1][1],a2,b2)>=cal_distance(meet2[1][0],meet2[1][1],a2,b2)): + meet_result[1][0]=meet2[1][0] + meet_result[1][1]=meet2[1][1] + else: + meet_result[1][0]=meet1[1][0] + meet_result[1][1]=meet1[1][1] + elif(line_position[1][0]!=0 and line_position[1][1]==0): + meet1[1][0]=line_position[1][2]/line_position[1][0] + meet2[1][0]=meet1[1][0] + meet1[1][1]=math.sqrt(r1*r1-(meet1[1][0]-a1)*(meet1[1][0]-a1))+b1 + meet2[1][1]=b1-math.sqrt(r1*r1-(meet1[1][0]-a1)*(meet1[1][0]-a1)) + if(cal_distance(meet1[1][0],meet1[1][1],a2,b2)>=cal_distance(meet2[1][0],meet2[1][1],a2,b2)): + meet_result[1][0]=meet2[1][0] + meet_result[1][1]=meet2[1][1] + else: + meet_result[1][0]=meet1[1][0] + meet_result[1][1]=meet1[1][1] + else: + a=1+((line_position[1][0]/line_position[1][1])*(line_position[1][0]/line_position[1][1])) + b=(-2*a1)+((-2*line_position[1][0]*line_position[1][2]) / (line_position[1][1]*line_position[1][1]))+((2*line_position[1][0]*b1)/line_position[1][1]) + c=((line_position[1][2]/line_position[1][1])*(line_position[1][2]/line_position[1][1])) + ((-2*line_position[1][2]*b1)/line_position[1][1]) + b1*b1 - r1*r1 + a1*a1 + meet1[1][0]=(-b+math.sqrt(b*b-4*a*c))/(2*a) + meet2[1][0]=(-b-math.sqrt(b*b-4*a*c))/(2*a) + meet1[1][1]=(-1*line_position[1][0]*meet1[1][0]/line_position[1][1])+(line_position[1][2]/line_position[1][1]) + meet2[1][1]=(-1*line_position[1][0]*meet2[1][0]/line_position[1][1])+(line_position[1][2]/line_position[1][1]) + if(cal_distance(meet1[1][0],meet1[1][1],a2,b2)>=cal_distance(meet2[1][0],meet2[1][1],a2,b2)): + meet_result[1][0]=meet2[1][0] + meet_result[1][1]=meet2[1][1] + else: + meet_result[1][0]=meet1[1][0] + meet_result[1][1]=meet1[1][1] + print("x2:",meet_result[1][0],"y2:",meet_result[1][1]) + #세번째 직선의 교점 + if(line_position[2][0]==0 and line_position[2][1]!=0): + meet1[2][1]=line_position[2][2]/line_position[2][1] + meet2[2][1]=meet1[2][1] + meet1[2][0]=math.sqrt(r2*r2-(meet1[2][1]-b2)*(meet1[2][1]-b2))+a2 + meet2[2][0]=a2-math.sqrt(r2*r2-(meet1[2][1]-b2)*(meet1[2][1]-b2)) + if(cal_distance(meet1[2][0],meet1[2][1],a1,b1)>=cal_distance(meet2[2][0],meet2[2][1],a1,b1)): + meet_result[2][0]=meet2[2][0] + meet_result[2][1]=meet2[2][1] + else: + meet_result[2][0]=meet1[2][0] + meet_result[2][1]=meet1[2][1] + elif(line_position[2][0]!=0 and line_position[2][1]==0): + meet1[2][0]=line_position[2][2]/line_position[2][0] + meet2[2][0]=meet1[2][0] + meet1[2][1]=math.sqrt(r2*r2-(meet1[2][0]-a2)*(meet1[2][0]-a2))+b2 + meet2[2][1]=b2-math.sqrt(r2*r2-(meet1[2][0]-a1)*(meet1[2][0]-a2)) + if(cal_distance(meet1[2][0],meet1[2][1],a1,b1)>=cal_distance(meet2[2][0],meet2[2][1],a1,b1)): + meet_result[2][0]=meet2[2][0] + meet_result[2][1]=meet2[2][1] + else: + meet_result[2][0]=meet1[2][0] + meet_result[2][1]=meet1[2][1] + else: + a=1+(line_position[2][0]/line_position[2][1])*(line_position[2][0]/line_position[2][1]) + b=(-2*a2)+((-2*line_position[2][0]*line_position[2][2]) / (line_position[2][1]*line_position[2][1]))+(2*line_position[2][0]*b1/line_position[2][1]) + c=(line_position[2][2]/line_position[2][1])*(line_position[2][2]/line_position[2][1]) + (-2*line_position[2][2]*b1/line_position[2][1]) + b2*b2 - r2*r2 +a2*a2 + meet1[2][0]=(-b+math.sqrt(b*b-4*a*c))/(2*a) + meet2[2][0]=(-b-math.sqrt(b*b-4*a*c))/(2*a) + meet1[2][1]=(-1*line_position[2][0]*meet1[2][0]/line_position[2][1])+(line_position[2][2]/line_position[2][1]) + meet2[2][1]=(-1*line_position[2][0]*meet2[2][0]/line_position[2][1])+(line_position[2][2]/line_position[2][1]) + if(cal_distance(meet1[2][0],meet1[2][1],a1,b1)>=cal_distance(meet2[2][0],meet2[2][1],a1,b1)): + meet_result[2][0]=meet2[2][0] + meet_result[2][1]=meet2[2][1] + else: + meet_result[2][0]=meet1[2][0] + meet_result[2][1]=meet1[2][1] + print("x3:",meet_result[2][0],"y3:",meet_result[2][1]) + print("x:",(meet_result[0][0]+meet_result[1][0]+meet_result[2][0])/3,"/y:",(meet_result[0][1]+meet_result[1][1]+meet_result[2][1])/3) + return 0 - #첫번째 직선 - line_position[0][0]=(rbp_position[0][0]-rbp_position[1][0])/(rbp_position[1][1]-rbp_position[0][1]) - line_position[0][1]=( (distance[0]*distance[0]-distance[1]*distance[1]) - (rbp_position[0][0]*rbp_position[0][0] - rbp_position[1][0]*rbp_position[1][0]) -(rbp_position[0][1]*rbp_position[0][1]-rbp_position[1][1]*rbp_position[1][1]) ) /(2*rbp_position[1][1]-2*rbp_position[0][1]) - print(line_position[0][0],"&",line_position[0][1]) - #두번째 직선 - line_position[1][0]=(rbp_position[0][0]-rbp_position[2][0])/(rbp_position[2][1]-rbp_position[0][1]) - line_position[1][1]=( (distance[0]*distance[0]-distance[2]*distance[2]) - (rbp_position[0][0]*rbp_position[0][0] - rbp_position[2][0]*rbp_position[2][0]) -(rbp_position[0][1]*rbp_position[0][1]-rbp_position[2][1]*rbp_position[2][1]) ) /(2*rbp_position[2][1]-2*rbp_position[0][1]) - print(line_position[1][0],"&",line_position[1][1]) - #세번째 직선 - line_position[2][0]=(rbp_position[2][0]-rbp_position[1][0])/(rbp_position[1][1]-rbp_position[2][1]) - line_position[2][1]=( (distance[2]*distance[2]-distance[1]*distance[1]) - (rbp_position[2][0]*rbp_position[2][0] - rbp_position[1][0]*rbp_position[1][0]) -(rbp_position[2][1]*rbp_position[2][1]-rbp_position[1][1]*rbp_position[1][1]) ) /(2*rbp_position[1][1]-2*rbp_position[2][1]) - print(line_position[2][0],"&",line_position[2][1]) - #첫번째 직선과 두번째 직선의 교점 - meet[0][0]=(line_position[1][1]-line_position[0][1])/(line_position[0][0]-line_position[1][0]) - meet[0][1]=(line_position[0][0] * (line_position[1][1]-line_position[0][1]) / (line_position[0][0]-line_position[1][0]) )+line_position[0][1] - #두번째 직선과 세번째 직선의 교점 - meet[1][0]=(line_position[1][1]-line_position[2][1])/(line_position[2][0]-line_position[1][0]) - meet[1][1]=(line_position[2][0] * (line_position[1][1]-line_position[2][1]) / (line_position[2][0]-line_position[1][0]) )+line_position[2][1] - #첫번째 직선과 세번째 직선의 교점 - meet[2][0]=(line_position[2][1]-line_position[0][1])/(line_position[0][0]-line_position[2][0]) - meet[2][1]=(line_position[0][0] * (line_position[2][1]-line_position[0][1]) / (line_position[0][0]-line_position[2][0]) )+line_position[0][1] +def second_situation(a1,b1,r1,a2,b2,r2,oa3,ob3,or3):#2번째 경우로 2개의 원만 겹치는 부분이 있는 경우이다. + line_position = [[0,0,0],[0,0,0],[0,0,0]]#Ax+By=c의 형태[A,B,C] + meet1 = [[0,0],[0,0],[0,0]] + meet2 = [[0,0],[0,0],[0,0]] + meet_result = [[0,0],[0,0],[0,0]] + #첫번째 직선(1,2) + line_position[0][0]=-2*a1+2*a2 + line_position[0][1]=-2*b1+2*b2 + line_position[0][2]=(r1*r1-r2*r2)+(a2*a2-a1*a1)+(b2*b2-b1*b1) + print(line_position[0]) + #첫번째 직선의 교점 + if(line_position[0][0]==0 and line_position[0][1]!=0): + meet1[0][1]=line_position[0][2]/line_position[0][1] + meet2[0][1]=meet1[0][1] + meet1[0][0]=math.sqrt(r1*r1-(meet1[0][1]-b1)*(meet1[0][1]-b1))+a1 + meet2[0][0]=a1-math.sqrt(r1*r1-(meet1[0][1]-b1)*(meet1[0][1]-b1)) + if(cal_distance(meet1[0][0],meet1[0][1],oa3,ob3)>=cal_distance(meet2[0][0],meet2[0][1],oa3,ob3)): + meet_result[0][0]=meet2[0][0] + meet_result[0][1]=meet2[0][1] + else: + meet_result[0][0]=meet1[0][0] + meet_result[0][1]=meet1[0][1] + elif(line_position[0][0]!=0 and line_position[0][1]==0): + meet1[0][0]=line_position[0][2]/line_position[0][0] + meet2[0][0]=meet1[0][0] + meet1[0][1]=math.sqrt(r1*r1-(meet1[0][0]-a1)*(meet1[0][0]-a1))+b1 + meet2[0][1]=b1-math.sqrt(r1*r1-(meet1[0][0]-a1)*(meet1[0][0]-a1)) + if(cal_distance(meet1[0][0],meet1[0][1],oa3,ob3)>=cal_distance(meet2[0][0],meet2[0][1],oa3,ob3)): + meet_result[0][0]=meet2[0][0] + meet_result[0][1]=meet2[0][1] + else: + meet_result[0][0]=meet1[0][0] + meet_result[0][1]=meet1[0][1] + else: + a=1+(line_position[0][0]/line_position[0][1])*(line_position[0][0]/line_position[0][1]) + b=(-2*a1)+((-2*line_position[0][0]*line_position[0][2]) / (line_position[0][1]*line_position[0][1]))+(2*line_position[0][0]*b1/line_position[0][1]) + c=(line_position[0][2]/line_position[0][1])*(line_position[0][2]/line_position[0][1]) + (-2*line_position[0][2]*b1/line_position[0][1]) + b1*b1 - r1*r1 + a1*a1 + print(a,b,c) + meet1[0][0]=(-b+math.sqrt(b*b-4*a*c))/(2*a) + meet2[0][0]=(-b-math.sqrt(b*b-4*a*c))/(2*a) + meet1[0][1]=(-1*line_position[0][0]*meet1[0][0]/line_position[0][1])+(line_position[0][2]/line_position[0][1]) + meet2[0][1]=(-1*line_position[0][0]*meet2[0][0]/line_position[0][1])+(line_position[0][2]/line_position[0][1]) + print(meet1[0][0],meet1[0][1]) + print(meet2[0][0],meet2[0][1]) + print(cal_distance(meet1[0][0],meet1[0][1],oa3,ob3)) + print(cal_distance(meet2[0][0],meet2[0][1],oa3,ob3)) + if(cal_distance(meet1[0][0],meet1[0][1],oa3,ob3)>=cal_distance(meet2[0][0],meet2[0][1],oa3,ob3)): + meet_result[0][0]=meet2[0][0] + meet_result[0][1]=meet2[0][1] + else: + meet_result[0][0]=meet1[0][0] + meet_result[0][1]=meet1[0][1] + print("x:",meet_result[0][0],"y:",meet_result[0][1]) - print("x:",(meet[0][0]+meet[1][0]+meet[2][0])/3,"/y:",(meet[0][1]+meet[1][1]+meet[2][1])/3) - print("-----------") - print("-----------") - print("-----------") \ No newline at end of file +#def third_situation(a1,b1,r1,a2,b2,r2,a3,b3,r3):#3번째 경우로 겹치는 원이 없는 경우이다.(값이 정확히지는 않음) +# a=2*(a1-a2) +# b=2*(b1-b2) +# c=(r2*r2-r1*r1)+(a1*a1-a2*a2)+(b1*b1-b2*b2) +# d=2*(a2-a3) +# e=2*(b2-b3) +# f=(r3*r3-r2*r2)+(a2*a2-a3*a3)+(b2*b2-b3*b3) +# y1=(f-((d*c) / a)) / (e-((b*d)/a)) +# x1=(c-b*y1)/a +# print("x:",x1,"y:",y1) + +def function(a1,b1,r1,a2,b2,r2,a3,b3,r3): + a=cal_distance(a1,b1,a2,b2) + b=cal_distance(a1,b1,a3,b3) + c=cal_distance(a2,b2,a2,b2) + if(a<=r1+r2 and b<=r1+r3 and c<=r2+r3): + first_situation(a1,b1,r1,a2,b2,r2,a3,b3,r3) + elif(a<=r1+r2 and b>r1+r3 and c>r2+r3): + second_situation(a1,b1,r1,a2,b2,r2,a3,b3,r3) + elif(a>r1+r2 and b<=r1+r3 and c>r2+r3): + second_situation(a1,b1,r1,a3,b3,r3,a2,b2,r2) + elif(a>r1+r2 and b>r1+r3 and c<=r2+r3): + second_situation(a2,b2,r2,a3,b3,r3,a1,b1,r1) + +#예시 +first_situation(20,10,10,9,10,1,10,9,1) +function(20,10,10,9,10,1,10,9,1) +print("-----------------------") +second_situation(0,3,1,0,4,1,5,3,1) +function(0,3,1,0,4,1,5,3,1) +print("-----------------------") \ No newline at end of file