def update(self, elapsed_seconds, force=None, extra_drag=0):
# Apply force if necessary.
if force is not None:
self.velocity = vec.add(
self.velocity,
vec.mul(force, (elapsed_seconds / self.mass)),
)
# Apply drag.
# Decrease the magnitude of the velocity vector by
# the amount of drag.
drag = (self.drag_rate + extra_drag) * elapsed_seconds
if drag > self.speed:
self.velocity = (0, 0)
elif drag != 0 and self.velocity != (0, 0):
drag_vector = vec.norm(self.velocity, -drag)
self.velocity = vec.add(
self.velocity,
drag_vector,
)
# Limit to maximum speed.
if self.speed > c.max_speed:
self.velocity = vec.norm(self.velocity, c.max_speed)
# Update position based on velocity.
self.last_pos = self.pos
self.pos = vec.add(
self.pos,
vec.mul(self.velocity, elapsed_seconds),
)
def draw_player(self, player):
# Show whether or not the player is thrusting.
if player.do_thrust:
trailing_point = vec.add(
player.pos,
vec.norm(player.direction, -1.5 * player.radius),
)
self.line(THRUST_COLOR, player.pos, trailing_point,
.5 * player.radius)
if player.boost_heavy_time_remaining > 0.0:
trailing_point = vec.add(
player.pos,
vec.norm(player.direction, -2.0 * player.radius),
)
self.line(THRUST_COLOR, player.pos, trailing_point,
1.5 * player.radius)
# Show braking and boosting charge up.
if player.do_brake:
# Redden the brake color as we charge.
color = vec.add(
BRAKE_COLOR,
vec.mul(THRUST_COLOR,
player.boost_charge_time / c.player_boost_ready_time))
# Vibrate if we are fully charged.
r = 1.2
if player.boost_charge_time == c.player_boost_ready_time:
r += 0.1 * math.sin(6 *
(2 * math.pi) * pg.time.get_ticks() / 1000)
self.circle(color, player.pos, r)
# Body.
self.circle(PLAYER_COLORS[player.number], player.pos, player.radius)
# Show the player pointing towards a direction.
leading_point = vec.add(
player.pos,
vec.norm(player.direction, player.radius),
)
self.line(DIRECTION_COLOR, player.pos, leading_point,
.2 * player.radius)
if SHOW_RUDDER_FORCE:
point = vec.add(
player.pos,
vec.mul(player.rudder_force, .1),
)
self.line(THRUST_COLOR, player.pos, point, .1 * player.radius)
if SHOW_INTENDED_DIRECTION and hasattr(player, 'intended_direction'):
point = vec.add(
player.pos,
player.intended_direction,
)
self.line(THRUST_COLOR, player.pos, point, .1 * player.radius)
def eval_spike_rush(self, ball_pos):
dist = norm(ball_pos - self.pos)
if dist > 160:
self.has_ball_spiked = False
rel_pos = dot(ball_pos - self.pos, self.rot)
self._ball_last_rel_poss[self._next_rel_pos_to_replace] = rel_pos
self._next_rel_pos_to_replace = (self._next_rel_pos_to_replace + 1) % 3
change = norm(self._ball_last_rel_poss[0] - self._ball_last_rel_poss[1]) + \
norm(self._ball_last_rel_poss[1] - self._ball_last_rel_poss[2])
self.has_ball_spiked = change < 1 and dist < 200
def intersect_circles(center1, radius1, center2, radius2):
radius1 = abs(radius1)
radius2 = abs(radius2)
if radius2 > radius1:
return intersect_circles(center2, radius2, center1, radius1)
transverse = vec.vfrom(center1, center2)
dist = vec.mag(transverse)
# Check for identical or concentric circles. These will have either
# no points in common or all points in common, and in either case, we
# return an empty list.
if points_equal(center1, center2):
return []
# Check for exterior or interior tangent.
radius_sum = radius1 + radius2
radius_difference = abs(radius1 - radius2)
if (float_equal(dist, radius_sum) or float_equal(dist, radius_difference)):
return [
vec.add(center1, vec.norm(transverse, radius1)),
]
# Check for non intersecting circles.
if dist > radius_sum or dist < radius_difference:
return []
# If we've reached this point, we know that the two circles intersect
# in two distinct points.
# Reference:
# http://mathworld.wolfram.com/Circle-CircleIntersection.html
# Pretend that the circles are arranged along the x-axis.
# Find the x-value of the intersection points, which is the same for both
# points. Then find the chord length "a" between the two intersection
# points, and use vector math to find the points.
dist2 = vec.mag2(transverse)
x = (dist2 - radius2**2 + radius1**2) / (2 * dist)
a = ((1 / dist) * sqrt(
(-dist + radius1 - radius2) * (-dist - radius1 + radius2) *
(-dist + radius1 + radius2) * (dist + radius1 + radius2)))
chord_middle = vec.add(
center1,
vec.norm(transverse, x),
)
perp = vec.perp(transverse)
return [
vec.add(chord_middle, vec.norm(perp, a / 2)),
vec.add(chord_middle, vec.norm(perp, -a / 2)),
]
def draw_player(self, player):
# Show whether or not the player is thrusting.
if player.do_thrust:
trailing_point = vec.add(
player.pos,
vec.norm(player.direction, -1.5 * player.radius),
)
self.line(THRUST_COLOR, player.pos, trailing_point, .5 * player.radius)
if player.boost_heavy_time_remaining > 0.0:
trailing_point = vec.add(
player.pos,
vec.norm(player.direction, -2.0 * player.radius),
)
self.line(THRUST_COLOR, player.pos, trailing_point, 1.5 * player.radius)
# Show braking and boosting charge up.
if player.do_brake:
# Redden the brake color as we charge.
color = vec.add(
BRAKE_COLOR,
vec.mul(THRUST_COLOR, player.boost_charge_time / c.player_boost_ready_time)
)
# Vibrate if we are fully charged.
r = 1.2
if player.boost_charge_time == c.player_boost_ready_time:
r += 0.1 * math.sin(6 * (2*math.pi) * pg.time.get_ticks() / 1000)
self.circle(color, player.pos, r)
# Body.
self.circle(PLAYER_COLORS[player.number], player.pos, player.radius)
# Show the player pointing towards a direction.
leading_point = vec.add(
player.pos,
vec.norm(player.direction, player.radius),
)
self.line(DIRECTION_COLOR, player.pos, leading_point, .2 * player.radius)
if SHOW_RUDDER_FORCE:
point = vec.add(
player.pos,
vec.mul(player.rudder_force, .1),
)
self.line(THRUST_COLOR, player.pos, point, .1 * player.radius)
if SHOW_INTENDED_DIRECTION and hasattr(player, 'intended_direction'):
point = vec.add(
player.pos,
player.intended_direction,
)
self.line(THRUST_COLOR, player.pos, point, .1 * player.radius)
def predict_hit_pos(self, from_pos):
speed = SUPER_SPEED if self.next_is_super else NORMAL_SPEED
ball_prediction = self.get_ball_prediction_struct()
if ball_prediction is not None:
TIME_PER_SLICES = 1 / 60.0
SLICES = 360
# Iterate to find the first location which can be hit
for i in range(0, 360):
time = i * TIME_PER_SLICES
rlpos = ball_prediction.slices[i].physics.location
pos = Vec3(rlpos.x, rlpos.y, rlpos.z)
dist = norm(from_pos - pos)
travel_time = dist / speed
if time >= travel_time:
# Add small bias for aiming
tsign = -1 if self.team == 0 else 1
enemy_goal = Vec3(0, tsign * -5030, 300)
bias_direction = normalize(pos - enemy_goal)
pos = pos + bias_direction * GOAL_AIM_BIAS_AMOUNT
return pos, travel_time
# Use last
rlpos = ball_prediction.slices[SLICES - 1].physics.location
return Vec3(rlpos.x, rlpos.y, rlpos.z), 6
return Vec3(0, 0, 0), 5
def sphere_line_intersectQ(line, r, pos):
v0 = line[0]
v1 = line[1]
dv = v1 - v0
dv_norm = vec.norm(dv)
dv = dv / dv_norm
v0_rel = v0 - pos
v0r_dv = vec.dot(v0_rel, dv)
discr = (v0r_dv)**2 - vec.dot(v0_rel, v0_rel) + r * r
#print('discr',discr)
#no intersection with line
if discr < 0:
return False
sqrt_discr = math.sqrt(discr)
tm = -v0r_dv - sqrt_discr
tp = -v0r_dv + sqrt_discr
#print('tm,tp',tm,tp)
#no intersection with line segment
if tm > dv_norm and tp > dv_norm:
return False
if tm < 0 and tp < 0:
return False
return True
def sphere_line_intersect(line, r):
v0 = line[0]
v1 = line[1]
dv = v1 - v0
dv_norm = vec.norm(dv)
dv = dv / dv_norm
#in our case, sphere center is the origin
v0_rel = v0 # - sphere_center
v0r_dv = vec.dot(v0_rel, dv)
discr = (v0r_dv)**2 - vec.dot(v0_rel, v0_rel) + r * r
#print('discr',discr)
#no intersection with line
if discr < 0:
return None
sqrt_discr = math.sqrt(discr)
tm = -v0r_dv - sqrt_discr
tp = -v0r_dv + sqrt_discr
#print('tm,tp',tm,tp)
#no intersection with line segment
if tm > dv_norm and tp > dv_norm:
return None
if tm < 0 and tp < 0:
return None
intersect_points = [v0 + tm * dv, v0 + tp * dv]
return intersect_points
def __init__(self, filename):
craftFile = open(filename,'r')
data = json.load(craftFile)
self.thrusterLocations = data[0]
self.thrusterOrientations = data[1]
self.accelerationMax = data[2]
self.angularAccelerationMax = data[3]
# each thruster provides accelerationMin = 0, and angularAccelerationMin = 0 (can't push backward)
self.radius = data[4] #until a more sophisticated collision algorithm is implemented, use a sphere
self.numThr = len(data[0])
self.accelMatrix = []
self.alphaMatrix = []
self.totalAccelMax = 0 # the maximum acceleration magnitude the spacecraft can reach
self.totalAlphaMax = 0 # as above for angular acceleration
for i in range(2**self.numThr):
thrusterFlags = [ (i//(2**j))%2 for j in range(self.numThr)] # creates a list w/length numThr
# filled with 1's and 0's, unique for
# each i, covering all possible
# combinations.
accel = [
sum(
[self.accelerationMax[j][k]*thrusterFlags[j] for j in range(self.numThr)])
for k in range(3)
]
if vec.norm(accel) > self.totalAccelMax:
self.totalAccelMax = vec.norm(accel)
alpha = [
sum(
[self.angularAccelerationMax[j][k]*thrusterFlags[j] for j in range(self.numThr)])
for k in range(3)
]
if vec.norm(alpha) > self.totalAlphaMax:
self.totalAlphaMax = vec.norm(alpha)
self.accelMatrix.append(accel)
self.alphaMatrix.append(alpha)
def collide_particles(p1, p2):
restitution = p1.restitution * p2.restitution
# Don't collide immovable particles.
if p1.immovable and p2.immovable:
return
# Test if p1 and p2 are actually intersecting.
if not intersect(p1, p2):
return
# If one particle is immovable, make it the first one.
if not p1.immovable and p2.immovable:
p1, p2 = p2, p1
# Vector spanning between the centers, normal to contact surface.
v_span = vec.vfrom(p1.pos, p2.pos)
# Split into normal and tangential components and calculate
# initial velocities.
normal = vec.norm(v_span)
tangent = vec.perp(normal)
v1_tangent = vec.proj(p1.velocity, tangent)
v2_tangent = vec.proj(p2.velocity, tangent)
p1_initial = vec.dot(p1.velocity, normal)
p2_initial = vec.dot(p2.velocity, normal)
# Don't collide if particles were actually moving away from each other, so
# they don't get stuck inside one another.
if p1_initial - p2_initial < 0:
return
# Handle immovable particles specially.
if p1.immovable:
p2_final = -p2_initial * restitution
p2.velocity = vec.add(
v2_tangent,
vec.mul(normal, p2_final),
)
return
# Elastic collision equations along normal component.
m1, m2 = p1.mass, p2.mass
m1plusm2 = (m1 + m2) / restitution
p1_final = (p1_initial * (m1 - m2) / m1plusm2 + p2_initial *
(2 * m2) / m1plusm2)
p2_final = (p2_initial * (m2 - m1) / m1plusm2 + p1_initial *
(2 * m1) / m1plusm2)
# Tangential component is unchanged, recombine.
p1.velocity = vec.add(
v1_tangent,
vec.mul(normal, p1_final),
)
p2.velocity = vec.add(
v2_tangent,
vec.mul(normal, p2_final),
)
def test_bumper():
northwest = vec.norm((-1, -1))
northeast = vec.norm((1, -1))
env = Environment()
env.load_level([
(Bumper, dict(pos=(0,0), velocity=(0,0), radius=3)),
(Particle, dict(pos=(6, 0), velocity=vec.norm(northwest, c.max_speed), mass=1, radius=3.1)),
])
# The particle starts out heading northwest.
bumper, particle = env.particles
assert_vectors_equal(vec.norm(particle.velocity), northwest)
# After it bounces, it goes northeast.
for _ in range(10):
env.update(1)
assert_vectors_equal(vec.norm(particle.velocity), northeast)
def calc_rudder_force(self):
# We continuously bring the direction of the player's movement to be
# closer in line with the direction it is facing.
target_velocity = vec.norm(self.direction, self.speed)
force = vec.vfrom(self.velocity, target_velocity)
if force == (0, 0):
return (0, 0)
# The strength of the rudder is highest when acting perpendicular to
# the direction of movement.
v_perp = vec.norm(vec.perp(self.velocity))
angle_multiplier = abs(vec.dot(v_perp, self.direction))
strength = self.speed * c.player_rudder_strength
strength = min(strength, c.player_max_rudder_strength)
strength *= angle_multiplier
if strength == 0:
return (0, 0)
force = vec.norm(force, strength)
return force
def calc_rudder_force(self):
# We continuously bring the direction of the player's movement to be
# closer in line with the direction it is facing.
target_velocity = vec.norm(self.direction, self.speed)
force = vec.vfrom(self.velocity, target_velocity)
if force == (0, 0):
return (0, 0)
# The strength of the rudder is highest when acting perpendicular to
# the direction of movement.
v_perp = vec.norm(vec.perp(self.velocity))
angle_multiplier = abs(vec.dot(v_perp, self.direction))
strength = self.speed * c.player_rudder_strength
strength = min(strength, c.player_max_rudder_strength)
strength *= angle_multiplier
if strength == 0:
return (0, 0)
force = vec.norm(force, strength)
return force
def draw_graph(graph):
gap_size = 0.25
p = canoepaddle.Pen()
p.stroke_mode(0.1, 'black')
for a, b in graph_edges(graph):
gap = vec.norm(vec.vfrom(a, b), gap_size)
p.move_to(vec.add(a, gap))
p.line_to(vec.sub(b, gap))
return p.paper
def update(self, elapsed_seconds, force=None, extra_drag=0):
# Apply force if necessary.
if force is not None:
self.velocity = vec.add(self.velocity, vec.mul(force, (elapsed_seconds / self.mass)))
# Apply drag.
# Decrease the magnitude of the velocity vector by
# the amount of drag.
drag = (self.drag_rate + extra_drag) * elapsed_seconds
if drag > self.speed:
self.velocity = (0, 0)
elif drag != 0 and self.velocity != (0, 0):
drag_vector = vec.norm(self.velocity, -drag)
self.velocity = vec.add(self.velocity, drag_vector)
# Limit to maximum speed.
if self.speed > c.max_speed:
self.velocity = vec.norm(self.velocity, c.max_speed)
# Update position based on velocity.
self.last_pos = self.pos
self.pos = vec.add(self.pos, vec.mul(self.velocity, elapsed_seconds))
def rebound(self, normal, point=None, restitution=1):
# Split into normal and tangential components.
tangent = vec.perp(normal)
v_tangent = vec.proj(self.velocity, tangent)
v_normal = vec.proj(self.velocity, normal)
# Invert normal component and recombine, with restitution.
v_normal = vec.neg(v_normal)
self.velocity = vec.add(v_tangent, vec.mul(v_normal, restitution))
# If the particle is partially inside the wall, move it out.
if point is not None:
v = vec.vfrom(point, self.pos)
if vec.mag2(v) < self.radius ** 2:
v = vec.norm(v, self.radius)
self.pos = vec.add(point, v)
def join_with_arc(self, other):
# Special case coincident arcs.
if points_equal(self.center, other.center):
if not (
float_equal(self.radius, other.radius) and
float_equal(self.width, other.width)
):
self.end_joint_illegal = True
other.start_joint_illegal = True
return
r = vec.vfrom(self.center, self.b)
if self.radius < 0:
r = vec.neg(r)
v_left = vec.norm(r, self.radius - self.width / 2)
self.b_left = other.a_left = Point(*vec.add(self.center, v_left))
v_right = vec.norm(r, self.radius + self.width / 2)
self.b_right = other.a_right = Point(*vec.add(self.center, v_right))
return
c1, r1 = self.offset_circle_left()
c2, r2 = other.offset_circle_left()
points_left = intersect_circles(c1, r1, c2, r2)
if len(points_left) > 0:
p = Point(*closest_point_to(self.b, points_left))
self.b_left = other.a_left = p
c1, r1 = self.offset_circle_right()
c2, r2 = other.offset_circle_right()
points_right = intersect_circles(c1, r1, c2, r2)
if len(points_right) > 0:
p = Point(*closest_point_to(self.b, points_right))
self.b_right = other.a_right = p
if len(points_left) == 0 or len(points_right) == 0:
self.end_joint_illegal = True
other.start_joint_illegal = True
def join_with_arc(self, other):
# Special case coincident arcs.
if points_equal(self.center, other.center):
if not (
float_equal(self.radius, other.radius)
and float_equal(self.width, other.width)
):
self.end_joint_illegal = True
other.start_joint_illegal = True
return
r = vec.vfrom(self.center, self.b)
if self.radius < 0:
r = vec.neg(r)
v_left = vec.norm(r, self.radius - self.width / 2)
self.b_left = other.a_left = Point(*vec.add(self.center, v_left))
v_right = vec.norm(r, self.radius + self.width / 2)
self.b_right = other.a_right = Point(*vec.add(self.center, v_right))
return
c1, r1 = self.offset_circle_left()
c2, r2 = other.offset_circle_left()
points_left = intersect_circles(c1, r1, c2, r2)
if len(points_left) > 0:
p = Point(*closest_point_to(self.b, points_left))
self.b_left = other.a_left = p
c1, r1 = self.offset_circle_right()
c2, r2 = other.offset_circle_right()
points_right = intersect_circles(c1, r1, c2, r2)
if len(points_right) > 0:
p = Point(*closest_point_to(self.b, points_right))
self.b_right = other.a_right = p
if len(points_left) == 0 or len(points_right) == 0:
self.end_joint_illegal = True
other.start_joint_illegal = True
def collide_particles(p1, p2):
restitution = p1.restitution * p2.restitution
# Don't collide immovable particles.
if p1.immovable and p2.immovable:
return
# Test if p1 and p2 are actually intersecting.
if not intersect(p1, p2):
return
# If one particle is immovable, make it the first one.
if not p1.immovable and p2.immovable:
p1, p2 = p2, p1
# Vector spanning between the centers, normal to contact surface.
v_span = vec.vfrom(p1.pos, p2.pos)
# Split into normal and tangential components and calculate
# initial velocities.
normal = vec.norm(v_span)
tangent = vec.perp(normal)
v1_tangent = vec.proj(p1.velocity, tangent)
v2_tangent = vec.proj(p2.velocity, tangent)
p1_initial = vec.dot(p1.velocity, normal)
p2_initial = vec.dot(p2.velocity, normal)
# Don't collide if particles were actually moving away from each other, so
# they don't get stuck inside one another.
if p1_initial - p2_initial < 0:
return
# Handle immovable particles specially.
if p1.immovable:
p2_final = -p2_initial * restitution
p2.velocity = vec.add(v2_tangent, vec.mul(normal, p2_final))
return
# Elastic collision equations along normal component.
m1, m2 = p1.mass, p2.mass
m1plusm2 = (m1 + m2) / restitution
p1_final = p1_initial * (m1 - m2) / m1plusm2 + p2_initial * (2 * m2) / m1plusm2
p2_final = p2_initial * (m2 - m1) / m1plusm2 + p1_initial * (2 * m1) / m1plusm2
# Tangential component is unchanged, recombine.
p1.velocity = vec.add(v1_tangent, vec.mul(normal, p1_final))
p2.velocity = vec.add(v2_tangent, vec.mul(normal, p2_final))
def rebound(self, normal, point=None, restitution=1):
# Split into normal and tangential components.
tangent = vec.perp(normal)
v_tangent = vec.proj(self.velocity, tangent)
v_normal = vec.proj(self.velocity, normal)
# Invert normal component and recombine, with restitution.
v_normal = vec.neg(v_normal)
self.velocity = vec.add(
v_tangent,
vec.mul(v_normal, restitution),
)
# If the particle is partially inside the wall, move it out.
if point is not None:
v = vec.vfrom(point, self.pos)
if vec.mag2(v) < self.radius**2:
v = vec.norm(v, self.radius)
self.pos = vec.add(point, v)
def join_with_line(self, other):
v_self = self._vector()
v_other = other._vector()
# Check turn angle.
self_heading = Heading.from_rad(vec.heading(v_self))
other_heading = Heading.from_rad(vec.heading(v_other))
turn_angle = self_heading.angle_to(other_heading)
# Special case equal widths.
if(
abs(turn_angle) <= MAX_TURN_ANGLE
and float_equal(self.width, other.width)
):
# When joints between segments of equal width are straight or
# almost straight, the line-intersection method becomes very
# numerically unstable, so use another method instead.
# For each segment, get a vector perpendicular to the
# segment, then add them. This is an angle bisector for
# the angle of the joint.
w_self = self._width_vector()
w_other = other._width_vector()
v_bisect = vec.add(w_self, w_other)
# Make the bisector have the correct length.
half_angle = vec.angle(v_other, v_bisect)
v_bisect = vec.norm(
v_bisect,
(self.width / 2) / math.sin(half_angle)
)
# Determine the left and right joint spots.
p_left = vec.add(self.b, v_bisect)
p_right = vec.sub(self.b, v_bisect)
else:
a, b = self.offset_line_left()
c, d = other.offset_line_left()
p_left = intersect_lines(a, b, c, d)
a, b = self.offset_line_right()
c, d = other.offset_line_right()
p_right = intersect_lines(a, b, c, d)
# Make sure the joint points are "forward" from the perspective
# of each segment.
if p_left is not None:
if vec.dot(vec.vfrom(self.a_left, p_left), v_self) < 0:
p_left = None
if p_right is not None:
if vec.dot(vec.vfrom(self.a_right, p_right), v_self) < 0:
p_right = None
# Don't join the outer sides if the turn angle is too steep.
if abs(turn_angle) > MAX_TURN_ANGLE:
if turn_angle > 0:
p_right = None
else:
p_left = None
if p_left is not None:
self.b_left = other.a_left = Point(*p_left)
if p_right is not None:
self.b_right = other.a_right = Point(*p_right)
if p_left is None or p_right is None:
self.end_joint_illegal = True
other.start_joint_illegal = True
def heuristic(self, state, spacecraft):
# print('placeholder4')
# DEFINE HEURISTIC HERE
# Don't refer to self.firstNode or self.firstHeuristic
# See notes file part 1
if not rotation:
amax = spacecraft.totalAccelMax
alpmax = spacecraft.totalAlphaMax
v0 = state.v
th0 = state.th
# worstTime = 0
# There is a critical speed along an axis such that if the spacecraft decelerates at its maximum
# rate it will stop exactly at the goal. The first contribution to the heuristic will be how
# different the spacecraft's actual velocity v0 is to the critical velocity, vc. The second
# contribution is the time it will take to decelerate to the goal if its speed were equal to the
# critical velocity
deltaP = []
deltaP.append(self.finalState.p[0] - state.p[0])
deltaP.append(self.finalState.p[1] - state.p[1])
deltaP.append(self.finalState.p[2] - state.p[2])
deltaTh = []
deltaTh.append(self.finalState.th[0] - state.th[0])
deltaTh.append(self.finalState.th[1] - state.th[1])
deltaTh.append(self.finalState.th[2] - state.th[2])
# h = []
# for i in range(3):
# if abs(deltaP[i]) < self.dp:
# h.append( v0[i]**4 / amax**4)
# # if h > worstTime:
# # worstTime = h
# # continue
# continue
# c1 = deltaP[i]**2 / (amax*amax)
# c2 = v0[i]*abs(v0[i]) / (amax*deltaP[i])
# h.append(c1*(c2**2 + 4*c2 + 8))
# # if h > worstTime:
# # worstTime = h
# # return worstTime
normDeltaP = vec.norm(deltaP)
normal = vec.smul(1/normDeltaP,deltaP)
normalV = vec.dot(v0,normal)
normV = vec.norm(v0)
tangentialV = math.sqrt(normV**2 - normalV**2)
vcrit = math.sqrt(2*amax*normDeltaP)
t1 = 0
if normalV < 0: #spacraft is heading the wrong way
tstop = -normalV/amax #time required to stop
t1 = tstop + self.ramp((1/2)*amax*(tstop**2) + normDeltaP,amax)
elif normalV > vcrit: #spacecraft is moving too fast and will overshoot
tstop = normalV/amax
t1 = tstop + self.ramp((1/2)*amax*(tstop**2) - normDeltaP,amax)
else: #spacecraft is heading the right way, slow enough to stop in time
negTStop = -normalV/amax #represents the time in the past the ramp would have started
t1 = negTStop + self.ramp((1/2)*amax*(negTStop**2) + normDeltaP,amax)
# t1 is the heuristic if there is no tangential velocity
# at the very least, the tangential velocity needs to be cancelled (taking extra time)
h = t1 + tangentialV/amax + math.sqrt((deltaTh[0]/alpmax)**2 + (deltaTh[1]/alpmax)**2 + (deltaTh[2]/alpmax)**2)
return h
raise NotImplementedError('Rotating Spacecraft Heuristic Not Implemented')
def _width_vector(self):
v = self._vector()
v = vec.perp(v)
v = vec.norm(v, self.width / 2)
return v
def intersect_circles(center1, radius1, center2, radius2):
radius1 = abs(radius1)
radius2 = abs(radius2)
if radius2 > radius1:
return intersect_circles(center2, radius2, center1, radius1)
transverse = vec.vfrom(center1, center2)
dist = vec.mag(transverse)
# Check for identical or concentric circles. These will have either
# no points in common or all points in common, and in either case, we
# return an empty list.
if points_equal(center1, center2):
return []
# Check for exterior or interior tangent.
radius_sum = radius1 + radius2
radius_difference = abs(radius1 - radius2)
if (
float_equal(dist, radius_sum) or
float_equal(dist, radius_difference)
):
return [
vec.add(
center1,
vec.norm(transverse, radius1)
),
]
# Check for non intersecting circles.
if dist > radius_sum or dist < radius_difference:
return []
# If we've reached this point, we know that the two circles intersect
# in two distinct points.
# Reference:
# http://mathworld.wolfram.com/Circle-CircleIntersection.html
# Pretend that the circles are arranged along the x-axis.
# Find the x-value of the intersection points, which is the same for both
# points. Then find the chord length "a" between the two intersection
# points, and use vector math to find the points.
dist2 = vec.mag2(transverse)
x = (dist2 - radius2**2 + radius1**2) / (2 * dist)
a = (
(1 / dist) *
sqrt(
(-dist + radius1 - radius2) *
(-dist - radius1 + radius2) *
(-dist + radius1 + radius2) *
(dist + radius1 + radius2)
)
)
chord_middle = vec.add(
center1,
vec.norm(transverse, x),
)
perp = vec.perp(transverse)
return [
vec.add(chord_middle, vec.norm(perp, a / 2)),
vec.add(chord_middle, vec.norm(perp, -a / 2)),
]
def join_with_line(self, other):
v_self = self._vector()
v_other = other._vector()
# Check turn angle.
self_heading = Heading.from_rad(vec.heading(v_self))
other_heading = Heading.from_rad(vec.heading(v_other))
turn_angle = self_heading.angle_to(other_heading)
# Special case equal widths.
if(
abs(turn_angle) <= MAX_TURN_ANGLE and
float_equal(self.width, other.width)
):
# When joints between segments of equal width are straight or
# almost straight, the line-intersection method becomes very
# numerically unstable, so use another method instead.
# For each segment, get a vector perpendicular to the
# segment, then add them. This is an angle bisector for
# the angle of the joint.
w_self = self._width_vector()
w_other = other._width_vector()
v_bisect = vec.add(w_self, w_other)
# Make the bisector have the correct length.
half_angle = vec.angle(v_other, v_bisect)
v_bisect = vec.norm(
v_bisect,
(self.width / 2) / math.sin(half_angle)
)
# Determine the left and right joint spots.
p_left = vec.add(self.b, v_bisect)
p_right = vec.sub(self.b, v_bisect)
else:
a, b = self.offset_line_left()
c, d = other.offset_line_left()
p_left = intersect_lines(a, b, c, d)
a, b = self.offset_line_right()
c, d = other.offset_line_right()
p_right = intersect_lines(a, b, c, d)
# Make sure the joint points are "forward" from the perspective
# of each segment.
if p_left is not None:
if vec.dot(vec.vfrom(self.a_left, p_left), v_self) < 0:
p_left = None
if p_right is not None:
if vec.dot(vec.vfrom(self.a_right, p_right), v_self) < 0:
p_right = None
# Don't join the outer sides if the turn angle is too steep.
if abs(turn_angle) > MAX_TURN_ANGLE:
if turn_angle > 0:
p_right = None
else:
p_left = None
if p_left is not None:
self.b_left = other.a_left = Point(*p_left)
if p_right is not None:
self.b_right = other.a_right = Point(*p_right)
if p_left is None or p_right is None:
self.end_joint_illegal = True
other.start_joint_illegal = True
def _width_vector(self):
v = self._vector()
v = vec.perp(v)
v = vec.norm(v, self.width / 2)
return v