def PolyCutBar(ktbar, ktnode1, vc, vch, btop):
assert not ktbar.bbardeleted, "polycutbar hits deletion"
Dktbarstart = ktbar
ktnoded1 = P3.Dot(vc, ktnode1.p)
assert btop == (P3.Dot(vc, ktbar.nodemid.p) < vch), ("fg", btop,
P3.Dot(
vc,
ktbar.nodemid.p),
vch)
#assert ktnoded0 < vch # not necessarily true if bar points back with mid on end
Dcount = 0
while True:
ktnode0 = ktnode1
ktnode1 = ktbar.GetNodeFore(ktbar.nodeback == ktnode0)
ktnoded0 = ktnoded1
ktnoded1 = P3.Dot(vc, ktnode1.p)
if btop == (ktnoded1 > vch):
break
ktbar = ktbar.GetForeRightBL(ktbar.nodefore == ktnode1)
assert not ktbar.bbardeleted
assert Dktbarstart != ktbar
Dcount += 1
assert Dcount < 1000
ktbarlam = (vch - ktnoded0) / (ktnoded1 - ktnoded0)
return ktnode0, ktbar, ktbarlam
def getbarendclos(self, barstosplit): # for plotting closest approaches
pcs = []
for bar in barstosplit:
if bar.bbardeleted:
continue
if not ((bar.nodeback.pointzone.izone == barmesh.PZ_BEYOND_R) and
(bar.nodefore.pointzone.izone == barmesh.PZ_BEYOND_R)):
continue
vb = bar.nodefore.p - bar.nodeback.p
vbsq = vb.Lensq()
if bar.nodeback.pointzone.v is not None:
lampcb = P3.Dot(bar.nodeback.pointzone.v, vb) / vbsq
if 0 < lampcb < 1:
dvb = bar.nodeback.pointzone.v - vb * lampcb
assert abs(P3.Dot(dvb, vb)) < 0.001
dvbsq = dvb.Lensq()
if dvbsq < self.rdsq:
pcs.append(bar.nodeback.p + bar.nodeback.pointzone.v)
if bar.nodefore.pointzone.v is not None:
lampcf = -P3.Dot(bar.nodefore.pointzone.v, vb) / vbsq
if 0 < lampcf < 1:
dvf = bar.nodefore.pointzone.v + vb * lampcf
assert abs(P3.Dot(dvf, vb)) < 0.001
dvfsq = dvf.Lensq()
if dvfsq < self.rdsq:
pcs.append(bar.nodeback.p + bar.nodefore.pointzone.v)
return pcs
def DistPtrianglePZ(self, p0, p1, p2):
v1 = p1 - p0
v2 = p2 - p0
v1sq = v1.Lensq()
v2sq = v2.Lensq()
v1dv2 = P3.Dot(v1, v2)
det = v1sq * v2sq - v1dv2**2
if det == 0.0:
return # near zero width triangle has no face to touch
invdet = 1.0 / det
# should do distance by the dot on the crossproduct normal
lv = self.p - p0
v1dlv = P3.Dot(v1, lv)
v2dlv = P3.Dot(v2, lv)
# solve vd = lv - v1 * lam1 - v2 * lam2, where vd.v1 = vd.v2 = 0
# (v1sq v1dv2) ( lam1 ) ( v1dlv )
# (v1dv2 v2sq) . ( lam2 ) = ( v2dlv )
lam1 = (v2sq * v1dlv - v1dv2 * v2dlv) * invdet
lam2 = (-v1dv2 * v1dlv + v1sq * v2dlv) * invdet
if 0 < lam1 and 0 < lam2 and lam1 + lam2 < 1:
vd = lv - v1 * lam1 - v2 * lam2
assert abs(P3.Dot(vd, v1)) < 0.001
assert abs(P3.Dot(vd, v2)) < 0.001
vdlen = vd.Len()
if vdlen < self.r:
self.r = vdlen
self.v = -vd
def InsertNodeIntoBarF(self, bar, newnode, bcolinear):
assert newnode.p != bar.nodeback.p and newnode.p != bar.nodefore.p, newnode.p
assert newnode in self.nodes
if __debug__:
if bcolinear:
Dv = bar.nodefore.p - bar.nodeback.p
Dv0 = newnode.p - bar.nodeback.p
Dlam = P3.Dot(Dv0, Dv) / Dv.Lensq()
assert 0 < Dlam < 1, Dlam
Dvd = Dv0 - Dv * Dlam
assert abs(P3.Dot(Dvd, Dv)) < 0.001
assert Dvd.Len() < 0.01, (Dv, Dv0)
barforeleft = bar.GetBarForeLeft()
barbackright = bar.GetBarBackRight()
barback = Bar(bar.nodeback, newnode)
barfore = Bar(bar.nodefore, newnode)
if bcolinear: # copy over so we can identify colinearity from creation and not attempt to split along it
assert (bar.barvecN - barback.barvecN).Len() < 1e-5, (
bar.nodeback.p, newnode.p, bar.nodefore.p)
assert (bar.barvecN + barfore.barvecN).Len() < 1e-5, (
bar.nodeback.p, newnode.p, bar.nodefore.p)
barback.barvecN = bar.barvecN
barfore.barvecN = -bar.barvecN
barback.cellmarkleft, barback.cellmarkright = bar.cellmarkleft, bar.cellmarkright
barfore.cellmarkright, barfore.cellmarkleft = bar.cellmarkleft, bar.cellmarkright
if barbackright:
barback.barforeright = barfore
barfore.barbackleft = bar.barforeright
barbackright.SetForeRightBL(barbackright.nodefore == bar.nodeback,
barback)
if barforeleft:
barback.barbackleft = bar.barbackleft
barfore.barforeright = barback
barforeleft.SetForeRightBL(barforeleft.nodefore == bar.nodefore,
barfore)
assert not barforeleft or barback.GetBarForeLeft(
) == barfore, barback.GetBarForeLeft()
assert not barbackright or barfore.GetBarForeLeft(
) == barback, barfore.GetBarForeLeft()
self.bars.append(barback)
self.bars.append(barfore)
bar.barforeright = barfore
bar.barbackleft = barback
bar.bbardeleted = True
return newnode
def DistPedgePZ(self, p0, p1):
lv = self.p - p0
v = p1 - p0
vsq = v.Lensq()
lam = P3.Dot(v, lv) / vsq
if 0.0 < lam < 1.0:
vd = lv - v * lam
assert abs(P3.Dot(vd, v)) < 0.0001
vdlen = vd.Len()
if vdlen < self.r:
self.r = vdlen
self.v = -vd
def DistLamPedgePZ(self, p0, p1):
v = p1 - p0
vsq = v.Lensq()
lv = self.p - p0
# solve |lv + vp * lam - v * mu| == r, where (lv + vp * lam - v * mu) . vp == 0
mu0 = P3.Dot(lv, v) / vsq
lvf = lv - v * mu0
vpdv = P3.Dot(self.vp, v)
muvp = vpdv / vsq
vpf = self.vp - v * muvp
vpfsq = vpf.Lensq()
if vpfsq == 0.0:
return
assert abs(vpfsq - (self.vpsq - muvp * vpdv)) < 0.001
lvfdvpf = P3.Dot(lvf, vpf)
lamc = -lvfdvpf / vpfsq
cp = lvf + vpf * lamc
cpsq = cp.Lensq()
lvfsq = lvf.Lensq()
assert abs(cpsq -
(lvfsq + 2 * lvfdvpf * lamc + vpfsq * lamc * lamc)) < 0.001
assert abs(P3.Dot(cp, vpf)) < 0.001
llamdsq = self.rsq - cp.Lensq()
if llamdsq < 0.0:
return
lamd = math.sqrt(llamdsq / vpfsq)
if lamc + lamd < 0.0:
return
lam = lamc - lamd
if lam < 0.0:
return # check closer stuff
if lam > self.lam:
return
mu = mu0 + muvp * lam
if mu < 0 or mu > 1:
return
dv = lv + self.vp * lam - v * mu
assert abs(dv.Len() - self.r) < 0.001
assert abs(P3.Dot(dv, v)) < 0.001
self.lam = lam
def calcsplithalfpos(self):
# midpoint, but could be informed by multiple subdivisions on the rate of curvature
# should also measure the bc.calccellcuttangency() and find a spot where it is properly wide and doesn't need to be tested
vch = P3.Dot(self.vc, self.bar.nodemid.p + self.vc * 0.5)
# find the splitting points
self.ktnodetop, self.ktbartop, self.ktbartoplam = PolyCutBar(
self.bar, self.node, self.vc, vch, True)
self.ktnodebot, self.ktbarbot, self.ktbarbotlam = PolyCutBar(
self.cbar, self.cbar.GetNodeFore(self.cbar.nodeback == self.cnode),
self.vc, vch, False)
def DistLamPtrianglePZ(self, p0, p1, p2):
# solve vd = lv + vp * lam - v1 * lam1 - v2 * lam2, where |vd| = r and vd.v1 = vd.v2 = 0
# solve +-r = (lv + vp * lam) . vnorm = lv . vnorm + vp . vnorm lam
v1 = p1 - p0
v2 = p2 - p0
vcross = P3.Cross(v1, v2)
assert abs(P3.Dot(vcross, v1)) < 0.001
assert abs(P3.Dot(vcross, v2)) < 0.001
vnorm = P3.ZNorm(vcross)
lv = self.p - p0
lvdvnorm = P3.Dot(lv, vnorm)
vpdvnorm = P3.Dot(self.vp, vnorm)
if vpdvnorm == 0.0:
return
# lam = (+-r - lvdvnorm)/vpdvnorm
if vpdvnorm > 0.0:
lam = (-self.r - lvdvnorm) / vpdvnorm
else:
lam = (self.r - lvdvnorm) / vpdvnorm
if lam < 0 or lam > self.lam:
return
lvl = lv + self.vp * lam
v1dlv = P3.Dot(v1, lvl)
v2dlv = P3.Dot(v2, lvl)
v1sq = v1.Lensq()
v2sq = v2.Lensq()
v1dv2 = P3.Dot(v1, v2)
det = v1sq * v2sq - v1dv2**2
if det == 0.0:
return # no face size
invdet = 1.0 / det
# solve vd = lv - v1 * lam1 - v2 * lam2, where vd.v1 = vd.v2 = 0
# (v1sq v1dv2) ( lam1 ) ( v1dlv )
# (v1dv2 v2sq) . ( lam2 ) = ( v2dlv )
lam1 = (v2sq * v1dlv - v1dv2 * v2dlv) * invdet
lam2 = (-v1dv2 * v1dlv + v1sq * v2dlv) * invdet
if 0 < lam1 and 0 < lam2 and lam1 + lam2 < 1:
vd = lvl - v1 * lam1 - v2 * lam2
assert abs(P3.Dot(vd, v1)) < 0.001
assert abs(P3.Dot(vd, v2)) < 0.001
assert abs(vd.Len() - self.r) < 0.001
self.lam = lam
def shoulddivide(self, rd, contourdelta, contourdotdiff,
b2dcontournormals):
if self.vc.Len() <= contourdelta:
return False
if b2dcontournormals:
nd = P2.Dot(
P2.ZNorm(
P2(self.bar.nodemid.pointzone.v.x,
self.bar.nodemid.pointzone.v.y)),
P2.ZNorm(
P2(self.cbar.nodemid.pointzone.v.x,
self.cbar.nodemid.pointzone.v.y)))
else:
nd = P3.Dot(self.bar.nodemid.pointzone.v,
self.cbar.nodemid.pointzone.v) / (rd * rd)
if nd >= contourdotdiff:
return False
return True
def DistLamPpointPZ(self, p0):
lv = p0 - self.p
if lv.z < min(self.vp.z, 0) - self.r or lv.z > max(self.vp.z,
0) + self.r:
return
# |lv - vp * lam| = r
# qa = vpsq
qb2 = -P3.Dot(self.vp, lv)
qc = lv.Lensq() - self.rsq
qdq = qb2 * qb2 - self.vpsq * qc
if qdq < 0.0:
return
qs = math.sqrt(qdq) / self.vpsq
qm = -qb2 / self.vpsq
assert abs(qc + (qm + qs) * (2 * qb2 + (qm + qs) * self.vpsq)) < 0.002
if qm + qs <= 0.0:
return
laml = qm - qs
if laml < 0.0:
self.lam = 0.0 # shouldn't happen
elif laml < self.lam:
self.lam = laml
def splitbarsdirectionchangesR(self, barstosplit):
barsplitsdc = []
bsdsq = self.barsplitdelta * self.barsplitdelta
for bar in barstosplit:
if bar.bbardeleted:
continue
if not ((bar.nodeback.pointzone.izone == barmesh.PZ_BEYOND_R) and
(bar.nodefore.pointzone.izone == barmesh.PZ_BEYOND_R)):
continue
vb = bar.nodefore.p - bar.nodeback.p
vbsq = vb.Lensq()
if vbsq < bsdsq:
continue
lamclossplit = -1
dvbsq = -1
if bar.nodeback.pointzone.v is not None:
lampcb = P3.Dot(bar.nodeback.pointzone.v, vb) / vbsq
if 0 < lampcb < 1:
dvb = bar.nodeback.pointzone.v - vb * lampcb
assert abs(P3.Dot(dvb, vb)) < 0.001
dvbsq = dvb.Lensq()
if dvbsq < self.rdsq:
lamclossplit = lampcb
if bar.nodefore.pointzone.v is not None:
lampcf = -P3.Dot(bar.nodefore.pointzone.v, vb) / vbsq
if 0 < lampcf < 1:
dvf = bar.nodefore.pointzone.v + vb * lampcf
assert abs(P3.Dot(dvf, vb)) < 0.001
dvfsq = dvf.Lensq()
if dvfsq < self.rdsq:
if lamclossplit == -1 or dvfsq < dvbsq:
lamclossplit = 1 - lampcf
if lamclossplit != -1:
barsplitsdc.append((bar, lamclossplit))
# split the bars
nowsplitbars = []
splitnodes = []
for bar, lamc in barsplitsdc:
splitnode = self.bm.InsertNodeIntoBarF(
bar,
self.bm.NewNode(Along(lamc, bar.nodeback.p, bar.nodefore.p)),
True)
assert bar.bbardeleted
splitnodes.append(splitnode)
nowsplitbars.append(bar.barforeright)
nowsplitbars.append(bar.barbackleft)
# make the measurements on all the new splitnodes
self.MakePointZoneRFS(splitnodes)
# make the measurements on all the new bars
newbarpolycuts = []
for bar in nowsplitbars:
if IsCutBar(bar, barmesh.PZ_BEYOND_R):
newbarpolycuts.append(bar)
self.CutbarRFS(newbarpolycuts)
#print("splitbarsdirectionchanges", len(barstosplit), len(nowsplitbars))
return nowsplitbars
def cperpbardotN(self, bar, v):
assert self == bar.nodeback or self == bar.nodefore
vb = (bar.nodefore.p - bar.nodeback.p)
vbs = (self == bar.nodeback and +1 or -1)
cperpvb = P3(vb.y * vbs, -vb.x * vbs, 0)
return P3.Dot(cperpvb, v) / vb.Len()