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