def update(self, p1x, p1y, p2x, p2y, width, height, mat = 0):
'''Update the quaterion with a new rotation position derived
from the first point (p1) and the second point (p2). The
the mat parameter is not currently used.'''
if p1x == p2x and p1y == p2y:
self.quat = quaternion(1, 0, 0, 0)
else:
# First, figure out z-coordinates for projection of p1 and p2 to
# deformed sphere
p1x_u = self.scale*p1x/width - 1.0
p1y_u = 1.0 - self.scale*p1y/height
p2x_u = self.scale*p2x/width - 1.0
p2y_u = 1.0 - self.scale*p2y/height
P1 = (p1x_u,p1y_u,self.__track_project_to_sphere(p1x_u, p1y_u))
P2 = (p2x_u,p2y_u,self.__track_project_to_sphere(p2x_u, p2y_u))
a = [(P2[1]*P1[2]) - (P2[2]*P1[1]),
(P2[2]*P1[0]) - (P2[0]*P1[2]),
(P2[0]*P1[1]) - (P2[1]*P1[0])]
# Figure out how much to rotate around that axis.
d = map(lambda x, y: x - y, P1, P2)
t = math.sqrt(d[0]**2 + d[1]**2 + d[2]**2) / (2.0 * self.size)
# Avoid problems with out-of-control values...
t = max(min(t, 1.0), -1.0)
scale = t*math.sqrt(a[0]**2 + a[1]**2 + a[2]**2)
q = map(lambda x, y: x*y, a, [scale]*3) + [math.sqrt(1.0-t**2)]
self.quat = quaternion([q[3], q[0], q[1], q[2]]).normalized()
def __init__(self, game_object):
self.game_object = game_object
self.local_position = vector3()
self.local_rotation = quaternion(1, 0, 0, 0)
self.local_scale = vector3.one()
self._parent = None
self.children = []
def __init__(self, name):
self.name = name
self.position = vector3()
self.rotation = quaternion(1, 0, 0, 0)
self.scale = vector3(1, 1, 1)
self.mesh = None
self.material = None
self.children = []
def get_rotation(self):
if (self._parent):
return self.local_rotation * self._parent.rotation
else:
q = quaternion()
local = self.local_rotation
q.x = local.x
q.y = local.y
q.z = local.z
q.w = local.w
return q
def test_rotate_point():
# A represents a point, the real part is zero
A_point = quaternion(0, 0, 1, 0)
# the angle
rot_vec = rotation_vector(1, 0, 0, np.pi / 2)
rot_quat = rot_vec.toQuat()
#print(rot_vec, rot_quat)
#after the rotation, the object should be
result = rot_quat * A_point * rot_quat.inv()
print(result)
def from_vector_and_angle(cls, v, theta_deg):
theta = radians(theta_deg)
q = quaternion(v.x * sin(theta / 2), v.y * sin(theta / 2),
v.z * sin(theta / 2), cos(theta / 2))
return q
def __init__(self): self.orientation = quaternion() self.rotation = quaternion()
from quaternion import * from math import * q1 = quaternion() q1.set_xyz_alpha(0.0, 1.0, 0.0, math.pi/2.0) q1.normalize() print "q1: " + str(q1) + " magnetude=" + str(q1.get_magnetude()) q2 = quaternion() q2.set_xyz_alpha(0.0, -1.0, 0.0, math.pi/2.0) print "q2: " + str(q2) + " magnetude=" + str(q2.get_magnetude()) q3 = (q1 * 0.25) + (q2 * 0.75) q3.normalize() print "q3: " + str(q3) + " magnetude=" + str(q3.get_magnetude()) q4 = quaternion() q4.set_rotations(math.pi, math.pi/2, 0.0) print "q4: " + str(q4) + " magnetude=" + str(q4.get_magnetude())
def reset(self):
self.quat = quaternion([1, 0, 0, 0])
def test_rotation_matrix():
A = quaternion(1, 0, 0, 0)
rot_vec = rotation_vector(100, 0, 0, np.pi / 2 * 100)
rot_quat = rot_vec.toQuat()
print(rot_quat.normalize().matrix())
def test_add():
A = quaternion(1, 0, 0, 0)
print(A)
B = quaternion(1, 0, 0, 0)
C = A * B
print(C)
def test_normalize():
A = quaternion(100.0, 0, 0, 0)
B = A.normalize()
print("B ", B, " A", A)
def test_inv():
A = quaternion(1.0, 0, 0, 0)
norm = A * A.inv()
print(norm)
def test_div():
A = quaternion(1, 0, 0, 0)
A = A / 4
print(A)
mag_y[i] = measurement(float(row["Mag_y"]), 14.92) mag_z[i] = measurement(float(row["Mag_z"]), 5.91) start[i] = start_sec[i] + (start_nsec[i] * math.pow(10, -9)) end[i] = end_sec[i] + (end_nsec[i] * math.pow(10, -9)) #Time if i > 0: dtime[i] = ((start[i] - start[i-1]) + (end[i] - end[i - 1]) / 2) else: dtime[i] = 1 #Calculate angles #Init current orientation measurement orientation_quat = quaternion() orientation_quat.set_rotations(mag_x[i].get_mean(), mag_y[i].get_mean(), mag_z[i].get_mean()) orientation[i] = measurement(orientation_quat, 1) #Calculate current rotation rot_x_deg_meas[i] = (rot_x[i] - rot_x_avg) * rot2deg rot_y_deg_meas[i] = (rot_x[i] - rot_x_avg) * rot2deg rot_z_deg_meas[i] = (rot_x[i] - rot_x_avg) * rot2deg rot_quat = quaternion() rot_quat.set_rotations(rot_x[i].mean(), rot_y[i].mean(), rot_z[i].mean()) rotation = measurement(rot_quat, 0.005) #DO NOT REMOVE
