def get_route(start_pt, end_pt, wind_direction, speed, plot_file): """ Polar OST algorithm caller. Args: start_pt (tuple): Cartesian coordinates of start end_pt (tuple): Cartesian coordinates of end wind_direction (double) : Cardinal direction in degrees plot_file (string) : path to csv file of a polar plot Returns: route (list): List of tuples representing the fastest route from start_pt -> end_pt!! """ plot = PolarPlot(plot_file) # Gets the unit circle direction of the desired direction desired_angle = m.atan2(end_pt[1] - start_pt[1], end_pt[0] - start_pt[0]) # Makes it a cardinal direction (bearings) desired_direction = radians_to_cardinal(desired_angle) # Finds vmg for route to destination vmg_direction = plot.find_optimal_angle(wind_direction, speed, desired_direction) # Route always includes start point route = [start_pt] # Evaluates if vmg is within degree of error in the plot # Also, if desired route is in no go zone, you must tack desired_diff = abs(angle_difference(vmg_direction, desired_direction)) if desired_diff > PLOT_ERROR_ANGLE or plot.in_nogo_zone( desired_direction, wind_direction, speed): # Construct vectors start = Vec2(start_pt[0], start_pt[1]) end = Vec2(end_pt[0], end_pt[1]) # Taking a Tack is optimal # Convert back to unit circle to get vector vmg_angle = cardinal_transform(vmg_direction) wind_angle = cardinal_transform(wind_direction) # Getting vectors vmg_vec = Vec2.polar(vmg_angle, 1.0) wind_vec = Vec2.polar(wind_angle, 1.0) # Needed for reflection perpwind_vec = wind_vec.perpendicular() # Reflect vmg along wind to get second tack line reflectvmg_vec = vmg_vec.reflect(perpwind_vec) #reflectvmg_vec = vmg_vec.reflect(reverse_wind) reflectvmg_vec = reflectvmg_vec.normalized() # Get another point on each line start2 = Vec2(start.x + vmg_vec.x, start.y + vmg_vec.y) end2 = Vec2(end.x + reflectvmg_vec.x, end.y + reflectvmg_vec.y) # Get intersection intersect_pt = find_intersection(start_pt, [start2.x, start2.y], end_pt, [end2.x, end2.y]) # Push to route route.append(intersect_pt) # The line from planar is trash, isn't useful at all # first_line = Line(reflectvmg_vec, start) # second_line = Line(reflectvmg_vec, end) # Return end point route.append(end_pt) return route
def test_character_collision_2_chars(self, patched): character = Character( character_id=1, initial_position=Vec2(0, 0), initial_viewport=Vec2(0, 1)) self.world.load_character(character, timestamp=0) character2 = Character( character_id=2, initial_position=Vec2(30, 30), initial_viewport=Vec2(0, 1)) self.world.load_character(character2, timestamp=0) self.world.update_character( character, velocity=Vec2(1, 1), viewport=Vec2.polar(angle=45, length=1), timestamp=0) self.world.update_character( character2, velocity=Vec2(-1, -1), viewport=Vec2.polar(angle=45, length=1), timestamp=0) self.loop.run_until_complete(asyncio.sleep(0.1, loop=self.loop)) self.assertEqual( patched.call_count, 2, "`World.notify` was not called on character passive move") self.assertEqual( tuple(patched.call_args), ((), dict( affects=[character], event="character_move", character=character, timestamp=0 ))) self.loop.run_until_complete(asyncio.sleep(1, loop=self.loop)) self.assertEqual( patched.call_count, 3, "`World.notify` was not called on character passive move") self.assertEqual( tuple(patched.call_args), ((), dict( affects=[character], event="character_move", character=character, timestamp=1 ))) self.assertEqual( character.position, Vec2(1, 1))
def test_character_collision_2_chars(self, patched): character = Character(character_id=1, initial_position=Vec2(0, 0), initial_viewport=Vec2(0, 1)) self.world.load_character(character, timestamp=0) character2 = Character(character_id=2, initial_position=Vec2(30, 30), initial_viewport=Vec2(0, 1)) self.world.load_character(character2, timestamp=0) self.world.update_character(character, velocity=Vec2(1, 1), viewport=Vec2.polar(angle=45, length=1), timestamp=0) self.world.update_character(character2, velocity=Vec2(-1, -1), viewport=Vec2.polar(angle=45, length=1), timestamp=0) self.loop.run_until_complete(asyncio.sleep(0.1, loop=self.loop)) self.assertEqual( patched.call_count, 2, "`World.notify` was not called on character passive move") self.assertEqual(tuple(patched.call_args), ((), dict(affects=[character], event="character_move", character=character, timestamp=0))) self.loop.run_until_complete(asyncio.sleep(1, loop=self.loop)) self.assertEqual( patched.call_count, 3, "`World.notify` was not called on character passive move") self.assertEqual(tuple(patched.call_args), ((), dict(affects=[character], event="character_move", character=character, timestamp=1))) self.assertEqual(character.position, Vec2(1, 1))
def fire_bullet(self, timestamp): dt = timestamp - self._timestamp position = self._position_in(dt) # Bullet is created at pin of the ship. Which is 4 units further than # center direction = Vec2.polar(self._angle) position += direction * 4 bullet = Bullet(position, direction, timestamp) self._world._add_bullet(timestamp, bullet)
def _spawn_asteroids(self, timestamp, asteroid_count): for i in range(asteroid_count): position = Vec2(random.randint(-self.width/2, self.width/2), random.randint(-self.height/2, self.height/2)) position = position * 0.9 angle = random.randint(0, 360) speed = random.randint( self.ASTEROID_SPEED_MIN, self.ASTEROID_SPEED_MAX) velocity = Vec2.polar(angle, speed) ast = Asteroid(position, velocity, timestamp) self._add_asteroid(timestamp, ast)
def fire_bullet(self, timestamp): dt = timestamp - self._timestamp position = self._position_in(dt) # Bullet is created at pin of the ship. Which is 4 units further than # center direction = Vec2.polar(self._angle) position += direction * 4 bullet = Bullet( position, direction, timestamp) self._world._add_bullet(timestamp, bullet)
def is_time_to_leave_wall(self, x, y, theta): """ This function is regularly called from the wallfollower state to check the brain's belief about whether it is the right time (or place) to leave the wall and move straight to the goal. """ theta = degrees(theta); self.current_theta =theta; self.wp_current_position = Point(x,y); self.current_direction = Vec2.polar(angle = theta,length = 1); #Robot Orientation Line. self.ln_current_orentation = Line(Vec2(x,y),self.current_direction); # the prependicular line to the path self.ln_distance = self.ln_path.perpendicular(self.wp_current_position); distance_to_path= self.ln_path.distance_to(Point(x,y)); self.distance_to_path = distance_to_path; distance_to_destination = self.ln_current_orentation.distance_to(self.wp_destination); if(abs(distance_to_path) > 1): self.path_started =True; self.distance_to_goal = self.wp_destination.distance_to(Point(x,y)); """ checking if distance to the straight path is approx. 0 and if destenation on the opposit side of wall then leave the path NOTE and TODO: works only for the circles not for complex path. """ if(abs(distance_to_path) < self.TOLERANCE() and self.distance_to_goal < self.distance_when_left and self.is_destination_opposite_to_wall(distance_to_destination) and self.path_started): # is robot started following wall! self.wp_wf_stop = Point(x,y); return True; return False
def get_motion_values(x_val, y_val, robot_rotation, field_centric): global x_joystick global y_joystick if x_val < x_joystick[1]: x_val = scale_value(x_val, x_joystick[0], x_joystick[1], -1.0, 0.0) elif x_val > x_joystick[1]: x_val = scale_value(x_val, x_joystick[1], x_joystick[2], 0.0, 1.0) else: x_val = 0.0 if y_val < y_joystick[1]: y_val = scale_value(y_val, y_joystick[0], y_joystick[1], -1.0, 0.0) elif y_val > y_joystick[1]: y_val = scale_value(y_val, y_joystick[1], y_joystick[2], 0.0, 1.0) else: y_val = 0.0 motion_vector = Vec2(x_val, y_val) motion_vector = Vec2.polar(angle=motion_vector.angle - (robot_rotation if field_centric else 0), length=motion_vector.length) return (motion_vector.x, motion_vector.y)
from timeit import timeit from planar import Vec2 from planar.polygon import Polygon times = 500 def rand_pt(span=10): return Vec2(random() * span - 0, random() * span - 0.5) pts = [Vec2(i, random() * 10.0 + 5.001) for i in range(359)] for sides in [4, 5, 6, 7, 8, 9, 10, 20, 40, 80, 160, 320, 640]: angles = sorted(set(random() * 360.0 for i in range(sides))) if random() > 0.5: angles.reverse() poly = Polygon((Vec2.polar(a, 5) for a in angles)) assert poly.is_convex tangents = poly._pt_tangents cvx_tangents = poly.tangents_to_point for pt in pts: assert not poly.contains_point(pt) tans = tangents(pt) cvx_tans = cvx_tangents(pt) assert tans == cvx_tans, (tans, cvx_tans, sides, pt, list(poly)) def null(): pt_tangents = poly._pt_tangents for pt in pts: pass
def bump(self, timestamp): self._update_time(timestamp) direction = Vec2.polar(self._angle) self._velocity += direction * self.BUMP_AMPLITUDE
from planar.polygon import Polygon times = 500 def rand_pt(span=10): return Vec2(random() * span - 0, random() * span - 0.5) pts = [Vec2(i, random() * 10.0 + 5.001) for i in range(359)] for sides in [4, 5, 6, 7, 8, 9, 10, 20, 40, 80, 160, 320, 640]: angles = sorted(set(random() * 360.0 for i in range(sides))) if random() > 0.5: angles.reverse() poly = Polygon((Vec2.polar(a, 5) for a in angles)) assert poly.is_convex tangents = poly._pt_tangents cvx_tangents = poly.tangents_to_point for pt in pts: assert not poly.contains_point(pt) tans = tangents(pt) cvx_tans = cvx_tangents(pt) assert tans == cvx_tans, (tans, cvx_tans, sides, pt, list(poly)) def null(): pt_tangents = poly._pt_tangents for pt in pts: pass