def __apply_transformation_vec4(self, a_point):
'''
Applies the transformation to a given Verctor4.
:param a_point: Vector4
'''
return (Matrix44.from_quaternion(self.__rotation_quaternion).T *
a_point) + self.__translation
def __init__(self, data):
self.name = data.get("name")
self.children = data.get("children")
self.matrix = data.get("matrix")
self.mesh = data.get("mesh")
self.camera = data.get("camera")
self.translation = data.get("translation")
self.rotation = data.get("rotation")
self.scale = data.get("scale")
if self.matrix:
self.matrix = Matrix44(self.matrix)
else:
self.matrix = Matrix44.identity()
if self.translation is not None:
self.matrix = self.matrix * Matrix44.from_translation(
self.translation)
if self.rotation is not None:
quat = quaternion.create(
x=self.rotation[0],
y=self.rotation[1],
z=self.rotation[2],
w=self.rotation[3],
)
self.matrix = self.matrix * Matrix44.from_quaternion(
quat).transpose()
if self.scale is not None:
self.matrix = self.matrix * Matrix44.from_scale(self.scale)
def __init__(self, data):
self.name = data.get('name')
self.children = data.get('children')
self.matrix = data.get('matrix')
self.mesh = data.get('mesh')
self.camera = data.get('camera')
self.translation = data.get('translation')
self.rotation = data.get('rotation')
self.scale = data.get('scale')
if self.matrix:
self.matrix = Matrix44(self.matrix)
else:
self.matrix = Matrix44.identity()
if self.translation is not None:
self.matrix = self.matrix * Matrix44.from_translation(self.translation)
if self.rotation is not None:
quat = quaternion.create(
x=self.rotation[0],
y=self.rotation[1],
z=self.rotation[2],
w=self.rotation[3],
)
self.matrix = self.matrix * Matrix44.from_quaternion(quat).transpose()
if self.scale is not None:
self.matrix = self.matrix * Matrix44.from_scale(self.scale)
def rotate(self, w, i, j, k):
ct = quaternion.create(i, j, k, w, dtype="float32")
rot_y = Matrix44.from_quaternion(ct).flatten().astype("float32")
c_rotate = numpy.ctypeslib.as_ctypes(rot_y)
glUniformMatrix4fv(self.rotate_loc, 1, GL_FALSE, c_rotate)
glUniformMatrix4fv(self.light_loc, 1, GL_FALSE, c_rotate)
def write_bl(self, pose, camera, m_shadow_bias, lightDir):
x, y, z, w, rx, ry, rz, *_ = pose
# y = -y
ry = -ry
rx = -rx
rz = -rz
rotz = Matrix44.from_quaternion([rx, ry, rz, w], dtype="f4")
mat1 = Matrix44.from_translation([x * 10, y * 10, z * 10], dtype="f4")
self.basic_light_prog["m_model"].write(mat1 * rotz)
self.basic_light_prog["m_proj"].write(camera.projection.matrix)
self.basic_light_prog["m_camera"].write(camera.matrix)
self.basic_light_prog["m_shadow_bias"].write(m_shadow_bias)
self.basic_light_prog["lightDir"].write(lightDir)
def update_transform_matrix(self):
scale_matrix = Matrix44.from_scale(self.scale)
translation_matrix = Matrix44.from_translation(self.location)
# TODO: For some reason the qx, qy, qz part of the quaternion must be flipped
# Need to understand why and fix it
# The change need to be made together with the coordinate conversion in NDDS
# rotation_matrix = Matrix44.from_quaternion(self.quaternion)
qx, qy, qz, qw = self.quaternion
test_quaternion = Quaternion([-qx, -qy, -qz, qw])
rotation_matrix = Matrix44.from_quaternion(test_quaternion)
# print('update_transform_matrix: rotation_matrix = {}'.format(rotation_matrix))
relative_matrix = (translation_matrix * scale_matrix * rotation_matrix)
# self.transform_matrix = relative_matrix * self.initial_matrix
self.transform_matrix = relative_matrix
def test_m44_q_equivalence(self):
"""Test for equivalance of matrix and quaternion rotations.
Create a matrix and quaternion, rotate each by the same values
then convert matrix<->quaternion and check the results are the same.
"""
m = Matrix44.from_x_rotation(np.pi / 2.)
mq = Quaternion.from_matrix(m)
q = Quaternion.from_x_rotation(np.pi / 2.)
qm = Matrix44.from_quaternion(q)
self.assertTrue(np.allclose(np.dot([1., 0., 0., 1.], m), [1., 0., 0., 1.]))
self.assertTrue(np.allclose(np.dot([1., 0., 0., 1.], qm), [1., 0., 0., 1.]))
self.assertTrue(np.allclose(q * Vector4([1., 0., 0., 1.]), [1., 0., 0., 1.]))
self.assertTrue(np.allclose(mq * Vector4([1., 0., 0., 1.]), [1., 0., 0., 1.]))
np.testing.assert_almost_equal(np.array(q), np.array(mq), decimal=5)
np.testing.assert_almost_equal(np.array(m), np.array(qm), decimal=5)
def test_m44_q_equivalence(self):
"""Test for equivalance of matrix and quaternion rotations.
Create a matrix and quaternion, rotate each by the same values
then convert matrix<->quaternion and check the results are the same.
"""
m = Matrix44.from_x_rotation(np.pi / 2.)
mq = Quaternion.from_matrix(m)
q = Quaternion.from_x_rotation(np.pi / 2.)
qm = Matrix44.from_quaternion(q)
self.assertTrue(
np.allclose(np.dot([1., 0., 0., 1.], m), [1., 0., 0., 1.]))
self.assertTrue(
np.allclose(np.dot([1., 0., 0., 1.], qm), [1., 0., 0., 1.]))
self.assertTrue(
np.allclose(q * Vector4([1., 0., 0., 1.]), [1., 0., 0., 1.]))
self.assertTrue(
np.allclose(mq * Vector4([1., 0., 0., 1.]), [1., 0., 0., 1.]))
np.testing.assert_almost_equal(q, mq, decimal=5)
np.testing.assert_almost_equal(m, qm, decimal=5)
def render(self, time, frametime):
self.ctx.enable_only(moderngl.DEPTH_TEST | moderngl.CULL_FACE)
scene_pos = Vector3((0, -5, -32), dtype='f4')
data = myDriver.get_pose()
hmd = data[:13]
cntl1 = data[13:13+22]
cntl2 = data[13+22:13+22+22]
# --- PASS 1: Render shadow map
self.offscreen.clear()
self.offscreen.use()
depth_projection = Matrix44.orthogonal_projection(-20, 20, -20, 20, -20, 40, dtype='f4')
depth_view = Matrix44.look_at(self.lightpos, (0, 0, 0), (0, 1, 0), dtype='f4')
depth_mvp = depth_projection * depth_view
self.shadowmap_program['mvp'].write(depth_mvp)
self.handL.render(self.shadowmap_program)
self.handL2.render(self.shadowmap_program)
self.handL3.render(self.shadowmap_program)
self.handR.render(self.shadowmap_program)
self.handR2.render(self.shadowmap_program)
self.handR3.render(self.shadowmap_program)
self.hmd.render(self.shadowmap_program)
self.hmd2.render(self.shadowmap_program)
self.floor.render(self.shadowmap_program)
# --- PASS 2: Render scene to screen
self.wnd.use()
# self.basic_light['m_proj'].write(self.camera.projection.matrix)
# self.basic_light['m_camera'].write(self.camera.matrix)
# self.basic_light['m_model'].write(Matrix44.from_translation(scene_pos, dtype='f4') * Matrix44.from_y_rotation(time, dtype='f4'))
bias_matrix = Matrix44(
[[0.5, 0.0, 0.0, 0.0],
[0.0, 0.5, 0.0, 0.0],
[0.0, 0.0, 0.5, 0.0],
[0.5, 0.5, 0.5, 1.0]],
dtype='f4',
)
# self.basic_light['m_shadow_bias'].write(matrix44.multiply(depth_mvp, bias_matrix))
# self.basic_light['lightDir'].write(self.lightpos)
self.offscreen_depth.use(location=0)
x, y, z, w, rx, ry, rz = cntl2[:7]
# y = -y
ry = -ry
rx = -rx
rz = -rz
rotz = Matrix44.from_quaternion([rx, ry, rz, w], dtype='f4')
mat1 = Matrix44.from_translation([x*10, y*10, z*10], dtype='f4')
self.basic_lightL['m_model'].write(mat1*rotz)
self.basic_lightL['m_proj'].write(self.camera.projection.matrix)
self.basic_lightL['m_camera'].write(self.camera.matrix)
self.basic_lightL['m_shadow_bias'].write(matrix44.multiply(depth_mvp, bias_matrix))
self.basic_lightL['lightDir'].write(self.lightpos)
x, y, z, w, rx, ry, rz = cntl1[:7]
# y = -y
ry = -ry
rx = -rx
rz = -rz
rotz = Matrix44.from_quaternion([rx, ry, rz, w], dtype='f4')
mat1 = Matrix44.from_translation([x*10, y*10, z*10], dtype='f4')
self.basic_lightR['m_model'].write(mat1*rotz)
self.basic_lightR['m_proj'].write(self.camera.projection.matrix)
self.basic_lightR['m_camera'].write(self.camera.matrix)
self.basic_lightR['m_shadow_bias'].write(matrix44.multiply(depth_mvp, bias_matrix))
self.basic_lightR['lightDir'].write(self.lightpos)
x, y, z, w, rx, ry, rz = hmd[:7]
# y = -y
ry = -ry
rx = -rx
rz = -rz
rotz = Matrix44.from_quaternion([rx, ry, rz, w], dtype='f4')
mat1 = Matrix44.from_translation([x*10, y*10, z*10], dtype='f4')
self.basic_lightHmd['m_model'].write(mat1*rotz)
self.basic_lightHmd['m_proj'].write(self.camera.projection.matrix)
self.basic_lightHmd['m_camera'].write(self.camera.matrix)
self.basic_lightHmd['m_shadow_bias'].write(matrix44.multiply(depth_mvp, bias_matrix))
self.basic_lightHmd['lightDir'].write(self.lightpos)
self.handL.render(self.basic_lightL)
self.handL2.render(self.basic_lightL)
self.handL3.render(self.basic_lightL)
self.handR.render(self.basic_lightR)
self.handR2.render(self.basic_lightR)
self.handR3.render(self.basic_lightR)
self.hmd.render(self.basic_lightHmd)
self.hmd2.render(self.basic_lightHmd)
self.basic_lightFloor['m_model'].write(Matrix44.from_translation((0, 0, 0), dtype='f4'))
self.basic_lightFloor['m_proj'].write(self.camera.projection.matrix)
self.basic_lightFloor['m_camera'].write(self.camera.matrix)
self.basic_lightFloor['m_shadow_bias'].write(matrix44.multiply(depth_mvp, bias_matrix))
self.basic_lightFloor['lightDir'].write(self.lightpos)
self.floor.render(self.basic_lightFloor)
# # Render the sun position
# self.sun_prog['m_proj'].write(self.camera.projection.matrix)
# self.sun_prog['m_camera'].write(self.camera.matrix)
# self.sun_prog['m_model'].write(Matrix44.from_translation(self.lightpos + scene_pos, dtype='f4'))
# self.sun.render(self.sun_prog)
# --- PASS 3: Debug ---
# self.ctx.enable_only(moderngl.NOTHING)
self.offscreen_depth.use(location=0)
self.offscreen_quad.render(self.raw_depth_prog)
def __get_rotation_matrix(self):
return Matrix44.from_quaternion(self.__rotation_quaternion)
with mydriver:
mydriver.send('hello')
while 1:
fbo.clear(0.0, 0.0, 0.0, 0.0)
data = mydriver.get_pose()
hmd = data[:13]
cntl1 = data[13:13 + 22]
cntl2 = data[13 + 22:13 + 22 + 22]
x, y, z, w, rx, ry, rz = hmd[:7]
y = -y
ry = -ry
rotz = Matrix44.from_quaternion([rx, ry, rz, w],
dtype='f4') * modelRotOffz
mat1 = Matrix44.from_translation([x, y, z], dtype='f4') * viewPosOffz
prog['transformMat'].write(mat1 * rotz)
x, y, z, w, rx, ry, rz = cntl1[:7]
y = -y
ry = -ry
rotz = Matrix44.from_quaternion([rx, ry, rz, w],
dtype='f4') * modelRotOffz
mat1 = Matrix44.from_translation([x, y, z], dtype='f4') * viewPosOffz
prog2['transformMat'].write(mat1 * rotz)
x, y, z, w, rx, ry, rz = cntl2[:7]
