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