def __test_normalize(): import vtk ref_point = PointWithRef() point2 = PointWithRef() for p in (ref_point, point2): p.point_pos = np.random.random(3) * 15 / 100 p.ref_pos = np.random.random(3) * 15 / 100 p.point_or = tuple(Quaternion.from_v_theta(np.random.random(3), np.random.random()).x) p.ref_or = tuple(Quaternion.from_v_theta(np.random.random(3), np.random.random()).x) ren_win = vtk.vtkRenderWindow() iren = vtk.vtkRenderWindowInteractor() iren.SetInteractorStyle(vtk.vtkInteractorStyleTrackballCamera()) iren.SetRenderWindow(ren_win) ren = vtk.vtkRenderer() ren_win.AddRenderer(ren) # draw ref __draw_point_and_uni(ref_point, (1, 1, 1), ren) # draw original point __draw_point_and_uni(point2, (0, 1, 0), ren) #normalize point2_n = normalize_point_to_ref(point2, ref_point) __draw_point_and_uni(point2_n, (1, 0, 0), ren) iren.Initialize() iren.Start()
def rotate_vector(v, q): if not isinstance(q, Quaternion): q = Quaternion(q) v_q = Quaternion([0, v[0], v[1], v[2]]) v_2_q = q * v_q * q.conj() v2 = v_2_q.x[1:] return v2
def normalize_point_to_ref(point, ref): out_point = point.copy() ref_pos = np.array(ref.ref_pos) ref_or = np.array(ref.ref_or) point_uni_pos = np.array(point.ref_pos) point_uni_or = np.array(point.ref_or) point_pos = np.array(point.point_pos) point_or = np.array(point.point_or) # v1 is the vector between the unicorn and the position v1 = point_pos - point_uni_pos # normalize position pos_delta = ref_pos - point_uni_pos # normalize orientation ref_q = Quaternion(ref_or) point_uni_q = Quaternion(point_uni_or) point_q = Quaternion(point_or) v1_q = Quaternion([0, v1[0], v1[1], v1[2]]) delta_q = ref_q * point_uni_q.inv() #left multiply everything by delta_q new_point_uni_q = delta_q * point_uni_q new_point_q = delta_q * point_q new_v1_q = delta_q * v1_q * delta_q.conj() new_v1 = new_v1_q.x[1:] #save results out_point.ref_pos = tuple(point_uni_pos + pos_delta) out_point.point_pos = tuple(out_point.ref_pos + new_v1) out_point.ref_or = new_point_uni_q.x out_point.point_or = new_point_q.x return out_point