def localize2d(self, node, localized):
p = []
r = []
for i in range(4):
p.append(self.global_pos[localized[i]])
r.append(dist2d(node, localized[i]))
if collinear(p):
return False
sol = cc_int(p[:-1], r[:-1])
if len(sol) == 0:
return False
if len(sol) == 1:
node.set_local(sol[0])
else:
p1 = abs(point.norm(point.vector(sol[0]) - point.vector(p[2])) - r[3]) <= r * 0.05
p2 = abs(point.norm(point.vector(sol[1]) - point.vector(p[2])) - r[3]) <= r * 0.05
if p1 and p2:
return False
if p1:
self.local_pos[node] = sol[0]
if p2:
self.local_pos[node] = sol[1]
return True
def localize3d(self, node, localized):
p = []
r = []
for i in range(4):
p.append(self.global_pos[localized[i]])
r.append(self.adj[i][localized[i]][1])
if coplanar(p):
return False
sol = sss_int(p[:-1], r[:-1])
if len(sol) == 0:
return False
if len(sol) == 1:
self.global_pos[node] = sol[0]
else:
p1 = abs(point.norm(point.vector(sol[0]) - point.vector(p[3])) - r[3]) <= r[3] * 0.05
p2 = abs(point.norm(point.vector(sol[1]) - point.vector(p[3])) - r[3]) <= r[3] * 0.05
if p1 and p2:
return False
if p1:
self.global_pos[node] = sol[0]
if p2:
self.global_pos[node] = sol[1]
return True
def actual_distance(self, other):
return point.norm(self.actualPos - other.actualPos)
