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
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