def first_triangle(self, p0, c0):
self.ale.clear()
self.triangles.clear()
self.points.clear()
n = norm_vec(self.func_gd(p0))
if n[1] > 0.5 or n[0] > 0.5:
tg_vec = np.array((n[1], -n[0], 0))
else:
tg_vec = np.array((-n[2], 0, n[0]))
tg_vec = norm_vec(tg_vec)
vinit = np.cross(tg_vec, n)
radius = self.estimate_spining_radius(p0 - tg_vec * c0 * 0.5, p0 + tg_vec * c0 * 0.5, vinit, c0)
p1 = self.search_on_plane(p0, radius, tg_vec, vinit)
plane_norm = p1 - p0
vinit = np.cross(plane_norm, n)
radius = self.estimate_spining_radius(p0, p1, vinit, vec_len(plane_norm))
p2 = self.search_on_plane((p0 + p1) * 0.5, radius, plane_norm, vinit)
self.points.extend((p0, p1, p2))
self.triangles.append((0, 2, 1))
e0 = _WingedEdge(p0, p1, 0, 1, p2)
e1 = _WingedEdge(p1, p2, 1, 2, p0)
e2 = _WingedEdge(p2, p0, 2, 0, p1)
e0.pred = e2
e0.succ = e1
e1.pred = e0
e1.succ = e2
e2.pred = e1
e2.succ = e0
self.ale.extend((e0, e1, e2))
def test_norm_vec():
while True:
vec = random_vec(100)
if vec_len(vec) != 1:
break
normed_vec = norm_vec(vec)
feqok_(np.dot(normed_vec, normed_vec), 1, 1e-6)
def _search_root_on_plane(self, p_init, plane_norm, normed=True):
# MARK: alpha_n_assuming see also self.edge_polygon
# in Cermak2005 it is not directly presented
# that grd should in the circle plan,
# but the Neighborhood test in S7.1 assumed that.
if not normed:
plane_norm = norm_vec(plane_norm)
grd = self.func_gd(p_init)
grd = projection_to_plan(grd, plane_norm)
func_val = self.func(p_init)
delta = self.line_search_dlt * -cmp(func_val, 0)
delta = delta * grd
pnew2 = p_init + delta
func_val2 = self.func(pnew2)
pnew = p_init
while func_val * func_val2 > 0:
pnew = pnew2
pnew2 = pnew + delta
func_val = func_val2
func_val2 = self.func(pnew2)
rslt = brentq(lambda t: self.func(pnew * (1 - t) + pnew2 * t),
0, 1)
pnew = pnew * (1 - rslt) + pnew2 * rslt
return pnew
def check_alpha_err(self, grd, pnew, normed=True):
if normed:
grdnew = norm_vec(self.func_gd(pnew))
if intersec_angle(grdnew, grd, True) < self._alpha_err:
return True
else:
grdnew = self.func_gd(pnew)
if intersec_angle(grdnew, grd) < self._alpha_err:
return True
return False
def spining_search(
self,
center,
radius,
plane_norm,
vinit
):
""" edge spinning method
p1->p2 is the activate edge,
grd_old is the triangle normal vector, no need with length 1
c the circle radius for estimation spining circle radius"""
# MARK: alpha_n_assuming see self.edge_polygon
# Estimation of the circle radius(S6.1), radius
# edge spin root finding (S6.2)
# determine init point pnew, init search direction of rotation
vinit = norm_vec(vinit)
vnew = vinit * radius
pnew = center + vnew
rot_mat = rotation_array(self.dlt_alpha, plane_norm)
vnew2 = rot_mat.dot(vnew)
pnew2 = vnew2 + center
func_val2 = self.func(pnew2)
func_val = self.func(pnew)
delta = self.dlt_alpha
if abs(func_val2) > abs(func_val) and func_val2 * func_val > 0 :
delta = -delta
rot_mat = rotation_array(-self.dlt_alpha, plane_norm)
vnew2 = rot_mat.dot(vnew)
pnew2 = vnew2 + center
func_val2 = self.func(pnew2)
alpha = delta
# find the sign different interval
while func_val * func_val2 > 0:
func_val = func_val2
vnew2 = rot_mat.dot(vnew2)
pnew = pnew2
pnew2 = vnew2 + center
func_val2 = self.func(pnew2)
alpha += delta
if abs(alpha) > self.alpha_lim:
return None
# interval search( not bisection search!!!
# that's different to Cermak2005)
rslt = brentq(lambda t: self.func(pnew * (1 - t) + pnew2 * t),
0, 1)
pnew = pnew * (1 - rslt) + pnew2 * rslt
return pnew
def estimate_spining_radius(self, p2, p1, vinit, c0):
p_s = (p1 + p2) / 2.0
vinit = norm_vec(vinit)
pinit = p_s + vinit * c0
grdinit = norm_vec(self.func_gd(pinit))
rcmin = None
for item in (p1, p2, p_s):
item_gd = norm_vec(self.func_gd(item))
t = rb_acos(item_gd.dot(grdinit))
if t == 0:
rc = self.rmax
else:
rc = vec_len(item - pinit) / t
if None is rcmin or rc < rcmin:
rc_min = rc
rc = rc_min
r2 = rc * self._surf_curv_coef
if r2 < self.rmin:
r2 = self.rmin
elif r2 > self.rmax:
r2 = self.rmax
return r2
