def rotate(self, q):
q1 = np.quaternion(q[0], q[1], q[2], q[3])
q2 = np.conjugate(q1)
v = np.quaternion(0, self.x, self.z, self.y)
rot = (q1 * v) * q2
return Nodes(rot.x, rot.z, rot.y)
def project(self, win_width, win_height, fov, viewer_distance):
transformation = [120, 220]
factor = fov / (viewer_distance + self.z)
x = self.x * factor + win_width / 1.6
y = -self.y * factor + win_height / 1.6
return Nodes(x - transformation[0], y - transformation[1], self.z)
if __name__ == '__main__':
i = 200
get_data = gdata()
print('Intro')
while (i > 0):
getmag = next(get_data)
fuse.calibrate(getmag)
i -= 1
print('Cal done. Magnetometer bias vector:', fuse.magbias, fuse.scale)
get_centre = init_quat()
centre = next(get_centre)
fuse.set_centre(centre)
print("Centre initialised")
main()
