def test_rotation_matrix():
sample_count = 10
vec_len_range = 10000
theta_range = pi # if > pi, may cause test fail, but don't worry.
tol = 1e-4
for i in xrange(sample_count):
axis_dir = random_vec(vec_len_range)
vec_to_rotate = random_vec(vec_len_range)
theta = random_float(theta_range)
mat = rotation_array(theta, axis_dir)
rotated_vec = mat.dot(vec_to_rotate)
rotated_vec2 = mat.dot(rotated_vec)
act_theta = intersec_angle(projection_to_plan(vec_to_rotate,
axis_dir),
projection_to_plan(rotated_vec,
axis_dir))
act_theta *= cmp(np.dot(np.cross(vec_to_rotate, rotated_vec),
axis_dir), 0)
feqok_(act_theta, theta, tol)
if theta < pi * tol:
continue
feqok_(vec_len(vec_to_rotate), vec_len(rotated_vec), tol)
(r, center) = circum(vec_to_rotate, rotated_vec, rotated_vec2)
feqok_(vec_len(np.cross(center, axis_dir)), 0, tol
* max(vec_len(axis_dir), vec_len(center)))
ok_(cmp(np.dot(center, axis_dir), 0)
== cmp(np.dot(vec_to_rotate, axis_dir), 0))
def _distance_test(
self,
e,
r,
center,
check_dir,
except_pts,
radius_check_pts,
):
##new code:
t_ale = deque()
(min_dis, emin) = (None, None)
e_mid = (e.start + e.end) * 0.5
e_grd = 0.5 * (self.func_gd(e.start) + self.func_gd(e.end))
e_v = e.end - e.start
for item in self.ale:
if is_circle_cross_segment(center, r, item.end, item.start):
t_ale.append(item)
for item in t_ale:
for pt in (item.start, item.end):
tv = pt - e_mid
if np.cross(e_v, tv).dot(e_grd) < 0 or self.func_gd(pt).dot(e_grd) < 0:
continue
dis = vec_len(tv)
tb = False
for i in except_pts:
if i is pt:
tb = True
break
if tb:
continue
tb = False
for i in radius_check_pts:
if pt is i:
tb = True
break
if not tb:
if (min_dis is None or dis < min_dis):
min_dis = dis
if pt is item.start:
emin = item.pred
else:
emin = item
else:
if vec_len(pt - center) < r and (min_dis is None or dis < min_dis):
min_dis = dis
if pt is item.start:
emin = item.pred
else:
emin = item
return emin
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 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 estimate_direct_search_init(self, center, plane_norm, p_failed, center_gd, p_failed_gd, failed_angle):
# algorithm from:
# (x - center) dot center_gd = 0
# (x - p_failed) dot grd_failed = 0
# (x - center) dot plane_norm = 0
a = np.array((center_gd, p_failed_gd, plane_norm))
b = np.array((np.dot(center, center_gd), np.dot(p_failed, p_failed_gd), np.dot(center, plane_norm)))
pinit = np.linalg.solve(a, b)
if np.cross(pinit - center, center_gd).dot(np.cross(p_failed - center, center_gd)) <= 0:
pinit = None
t = self.k * self._alpha_err / failed_angle
pinit2 = center * (1 - t) + p_failed * t
if pinit is None or vec_len(pinit2 - center) > vec_len(pinit - center):
return pinit2
else:
return pinit
def test_edge_spining():
tol=1e-10
center=array((0,0,1))
r=1.1
samp=sphere_CSTri(center=center,r=r)
v1=array((1,2,3))
v2=array((1.1,2.3,2.9))
v3=array((0.7,2.2,3))
v1=norm_vec(v1)*r
v2=norm_vec(v2)*r
v3=norm_vec(v3)*r
p1=v1+center
p2=v2+center
pold=v3+center
c=0.1
pnew=samp.spining_search(p1,p2,np.cross(p1-p2,pold-p2),c)
feqok_(vec_len(pnew-p1),vec_len(pnew-p2),tol)
feqok_(np.dot(pnew-(p1+p2)/2,p1-p2),0,tol*vec_len(p1-p2))
samp=sphere_CSTri(center=center,r=r,is_hole=True)
pnew=samp.spining_search(p1,p2,np.cross(p1-p2,pold-p2),c)
feqok_(vec_len(pnew-p1),vec_len(pnew-p2),tol)
feqok_(np.dot(pnew-(p1+p2)/2,p1-p2),0,tol*vec_len(p1-p2))
c_fail=r*100
pnew=samp.spining_search(p1,p2,np.cross(p1-p2,pold-p2),c_fail)
def test_circum():
vec_len_range = 10000
sample_count = 1000
tol = 1e-6
for i in xrange(sample_count):
p1 = random_vec(vec_len_range)
p2 = random_vec(vec_len_range)
p3 = random_vec(vec_len_range)
(r, center) = circum(p1, p2, p3)
err = max(vec_len(p1 - p2), vec_len(p2 - p3), vec_len(p3 - p1)) \
* tol
feqok_(vec_len(p1 - center), r, err)
feqok_(vec_len(p2 - center), r, err)
feqok_(vec_len(p3 - center), r, err)
feqok_((p1 - center).dot(np.cross(p2 - center, p3 - center)),
0, tol * vec_len(np.cross(p2 - center, p3 - center))
* vec_len(p1 - center))
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
def sphere_func_gd(center,r,is_hole=False):
if is_hole:
return lambda p:center-p/vec_len(center-p)
else:
return lambda p:p-center/vec_len(p-center)
def sphere_func(center,r,is_hole=False):
if is_hole:
return lambda p:r-vec_len(p-center)
else:
return lambda p:vec_len(p-center)-r
def test_vec_len():
vec = np.array((1, 3, 2))
expected = sqrt(1 + 9 + 4)
result = vec_len(vec)
eq_(result, expected)
def edge_polygon(self):
e = self.ale.pop()
p1 = e.start
p2 = e.end
p_s = (p1 + p2) / 2.0
#grd_s = norm_vec(self.func_gd(p_s))
pold = e.tri_pt
v12 = p2 - p1
grd_old = np.cross(v12, pold - p2)
# not like Cermak2005!!!
c0 = vec_len(grd_old) / vec_len(v12)
vinit = np.cross(v12, grd_old)
radius = self.estimate_spining_radius(p1, p2, vinit, c0)
pnew = self.search_on_plane(p_s, radius, v12, vinit)
v_succ = e.succ.end - e.end
v_pred = e.start - e.pred.start
alpha2 = intersec_angle(v_succ, -v12)
if np.cross(v12, v_succ).dot(grd_old) > 0:
alpha2 = 2 * pi - alpha2
alpha1 = intersec_angle(v12, -v_pred)
if np.cross(v_pred, v12).dot(grd_old) > 0:
alpha1 = 2 * pi - alpha1
alpha = min(alpha1, alpha2)
# MARK: alpha_n_assuming the pnew made by edge_spining is on the plan P:
# P across the mid point of e
# P is perpendicular to e
if pnew is not None:
alpha_n = intersec_angle(pnew - p1, v12)
alpha_shape = alpha_n + self.alpha_shape_min
else:
alpha_shape = self.alpha_shape_big
# neibourhood test
if alpha < alpha_shape:
if e.succ.succ is e.pred:
self.ale.remove(e.succ)
self.ale.remove(e.pred)
self.triangles.append((e.end_index, e.start_index, e.pred.start_index))
return
# situation a) in Fig8 & Fig9
if alpha == alpha1:
case = 1
(r, center) = smallest_triangle_sphere(p1, p2,
e.pred.start)
except_pts = (p1, p2, e.pred.start)
check_dir = p2 - e.pred.start
radius_check_pts = (e.pred.pred.start, e.succ.end)
else:
case = 2
(r, center) = smallest_triangle_sphere(p1, p2,
e.succ.end)
except_pts = (p1, p2, e.succ.end)
check_dir = e.succ.end - p1
radius_check_pts = (e.pred.start, e.succ.succ.end)
elif pnew is None:
r = vec_len(v12) * 0.5
center = (p1 + p2) * 0.5
except_pts = (p1, p2)
check_dir = e
radius_check_pts = (e.pred.start, e.succ.end)
case = 3
else:
(r, center) = smallest_triangle_sphere(p1, p2, pnew)
r = r * self.dist_test_cf
except_pts = (p1, p2)
check_dir = e
radius_check_pts = (e.pred.start, e.succ.end)
case = 4
# filet out a NAEL,distance test
emin = self._distance_test(e, r, center, check_dir, except_pts, radius_check_pts)
if emin is e.pred.pred:
emin = None
case = 1
elif emin is e.succ:
emin = None
case = 2
if emin is None:
if 1 == case:
self.ale.remove(e.pred)
self._add_tri_ale_A(e.pred, e)
elif 2 == case:
self.ale.remove(e.succ)
self._add_tri_ale_A(e, e.succ)
elif 3 == case:
self.ale.appendleft(e)
elif 4 == case:
pnew_index = len(self.points)
self.points.append(pnew)
self.triangles.append((pnew_index, e.end_index,
e.start_index))
e_new1 = _WingedEdge(e.start, pnew, e.start_index,
pnew_index, e.end)
e_new2 = _WingedEdge(pnew, e.end, pnew_index,
e.end_index, e.start)
e_new1.succ = e_new2
e_new1.pred = e.pred
e_new2.pred = e_new1
e_new2.succ = e.succ
e.pred.succ = e_new1
e.succ.pred = e_new2
self.ale.append(e_new1)
self.ale.append(e_new2)
else:
emin1 = emin
emin2 = emin.succ
alpha_m1 = intersec_angle(emin2.end - emin2.start, e.start
- emin2.start)
alpha_m2 = intersec_angle(emin1.start - emin1.end, e.end
- emin1.end)
if alpha_m1 < alpha_m2:
te = _WingedEdge(e.start, emin2.end)
(r, center) = smallest_triangle_sphere(e.start,
emin2.start, emin2.end)
except_pts = (emin2.start, emin2.end, e.start, e.end)
radius_check_pts = (emin2.succ.end, emin1.start, e.pred.start)
else:
te = _WingedEdge(emin1.start, e.end)
(r, center) = smallest_triangle_sphere(e.end,
emin1.start, emin1.end)
except_pts = (emin1.start, emin1.end, e.start, e.end)
radius_check_pts = (emin2.end, emin1.pred.start, e.succ.end)
check_dir = te.end - te.start
t_emin = self._distance_test(te, r, center, check_dir, except_pts, radius_check_pts)
if t_emin is not None:
self.ale.appendleft(e)
return
if alpha_m1 < alpha_m2:
self.ale.remove(emin2)
if emin2.succ is e.pred:
self.ale.remove(e.pred)
else:
enew1 = _WingedEdge(
e.start,
emin2.end,
e.start_index,
emin2.end_index,
emin2.start,
e.pred,
emin2.succ,
)
e.pred.succ = enew1
emin2.succ.pred = enew1
self.ale.append(enew1)
enew2 = _WingedEdge(
emin2.start,
e.end,
emin2.start_index,
e.end_index,
e.start,
emin1,
e.succ,
)
emin1.succ = enew2
e.succ.pred = enew2
self.ale.append(enew2)
self.triangles.append((emin2.end_index,
emin2.start_index, e.start_index))
else:
self.ale.remove(emin1)
if e.succ is emin1.pred:
self.ale.remove(emin1.pred)
else:
enew1 = _WingedEdge(
emin1.start,
e.end,
emin1.start_index,
e.end_index,
emin1.end,
emin1.pred,
e.succ,
)
emin1.pred.succ = enew1
e.succ.pred = enew1
self.ale.append(enew1)
enew2 = _WingedEdge(
e.start,
emin1.end,
e.start_index,
emin1.end_index,
e.end,
e.pred,
emin2,
)
e.pred.succ = enew2
emin2.pred = enew2
self.ale.append(enew2)
self.triangles.append((emin1.end_index,
emin1.start_index, e.end_index))
self.triangles.append((emin2.start_index, e.end_index,
e.start_index))
pass
