def ComputeColor(obj, intersection, light_position): # intersection_position = intersection.position intersection_normal = intersection.surface_normal # light_direction = -light_position.normalize().xyz() light_direction = glm.normalize(light_position) albedo = obj.material.color.AsVec3() # * obj.material.ambient dot_product = max(0.0, glm.dot(light_direction, intersection_normal)) color = albedo * dot_product return color
def check_in_frustum(self, chunk_pos): """Frustum check of each chunk. If the chunk is not in the view frustum, it is discarded""" planes = (Frustum.left, Frustum.right, Frustum.bottom, Frustum.top, Frustum.near, Frustum.far) result = 2 center = glm.vec3(chunk_pos * glm.ivec3(chunk.CHUNK_WIDTH, 0, chunk.CHUNK_LENGTH) + glm.ivec3(chunk.CHUNK_WIDTH / 2, chunk.CHUNK_HEIGHT / 2, chunk.CHUNK_LENGTH / 2)) for plane in planes: _in = 0 _out = 0 normal = plane.xyz w = plane.w if glm.dot(normal, center + glm.vec3(chunk.CHUNK_WIDTH / 2, chunk.CHUNK_HEIGHT / 2, chunk.CHUNK_LENGTH / 2)) + w < 0: _out += 1 else: _in += 1 if glm.dot(normal, center + glm.vec3(-chunk.CHUNK_WIDTH / 2, chunk.CHUNK_HEIGHT / 2, chunk.CHUNK_LENGTH / 2)) + w < 0: _out += 1 else: _in += 1 if glm.dot(normal, center + glm.vec3(chunk.CHUNK_WIDTH / 2, chunk.CHUNK_HEIGHT / 2, -chunk.CHUNK_LENGTH / 2)) + w < 0: _out += 1 else: _in += 1 if glm.dot(normal, center + glm.vec3(-chunk.CHUNK_WIDTH / 2, chunk.CHUNK_HEIGHT / 2, -chunk.CHUNK_LENGTH / 2)) + w < 0: _out += 1 else: _in += 1 if glm.dot(normal, center + glm.vec3(chunk.CHUNK_WIDTH / 2, -chunk.CHUNK_HEIGHT / 2, chunk.CHUNK_LENGTH / 2)) + w < 0: _out += 1 else: _in += 1 if glm.dot(normal, center + glm.vec3(-chunk.CHUNK_WIDTH / 2, -chunk.CHUNK_HEIGHT / 2, chunk.CHUNK_LENGTH / 2)) + w < 0: _out += 1 else: _in += 1 if glm.dot(normal, center + glm.vec3(chunk.CHUNK_WIDTH / 2, -chunk.CHUNK_HEIGHT / 2, -chunk.CHUNK_LENGTH / 2)) + w < 0: _out += 1 else: _in += 1 if glm.dot(normal, center + glm.vec3(-chunk.CHUNK_WIDTH / 2, -chunk.CHUNK_HEIGHT / 2, -chunk.CHUNK_LENGTH / 2)) + w < 0: _out += 1 else: _in += 1 if not _in: return 0 elif _out: result = 1 return result
def Intersect(self, ray): ray_origin = ray.origin ray_direction = ray.direction sphere_center = glm.vec3(self.position) oc = ray_origin - sphere_center a = glm.dot(ray_direction, ray_direction) # b = 2.0 * glm.dot(oc, ray_direction) half_b = glm.dot(oc, ray_direction) c = glm.dot(oc, oc) - self.radius * self.radius # discriminant = b*b - 4*a*c discriminant = half_b * half_b - a * c if discriminant > 0: # ray_intersection_offset = (-b - math.sqrt(discriminant)) / (2 * a) ray_intersection_offset = (-half_b - math.sqrt(discriminant)) / a if ray_intersection_offset < ray.t_max and ray_intersection_offset > ray.t_min: intersection_point = ray.PointAtOffset(ray_intersection_offset) # surface_normal = glm.normalize(intersection_point - sphere_center) # if radius is negative then front-face is inside the sphere surface_normal = (intersection_point - sphere_center) / self.radius intersection_result = IntersectionResult( self, intersection_point, ray_intersection_offset, surface_normal) return intersection_result # ray_intersection_offset = (-b + math.sqrt(discriminant)) / (2 * a) ray_intersection_offset = (-half_b + math.sqrt(discriminant)) / a if ray_intersection_offset < ray.t_max and ray_intersection_offset > ray.t_min: intersection_point = ray.PointAtOffset(ray_intersection_offset) # surface_normal = glm.normalize(intersection_point - sphere_center) # if radius is negative then front-face is inside the sphere surface_normal = (intersection_point - sphere_center) / self.radius intersection_result = IntersectionResult( self, intersection_point, ray_intersection_offset, surface_normal) return intersection_result return None
def refract(v, n, ni_over_nt, refracted): uv = glm.vec3(v / glm.length(v)) dt = glm.dot(uv, n) discriminant = 1.0 - ni_over_nt * ni_over_nt * (1 - dt * dt) if (discriminant > 0): refracted = ni_over_nt * (uv - n * dt) - n * math.sqrt(discriminant) return (True, refracted) else: refracted = glm.vec3 return (False, refracted)
def Intersect(self, ray): ray_direction = ray.direction denominator = glm.dot(self.normal, ray_direction) # possible collision # plane normal and ray direction are not in the same direction if denominator < 0: plane_position = glm.vec3(self.position) ray_origin = ray.origin oc = ray_origin - plane_position ray_intersection_offset = glm.dot(oc, self.normal) / -denominator # ray_intersection_offset > ray.t_min plane is in front of the ray if ray_intersection_offset < ray.t_max and ray_intersection_offset > ray.t_min: intersection_point = ray.PointAtOffset(ray_intersection_offset) intersection_result = IntersectionResult( self, intersection_point, ray_intersection_offset, self.normal) return intersection_result return None
def heuristic_step_cost(self, n1, n2, goal): if n1 == n2: return 0 d = self.nodes.distance(n1, n2) p1 = self.nodes.id_to_pos[n1] p2 = self.nodes.id_to_pos[n2] d += abs(p1[2] - p2[2]) * 5 if 0: # avoid towards goal dir1 = self.nodes.direction(n1, n2) dir2 = self.nodes.direction(n1, goal) return glm.dot(dir1, dir2) * d return d
def build_view_matrix(position, look_at, up): forward = glm.normalize(position - look_at) right = glm.normalize(glm.cross(up, forward)) up = glm.normalize(glm.cross(forward, right)) mat = glm.mat4(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)) return mat
def intersect(self, my_ray, inter_rec): eo = self.center - my_ray.origin v = glm.dot(eo, my_ray.direction) disc = (self.radius * self.radius) - (glm.dot(eo, eo) - (v * v)) if disc < 0.0: return False else: d = sqrt(disc) t1 = v - d t2 = v + d if t1 > 0.00001: inter_rec.t = t1 else: inter_rec.t = t2 inter_rec.position = my_ray.origin + inter_rec.t * my_ray.direction inter_rec.normal = glm.normalize(inter_rec.position - self.center) inter_rec.material = self.material return True
def shade(self, ray, pos, normal, light): output = glm.vec3(0.0) nlightDirection, lightColor = light.getIllumination(pos) nnormal = glm.normalize(normal) diffuseIntensity = glm.dot(nnormal, nlightDirection) output = output + self.diffuse * lightColor * glm.clamp( diffuseIntensity, 0.0, 1.0) nview = glm.normalize(ray.origin - pos) nhalf = glm.normalize(nview + nlightDirection) specularIntensity = glm.pow(glm.dot(nhalf, nnormal), self.shininess) output = output + self.specular * lightColor * glm.clamp( specularIntensity, 0.0, 1.0) output = output + self.emission return glm.vec3(glm.clamp(output.x, 0.0, 1.0), glm.clamp(output.y, 0.0, 1.0), glm.clamp(output.z, 0.0, 1.0))
def hit(self, r, t_min, t_max, rec): oc = glm.vec3(r.origin - self.center) a = glm.dot(r.direction, r.direction) b = glm.dot(oc, r.direction) c = glm.dot(oc, oc) - self.radius * self.radius discriminant = b * b - a * c if (discriminant > 0): temp = (-b - math.sqrt(b * b - a * c)) / a if (temp < t_max and temp > t_min): rec.t = temp rec.p = r.point_at_parameter(rec.t) rec.normal = (rec.p - self.center) / self.radius rec.mat_ptr = self.material return True temp = (-b + math.sqrt(b * b - a * c)) / a if (temp < t_max and temp > t_min): rec.t = temp rec.p = r.point_at_parameter(rec.t) rec.normal = (rec.p - self.center) / self.radius rec.mat_ptr = self.material return True return False
def RayTriangleCollisionCheck(self, ray, a, b, c): ray_direction = ray.direction ray_origin = ray.origin edge1 = b - a edge2 = c - a normal = glm.cross(edge1, edge2) det = -glm.dot(ray_direction, normal) invertDet = 1.0 / det AO = ray_origin - a DAO = glm.cross(AO, ray_direction) u = glm.dot(edge2, DAO) * invertDet v = -glm.dot(edge1, DAO) * invertDet ray_intersection_offset = glm.dot(AO, normal) * invertDet if det >= 1e-6 and ray_intersection_offset >= 0.0 and u >= 0.0 and v >= 0.0 and ( u + v) <= 1.0: intersection_point = ray.PointAtOffset(ray_intersection_offset) normal = glm.normalize(normal) intersection_result = IntersectionResult(self, intersection_point, ray_intersection_offset, normal) return intersection_result return None
def world_to_screen(self, world_pos: vec3) -> Union[vec2, None]: """ Convert a world 3D position to a screen 2D position. Returns None if the position is not in the screen """ rel = world_pos - self.position # distance along the screen's z axis dist = dot(rel, self.direction) if dist < 10: return None absolute_y = dot(rel, self.up) absolute_x = dot(rel, self.horizontal) pos = (vec2( absolute_x / dist * self.screen_dist, absolute_y / dist * self.screen_dist, ) + self.screen_size / 2) pos.y = self.screen_size.y - pos.y return pos
def intersect(self, my_ray, inter_rec): a = self.vertex1.x - self.vertex2.x b = self.vertex1.y - self.vertex2.y c = self.vertex1.z - self.vertex2.z d = self.vertex1.x - self.vertex3.x e = self.vertex1.y - self.vertex3.y f = self.vertex1.z - self.vertex3.z g = my_ray.direction.x h = my_ray.direction.y i = my_ray.direction.z j = self.vertex1.x - my_ray.origin.x k = self.vertex1.y - my_ray.origin.y l = self.vertex1.z - my_ray.origin.z ei_minus_hf = float(e * i - h * f) gf_minus_di = float(g * f - d * i) dh_minus_eg = float(d * h - e * g) ak_minus_jb = float(a * k - j * b) jc_minus_al = float(j * c - a * l) bl_minus_kc = float(b * l - k * c) M = a * (ei_minus_hf) + b * (gf_minus_di) + c * (dh_minus_eg) t = -(f * (ak_minus_jb) + e * (jc_minus_al) + d * (bl_minus_kc)) / M if t < 0.0: return False gamma = (i * (ak_minus_jb) + h * (jc_minus_al) + g * (bl_minus_kc)) / M if gamma < 0.0 or gamma > 1.0: return False beta = (j * (ei_minus_hf) + k * (gf_minus_di) + l * (dh_minus_eg)) / M if beta < 0.0 or beta > 1.0 - gamma: return False inter_rec.t = t inter_rec.position = my_ray.origin + inter_rec.t * my_ray.direction inter_rec.normal = glm.normalize( glm.cross(self.vertex2 - self.vertex1, self.vertex3 - self.vertex1)) if glm.dot(inter_rec.normal, my_ray.direction) > 0: inter_rec.normal = -inter_rec.normal inter_rec.material = self.material return True
def cull(self, C, cull_planes): if len(cull_planes) > 0: # Recalculate center and radius based on scene graph traversal center = glm.vec3(C * glm.vec4(self.center, 1)) radius = self.radius * glm.length( C * glm.vec4(1, 1, 1, 0)) / glm.sqrt(3) for point, normal in cull_planes: dist = glm.dot(center - point, normal) if dist > radius: # Cull if beyond plane if self.name == None: print( 'name:{} point:{!r} norm:{!r} dist:{} r:{} center:{!r} prev_center:{!r}' .format(self.name, point, normal, dist, radius, center, self.center)) return True return False
def get_vect_step(self): # Get the vector to move by between pixels n = maths.unit(self.goal - self.point) # Normal to plane center = self.point + n * self.fl # center of the ccd d = glm.dot(center, n) # dot of the plane upish = n + self.up # Up on the plane a = self.point # Makes it look nicer up_point = a + upish * (d - (a[0] * n[0] + a[1] * n[1] + a[2] * n[2])) / ( upish[0] * n[0] + upish[1] * n[1] + upish[2] * n[2]) upstep = maths.unit(up_point - center) * ( self.ccd_size[1] / self.res[1]) # Step for up pixels sidestep = maths.unit(glm.cross( n, upstep)) * (self.ccd_size[0] / self.res[0]) return upstep, sidestep, center
def update(self, dt): d = min(1, dt * 5) # gravity if 1: newpos = self.position + d * glm.vec3(0, 0, -3) if int(newpos.z) >= 1: if self.chunk.block(int(newpos.x), int(newpos.y), int(newpos.z)).space_type == 0: self.position = newpos else: t, hit = self.chunk.cast_voxel_ray(self.position + (0, 0, 0), (0, 0, -1)) if hit: self.position.z -= t self.sposition.z -= t # advance spos to pos move = (self.position - self.sposition) + self.velocity if 0: newpos = self.sposition + d * move if self.chunk.block(int(newpos.x), int(newpos.y), int(newpos.z)).space_type == 0: self.sposition = newpos else: nextpos = glm.vec3(self.sposition) for i, ax in enumerate(((1, 0, 0), (0, 1, 0), (0, 0, 1))): newpos = self.sposition + d * move * ax if self.chunk.block(int(newpos.x-.2), int(newpos.y-.2), int(newpos.z)).space_type == 0\ and self.chunk.block(int(newpos.x+.2), int(newpos.y-.2), int(newpos.z)).space_type == 0\ and self.chunk.block(int(newpos.x-.2), int(newpos.y+.2), int(newpos.z)).space_type == 0\ and self.chunk.block(int(newpos.x+.2), int(newpos.y+.2), int(newpos.z)).space_type == 0: nextpos[i] = newpos[i] self.sposition = nextpos self.velocity -= self.velocity * d * .5 amt = glm.dot(move, move) if amt > .1: self.anim_stage += dt * 7. if self.anim_stage > 3.: self.anim_stage = 1. else: self.anim_stage = 0.
def find_aim(self): # Find closest enemy dist = float("inf") closest = None for e in self.scene.iter_entities(Enemy): if not e.alive or e.position.z > self.position.z: continue dir = e.position - self.position v = vec3(self.velocity.xy * 10, self.velocity.z) d = vec3(dir.xy * 10, dir.z) if abs(dot(normalize(v), normalize(d))) < 0.9: # Angles are too different continue d = length(e.position - self.position) if 200 < d < dist: dist = d closest = e return closest
def CheckDirection(target): compass = [ glm.vec2(0.0, 1.0), glm.vec2(1.0, 0.0), glm.vec2(0.0, -1.0), glm.vec2(-1.0, 0.0) ] direction = ["UP", "RIGHT", "DOWN", "LEFT"] maxVal = 0.0 bestVal = -1 index = 0 for value in compass: dotProduct = glm.dot(glm.normalize(target), value) if dotProduct > maxVal: maxVal = dotProduct bestVal = index index = index + 1 return direction[bestVal]
def collide_static(self, other): collision_info = self.collide(other) if collision_info: # started colliding in 2D top = other.get_top(self.pos.xz) step_top = top - self.step_height if other.get_y() - self.get_height(None) < self.pos.y < step_top: # colliding in 3D hit_point, normal_vector, radius = collision_info new_pos = hit_point + normal_vector * (other.radius + self.radius + .01) self.pos.x, self.pos.z = new_pos # # force = glm.reflect(self.vel.xz, -normal_vector) force = normal_vector * max( 0, -glm.dot(self.vel.xz, normal_vector)) self.force.x += force.x * self.bounciness self.force.z += force.y * self.bounciness # self.vel.x, self.vel.z = new_vel * self.bounciness self.on_collision(other, hit_point, normal_vector, radius) return True else: # not colliding in 3D self.colliding.add(other) if step_top < self.pos.y < top: # stepped on the object self.pos.y = top self.set_platform(other) return True
def distance(self, world_pos): """Distance from the camera along the `direction` axis""" return dot(world_pos - self.position, self.direction)
def refract(uv, n, etaI_over_etaT): cos_theta = glm.dot(-uv, n) r_out_parallel = etaI_over_etaT * (uv + cos_theta * n) r_out_perp = n * (-math.sqrt(1.0 - glm.length2(r_out_parallel))) return r_out_parallel + r_out_perp
def reflect(v, n): return (v - 2 * glm.dot(v, n) * n)
def random_in_unit_disk(): p = 2.0 * glm.vec3(ran(), ran(), 0.0) - glm.vec3(1.0, 1.0, 0.0) while (glm.dot(p, p) >= 1.0): p = 2.0 * glm.vec3(ran(), ran(), 0.0) - glm.vec3(1.0, 1.0, 0.0) return p
def multiply(self, matrix4): self.__matrix = glm.dot(matrix4, self.__matrix)
def MoveCursorTo(self, cursor_pos): view_changed = False # get view matrix and viewport rectangle view, inv_view = self.ViewMat() view_rect = self.VpRect() if self.__pan: # get drag start and end wnd_from = self.__pan_start wnd_to = glm.vec3(*cursor_pos, self.__pan_start[2]) self.__pan_start = wnd_to # get projection and window matrix proj, inv_proj = self.ProjectionMat() wnd, inv_wnd = self.WindowMat() # calculate drag start and world coordinates pt_h_world = [ inv_view * inv_proj * inv_wnd * glm.vec4(*pt, 1) for pt in [wnd_from, wnd_to] ] pt_world = [glm.vec3(pt_h) / pt_h.w for pt_h in pt_h_world] # calculate drag world translation world_vec = pt_world[1] - pt_world[0] # translate view position and update view matrix inv_view = glm.translate(glm.mat4(1), world_vec * -1) * inv_view view = glm.inverse(inv_view) view_changed = True elif self.__orbit == self.ORBIT: # get the drag start and end wnd_from = self.__orbit_start wnd_to = glm.vec3(*cursor_pos, self.__orbit_start[2]) self.__orbit_start = wnd_to # calculate the pivot, rotation axis and angle pivot = glm.vec3(view * glm.vec4(*self.__pivot_world, 1)) orbit_dir = wnd_to - wnd_from axis = glm.vec3(-orbit_dir.y, orbit_dir.x, 0) angle = glm.length( glm.vec2(orbit_dir.x / (view_rect[2] - view_rect[0]), orbit_dir.y / (view_rect[3] - view_rect[1]))) * math.pi # calculate the rotation matrix and the rotation around the pivot rot_mat = glm.rotate(glm.mat4(1), angle, axis) rot_pivot = glm.translate(glm.mat4(1), pivot) * rot_mat * glm.translate( glm.mat4(1), -pivot) #transform and update view matrix view = rot_pivot * view view_changed = True elif self.__orbit == self.ROTATE: # get the drag start and end wnd_from = self.__orbit_start wnd_to = glm.vec3(*cursor_pos, self.__orbit_start[2]) self.__orbit_start = wnd_to # calculate the pivot, rotation axis and angle pivot_view = glm.vec3(view * glm.vec4(*self.__pivot_world, 1)) orbit_dir = wnd_to - wnd_from # get the projection of the up vector to the view port # TODO # calculate the rotation components for the rotation around the view space x axis and the world up vector orbit_dir_x = glm.vec2(0, 1) orbit_vec_x = glm.vec2(0, orbit_dir.y) orbit_dir_up = glm.vec2(1, 0) orbit_vec_up = glm.vec2(orbit_dir.x, 0) # calculate the rotation matrix around the view space x axis through the pivot rot_pivot_x = glm.mat4(1) if glm.length(orbit_vec_x) > 0.5: axis_x = glm.vec3(-1, 0, 0) angle_x = glm.dot( orbit_dir_x, glm.vec2(orbit_vec_x.x / (view_rect[2] - view_rect[0]), orbit_vec_x.y / (view_rect[3] - view_rect[1]))) * math.pi rot_mat_x = glm.rotate(glm.mat4(1), angle_x, axis_x) rot_pivot_x = glm.translate( glm.mat4(1), pivot_view) * rot_mat_x * glm.translate( glm.mat4(1), -pivot_view) # calculate the rotation matrix around the world space up vector through the pivot rot_pivot_up = glm.mat4(1) if glm.length(orbit_vec_up) > 0.5: axis_up = glm.vec3(0, 0, 1) angle_up = glm.dot( orbit_dir_up, glm.vec2(orbit_vec_up.x / (view_rect[2] - view_rect[0]), orbit_vec_up.y / (view_rect[3] - view_rect[1]))) * math.pi rot_mat_up = glm.rotate(glm.mat4(1), angle_up, axis_up) rot_pivot_up = glm.translate( glm.mat4(1), self.__pivot_world) * rot_mat_up * glm.translate( glm.mat4(1), -self.__pivot_world) #transform and update view matrix view = rot_pivot_x * view * rot_pivot_up view_changed = True # return the view matrix return view, view_changed
def _angle_between(v1, v2): v1_u = glm.normalize(v1) v2_u = glm.normalize(v2) return glm.acos(np.clip(glm.dot(v1_u, v2_u), -1.0, 1.0))
def __init__(self, engine, sx, sy, length, height, seed): self.size = glm.vec3(sx * length, height, sy * length) self.tex_size = glm.vec2(sx, sy) self.texture = engine.graphics.get_texture('dirt') self.collider = physics_objects.Box((0, 0, 0), self.size) self.collider.get_height = self.get_height engine.physics.static.append(self.collider) self.positions = np.empty((sx * sy, 3), np.float32) normals = np.empty((sx * sy, 3), np.float32) texcoords = np.empty((sx * sy, 2), np.float32) colors = np.empty((sx * sy, 3), np.float32) indices = np.empty(6 * (sx - 1) * (sy - 1), np.uint32) # self.image = Image.new('L', (sx, sy), 'black') h = hash(seed) sample = glm.vec2(h, hash(h)) # Generate vertex positions for x in range(0, sx): for y in range(0, sy): pos = glm.vec3(x / float(sx), 0.0, y / float(sy)) for i in range(0, 4): pos.y += pow(1.0, -i) * noise2(pos.x + sample.x, pos.z + sample.y, i + 1) pos.y /= 4.0 pos.y = 0.5 * pos.y + 0.5 pos.y *= abs(pos.x-.5) + abs(pos.z-.5) # self.image.putpixel((x, y), int(pos.y * 255)) # pos.y = pos.x self.positions[y * sx + x] = list(pos) texcoords[y * sx + x] = [pos.x, pos.z] # image.close() # Specify indices and normals i = 0 for x in range(0, sx - 1): for y in range(0, sy - 1): indices[i] = y * sx + x indices[i + 1] = (y + 1) * sx + x indices[i + 2] = y * sx + x + 1 v0 = self.positions[indices[i]] v1 = self.positions[indices[i + 1]] v2 = self.positions[indices[i + 2]] normal = np.cross(v1 - v0, v2 - v0) normal /= np.linalg.norm(normal) normals[indices[i]] = normal normals[indices[i + 1]] = normal normals[indices[i + 2]] = normal i += 3 indices[i] = y * sx + (x + 1) indices[i + 1] = (y + 1) * sx + x indices[i + 2] = (y + 1) * sx + (x + 1) v0 = self.positions[indices[i]] v1 = self.positions[indices[i + 1]] v2 = self.positions[indices[i + 2]] normal = np.cross(v1 - v0, v2 - v0) normal /= np.linalg.norm(normal) normals[indices[i]] = normal normals[indices[i + 1]] = normal normals[indices[i + 2]] = normal i += 3 GRASS = glm.vec3(0.35, 0.65, 0.25) SNOW = glm.vec3(0.925, 0.93, 0.95) for x in range(0, sx): for y in range(0, sy): i = y * sx + x p = self.positions[i] normal = glm.vec3(float(normals[i, 0]), float(normals[i, 1]), float(normals[i, 2])) angle = max(glm.dot(normal, glm.vec3(0.0, 1.0, 0.0)), 0.0) h = float(p[1]) color = glm.mix(GRASS, SNOW * h, pow(angle, 10.0)) colors[i] = [color.x, color.y, color.z] self.mesh = mesh.Mesh(self.positions, normals, texcoords, colors, indices)
def stepPG(self, a): #cp.enable() if self.robotLibOn: #print(self.last_state) if self.frame == 50: self.robotLib.executeCommand("cmd#initIdle") if self.frame == 199: forDebug = 0 if self.frame == 200: forDebug = 0 self.robotLib.executeCommand("cmd#testAction") a = self.calculateAnalyticalActions( self.last_state) / self.actionsMult if not self.scene.multiplayer: # if multiplayer, action first applied to all robots, then global step() called, then step() for all robots with the same actions self.apply_action(a) self.scene.global_step() state = self.calc_state() # also calculates self.joints_at_limit alive_multiplier = 1.0 alive = float(self.alive_bonus(state)) if alive < 0 and self.frame == 0: print("bad transition") alive *= alive_multiplier #if alive<0: # alive = -100.0 done = alive < 0 if not np.isfinite(state).all(): print("~INF~", state) done = True potential_old = self.potential self.potential = self.calc_potential() progress = float(self.potential - potential_old) self.progressHistory.append(progress) progressMultiplier = 2.0 progress *= progressMultiplier #''' progressDirMultiplier = 1.0 #10.0 self.moveDirNormalizer = glm.normalize(self.movement_per_frame) progressDir = glm.dot(self.moveDirNormalizer, self.movement_dir) progressDir *= progressDirMultiplier progress += progressDir progressDirMultiplier = 0.2 #10.0 progressDir = glm.dot(self.body_frontVec, self.movement_dir) progressDir *= progressDirMultiplier progress += progressDir #''' ''' if progress<0: progress = -1.0 else: progress = 1.0 ''' ''' if self.walk_target_dist>0.1 and self.prev_body_xyz is not None: vecStep = [self.prev_body_xyz[0] - self.body_xyz[0],self.prev_body_xyz[1] - self.body_xyz[1]] #, self.prev_body_xyz[2] - self.body_xyz[2]] vecMovement = np.linalg.norm( vecStep ) minSpeedToPunishPerSec = 0.01 # 1 cm minMovePerFrame = minSpeedToPunishPerSec/self.numStepsPerSecond if vecMovement<minMovePerFrame: progress = -1 ''' for i, f in enumerate(self.feet): contact_names = set(x.name for x in f.contact_list()) #print("CONTACT OF '%s' WITH %s" % (f.name, ",".join(contact_names)) ) self.feet_contact[i] = 1.0 if (self.foot_ground_object_names & contact_names) else 0.0 maxServoAnglePerFrame = self.max_servo_speed * self.physics_time_step angleDiffSpeed_cost_max = 0.0 angleDiffSpeed_cost_min = 1.0 angleDiffSpeed_cost_multiplier = -0.08 for n, j in enumerate(self.ordered_joints): ratio = min(abs(j.targetAngleDelta), np.pi) / np.pi #ratio = min(abs(j.targetAngleDeltaFrame),maxServoAnglePerFrame)/maxServoAnglePerFrame ratio *= j.energy_cost ratio += (1.0 - j.newCurTargetReachable) * 0.6 angleDiffSpeed_cost_min = min(ratio, angleDiffSpeed_cost_min) angleDiffSpeed_cost_max = max(ratio, angleDiffSpeed_cost_max) lerpFactor = 0.7 angleDiffSpeed_cost = angleDiffSpeed_cost_min + lerpFactor * ( angleDiffSpeed_cost_max - angleDiffSpeed_cost_min) angleDiffSpeed_cost *= angleDiffSpeed_cost_multiplier #energy cost for non contacted chain is better # use joint_to_foot self.rewards_progress.append(progress) self.reward_alive.append(alive) self.reward_angleDiff.append(angleDiffSpeed_cost) self.rewards = [ alive, progress, angleDiffSpeed_cost, #electricity_cost, #joints_at_limit_cost, #feet_collision_cost ] self.frame += 1 rewardSummary = {} if (done and not self.done) or self.frame == self.spec.max_episode_steps: self.episode_over(self.frame) done = True reward_progress = np.sum(self.rewards_progress) reward_angleDiff = np.sum(self.reward_angleDiff) reward_alive = np.sum(self.reward_alive) rewardTotal = reward_progress + reward_angleDiff + reward_alive rewardSummary = { "alive": reward_alive, "progress": reward_progress, "servo": reward_angleDiff, "distToTarget": self.walk_target_dist, "episode_steps": self.frame, "episode_reward": rewardTotal } if self.debugStats: print("Episode stats::") print("Reward_progress: ", reward_progress) print("Reward_angleDiff: ", reward_angleDiff) print("Reward_alive: ", reward_alive) print("Reward_total: ", reward_alive + reward_progress + reward_angleDiff) self.reward_alive.clear() self.rewards_progress.clear() self.reward_angleDiff.clear() #find projection to target vector to figureout dist walked #distWalked = np.linalg.norm( [self.body_xyz[1],self.body_xyz[0]]) #print("Dist Walked: ",distWalked) self.done += done # 2 == 1+True if bool(done) and self.frame == 1: print("First frame done - something bad happended") self.reward += sum(self.rewards) self.HUD(state, a, done) #cp.disable() #cp.print_stats() if done: debugBrek = 0 return state, sum(self.rewards), bool(done), rewardSummary
glm_create_mat4 = lambda: glm.mat4() numpy_create_mat4 = lambda: numpy.identity(4, dtype=numpy.float32) glm_v3 = glm.vec3() numpy_v3 = numpy.array((0, 0, 0), dtype=numpy.float32) glm_v4 = glm.vec4() numpy_v4 = numpy.zeros( (4, ), dtype=numpy.float32) #numpy.array((0,0,0,0), dtype=numpy.float32) glm_m44 = glm.mat4() numpy_m44 = numpy.identity(4, dtype=numpy.float32) glm_dot = lambda: glm.dot(glm_v3, glm_v3) numpy_dot = lambda: numpy.vdot(numpy_v3, numpy_v3) glm_transpose = lambda: glm.transpose(glm_m44) numpy_transpose = lambda: numpy.transpose(numpy_m44) glm_mul_v4_v4 = lambda: glm_v4 * glm_v4 numpy_mul_v4_v4 = lambda: numpy_v4 * numpy_v4 glm_mul_m44_v4 = lambda: glm_m44 * glm_v4 numpy_mul_m44_v4 = lambda: numpy_m44 * numpy_v4 glm_mat4_getitem = lambda: glm_m44[0] numpy_mat4_getitem = lambda: numpy_m44[0] print("How PyGLM's performance roughly compares to NumPy's performance:")
def get_height(self, pos): # return self.collider.half_size.y return self.collider.half_size.y + glm.dot( self.pos.xz - pos, self.collider.forward) * -self.slope