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
