def DistLamPedgePZF(self, p0z, p1z):
self.Dllist.append([p0z, p1z])
assert self.zlo <= p0z.z <= p1z.z <= self.zhi
v = p1z - p0z
vf = P2(v.x, v.y)
vfsq = vf.Lensq()
if vfsq == 0.0:
return
vflen = vf.Len()
vfperpnorm = P2.CPerp(vf) * (1.0 / vflen)
lv = self.p - p0z
dp = P2.DotLZ(vfperpnorm, lv)
dpm = P2.DotLZ(vfperpnorm, self.vp)
if dpm > 0.0:
laml = (-self.r - dp) / dpm
elif dpm < 0.0:
laml = (self.r - dp) / dpm
else:
return
if 0 < laml < self.lam:
lvp = lv + self.vp * laml
assert abs(abs(P2.DotLZ(vfperpnorm, lvp)) - self.r) < 0.001
mu = P2.DotLZ(vf, lvp) / vfsq
if 0 < mu < 1:
assert abs(P2.DotLZ(vf, lvp - v * mu)) < 0.001
self.lam = laml
def DistPedgePZF(self, p0z, p1z):
assert self.zlo <= p0z.z <= p1z.z <= self.zhi
v = p1z - p0z
vf = P2(v.x, v.y)
vfsq = vf.Lensq()
if vfsq == 0.0:
return
vflen = vf.Len()
lv = self.p - p0z
dp = abs(P2.DotLZ(P2.CPerp(vf), lv) / vflen)
if dp >= self.r:
return
lam = P2.DotLZ(vf, lv) / vfsq
if 0 < lam < 1:
self.r = dp
self.v = v * lam - lv
assert abs(P2.DotLZ(vf, self.v)) < 0.001
assert abs(P2(self.v.x, self.v.y).Len() - self.r) < 0.001
assert self.zlo <= self.p.z + self.v.z <= self.zhi, (self.zlo,
self.p.z,
self.p.z +
self.v.z,
self.zhi)
self.Dpp = [p0z, p1z]