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 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))
