def check_collision(self, segment: List[Tuple[float, float]], compute_distance: bool = False) -> \
Tuple[Tuple[float, float], float]:
"""Determines if a segment intersects with the map.
Args:
segment: Sensor ray or motion trajectory in the format [(start), (end)].
compute_distance: True to compute the distance between the robot and the intersection point.
Returns:
intersection: Closest collision point (x, y) [m].
distance: Distance to the obstacle [m]. inf if not computed.
"""
intersections = []
distance = float('inf')
index = 0
try:
if self._region_segments is not None:
r, c = self._xy_to_rc(segment[0])
map_segments = self._region_segments[r][c]
else:
map_segments = self._map_segments
if self._intersect is not None:
xi = ct.c_double(0.0)
yi = ct.c_double(0.0)
xp = ct.POINTER(ct.c_double)(xi)
yp = ct.POINTER(ct.c_double)(yi)
for map_segment in map_segments:
found = self._intersect.segment_intersect(
xp, yp,
segment[0][0], segment[0][1], segment[1][0], segment[1][1],
map_segment[0][0], map_segment[0][1], map_segment[1][0], map_segment[1][1]
)
if found:
intersections.append((xi.value, yi.value))
else:
from intersect import Intersect
intersect = Intersect()
for map_segment in map_segments:
pt = intersect.segment_intersect(segment, map_segment)
if pt is not None:
intersections.append(pt)
if (compute_distance and intersections) or len(intersections) > 1:
distances = [math.sqrt((pt[0] - segment[0][0]) ** 2 + (pt[1] - segment[0][1]) ** 2) for pt in intersections]
index = int(np.argmin(distances))
distance = distances[index]
except IndexError:
pass # Sensor rays may be outside the map even though the center of the robot is within it.
intersection = intersections[index] if intersections else []
return intersection, distance
def play_game(self):
"""move the ball, check for collisions against edges and objects"""
blnCollision = False
# Check if there are any blocks left | Increment level if all destroyed
if len(self.block_manager.blocks) == 0:
self.level += 1
self.level_label.increment_level()
self.block_manager.add_blocks(lvl=self.level)
# Check if collided with gutter | Computer wins
elif self.ball.location[1][0] <= self.screen_dims[1][0]:
blnCollision = True
self.computer_scoreboard.increment_score(
) # Computer player gets a point
self.reset_game() # Reset Game
# Check if collided with wall (Left/Right) | Invert Ball
elif self.ball.location[0][
0] - self.ball.move_increment < self.screen_dims[0][
0] or self.ball.location[0][
1] + self.ball.move_increment > self.screen_dims[0][1]:
blnCollision = True
self.ball.invert_ball(blnReverse=True)
self.ball.move()
# Check if collided with wall (Top) | Invert Ball & Change Direction
elif self.ball.location[1][
1] + self.ball.move_increment > self.screen_dims[1][1]:
blnCollision = True
self.ball.change_direction()
self.ball.invert_ball(blnReverse=False)
self.ball.move()
# Check if collided with paddle | Invert Ball & Change Direction
elif Intersect(self.ball.location, self.paddle.location):
blnCollision = True
self.ball.change_direction()
self.ball.invert_ball(blnReverse=False)
self.ball.move()
# Check if collided with block | Human wins
else:
blocks_to_remove = []
for key in self.block_manager.blocks:
block_x = self.block_manager.blocks[key]
if Intersect(self.ball.location, block_x.location):
blnCollision = True
block_x.destroy_block() # Hide Block
blocks_to_remove.append(key) # Mark for removal
# Invert Ball
self.ball.change_direction()
self.ball.invert_ball(blnReverse=False)
self.ball.move()
# Human player gets a point
self.human_scoreboard.increment_score()
break
# Remove block from block manager
for key in blocks_to_remove:
self.block_manager.blocks.pop(key)
# Otherwise just move the ball
if not blnCollision:
self.ball.move()
return blnCollision
def rayIntersect(self, orig, direction):
epsilon = 0.001
boundsMin = [0, 0, 0]
boundsMax = [0, 0, 0]
for i in range(3):
boundsMin[i] = self.position[i] - (epsilon + self.size / 2)
boundsMax[i] = self.position[i] + (epsilon + self.size / 2)
t = float('inf')
intersect = None
for plane in self.planes:
planeInter = plane.rayIntersect(orig, direction)
if planeInter is not None:
if planeInter.point[0] >= boundsMin[0] and planeInter.point[
0] <= boundsMax[0]:
if planeInter.point[1] >= boundsMin[
1] and planeInter.point[1] <= boundsMax[1]:
if planeInter.point[2] >= boundsMin[
2] and planeInter.point[2] <= boundsMax[2]:
if planeInter.distance < t:
t = planeInter.distance
intersect = planeInter
if intersect is None:
return None
return Intersect(distance=intersect.distance,
point=intersect.point,
normal=intersect.normal)
def side(self, v0, v1, v2, origin, direction):
v0v1 = sub(v1, v0)
v0v2 = sub(v2, v0)
N = cross(v0v1, v0v2)
raydirection = dot(N, direction)
if abs(raydirection) < 0.0001:
return None
d = dot(N, v0)
t = (dot(N, origin) + d) / raydirection
if t < 0:
return None
P = sum(origin, mul(direction, t))
U, V, W = barycentric(v0, v1, v2, P)
if U < 0 or V < 0 or W < 0:
return None
else:
return Intersect(distance = d,
point = P,
normal = norm(N))
def getIntersect(self, ray):
e1 = self.v[1] - self.v[0]
e2 = self.v[2] - self.v[0]
h = ray.v.cross(e2)
a = e1.dot(h)
if -0.00001 < a < 0.00001:
return None
f = 1 / a
s = ray.o - self.v[0]
u = f * s.dot(h)
if u < 0 or u > 1:
return None
q = s.cross(e1)
v = f * q.dot(ray.v)
if v < 0 or v + u > 1:
return None
t = f * e2.dot(q)
if t > 0.00001:
p = ray.o + t * ray.v
if self.n is not None:
n = (u * self.n[1] + v * self.n[2] +
(1 - u - v) * self.n[0]).normalized()
else:
n = e1.cross(e2).normalized()
if self.m.bump is not None:
coord = u * self.c[1] + v * self.c[2] + (1 - u - v) * self.c[0]
n = self.m.getNormal(coord.arr[0], coord.arr[1])
if self.c is not None:
coord = u * self.c[1] + v * self.c[2] + (1 - u - v) * self.c[0]
c = self.m.getColor(coord.arr[0], coord.arr[1])
else:
c = array([1., 1., 1.])
return Intersect(p, n, self.m, c)
def getIntersect(self, ray):
l = (self.pos - ray.o).dot(ray.v)
if l < 0:
return None
l *= l
d = (self.pos - ray.o).dot(self.pos - ray.o)
t = self.r**2 - d + l
if t < 0:
return None
s = l**0.5 - t**0.5
p = ray.o + ray.v * s
n = (p - self.pos).normalized()
if self.m.tex is not None:
x = n.dot(self.X)
y = n.dot(self.Y)
z = n.dot(self.Z)
u = 0.5 + atan2(z, x) / (2 * pi)
v = 0.5 - asin(1 if y > 1 else -1 if y < -1 else y) / pi
c = self.m.getColor(u, v)
else:
c = array([1., 1., 1.])
if self.m.bump is not None:
Z = n
X = self.X
Y = Z.cross(X)
b = self.m.getNormal(u, v)
n = (X * b.arr[0] + Y * b.arr[1] + Z * b.arr[2]).normalized()
return Intersect(p, n, self.m, c)
def detect_collision(self, player_location):
"""identify if turtle has is within any car location"""
blnCollision = False
for key in self.cars:
car_x = self.cars[key]
if Intersect(player_location, car_x.location):
blnCollision = True
break
return blnCollision
def rayIntersect(self, orig, dir):
# t = (( position - origRayo) dot normal) / (dirRayo dot normal)
denom = dot(dir, self.normal)
if abs(denom) > 0.0001:
t = dot(self.normal, sub(self.position, orig)) / denom
if t > 0:
# P = O + tD
hit = sum(orig, mul(dir, t))
return Intersect(distance = t,
point = hit,
normal = self.normal)
return None
def ray_intersect(self, orig, direction):
L = sub(self.center, orig)
tca = dot(L, direction)
l = length(L)
d2 = l**2 - tca**2
if d2 > self.radius**2:
return None
thc = (self.radius**2 - d2)**1 / 2
t0 = tca - thc
t1 = tca + thc
if t0 < 0:
t0 = t1
if t0 < 0:
return None
hit = sum(orig, mul(direction, t0))
normal = norm(sub(hit, self.center))
return Intersect(distance=t0, point=hit, normal=normal)
def rayIntersect(self, origin, direction):
v0, v1, v2, v3 = self.arrPoints
sides = [
self.side(v0, v2, v3, origin, direction),
self.side(v0, v2, v1, origin, direction),
self.side(v1, v2, v3, origin, direction),
self.side(v0, v3, v1, origin, direction)
]
t = float('inf')
intersect = None
for side in sides:
if side is not None:
if side.distance < t:
t = side.distance
intersect = side
if intersect is None:
return None
return Intersect(distance = intersect.distance,
point = intersect.point,
normal = intersect.normal)
def play_game(self):
"""move the laser point, check for collisions against edges and objects. Move the space invaders"""
# Set equal to false incase there aren't any lasers yet
blnCollision = False
lasers_to_remove = []
for laser_key in self.laser_manager.lasers:
# Define laser_point
laser_point = self.laser_manager.lasers[laser_key]
blnCollision = False
# Check if there are any invaders left | Increment level if all destroyed
if len(self.invader_manager.invaders) == 0:
self.invader_manager.add_invaders()
# Check if collided with gutter | Remove laser point
elif laser_point.location[1][0] <= self.screen_dims[1][0]:
blnCollision = True
laser_point.destroy_laser() # Hide Laser Point
lasers_to_remove.append(laser_key) # Mark for removal
# Check if collided with wall (Left/Right) | Remove laser point
elif laser_point.location[0][
0] - laser_point.move_increment < self.screen_dims[0][
0] or laser_point.location[0][
1] + laser_point.move_increment > self.screen_dims[
0][1]:
blnCollision = True
laser_point.destroy_laser() # Hide Laser Point
lasers_to_remove.append(laser_key) # Mark for removal
# Check if collided with wall (Top) | Remove laser point
elif laser_point.location[1][
1] + laser_point.move_increment > self.screen_dims[1][1]:
blnCollision = True
laser_point.destroy_laser() # Hide Laser Point
lasers_to_remove.append(laser_key) # Mark for removal
# Check if collided with spaceship | Lose Life!
elif Intersect(laser_point.location, self.spaceship.location
) and laser_point.direction == "Down":
blnCollision = True
laser_point.destroy_laser() # Hide Laser Point
lasers_to_remove.append(laser_key) # Mark for removal
if self.lives_board.lives > 0:
# Continue playing, spaceship loses a life
self.lives_board.decrease_lives()
if self.lives_board.lives == 0:
self.game_over = True
break
else:
# Exit loop
self.game_over = True
break
# Check if collided with invader | Spaceship wins
else:
invaders_to_remove = []
for invader_key in self.invader_manager.invaders:
invader_x = self.invader_manager.invaders[invader_key]
if Intersect(laser_point.location, invader_x.location
) and laser_point.direction == "Up":
blnCollision = True
score_x = invader_x.invader_score
invader_x.destroy_invader() # Hide Invader
invaders_to_remove.append(
invader_key) # Mark for removal
laser_point.destroy_laser() # Hide Laser Point
lasers_to_remove.append(laser_key) # Mark for removal
# Spaceship gets points
self.score_board.increment_score(score_inc=score_x)
break
# Remove invader from invader manager
for invader_key in invaders_to_remove:
self.invader_manager.invaders.pop(invader_key)
# Otherwise just move the laser point
if not blnCollision:
laser_point.move()
# Remove laser points from laser manager
for laser_key in lasers_to_remove:
self.laser_manager.lasers.pop(laser_key)
return blnCollision