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)