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
