def look_at(self, position, target, world_up):
# 1.Position = known
# 2.Calculate cameraDirection
zaxis = vector.normalise(position - target)
# 3.Get positive right axis vector
xaxis = vector.normalise(
vector3.cross(vector.normalise(world_up), zaxis))
# 4.Calculate the camera up vector
yaxis = vector3.cross(zaxis, xaxis)
# create translation and rotation matrix
translation = glm.mat4x4(1.0)
translation[3][0] = -position.x
translation[3][1] = -position.y
translation[3][2] = -position.z
rotation = glm.mat4x4(1.0)
rotation[0][0] = xaxis[0]
rotation[1][0] = xaxis[1]
rotation[2][0] = xaxis[2]
rotation[0][1] = yaxis[0]
rotation[1][1] = yaxis[1]
rotation[2][1] = yaxis[2]
rotation[0][2] = zaxis[0]
rotation[1][2] = zaxis[1]
rotation[2][2] = zaxis[2]
return translation * rotation
def process(self):
# build view matrix
position = self.world.component_for_entity(self.world.camera_id, com.Transformation).position
orientation = self.world.component_for_entity(self.world.camera_id, com.CameraOrientation)
forward = glm.normalize(position - orientation.look_at)
right = glm.normalize(glm.cross(orientation.up, forward))
up = glm.normalize(glm.cross(forward, right))
mat = glm.mat4x4(1.0)
mat[0][0] = right.x
mat[1][0] = right.y
mat[2][0] = right.z
mat[0][1] = up.x
mat[1][1] = up.y
mat[2][1] = up.z
mat[0][2] = forward.x
mat[1][2] = forward.y
mat[2][2] = forward.z
mat[3][0] = -(glm.dot(right, position))
mat[3][1] = -(glm.dot(up, position))
mat[3][2] = -(glm.dot(forward, position))
self.world.view_matrix = mat
# Upload shader data
self.world.standard_shader.start()
self.world.standard_shader.set_view_matrix(self.world.view_matrix)
self.world.standard_shader.load_light_setup(self.world.light_setup)
def build_transformation_matrix(position, rotation, scale):
mat = glm.mat4x4(1.0)
mat = glm.translate(mat, position)
mat = glm.rotate(mat, rotation.z, glm.vec3(1, 0, 0))
mat = glm.rotate(mat, rotation.y, glm.vec3(0, 1, 0))
mat = glm.rotate(mat, rotation.x, glm.vec3(0, 0, 1))
mat = glm.scale(mat, scale)
return mat
def drawAABB(self):
# Set the polygon mode to lines so we can see the actual object inside the AABB
gl.glPolygonMode(gl.GL_FRONT_AND_BACK, gl.GL_LINE)
# Set the vao
gl.glBindVertexArray(primitive_objs["cube"].vao)
# Set material uniforms
gl.glUniform3fv(ambient_color_location, 1,
glm.value_ptr(self.obj_data.meshes[0].material.Ka))
gl.glUniform3fv(diffuse_color_location, 1,
glm.value_ptr(self.obj_data.meshes[0].material.Kd))
gl.glUniform3fv(specular_color_location, 1,
glm.value_ptr(self.obj_data.meshes[0].material.Ks))
gl.glUniform1fv(shininess_location, 1,
self.obj_data.meshes[0].material.Ns)
# Set material textures
gl.glActiveTexture(gl.GL_TEXTURE0)
gl.glBindTexture(gl.GL_TEXTURE_2D,
self.obj_data.meshes[0].material.map_Ka)
gl.glActiveTexture(gl.GL_TEXTURE1)
gl.glBindTexture(gl.GL_TEXTURE_2D,
self.obj_data.meshes[0].material.map_Kd)
AABB = self.get_AABB()
center = glm.vec3((AABB[0][0] + AABB[0][1]) / 2.0,
(AABB[1][0] + AABB[1][1]) / 2.0,
(AABB[2][0] + AABB[2][1]) / 2.0)
scale = glm.vec3((AABB[0][1] - AABB[0][0]) / 2.0,
(AABB[1][1] - AABB[1][0]) / 2.0,
(AABB[2][1] - AABB[2][0]) / 2.0)
AABB_transformation = glm.mat4x4()
AABB_transformation = glm.translate(AABB_transformation, center)
AABB_transformation = glm.scale(AABB_transformation, scale)
# Set the transform uniform to self.transform
gl.glUniformMatrix4fv(transformation_location, 1, False,
glm.value_ptr(AABB_transformation))
# Draw the mesh with an element buffer.
gl.glBindBuffer(gl.GL_ELEMENT_ARRAY_BUFFER,
primitive_objs["cube"].meshes[0].element_array_buffer)
# When the last parameter in 'None', the buffer bound to the GL_ELEMENT_ARRAY_BUFFER will be used.
gl.glDrawElements(gl.GL_TRIANGLES,
primitive_objs["cube"].meshes[0].element_count,
gl.GL_UNSIGNED_INT, None)
# Set the state to its initial
gl.glPolygonMode(gl.GL_FRONT_AND_BACK, gl.GL_FILL)
def get_matrix(self):
if self.__transformations_changed:
transformation = glm.mat4x4()
transformation = glm.translate(transformation, self.position)
transformation = transformation * glm.mat4_cast(self.rotation)
transformation = glm.scale(transformation, self.scale)
self.__transformation_matrix = transformation
self.__transformations_changed = False
if self.parent is None:
return self.__transformation_matrix
else:
return self.parent.get_matrix() * self.__transformation_matrix
def getLocalXYZFromAngles(self, angles, len1, len2, joint2Offs):
#state_start = time.time()
resultTm = glm.mat4x4()
resultTm = glm.rotate(resultTm, angles[0], glm.vec3([1.0, 0.0, 0.0]))
resultTm = glm.translate(resultTm, joint2Offs)
resultTm = glm.rotate(resultTm, angles[1], glm.vec3([0.0, 1.0, 0.0]))
resultTm = glm.translate(resultTm, len1)
resultTm = glm.rotate(resultTm, angles[2], glm.vec3([0.0, 1.0, 0.0]))
resultTm = glm.translate(resultTm, len2)
point = glm.vec3(glm.column(resultTm, 3))
#print("tm",(time.time()-state_start)*1000.0)
return point
'''
def process(self):
for _id, (mat_target, transformation) in self.world.get_components(
com.TransformationMatrix,
com.Transformation):
mat = glm.mat4x4(1.0)
mat = glm.translate(mat, transformation.position)
# No rotation for you
# mat = glm.rotate(mat, transformation.rotation.z, glm.vec3(1, 0, 0))
mat = glm.rotate(mat, transformation.rotation.y, glm.vec3(1, 0, 0))
mat = glm.rotate(mat, transformation.rotation.x, glm.vec3(0, 0, 1))
mat = glm.scale(mat, transformation.scale)
mat_target.value = mat
def render_view(cur_camera: data.Camera, cur_camera_data: data.CameraData):
camera.position = cur_camera_data.position
tmp = tuple([- i for i in cur_camera_data.rotation])
# tmp = tuple(cur_camera_data.rotation)
view = cv2.Rodrigues(tmp)[0]
# camera.yaw = cur_camera.camera_data.yaw - 90
# camera.pitch = cur_camera.camera_data.pitch
view = glm.mat4x4(*view[0], 0, *view[1], 0, *view[2], 0, -camera.position[0], -camera.position[1], -camera.position[2], 1)
# print(view)
# render_background_image(cur_camera.bg.Model, window_width=cur_camera.bg.window_size[0],
# window_height=cur_camera.bg.window_size[1], camera_view=view)
render_background_image(cur_camera.bg.Model, camera_view=view, fx=cur_camera.bg.window_size[0])
# render_side_view(left_camera_intrinsic, left_camera_extrinsic, window.window_width / 2, window.window_height,
# stereo_left_index)
if cur_camera.bg.flag:
cur_camera.bg.Model.change_mesh(cur_camera.bg.path)
cur_camera.bg.flag = False
def display():
if mosquito_Object.transform.position.x < -3:
mosquito_Object.transform.position.x += 0.1
camera.transform.position.x += 0.1
mosquito_Object.velocity = 0
camera.velocity = 0
elif mosquito_Object.transform.position.x > 3:
mosquito_Object.transform.position.x -= 0.1
camera.transform.position.x -= 0.1
mosquito_Object.velocity = 0
camera.velocity = 0
if mosquito_Object.transform.position.y < 0:
mosquito_Object.transform.position.y += 0.1
camera.transform.position.y += 0.1
mosquito_Object.velocity = 0
camera.velocity = 0
elif mosquito_Object.transform.position.y > 4:
mosquito_Object.transform.position.y -= 0.1
camera.transform.position.y -= 0.1
mosquito_Object.velocity = 0
camera.velocity = 0
# Calculate time between frames
global time_passed, timerForColor, gameFinished
delta_time = time.perf_counter() - time_passed
time_passed += delta_time
# Update all the AABBs
for collider in GameObject.WithAABB:
collider.AABB.update(collider.transform)
global fork_target_positions
global last_thrown, fork_throw_cooldown, fork_target_position
if time_passed >= last_thrown + fork_throw_cooldown and mosquito_Object.started:
for i in range(3):
fork_target_positions.append(throw_fork(i))
last_thrown = time_passed
# Do the collision checking
global last_hit_at, lives, boosts
if time_passed >= last_hit_at + hit_recovery_time:
for collision_tester in set(collision_testers):
if mosquito_Object.AABB.check_collision(collision_tester.AABB):
last_hit_at = time_passed
lives -= 1
if not heart_remove_order.empty():
heart_to_remove = heart_remove_order.get()
heart_to_remove.leave_set(hearts)
global color_r, color_g, color_b, color_r_Instant
color_r += random.randint(-1, 2)
color_g += random.randint(-1, 2)
color_b += random.randint(-1, 2)
if color_r > 255:
color_r %= 255
if color_g > 255:
color_g %= 255
if color_b > 255:
color_b %= 255
if time_passed <= last_hit_at + timerForColor and not gameFinished:
color_r_Instant += 127
if color_r_Instant > 255:
color_r_Instant %= 255
mosquito_Object.obj_data.meshes[0].material.Kd = glm.vec3(
float(color_r / 255), 0.0, 0.0)
else:
mosquito_Object.obj_data.meshes[0].material.Kd = glm.vec3(
0.0, 0.0, 0.0)
color_r_Instant = 0
global fork_angle
if len(fork_objects) > 0 and not gameFinished:
for fork_obj, fork_target in zip(fork_objects, fork_target_positions):
fork_obj.transform.position += (fork_target * delta_time * 2)
fork_obj.transform.rotation = glm.quat(
glm.vec3(glm.radians(fork_angle), 0.0, 0.0))
fork_angle += delta_time * 90
# print(moving_obstacle_object.transform.position.x > mosquito_Object.transform.position.x)
if mosquito_Object.started:
if obstacle_objects[
0].transform.position.x > mosquito_Object.transform.position.x:
obstacle_objects[0].transform.position -= glm.vec3(0.001, 0.0, 0.0)
else:
obstacle_objects[0].transform.position += glm.vec3(0.001, 0.0, 0.0)
if mosquito_Object.transform.position.z < obstacle_objects[
0].transform.position.z:
if obstacle_objects[
1].transform.position.x > mosquito_Object.transform.position.x:
obstacle_objects[1].transform.position -= glm.vec3(
0.0015, 0.0, 0.0)
else:
obstacle_objects[1].transform.position += glm.vec3(
0.0015, 0.0, 0.0)
if mosquito_Object.transform.position.z < obstacle_objects[
1].transform.position.z:
if obstacle_objects[
2].transform.position.x > mosquito_Object.transform.position.x:
obstacle_objects[2].transform.position -= glm.vec3(
0.002, 0.0, 0.0)
else:
obstacle_objects[2].transform.position += glm.vec3(
0.002, 0.0, 0.0)
soup_Object.obj_data.meshes[0].material.Kd = glm.vec3(
float(color_r / 255), float(color_g / 255), float(color_b / 255))
if keyboard.is_pressed(" "):
mosquito_Object.started = True
if not gameFinished:
if mosquito_Object.started:
mosquito_Object.velocity += mosquito_Object.gravity * delta_time
mosquito_Object.transform.position += mosquito_Object.velocity * delta_time
camera.velocity += mosquito_Object.gravity * delta_time
camera.transform.position += camera.velocity * delta_time
"""if mosquito_Object.transform.position.y < 0.2:
mosquito_Object.transform.position = glm.vec3(0.0, 3.0, 3.0)
camera.transform.position = glm.vec3(0.0, 4.3, 4.5)"""
custom_keyboard_input()
# Do updates
# collision_tester_parent.transform.rotation = glm.quat(glm.vec3(0.0, 0.0, glm.radians(time_passed * 10.0)))
# Drawing part
# Clear screen
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glDepthMask(gl.GL_FALSE)
gl.glCullFace(gl.GL_FRONT)
skybox_shader.draw(
perspective_projection,
glm.mat4x4(glm.mat3x3(camera.get_view())),
# glm.mat4x4(),
skybox_texture)
gl.glDepthMask(gl.GL_TRUE)
gl.glCullFace(gl.GL_BACK)
# Draw objects in a standard way
standard_shader.draw(perspective_projection, camera.get_view(),
camera.transform.get_final_position(), light_position,
GameObject.WithObjData)
# Draw AABB debuggers
"""AABB_shader.draw(
perspective_projection,
camera.get_view(),
(game_object.AABB for game_object in GameObject.WithAABB)
)"""
global win, lose
if mosquito_Object.AABB.check_collision(
soup_Object.AABB) and not gameFinished and mosquito_Object.started:
win.join_set(win_and_lose_obj)
# check transparency in here! and position again with mosq
gameFinished = True
# print(mosquito_Object.transform.position)
if lives == 0 and not gameFinished and mosquito_Object.started:
lose.join_set(win_and_lose_obj)
# check transparency in here! and position again with mosq
gameFinished = True
UI_shader.draw(
orthogonal_projection,
hearts,
boosts,
win_and_lose_obj,
)
"""if mosquito_Object.AABB.check_collision(soup_Object.AABB):
time.sleep(4)
exit(0)
# print(mosquito_Object.transform.position)
if lives == 0:
time.sleep(4)
exit(0)"""
# Swap the buffer we just drew on with the one showing on the screen
glut.glutSwapBuffers()
def __init__(self):
self.value = glm.mat4x4(1.0)
def calc_state_single(self):
#state_start = time.time()
body_pose = self.robot_body.pose()
self.body_rpy = body_pose.rpy()
r, p, yaw = self.body_rpy
j = []
#self.bodyRot = glm.rotate(yaw, glm.vec3(0.0, 0.0, 1.0)) *
# glm.rotate(p, glm.vec3(0.0, 1.0, 0.0)) *
# glm::rotate(r, glm::vec3(1.0, 0.0, 0.0))
if self.inputsIsIKTargets:
fl = [
self.jdict["fl1"].servo_target, self.jdict["fl2"].servo_target,
self.jdict["fl3"].servo_target
]
fr = [
self.jdict["fr1"].servo_target, self.jdict["fr2"].servo_target,
self.jdict["fr3"].servo_target
]
bl = [
self.jdict["bl1"].servo_target, self.jdict["bl2"].servo_target,
self.jdict["bl3"].servo_target
]
br = [
self.jdict["br1"].servo_target, self.jdict["br2"].servo_target,
self.jdict["br3"].servo_target
]
self.syncLocalFromXYZ(fl, fr, bl, br, self.inputsInChassisSpace)
j = np.concatenate([self.flPosN] + [self.frPosN] + [self.blPosN] +
[self.brPosN])
self.lastJointsStates = j
else:
servo_angles = []
for j in self.ordered_joints:
# scaled beetween -1..1 between limits
limits = j.limits()
limitsMiddle = (limits[0] + limits[1]) * 0.5
jointObservation = 2 * (j.servo_target -
limitsMiddle) / (limits[1] - limits[0])
servo_angles.append(jointObservation)
j = servo_angles
#print("Joints3",(time.time()-state_start)*1000.0)
#print("Joints",time.time()-state_start)
#print(j)
#parts_xyz = np.array( [p.pose().xyz() for p in self.parts.values()] ).flatten()
curBodyXYZ = (body_pose.xyz()[0], body_pose.xyz()[1],
body_pose.xyz()[2])
self.prev_body_xyz = self.body_xyz
self.body_xyz = curBodyXYZ
self.movement_per_frame = [
self.body_xyz[0] - self.prev_body_xyz[0],
self.body_xyz[1] - self.prev_body_xyz[1],
self.body_xyz[2] - self.prev_body_xyz[2]
]
self.walk_target_theta = np.arctan2(
self.walk_target_y - self.body_xyz[1],
self.walk_target_x - self.body_xyz[0])
self.vecToTarget = [
self.walk_target_x - self.body_xyz[0],
self.walk_target_y - self.body_xyz[1],
self.walk_target_z - self.body_xyz[2]
]
self.walk_target_dist = np.linalg.norm(self.vecToTarget)
self.angle_to_target = self.walk_target_theta - yaw
frontRot = glm.rotate(glm.mat4x4(), yaw, glm.vec3(0.0, 0.0, 1.0))
self.body_frontVec = glm.mat3(frontRot) * glm.vec3(1.0, 0.0, 0.0)
'''
self.rot_minus_yaw = np.array(
[[np.cos(-yaw), -np.sin(-yaw), 0],
[np.sin(-yaw), np.cos(-yaw), 0],
[ 0, 0, 1]]
)
'''
observationsBase = np.array([
r, p, yaw,
np.sin(self.angle_to_target),
np.cos(self.angle_to_target)
],
dtype=np.float32)
observations = np.concatenate([observationsBase] +
[self.feet_contact] + [np.float32(j)])
# 0.3 is just scaling typical speed into -1..+1, no physical sense here
if self.velObservations > 0:
vx, vy, vz = self.robot_body.speed()
body_vel = np.array([0.3 * vx, 0.3 * vy, 0.3 * vz],
dtype=np.float32)
observations = np.append(observations, body_vel)
self.last_single_state = observations
#print("all",time.time()-state_start)
return observations
def getJ1TM(self, jointName):
resultTm = glm.mat4x4()
#resultTm = glm.rotate(resultTm,angles[0], glm.vec3([1.0,0.0,0.0]))
resultTm = glm.translate(resultTm, self.jointsLocalPos[jointName])
return resultTm
def value(self):
v = b'\x00' * 64
native.n_svmat4x4_value(self.m_cptr, ffi.from_buffer('float[]', v))
elems = struct.unpack('16f', v)
return glm.mat4x4(elems[0:4], elems[4:8], elems[8:12], elems[12:16])
def get_transformation(self):
transformation = glm.mat4x4()
transformation = glm.translate(transformation, self.position)
transformation = glm.scale(transformation, self.scale)
return transformation
def __init__(self):
self._m = glm.mat4x4(1)
self._stack = []
def load_identity(self):
self._m = glm.mat4x4(1)
