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: