def _wrappyramid(atmidx, coords, dismat):
'''_wrappyramid((i1, i2, i3, i4), coords, dismat)
coords是坐标，dismat是距离矩阵，已知i2, i3, i4三个点的坐标，求i1的坐标。
一般情况下i1有两个解(r1, r2), 这时返回 (r1, r2)，
如果没有解，返回 (None, None)
浮点数误差部分参见 config.Mezei_p24_threshold
'''
i1, i2, i3, i4 = atmidx
A = coords[i2]
B = coords[i3]
C = coords[i4]
rAX = dismat[i1, i2]
rBX = dismat[i1, i3]
rCX = dismat[i1, i4]
results, error = pyramid.pyramid(A, B, C, rAX, rBX, rCX)
threshold = config.config.get('Mezei_p24_threshold', -0.01*0.01)
if error < threshold:
yield None
yield None
else:
yield results[0]
yield results[1]
def _calc_p2_p4(self, points, len1, len2, p3_result, angle):
p0, p1, p5, p6 = tuple(points)
d12, d23, d34, d45 = tuple(len1)
d02, d13, d24, d35, d46 = tuple(len2)
threshold = config.get('Mezei_p24_threshold', -0.01*0.01)
p3o, p3x, p3y = p3_result
p3 = p3o + p3x * cos(angle) + p3y * sin(angle)
p2s, z2s = pyramid(p0, p1, p3, d02, d12, d23)
if z2s < threshold:
return None
p4s, z4s = pyramid(p6, p5, p3, d46, d45, d34)
if z4s < threshold:
return None
return (p2s, p4s)
def __call__(self, angle):
self.p3 = self.p3o + self.p3x * cos(angle) + self.p3y * sin(angle)
p2s, z2s = pyramid(self.p0, self.p1, self.p3, self.d02,
self.d12, self.d23)
if z2s < self.threshold:
return None
p4s, z4s = pyramid(self.p6, self.p5, self.p3, self.d46,
self.d45, self.d34)
if z4s < self.threshold:
return None
if self.mode == -1:
return (p2s, p4s)
else:
i2 = self.mode / 2
i4 = self.mode % 2
self.p2 = p2s[i2]
self.p4 = p4s[i4]
return tools.length(self.p2-self.p4) - self.d24
def get_p5(self, p3, p4, p6):
results, error = pyramid.pyramid(p3, p4, p6,
self._dismatrix(5, 3),
self._dismatrix(5, 4),
self._dismatrix(5, 6))
if error >= self.threshold:
yield results[0]
yield results[1]
else:
yield None
yield None
def get_p4(self, p2, p3, p6):
results, error = pyramid.pyramid(p2, p3, p6,
self._dismatrix(4, 2),
self._dismatrix(4, 3),
self._dismatrix(4, 6))
if error >= self.threshold:
for result in results:
yield result
else:
yield None
yield None
def get_p3(self, p2):
results, error = pyramid.pyramid(self._coord(0),
self._coord(1),
p2,
self._dismatrix(0, 3),
self._dismatrix(1, 3),
self._dismatrix(2, 3))
if error >= self.threshold:
yield (results[0] + results[1]) / 2.0
else:
yield None
def get_p7(self, p6):
results, error = pyramid.pyramid(p6,
self._coord(8),
self._coord(9),
self._dismatrix(7, 6),
self._dismatrix(7, 8),
self._dismatrix(7, 9))
if error >= self.threshold:
yield results[0]
yield results[1]
else:
yield None
yield None
def get_p6(self, p3):
results, error = pyramid.pyramid(p3,
self._coord(8),
self._coord(9),
self._dismatrix(6, 3),
self._dismatrix(6, 8),
self._dismatrix(6, 9))
if error >= self.threshold:
for result in results:
yield result
else:
yield None
yield None
