def __init__(self, data): self.numSteps = 4000 self.data = data self.states = data["state"] self.numStates = len(self.states) self.actions = data["action"] self.next_states = data["next_state"] high = np.ones([len(self.states[0])]) self.observation_space = gym.spaces.Box(-high, high) high = np.ones([len(self.actions[0])]) self.action_space = gym.spaces.Box(-high, high) self.index = 0 self.restartDataOnRestart = True #False #self.actionMults = np.array([2.0,1.0,1.0,2.0,1.0,1.0,2.0,1.0,1.0,2.0,1.0,1.0]) self.aMin = np.array([ -0.398132, -1.22173, -glm.pi(), -0.698132, -1.22173, -glm.pi(), -0.398132, -1.22173, -glm.pi(), -0.698132, -1.22173, -glm.pi() ]) self.aMax = np.array([ 0.698132, 1.22173, 0.0, 0.398132, 1.22173, 0.0, 0.698132, 1.22173, 0.0, 0.398132, 1.22173, 0.0 ]) self.limitsMiddle = (self.aMin + self.aMax) * 0.5 self.limitsDiff = (self.aMax - self.aMin)
def _init_rotation(self): self._rotation = glm.vec3(0) if self.projection == "o": self._rotation[0] = -glm.pi() / 4 elif self.projection in "e": self._rotation[0] = -glm.pi() / 2. elif self.projection in "p": pass elif self.projection == "i": self._rotation[0] = -glm.pi() / 3.3 self._rotation[2] = glm.pi() / 4.
def transformation_matrix_4(self) -> glm.mat4: m = glm.rotate( glm.mat4(1), -self.rotation_deg / 180 * glm.pi(), glm.vec3(0, 0, 1) ) m = m * glm.scale(glm.mat4(), glm.vec3(2. / self.scale)) m = m * glm.translate(glm.mat4(), glm.vec3(-self.location.x, -self.location.y, 0)) return m
def rotation(self, orig, dest): identityQuat = glm.quat(1.0, 0.0, 0.0, 0.0) epsilon = 0.00001 cosTheta = glm.dot(orig, dest) if cosTheta >= 1.0 - epsilon: #// orig and dest point in the same direction return identityQuat if cosTheta < -1.0 + epsilon: ''' // special case when vectors in opposite directions : // there is no "ideal" rotation axis // So guess one; any will do as long as it's perpendicular to start // This implementation favors a rotation around the Up axis (Y), // since it's often what you want to do. ''' rotationAxis = glm.cross(glm.vec3(0.0, 0.0, 1.0), orig) if glm.length( rotationAxis ) < epsilon: # // bad luck, they were parallel, try again! rotationAxis = glm.cross(glm.vec3(1.0, 0.0, 0.0), orig) rotationAxis = glm.normalize(rotationAxis) return glm.angleAxis(glm.pi(), rotationAxis) #// Implementation from Stan Melax's Game Programming Gems 1 article rotationAxis = glm.cross(orig, dest) s = math.sqrt((1.0 + cosTheta) * 2.0) invs = 1.0 / s return glm.quat(s * 0.5, rotationAxis.x * invs, rotationAxis.y * invs, rotationAxis.z * invs)
def check_keys(self, dt): dir_mapping = { pyglet.window.key.LEFT: glm.vec3(-1, 0, 0), pyglet.window.key.RIGHT: glm.vec3(1, 0, 0), pyglet.window.key.UP: glm.vec3(0, 1, 0), pyglet.window.key.DOWN: glm.vec3(0, -1, 0), #pyglet.window.key.SPACE: glm.vec3(0,0,1), } sum_dir = glm.vec3(0) num = 0 for symbol in dir_mapping: if self.keys[symbol]: dir = dir_mapping[symbol] if self._projection.projection == self._projection.P_ISOMETRIC: dir = glm.vec3( glm.rotate(glm.mat4(1), -glm.pi() / 4., (0, 0, 1)) * glm.vec4(dir, 0)) #dir *= min(1, dt*10.) sum_dir += dir num += 1 if num: self.agents["player"].move(sum_dir / num * 1.5) if self.keys[pyglet.window.key.SPACE]: self.agents["player"].jump()
def normAngle(self, angle): #// 180-270 if angle >= glm.pi() and angle <= glm.three_over_two_pi(): return angle - glm.two_pi() #// 270-360 if angle >= glm.three_over_two_pi() and angle <= glm.two_pi(): return angle - glm.two_pi() return angle
def lookAt(self, worldDestinationDirection): # direction newWorldDestinationRotation = glm.atan( worldDestinationDirection[1], worldDestinationDirection[0]) * 180.0 / glm.pi() if newWorldDestinationRotation != self.worldDestinationRotation: self.finishedRotate = False self.worldDestinationRotation = newWorldDestinationRotation
def draw_cylinder(x, y, z, radius, height): px = 0 pz = 0 c_angle = 0 angle_stepsize = 0.1 glBegin(GL_QUAD_STRIP) c_angle = 0 while c_angle < 2 * glm.pi() + 1: px = radius * glm.cos(c_angle) pz = radius * glm.sin(c_angle) glVertex3f(x + px, y + height, z + pz) glVertex3f(x + px, y, z + pz) c_angle += angle_stepsize glEnd() glBegin(GL_POLYGON) c_angle = 0 while c_angle < 2 * glm.pi(): px = radius * glm.cos(c_angle) pz = radius * glm.sin(c_angle) glVertex3f(x + px, y + height, z + pz) c_angle += angle_stepsize glEnd() glBegin(GL_POLYGON) c_angle = 0 while c_angle < 2 * glm.pi(): px = radius * glm.cos(c_angle) pz = radius * glm.sin(c_angle) glVertex3f(x + px, y + height, z + pz) c_angle += angle_stepsize glEnd() glBegin(GL_POLYGON) c_angle = 0 while c_angle < 2 * glm.pi(): px = radius * glm.cos(c_angle) pz = radius * glm.sin(c_angle) glVertex3f(x + px, y, z + pz) c_angle += angle_stepsize glEnd()
def on_text(self, text): if text == "o": self._projection = "o" self._init_rotation() if text == "i": self._projection = "i" self._init_rotation() self.rotate_z = glm.pi() / 4 if text == "p": self._projection = "p" self._init_rotation()
def update(self, dt: float): """Call this repeatedly with the time in seconds since last frame""" rotations = self._rotations self._rotations = [] for i, rot in enumerate(rotations): # print(i, rot) amount = rot["amount"] * dt * .1 * glm.sin(rot["t"] * glm.pi()) self._matrix = glm.rotate(self._matrix, amount, rot["axis"]) rot["t"] += dt / (1. + abs(rot["amount"])) * 5. if rot["t"] < 0.99: self._rotations.append(rot) translations = self._translations self._translations = [] for i, rot in enumerate(translations): # print(i, rot) amount = rot["amount"] * dt * .1 * glm.sin(rot["t"] * glm.pi()) self._matrix = glm.translate(self._matrix, amount * rot["axis"]) rot["t"] += dt / (1. + abs(rot["amount"])) * 5. if rot["t"] < 0.99: self._translations.append(rot)
def transformation_matrix(self) -> glm.mat4: if not self.has_physics: trans = glm.translate(glm.mat4(1), self._location) trans *= glm.rotate(glm.mat4(1), self._rotation / 180 * glm.pi(), glm.vec3(0, 0, 1)) return trans pos, quat = pybullet.getBasePositionAndOrientation( bodyUniqueId=self._body_id, physicsClientId=self._physics_client_id, ) mat3 = pybullet.getMatrixFromQuaternion(quat) return glm.rotate(glm.mat4( mat3[0], mat3[3], mat3[6], 0, mat3[1], mat3[4], mat3[7], 0, mat3[2], mat3[5], mat3[8], 0, pos[0], pos[1], pos[2], 1, ), math.pi/2., [1, 0, 0])
def calc_collide_rays(self, numViewDirections, length): directions=[] goldenRatio = (1 + glm.sqrt(5)) / 2; angleIncrement = glm.pi() * 2 * goldenRatio; i=0 while i < (numViewDirections): t = i / numViewDirections; inclination = glm.acos(1 - 2 * t); azimuth = angleIncrement * i; x = glm.sin(inclination) * glm.cos(azimuth); y = glm.sin (inclination) * glm.sin(azimuth); z = glm.cos (inclination); directions.append(glm.vec3(x, y, z)*length) i+=1 return directions
def render(self, projection, pos, dir, frame_num): self._update() trans = projection.matrix mat = glm.translate(trans, pos + (0, 0, 0)) p1 = mat * glm.vec4(0, 0, 0, 1) p2 = mat * glm.vec4(0, 1, 0, 1) p = p2 - p1 a = glm.atan(p.x, p.y) mat = glm.rotate(mat, a, (0, 0, 1)) mat = glm.rotate(mat, glm.pi() / 2., (1, 0, 0)) tile_idx = self.get_tileset_idx(dir, frame_num) self.texture.bind() self.drawable.shader.set_uniform("u_projection", mat) self.drawable.shader.set_uniform("u_tex_offset", self.tileset.get_uv_offset(tile_idx)) self.drawable.draw()
def on_text(self, text): if text == "o": self._projection = "o" self._init_rotation() if text == "i": self._projection = "i" self._init_rotation() self.rotate_z = glm.pi() / 4 if text == "p": self._projection = "p" self._init_rotation() if text == "e": self._projection = "e" self._init_rotation() if text == "f": self.set_fullscreen(not self.fullscreen) if text == "+": self.zoom += 1. if text == "-": self.zoom -= 1. if text == "a": self.fov += 0.01 print(self.fov)
def __init__(self, name): super().__init__(name) factory = MeshFactory() self.mesh = TriangleMesh() factory.add_cube(self.mesh, 1.) self.drawable = self.mesh.create_drawable() self.drawable.shader.set_vertex_source(DEFAULT_SHADER_VERSION + """ #line 19 uniform mat4 u_projection; uniform mat4 u_transformation; uniform mat4 u_transformation_inv; in vec4 a_position; in vec3 a_normal; in vec4 a_color; in vec2 a_texcoord; in vec3 a_ambient; in vec4 a_instance_color; in mat4 a_instance_transform; out vec4 v_pos; out vec3 v_normal; out vec4 v_color; out vec2 v_texcoord; out vec3 v_ambient; out vec3 v_world_normal; void main() { vec4 position = a_instance_transform * a_position; v_pos = position; v_normal = a_normal; v_world_normal = normalize((vec4(a_normal, 0.) * u_transformation_inv).xyz); v_color = a_instance_color; v_texcoord = a_texcoord; v_ambient = a_ambient; gl_Position = u_projection * u_transformation * position; } """) self.drawable.shader.set_fragment_source(DEFAULT_SHADER_VERSION + """ #line 18 in vec4 v_pos; in vec4 v_color; in vec3 v_normal; in vec2 v_texcoord; uniform float u_time; out vec4 fragColor; vec3 lighting(in vec3 pos, in vec3 norm, in vec3 light_pos, in vec3 color) { vec3 light_norm = normalize(light_pos - pos); float d = max(0., dot(norm, light_norm)); return color * pow(d, 5.); } void main() { vec3 col = v_color.xyz + .02 * v_normal; vec3 norm = v_normal; //normalize(v_normal + .1 * sin(length(v_pos.xyz) * 34. - 5. * u_time)); col += .3 * lighting(v_pos.xyz, norm, vec3(3, 0, 1), vec3(1, .6, .4)); col += .3 * lighting(v_pos.xyz, norm, vec3(-3, 1, 2), vec3(.5, 1, .4)); col += .3 * lighting(v_pos.xyz, norm, vec3(1, 5, 3), vec3(.3, .6, 1)); fragColor = vec4(col, 1); } """) if 0: self.instance_colors = [ 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 1, 1, 1, 0, 1, ] self.instance_transforms = [ glm.mat4(1), glm.translate(glm.mat4(1), glm.vec3(1, 0, 0)), glm.translate(glm.mat4(1), glm.vec3(1, 1, 0)), glm.rotate(glm.translate(glm.mat4(1), glm.vec3(-.5, 1, 0)), glm.pi() / 4., glm.vec3(0, 1, 0)), ] else: self.instance_colors = [] self.instance_transforms = [] for i in range(500): self.instance_colors.append(random.uniform(0.1, 1.)) self.instance_colors.append(random.uniform(0.1, 1.)) self.instance_colors.append(random.uniform(0.1, 1.)) self.instance_colors.append(1.) self.instance_transforms.append( glm.translate( glm.mat4(1), 20. * glm.vec3(random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1))))
def calculateAnalyticalActions(self, state): actions = self.robotLib.getActions(state.tolist(), self.physics_time_step) a = np.asarray(actions) if self.ActionIsAngles == False: if self.ActionsIsAdditive: fl = [a[0] * glm.pi(), a[1] * glm.pi(), a[2] * glm.pi()] fr = [a[3] * glm.pi(), a[4] * glm.pi(), a[5] * glm.pi()] bl = [a[6] * glm.pi(), a[7] * glm.pi(), a[8] * glm.pi()] br = [a[9] * glm.pi(), a[10] * glm.pi(), a[11] * glm.pi()] self.syncLocalFromXYZ(fl, fr, bl, br, self.inputsInChassisSpace) a = np.concatenate([self.flPosN] + [self.frPosN] + [self.blPosN] + [self.brPosN]) a = np.subtract(a, self.lastJointsStates) else: fl = [a[0] * glm.pi(), a[1] * glm.pi(), a[2] * glm.pi()] fr = [a[3] * glm.pi(), a[4] * glm.pi(), a[5] * glm.pi()] bl = [a[6] * glm.pi(), a[7] * glm.pi(), a[8] * glm.pi()] br = [a[9] * glm.pi(), a[10] * glm.pi(), a[11] * glm.pi()] self.syncLocalFromXYZ(fl, fr, bl, br, self.inputsInChassisSpace) a = np.concatenate([self.flPosN] + [self.frPosN] + [self.blPosN] + [self.brPosN]) return a
def _init_rotation(self): self.rotate_x = glm.pi() / (3.3 if self._projection == "i" else 4) self.rotate_y = 0 #-glm.pi()/4. self.rotate_z = 0
def angleDiff(self, a, b): dif = math.fmod(b - a + glm.pi(), 2.0 * glm.pi()) if dif < 0: dif += 2.0 * glm.pi() return dif - glm.pi()