def adjust_position(self):
self.prev_x = self.x
if len(self.curve_queue) > 1 and self.missile_fired is False:
slope = (player.y - self.y) / (player.x - self.x)
print("==============")
print("Enemy location:", self.x, self.y)
print("PLayer location:", player.x, player.y)
self.fire_enemy_missile(slope)
print("============")
self.missile_fired = True
if len(self.curve_queue) != 0:
self.waypoint = self.curve_queue[len(self.curve_queue) - 1].end_pnt
if len(self.curve_queue) == 1:
self.curve_queue[len(self.curve_queue) - 1].increase_velocity()
self.x = self.curve_queue[len(self.curve_queue) -
1].calculate_point()[0]
self.y = self.curve_queue[len(self.curve_queue) -
1].calculate_point()[1]
if self.curve_queue[len(self.curve_queue) - 1].t >= 1:
self.curve_queue.pop()
if len(self.curve_queue
) == 0 and self.y > self.initial_position[1]:
self.x = self.initial_position[0]
self.y = -1
self.curve_queue = [
BezierCurve([self.x, self.y], [self.x, self.y],
[self.x, self.initial_position[1]],
[self.x, self.initial_position[1]])
]
def init_interpolator(self):
if self._waypoints is None:
return False
self._markers_msg = MarkerArray()
self._marker_id = 0
self._interp_fcns['pos'] = list()
self._segment_to_wp_map = [0]
if self._waypoints.num_waypoints == 2:
self._interp_fcns['pos'].append(
LineSegment(
self._waypoints.get_waypoint(0).pos,
self._waypoints.get_waypoint(1).pos))
self._segment_to_wp_map.append(1)
elif self._waypoints.num_waypoints > 2:
self._interp_fcns['pos'] = BezierCurve.generate_cubic_curve([
self._waypoints.get_waypoint(i).pos
for i in range(self._waypoints.num_waypoints)
])
else:
return False
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns['pos']]
lengths = [0] + lengths
self._s = np.cumsum(lengths) / np.sum(lengths)
mean_vel = np.mean([
self._waypoints.get_waypoint(k).max_forward_speed
for k in range(self._waypoints.num_waypoints)
])
if self._duration is None:
self._duration = np.sum(lengths) / mean_vel
if self._start_time is None:
self._start_time = 0.0
if self._waypoints.num_waypoints == 2:
head_offset_line = deepcopy(
self._waypoints.get_waypoint(1).heading_offset)
self._interp_fcns['heading'] = lambda x: head_offset_line
else:
# Set a simple spline to interpolate heading offset, if existent
heading = [
self._waypoints.get_waypoint(i).heading_offset
for i in range(self._waypoints.num_waypoints)
]
self._heading_spline = splrep(self._s, heading, k=3, per=False)
self._interp_fcns['heading'] = lambda x: splev(
x, self._heading_spline)
return True
return True
def generate_bee_curves(self, choice, gunship):
if choice == 0:
self.curve_queue = [
BezierCurve(
[self.x - 1, self.y + 253], [self.x - 50, self.y + 330],
[self.x - 15, self.y + 259],
[(gunship.x - random.randint(20, 50)), self.y + 480]),
BezierCurve([self.x, self.y], [self.x - 75, self.y + 138],
[self.x + 194, self.y + 93],
[self.x - 1, self.y + 253])
]
else:
self.curve_queue = [
BezierCurve(
[self.x + 1, self.y + 253], [self.x + 50, self.y + 330],
[self.x + 15, self.y + 259],
[(gunship.x + random.randint(20, 50)), self.y + 480]),
BezierCurve([self.x, self.y], [self.x + 75, self.y + 138],
[self.x - 194, self.y + 93],
[self.x + 1, self.y + 253])
]
self.initial_dive = True
def generate_boss_curves(self, choice, gunship):
if choice == 0:
self.curve_queue = [
BezierCurve(
[self.x + 150.7, self.y + 264],
[self.x + 134.2, self.y + 321.6],
[self.x + 0.2, self.y + 433],
[(gunship.x - random.randint(20, 60)), self.y + 464]),
BezierCurve([self.x + 0.5, self.y + 95],
[self.x + 17, self.y + 152.6],
[self.x + 155, self.y + 182],
[self.x + 150.7, self.y + 264]),
BezierCurve([self.x + 98, self.y + 96],
[self.x + 106, self.y + 25],
[self.x + -1, self.y + 10.5],
[self.x + 0.5, self.y + 95]),
BezierCurve([self.x, self.y], [self.x - 48, self.y + 177],
[self.x + 102, self.y + 172],
[self.x + 98, self.y + 96])
]
else:
self.curve_queue = [
BezierCurve(
[self.x - 150.7, self.y + 264],
[self.x - 134.2, self.y + 321.6],
[self.x - 0.2, self.y + 433],
[(gunship.x + random.randint(20, 60)), self.y + 464]),
BezierCurve([self.x - 0.5, self.y + 95],
[self.x - 17, self.y + 152.6],
[self.x - 155, self.y + 182],
[self.x - 150.7, self.y + 264]),
BezierCurve([self.x - 98, self.y + 96],
[self.x - 106, self.y + 25],
[self.x - -1, self.y + 10.5],
[self.x - 0.5, self.y + 95]),
BezierCurve([self.x, self.y], [self.x + 48, self.y + 177],
[self.x - 102, self.y + 172],
[self.x - 98, self.y + 96])
]
self.initial_dive = True
def init_interpolator(self):
if self._waypoints is None:
return False
self._interp_fcns['pos'] = list()
self._segment_to_wp_map = [0]
if self._waypoints.num_waypoints == 2:
self._interp_fcns['pos'].append(
LineSegment(self._waypoints.get_waypoint(0).pos,
self._waypoints.get_waypoint(1).pos))
self._segment_to_wp_map.append(1)
# Set a simple spline to interpolate heading offset, if existent
heading = [self._waypoints.get_waypoint(k).heading_offset for k in range(self._waypoints.num_waypoints)]
elif self._waypoints.num_waypoints > 2:
q_seg = self._waypoints.get_waypoint(0).pos
q_start_line = q_seg
heading = [0]
for i in range(1, self._waypoints.num_waypoints):
first_line = LineSegment(q_start_line, self._waypoints.get_waypoint(i).pos)
radius = min(self._radius, first_line.get_length() / 2)
if i + 1 < self._waypoints.num_waypoints:
second_line = LineSegment(self._waypoints.get_waypoint(i).pos,
self._waypoints.get_waypoint(i + 1).pos)
radius = min(radius, second_line.get_length() / 2)
if i < self._waypoints.num_waypoints - 1:
q_seg = np.vstack(
(q_seg, first_line.interpolate((first_line.get_length() - radius) / first_line.get_length())))
self._interp_fcns['pos'].append(LineSegment(q_start_line, q_seg[-1, :]))
heading.append(0)
self._segment_to_wp_map.append(i)
if i == self._waypoints.num_waypoints - 1:
q_seg = np.vstack((q_seg, self._waypoints.get_waypoint(i).pos))
self._interp_fcns['pos'].append(LineSegment(q_seg[-2, :], q_seg[-1, :]))
heading.append(0)
self._segment_to_wp_map.append(i)
elif i + 1 < self._waypoints.num_waypoints:
q_seg = np.vstack((q_seg, second_line.interpolate(radius / second_line.get_length())))
self._interp_fcns['pos'].append(
BezierCurve([q_seg[-2, :], self._waypoints.get_waypoint(i).pos, q_seg[-1, :]], 5))
heading.append(0)
self._segment_to_wp_map.append(i)
q_start_line = deepcopy(q_seg[-1, :])
else:
return False
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns['pos']]
lengths = [0] + lengths
self._s = np.cumsum(lengths) / np.sum(lengths)
mean_vel = np.mean(
[self._waypoints.get_waypoint(k).max_forward_speed for k in range(self._waypoints.num_waypoints)])
if self._duration is None:
self._duration = np.sum(lengths) / mean_vel
if self._start_time is None:
self._start_time = 0.0
if self._waypoints.num_waypoints == 2:
head_offset_line = deepcopy(self._waypoints.get_waypoint(1).heading_offset)
self._interp_fcns['heading'] = lambda x: head_offset_line
else:
# Set a simple spline to interpolate heading offset, if existent
self._heading_spline = splrep(self._s, heading, k=3, per=False)
self._interp_fcns['heading'] = lambda x: splev(x, self._heading_spline)
return True
def init_interpolator(self):
if self._waypoints is None:
return False
self._interp_fcns['pos'] = list()
self._segment_to_wp_map = [0]
if self._waypoints.num_waypoints == 2:
self._interp_fcns['pos'].append(
LineSegment(
self._waypoints.get_waypoint(0).pos,
self._waypoints.get_waypoint(1).pos))
self._segment_to_wp_map.append(1)
elif self._waypoints.num_waypoints > 2:
tangents = [
np.zeros(3) for _ in range(self._waypoints.num_waypoints)
]
lengths = [
self._waypoints.get_waypoint(i + 1).dist(
self._waypoints.get_waypoint(i).pos)
for i in range(self._waypoints.num_waypoints - 1)
]
lengths = [0] + lengths
# Initial vector of parametric variables for the curve
u = np.cumsum(lengths) / np.sum(lengths)
delta_u = lambda k: u[k] - u[k - 1]
delta_q = lambda k: self._waypoints.get_waypoint(
k).pos - self._waypoints.get_waypoint(k - 1).pos
lamb_k = lambda k: delta_q(k) / delta_u(k)
alpha_k = lambda k: delta_u(k) / (delta_u(k) + delta_u(k + 1))
for i in range(1, len(u) - 1):
tangents[i] = (
1 - alpha_k(i)) * lamb_k(i) + alpha_k(i) * lamb_k(i + 1)
if i == 1:
tangents[0] = 2 * lamb_k(i) - tangents[1]
tangents[-1] = 2 * lamb_k(len(u) - 1) - tangents[-2]
# Normalize tangent vectors
for i in range(len(tangents)):
tangents[i] = tangents[i] / np.linalg.norm(tangents[i])
# Generate the cubic Bezier curve segments
for i in range(len(tangents) - 1):
self._interp_fcns['pos'].append(
BezierCurve([
self._waypoints.get_waypoint(i).pos,
self._waypoints.get_waypoint(i + 1).pos
], 3, tangents[i:i + 2]))
self._segment_to_wp_map.append(i + 1)
else:
return False
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns['pos']]
lengths = [0] + lengths
self._s = np.cumsum(lengths) / np.sum(lengths)
mean_vel = np.mean([
self._waypoints.get_waypoint(k).max_forward_speed
for k in range(self._waypoints.num_waypoints)
])
if self._duration is None:
self._duration = np.sum(lengths) / mean_vel
if self._start_time is None:
self._start_time = 0.0
# Set a simple spline to interpolate heading offset, if existent
heading = [
self._waypoints.get_waypoint(k).heading_offset
for k in range(self._waypoints.num_waypoints)
]
self._heading_spline = splrep(self._s, heading, k=3, per=False)
self._interp_fcns['heading'] = lambda x: splev(x, self._heading_spline)
return True
def main():
config = ConfigParser({"draw_curve":"True", "draw_bounds":"True", "draw_controls":"True",
"curve_color":"[0,0,0]", "control_color":"[0,0,0]", "bounds_color":"[0,0,0]",
"animation_color":"[0,\"0xffff\",0]", "throttle":"0.001"})
config.read("config.ini")
curve = BezierCurve(width=800, height=600, caption='Beziers', resizable=True)
curve.toggle_curve(config.getboolean('draw_modes', 'draw_curve'))
curve.toggle_controls(config.getboolean('draw_modes', 'draw_controls'))
curve.toggle_bounds(config.getboolean('draw_modes', 'draw_bounds'))
curve.set_curve_color(json.loads(config.get('colors', 'curve_color')))
curve.set_control_color(json.loads(config.get('colors', 'control_color')))
curve.set_bounds_color(json.loads(config.get('colors', 'bounds_color')))
curve.set_animation_color(json.loads(config.get('colors', 'animation_color')))
curve.set_throttle(config.getfloat('attributes', 'throttle'))
"""
The test that compares interpolate to static_calc_line_layer
"""
"""
controls = []
k = 200
for i in range(0, k):
controls.append((random.randint(0, 100), random.randint(0, 100)))
pool = multiprocessing.Pool()
n = 400
interp_args = []
calc_args = []
for i in range(0, n):
interp_args.append((controls, .5))
calc_args.append((controls, .5))
t1 = time()
vals = pool.map(interpolate, interp_args)
t2 = time()
print "{0} interpolates on {2} controls took {1}ms".format(n, (t2 - t1)*1000, k)
t1 = time()
vals = pool.map(static_calc_line_layer, calc_args)
t2 = time()
print "{0} static_calc_line_layers on {2} controls took {1}ms".format(n, (t2 - t1)*1000, k)
exit()
"""
curve.run()
def init_interpolator(self):
if self._waypoints is None:
return False
if self._waypoints.num_waypoints < 2:
print 'At least 2 waypoints are necessary'
return False
self._markers_msg = MarkerArray()
self._marker_id = 0
self._interp_fcns = list()
path = list()
last_heading = self._waypoints.get_waypoint(0).heading_offset
dist = lambda x, y: np.sqrt(np.sum((x - y)**2))
for i in range(1, self._waypoints.num_waypoints):
heading_init = 0.0
heading_final = 0.0
if i - 1 == 0:
heading_init = self._waypoints.get_waypoint(i - 1).heading_offset
else:
if not np.isclose(dist(self._waypoints.get_waypoint(i - 1).pos[0:2], self._waypoints.get_waypoint(i).pos[0:2]), 0):
heading_init = self._waypoints.get_waypoint(i - 1).calculate_heading(self._waypoints.get_waypoint(i))
else:
heading_init = last_heading
if i == self._waypoints.num_waypoints - 1:
if not np.isclose(dist(self._waypoints.get_waypoint(i - 1).pos[0:2], self._waypoints.get_waypoint(i).pos[0:2]), 0):
heading_final = self._waypoints.get_waypoint(i - 1).calculate_heading(self._waypoints.get_waypoint(i))
else:
heading_final = last_heading
else:
if not np.isclose(dist(self._waypoints.get_waypoint(i + 1).pos[0:2], self._waypoints.get_waypoint(i).pos[0:2]), 0):
heading_final = self._waypoints.get_waypoint(i).calculate_heading(self._waypoints.get_waypoint(i + 1))
else:
heading_final = last_heading
last_heading = heading_final
path += self._generate_path(
self._waypoints.get_waypoint(i - 1), heading_init,
self._waypoints.get_waypoint(i), heading_final)
inter_pnts = list()
for i in range(len(path) - 1):
if not np.isclose(np.sqrt(np.sum((path[i + 1] - path[i])**2)), 0):
inter_pnts += [path[i]]
if not np.isclose(np.sqrt(np.sum((path[-1] - path[-2])**2)), 0):
inter_pnts += [path[-1]]
self._interp_fcns = BezierCurve.generate_cubic_curve(inter_pnts)
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns]
lengths = [0] + lengths
self._s = np.cumsum(lengths) / np.sum(lengths)
mean_vel = np.mean(
[self._waypoints.get_waypoint(k).max_forward_speed for k in range(self._waypoints.num_waypoints)])
if self._duration is None:
self._duration = np.sum(lengths) / mean_vel
if self._start_time is None:
self._start_time = 0.0
return True
segments = list()
q_seg = np.array([q_x[0], q_y[0], q_z[0]])
q_start_line = deepcopy(q_seg)
for i in range(1, len(q_x)):
line = LineSegment(q_start_line, q[i, :])
if i < len(q_x) - 1:
q_seg = np.vstack((q_seg, line.interpolate((line.get_length() - radius) / line.get_length())))
segments.append(LineSegment(q_start_line, q_seg[-1, :]))
if i == len(q_x) - 1:
q_seg = np.vstack((q_seg, q[i, :]))
segments.append(LineSegment(q_seg[-2, :], q_seg[-1, :]))
elif i + 1 < len(q_x):
line = LineSegment(q[i, :], q[i + 1, :])
q_seg = np.vstack((q_seg, line.interpolate(radius / line.get_length())))
segments.append(BezierCurve([q_seg[-2, :], q[i, :], q_seg[-1, :]], 5))
q_start_line = deepcopy(q_seg[-1, :])
ax.plot(q_seg[:, 0], q_seg[:, 1], q_seg[:, 2], 'go')
lengths = [seg.get_length() for seg in segments]
lengths = [0] + lengths
total_length = np.sum(lengths)
u = np.cumsum(lengths) / total_length
pnts = None
deriv = None
for i in np.linspace(0, 1, 100):
idx = (u - i >= 0).nonzero()[0][0]
if idx == 0:
line = LineSegment(q_start_line, q[i, :])
if i < len(q_x) - 1:
q_seg = np.vstack(
(q_seg,
line.interpolate(
(line.get_length() - radius) / line.get_length())))
segments.append(LineSegment(q_start_line, q_seg[-1, :]))
if i == len(q_x) - 1:
q_seg = np.vstack((q_seg, q[i, :]))
segments.append(LineSegment(q_seg[-2, :], q_seg[-1, :]))
elif i + 1 < len(q_x):
line = LineSegment(q[i, :], q[i + 1, :])
q_seg = np.vstack(
(q_seg, line.interpolate(radius / line.get_length())))
segments.append(
BezierCurve([q_seg[-2, :], q[i, :], q_seg[-1, :]], 5))
q_start_line = deepcopy(q_seg[-1, :])
ax.plot(q_seg[:, 0], q_seg[:, 1], q_seg[:, 2], 'go')
lengths = [seg.get_length() for seg in segments]
lengths = [0] + lengths
total_length = np.sum(lengths)
u = np.cumsum(lengths) / total_length
pnts = None
deriv = None
for i in np.linspace(0, 1, 100):
idx = (u - i >= 0).nonzero()[0][0]
if idx == 0:
def init_interpolator(self):
if self._waypoints is None:
return False
if self._waypoints.num_waypoints < 2:
print 'At least 2 waypoints are necessary'
return False
self._markers_msg = MarkerArray()
self._marker_id = 0
self._interp_fcns = list()
path = list()
last_heading = self._waypoints.get_waypoint(0).heading_offset
dist = lambda x, y: np.sqrt(np.sum((x - y)**2))
for i in range(1, self._waypoints.num_waypoints):
heading_init = 0.0
heading_final = 0.0
if i - 1 == 0:
heading_init = self._waypoints.get_waypoint(i - 1).heading_offset
else:
if not np.isclose(dist(self._waypoints.get_waypoint(i - 1).pos[0:2], self._waypoints.get_waypoint(i).pos[0:2]), 0):
heading_init = self._waypoints.get_waypoint(i - 1).calculate_heading(self._waypoints.get_waypoint(i))
else:
heading_init = last_heading
if i == self._waypoints.num_waypoints - 1:
if not np.isclose(dist(self._waypoints.get_waypoint(i - 1).pos[0:2], self._waypoints.get_waypoint(i).pos[0:2]), 0):
heading_final = self._waypoints.get_waypoint(i - 1).calculate_heading(self._waypoints.get_waypoint(i))
else:
heading_final = last_heading
else:
if not np.isclose(dist(self._waypoints.get_waypoint(i + 1).pos[0:2], self._waypoints.get_waypoint(i).pos[0:2]), 0):
heading_final = self._waypoints.get_waypoint(i).calculate_heading(self._waypoints.get_waypoint(i + 1))
else:
heading_final = last_heading
last_heading = heading_final
path += self._generate_path(
self._waypoints.get_waypoint(i - 1), heading_init,
self._waypoints.get_waypoint(i), heading_final)
inter_pnts = list()
for i in range(len(path) - 1):
if not np.isclose(np.sqrt(np.sum((path[i + 1] - path[i])**2)), 0):
inter_pnts += [path[i]]
if not np.isclose(np.sqrt(np.sum((path[-1] - path[-2])**2)), 0):
inter_pnts += [path[-1]]
self._interp_fcns = BezierCurve.generate_cubic_curve(inter_pnts)
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns]
lengths = [0] + lengths
self._s = np.cumsum(lengths) / np.sum(lengths)
mean_vel = np.mean(
[self._waypoints.get_waypoint(k).max_forward_speed for k in range(self._waypoints.num_waypoints)])
if self._duration is None:
self._duration = np.sum(lengths) / mean_vel
if self._start_time is None:
self._start_time = 0.0
return True
