def draw(self, game, surface, erase):
color = (255., 255., 255.) if erase else (200., 200., 200.)
N = 72
phi = 4 * math.pi * self._time_to_charge / self._recharge_period
for i in range(N):
if self._time_to_charge == 0:
vector = utils.polar2cartesian(
[self._max_range, 2. * math.pi * i / N])
pos = [
self._holder.pos[0] + vector[0],
self._holder.pos[1] + vector[1]
]
draw_pos = [int(pos[0]), int(pos[1])]
surface.set_at(draw_pos, color)
else:
vector = utils.polar2cartesian([
self._holder.radius +
(self._max_range - self._holder.radius) * i / N,
phi + 2. * math.pi * i / N
])
pos = [
self._holder.pos[0] + vector[0],
self._holder.pos[1] + vector[1]
]
draw_pos = [int(pos[0]), int(pos[1])]
surface.set_at(draw_pos, color)
def draw(self, game, surface, erase):
N = 50
if erase:
color = (255., 255., 255.)
else:
color = (0., 0.,
255.) if self._holder == game.get_player() else (255., 0.,
0.)
self._gfx_phi.clear()
self._gfx_radii.clear()
for i in range(N):
self._gfx_phi.append(random() * 2 * math.pi)
self._gfx_radii.append(self._holder.radius + 6. +
random() * 6.0)
for i in range(N):
vector_start = utils.polar2cartesian(
[self._gfx_radii[i], self._gfx_phi[i]])
vector_end = utils.polar2cartesian(
[self._holder.radius + 5., self._gfx_phi[i]])
pos_start = [
round(self._holder.pos[0] + vector_start[0] - 1.),
round(self._holder.pos[1] + vector_start[1] - 1.)
]
pos_end = [
round(self._holder.pos[0] + vector_end[0] - 1.),
round(self._holder.pos[1] + vector_end[1] - 1.)
]
pygame.draw.line(surface, color, pos_start, pos_end)
def draw(self, game, surface, erase):
if erase:
color_inner = (255., 255., 255.)
color_outer = (255., 255., 255.)
color_charged = (255., 255., 255.)
else:
color_inner = [255. - 155. * (self.hp % 1.)] * 3
color_outer = (0., 0., 0.)
color_intensity = 150. * self.charge if self.charge < 1. else 255.
color_charged = (
0., 0., color_intensity) if self == game.get_player() else (
color_intensity, 0., 0.)
draw_pos = (int(self.pos[0]), int(self.pos[1]))
for i in range(int(self.hp)):
if i == 0:
pygame.draw.circle(surface, color_charged, draw_pos,
int(self.radius), 1)
elif self.radius > 2 * i:
pygame.draw.circle(surface, color_outer, draw_pos,
int(self.radius) - 2 * i, 1)
if self.hp % 1 > 0.1 and self.radius > 2 * (i + 1):
pygame.draw.circle(surface, color_inner, draw_pos,
int(self.radius) - 2 * (i + 1), 1)
if self.target_shoot:
color = (
255.,
255.,
255.,
) if erase else (50., 50., 50.)
vector = [
self.target_shoot[0] - self.pos[0],
self.target_shoot[1] - self.pos[1]
]
phi = utils.cartesian2polar(vector)[1]
points = []
for vertex in [
utils.polar2cartesian(
[self.radius + 3., phi - 0.05 * math.pi]),
utils.polar2cartesian([self.radius + 6., phi]),
utils.polar2cartesian(
[self.radius + 3., phi + 0.05 * math.pi])
]:
points.append([
round(self.pos[0] + vertex[0]),
round(self.pos[1] + vertex[1])
])
pygame.draw.lines(surface, color, True, points, 1)
def draw(self, game, surface, erase):
if erase:
color = (255., 255., 255.)
else:
color = (50., 50.,
200.) if self._holder == game.get_player() else (200.,
50., 50.)
for i in range(self._count):
phi = utils.cartesian2polar([
self._holder.target_shoot[0] -
self._holder.pos[0], self._holder.target_shoot[1] -
self._holder.pos[1]
])[1] if self._holder.target_shoot else -0.5 * math.pi
vector = utils.polar2cartesian([
1.,
phi - self._spread + 2. * self._spread * i / (self._count - 1.)
])
N = 10
for idx in range(N // 2, N + N // 2):
draw_pos = [
int(self._holder.pos[0] +
2. * self._holder.radius * vector[0] * idx / N),
int(self._holder.pos[1] +
2. * self._holder.radius * vector[1] * idx / N),
]
surface.set_at(draw_pos, color)
def position_shift(obj, game):
max_speed = 5.
if utils.norm(obj.speed) > max_speed:
obj.speed = utils.normalize(obj.speed, max_speed)
if obj.target_move:
vector = [
obj.target_move[0] - obj.pos[0],
obj.target_move[1] - obj.pos[1]
]
angle_target = utils.cartesian2polar(vector)[1]
if utils.dist(obj.pos, obj.target_move) < obj.rotation_radius:
angle_target += 0.5 * math.pi + math.pi / 60.
angle_max_shift = 2. * math.pi
elif utils.dist(obj.pos,
obj.target_move) > obj.rotation_radius > 0:
angle_target += 0.5 * math.pi - math.pi / 60.
angle_max_shift = 2. * math.pi
else:
angle_max_shift = math.pi / 60.
obj_speed_polar = utils.cartesian2polar(obj.speed)
obj_speed_polar[1] = utils.periodic_shift_to(
obj_speed_polar[1], angle_target, angle_max_shift)
obj.speed = utils.polar2cartesian(obj_speed_polar)
game.update_obj_pos(obj)
def spread_shot(obj, game, bullet):
ro, phi = utils.cartesian2polar(bullet.speed)
for i in range(self._count):
angle = phi - self._spread + 2. * self._spread * i / (
self._count - 1.)
bullet_new = copy.copy(bullet)
bullet_new.pos = bullet.pos.copy()
bullet_new.speed = utils.polar2cartesian([ro, angle])
game.add_effect(bullet_new)
bullet.is_active = False
def draw(self, game, surface, erase):
if erase:
color = (255., 255., 255.)
else:
color = (0., 0.,
255.) if self._holder == game.get_player() else (255., 0.,
0.)
points = []
for vertex in [
utils.polar2cartesian(
[self._holder.radius + 6., self._phi - 0.05 * math.pi]),
utils.polar2cartesian([self._holder.radius + 9., self._phi]),
utils.polar2cartesian(
[self._holder.radius + 6., self._phi + 0.05 * math.pi])
]:
points.append([
round(self._holder.pos[0] + vertex[0]),
round(self._holder.pos[1] + vertex[1])
])
pygame.draw.lines(surface, color, True, points, 1)
def homing_shot(obj, game, to_hit):
o = obj.source
if not o or to_hit.defenders:
return
if o != self._holder:
raise EnvironmentError(
"Source of bullet is not holder of modifier")
bullet = self._holder.shoot(
o, game, 0., utils.polar2cartesian([o.bullet_speed,
self._phi]), self._damage)
bullet.target_move = to_hit.pos
def draw(self, game, surface, erase):
if erase:
color = (
255.,
255.,
255.,
)
else:
color = (0., 0.,
255.) if self._holder == game.get_player() else (255., 0.,
0.)
shift = utils.polar2cartesian([
self._rotation_radius,
2. * math.pi * self._spawn_countdown / self._spawn_period
])
draw_pos = [
round(self._holder.pos[0] + shift[0]),
round(self._holder.pos[1] + shift[1])
]
pygame.draw.circle(surface, color, draw_pos, 3, 1)
def shoot(obj, game, charge_cost=0.1, speed=None, damage=None):
if obj.charge < charge_cost:
return
obj.charge -= charge_cost
if not speed:
if obj.target_shoot:
vector = [
obj.target_shoot[0] - obj.pos[0],
obj.target_shoot[1] - obj.pos[1]
]
speed = utils.normalize(vector, obj.bullet_speed)
else:
speed = utils.polar2cartesian(
[obj.bullet_speed, 2 * math.pi * random()])
if not damage:
damage = obj.bullet_damage
bullet = Bullet(obj.pos.copy(), speed, obj, damage)
game.add_effect(bullet)
for func in obj.on_shoot:
func(obj, game, bullet)
return bullet
def spawn_bullet(self, game):
if not self.is_active or not self._holder.is_active:
return #TODO find why this happens
if self._spawn_countdown == 0:
self._spawn_countdown = self._spawn_period
self._bullets = [o for o in self._bullets if o.is_active]
if len(self._bullets) == self._max_bullets:
return
shift = utils.polar2cartesian([
self._rotation_radius,
2. * math.pi * self._spawn_countdown / self._spawn_period
])
spawn_pos = [
self._holder.pos[0] + shift[0], self._holder.pos[1] + shift[1]
]
bullet = Bullet(spawn_pos, [0., 3.], self._holder, self._damage)
bullet.target_move = self._holder.pos
bullet.rotation_radius = self._rotation_radius
game.add_effect(bullet)
self._bullets.append(bullet)
self._game.append(game)
self._spawn_countdown -= 1
def add_landmarks(self, eta: np.ndarray, P: np.ndarray,
z: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
"""Calculate new landmarks, their covariances and add them to the state.
Parameters
eta : np.ndarray, shape=(3 + 2*#landmarks,) : the robot state and map concatenated
P : np.ndarray, shape=(3 + 2*#landmarks,)*2 : the covariance of eta
z : np.ndarray, shape(2 * #newlandmarks,) : A set of measurements to create landmarks for
Returns
Tuple[np.ndarray, np.ndarray], shapes=(3 + 2*(#landmarks + #newlandmarks,), (3 + 2*(#landmarks + #newlandmarks,)*2
eta with new landmarks appended, and its covariance
"""
n = P.shape[0]
assert z.ndim == 1, "SLAM.add_landmarks: z must be a 1d array"
numLmk = z.shape[0] // 2
lmnew = np.empty_like(z)
Gx = np.empty((numLmk * 2, 3))
Rall = np.zeros((numLmk * 2, numLmk * 2))
I2 = np.eye(2) # Preallocate, used for Gx
sensor_offset_world = rotmat2d(
eta[2]
) @ self.sensor_offset # For transforming landmark position into world frame
sensor_offset_world_der = rotmat2d(
eta[2] + np.pi / 2) @ self.sensor_offset # Used in Gx
for j in range(numLmk):
ind = 2 * j
inds = slice(ind, ind + 2)
zj = z[inds]
# calculate pos of new landmark in world frame
zj_x, zj_y = utils.polar2cartesian(zj[0], zj[1])
lmnew[inds] = rotmat2d(eta[2]) @ np.array([zj_x, zj_y]).reshape(
2, ) - sensor_offset_world
Gx[inds, :2] = I2
Gx[inds, 2] = zj[0] * np.array([
-np.sin(zj[1] + eta[2]),
np.cos(zj[1] + eta[2])
]).reshape(2, ) + sensor_offset_world_der
Gz = rotmat2d(zj[1] + eta[2]) @ np.diag((1, zj[0]))
# Gz * R * Gz^T, transform measurement covariance from polar to cartesian coordinates
Rall[inds, inds] = Gz @ self.R @ Gz.T
assert len(lmnew) % 2 == 0, "SLAM.add_landmark: lmnew not even length"
etaadded = np.append(
eta, lmnew) # [eta; G() ] : append new landmarks to state vector
low_r = Gx @ P[:3, :3] @ Gx.T + Rall # + Gz @ self.R @ Gz.T
P_added = la.block_diag(P, low_r) # block diagonal of P_new
P_added[:n, n:] = P[:, :3] @ Gx.T # top right corner of P_new
P_added[n:, :n] = P_added[:n, n:].T # transpose of above
assert (
etaadded.shape * 2 == P_added.shape
), "EKFSLAM.add_landmarks: calculated eta and P has wrong shape"
assert np.allclose(
P_added, P_added.T), "EKFSLAM.add_landmarks: P_added not symmetric"
assert np.all(np.linalg.eigvals(P_added) >= 0
), "EKFSLAM.add_landmarks: P_added not PSD"
return etaadded, P_added