Skip to content
Snippets Groups Projects
Commit d149f576 authored by KWAK's avatar KWAK
Browse files

triangulation

parent 2ccb1c5e
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Server_CoAP/triangulation.py
+ 220
− 82
View file @ d149f576
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])
#두번째 직선의 교점
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
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("-----------")
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])
#첫번째 직선
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])
#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)
#첫번째 직선과 두번째 직선의 교점
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 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)
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
#예시
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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment

feedback to tooltime@ajou.ac.kr or hwan@ajou.ac.kr