def intersect_lines(a, b, c, d, segment=False):
"""
Find the intersection of lines a-b and c-d.
If the "segment" argument is true, treat the lines as segments, and check
whether the intersection point is off the end of either segment.
"""
# Reference:
# http://geomalgorithms.com/a05-_intersect-1.html
u = vec.vfrom(a, b)
v = vec.vfrom(c, d)
w = vec.vfrom(c, a)
u_perp_dot_v = vec.dot(vec.perp(u), v)
if float_equal(u_perp_dot_v, 0):
return None # We have collinear segments, no single intersection.
v_perp_dot_w = vec.dot(vec.perp(v), w)
s = v_perp_dot_w / u_perp_dot_v
if segment and (s < 0 or s > 1):
return None
u_perp_dot_w = vec.dot(vec.perp(u), w)
t = u_perp_dot_w / u_perp_dot_v
if segment and (t < 0 or t > 1):
return None
return vec.add(a, vec.mul(u, s))
def test_straight_joint_headings():
# The math in calculating joint geometry can get numerically unstable
# very close to straight joints at various headings.
for heading_angle in range(0, 45):
p = Pen()
p.stroke_mode(1.0)
p.move_to((0, 0))
p.turn_to(heading_angle)
p.line_forward(10)
p.line_forward(10)
path = p.paper.paths[0]
path.render_path(2) # Doesn't crash.
# Check that the joint angle is 90 degrees from the heading.
assert_equal(len(p.paper.paths), 1)
segments = p.paper.paths[0].segments
assert_equal(len(segments), 2)
s0, s1 = segments
target_angle = (heading_angle + 90) % 180
joint_angle = math.degrees(vec.heading(vec.vfrom(s0.b_right, s0.b_left)))
assert_almost_equal(joint_angle % 180, target_angle)
joint_angle = math.degrees(vec.heading(vec.vfrom(s1.a_right, s1.a_left)))
assert_almost_equal(joint_angle % 180, target_angle)
def intersect_lines(a, b, c, d, segment=False):
"""
Find the intersection of lines a-b and c-d.
If the "segment" argument is true, treat the lines as segments, and check
whether the intersection point is off the end of either segment.
"""
# Reference:
# http://geomalgorithms.com/a05-_intersect-1.html
u = vec.vfrom(a, b)
v = vec.vfrom(c, d)
w = vec.vfrom(c, a)
u_perp_dot_v = vec.dot(vec.perp(u), v)
if float_equal(u_perp_dot_v, 0):
return None # We have collinear segments, no single intersection.
v_perp_dot_w = vec.dot(vec.perp(v), w)
s = v_perp_dot_w / u_perp_dot_v
if segment and (s < 0 or s > 1):
return None
u_perp_dot_w = vec.dot(vec.perp(u), w)
t = u_perp_dot_w / u_perp_dot_v
if segment and (t < 0 or t > 1):
return None
return vec.add(a, vec.mul(u, s))
def arc_to(self, endpoint, center=None, start_slant=None, end_slant=None):
"""
Draw an arc ending at the specified point, starting tangent to the
current position and heading.
"""
if points_equal(self._position, endpoint):
return
# Handle unspecified center.
# We need to find the center of the arc, so we can find its radius. The
# center of this arc is uniquely defined by the intersection of two
# lines:
# 1. The first line is perpendicular to the pen heading, passing
# through the pen position.
# 2. The second line is the perpendicular bisector of the pen position
# and the target arc end point.
v_pen = self._vector()
v_perp = vec.perp(self._vector())
v_chord = vec.vfrom(self._position, endpoint)
if center is None:
midpoint = vec.div(vec.add(self._position, endpoint), 2)
v_bisector = vec.perp(v_chord)
center = intersect_lines(
self._position,
vec.add(self._position, v_perp),
midpoint,
vec.add(midpoint, v_bisector),
)
# Determine true start heading. This may not be the same as the
# original pen heading in some circumstances.
assert not points_equal(center, self._position)
v_radius_start = vec.vfrom(center, self._position)
v_radius_perp = vec.perp(v_radius_start)
if vec.dot(v_radius_perp, v_pen) < 0:
v_radius_perp = vec.neg(v_radius_perp)
start_heading = math.degrees(vec.heading(v_radius_perp))
self.turn_to(start_heading)
# Refresh v_pen and v_perp based on the new start heading.
v_pen = self._vector()
v_perp = vec.perp(self._vector())
# Calculate the arc angle.
# The arc angle is double the angle between the pen vector and the
# chord vector. Arcing to the left is a positive angle, and arcing to
# the right is a negative angle.
arc_angle = 2 * math.degrees(vec.angle(v_pen, v_chord))
radius = vec.mag(v_radius_start)
# Check which side of v_pen the goes toward.
if vec.dot(v_chord, v_perp) < 0:
arc_angle = -arc_angle
radius = -radius
self._arc(
center,
radius,
endpoint,
arc_angle,
start_slant,
end_slant,
)
def arc_to(self, endpoint, center=None, start_slant=None, end_slant=None):
"""
Draw an arc ending at the specified point, starting tangent to the
current position and heading.
"""
if points_equal(self._position, endpoint):
return
# Handle unspecified center.
# We need to find the center of the arc, so we can find its radius. The
# center of this arc is uniquely defined by the intersection of two
# lines:
# 1. The first line is perpendicular to the pen heading, passing
# through the pen position.
# 2. The second line is the perpendicular bisector of the pen position
# and the target arc end point.
v_pen = self._vector()
v_perp = vec.perp(self._vector())
v_chord = vec.vfrom(self._position, endpoint)
if center is None:
midpoint = vec.div(vec.add(self._position, endpoint), 2)
v_bisector = vec.perp(v_chord)
center = intersect_lines(
self._position,
vec.add(self._position, v_perp),
midpoint,
vec.add(midpoint, v_bisector),
)
# Determine true start heading. This may not be the same as the
# original pen heading in some circumstances.
assert not points_equal(center, self._position)
v_radius_start = vec.vfrom(center, self._position)
v_radius_perp = vec.perp(v_radius_start)
if vec.dot(v_radius_perp, v_pen) < 0:
v_radius_perp = vec.neg(v_radius_perp)
start_heading = math.degrees(vec.heading(v_radius_perp))
self.turn_to(start_heading)
# Refresh v_pen and v_perp based on the new start heading.
v_pen = self._vector()
v_perp = vec.perp(self._vector())
# Calculate the arc angle.
# The arc angle is double the angle between the pen vector and the
# chord vector. Arcing to the left is a positive angle, and arcing to
# the right is a negative angle.
arc_angle = 2 * math.degrees(vec.angle(v_pen, v_chord))
radius = vec.mag(v_radius_start)
# Check which side of v_pen the goes toward.
if vec.dot(v_chord, v_perp) < 0:
arc_angle = -arc_angle
radius = -radius
self._arc(
center,
radius,
endpoint,
arc_angle,
start_slant,
end_slant,
)
def test_straight_joint_headings():
# The math in calculating joint geometry can get numerically unstable
# very close to straight joints at various headings.
for heading_angle in range(0, 45):
p = Pen()
p.stroke_mode(1.0)
p.move_to((0, 0))
p.turn_to(heading_angle)
p.line_forward(10)
p.line_forward(10)
path = p.paper.paths[0]
path.render_path(2) # Doesn't crash.
# Check that the joint angle is 90 degrees from the heading.
assert_equal(len(p.paper.paths), 1)
segments = p.paper.paths[0].segments
assert_equal(len(segments), 2)
s0, s1 = segments
target_angle = (heading_angle + 90) % 180
joint_angle = math.degrees(vec.heading(vec.vfrom(s0.b_right, s0.b_left)))
assert_almost_equal(joint_angle % 180, target_angle)
joint_angle = math.degrees(vec.heading(vec.vfrom(s1.a_right, s1.a_left)))
assert_almost_equal(joint_angle % 180, target_angle)
def set_slants(self, start_slant, end_slant):
if start_slant is not None:
start_slant = Heading(start_slant)
if end_slant is not None:
end_slant = Heading(end_slant)
self.start_slant = start_slant
self.end_slant = end_slant
# Intersect the slant lines with the left and right offset circles
# to find the corners.
center_left, radius_left = self.offset_circle_left()
center_right, radius_right = self.offset_circle_right()
# Start corners.
if start_slant is None:
v_slant = vec.vfrom(self.center, self.a)
else:
v_slant = vec.from_heading(start_slant.rad)
a = self.a
b = vec.add(self.a, v_slant)
points_left = intersect_circle_line(center_left, radius_left, a, b)
points_right = intersect_circle_line(center_right, radius_right, a, b)
if len(points_left) == 0 or len(points_right) == 0:
self.start_joint_illegal = True
return
self.a_left = Point(*closest_point_to(self.a, points_left))
self.a_right = Point(*closest_point_to(self.a, points_right))
# End corners.
if end_slant is None:
v_slant = vec.vfrom(self.center, self.b)
else:
v_slant = vec.from_heading(end_slant.rad)
a = self.b
b = vec.add(self.b, v_slant)
points_left = intersect_circle_line(center_left, radius_left, a, b)
points_right = intersect_circle_line(center_right, radius_right, a, b)
if len(points_left) == 0 or len(points_right) == 0:
self.end_joint_illegal = True
return
self.b_left = Point(*closest_point_to(self.b, points_left))
self.b_right = Point(*closest_point_to(self.b, points_right))
self.check_degenerate_segment()
def set_slants(self, start_slant, end_slant):
if start_slant is not None:
start_slant = Heading(start_slant)
if end_slant is not None:
end_slant = Heading(end_slant)
self.start_slant = start_slant
self.end_slant = end_slant
# Intersect the slant lines with the left and right offset circles
# to find the corners.
center_left, radius_left = self.offset_circle_left()
center_right, radius_right = self.offset_circle_right()
# Start corners.
if start_slant is None:
v_slant = vec.vfrom(self.center, self.a)
else:
v_slant = vec.from_heading(start_slant.rad)
a = self.a
b = vec.add(self.a, v_slant)
points_left = intersect_circle_line(center_left, radius_left, a, b)
points_right = intersect_circle_line(center_right, radius_right, a, b)
if len(points_left) == 0 or len(points_right) == 0:
self.start_joint_illegal = True
return
self.a_left = Point(*closest_point_to(self.a, points_left))
self.a_right = Point(*closest_point_to(self.a, points_right))
# End corners.
if end_slant is None:
v_slant = vec.vfrom(self.center, self.b)
else:
v_slant = vec.from_heading(end_slant.rad)
a = self.b
b = vec.add(self.b, v_slant)
points_left = intersect_circle_line(center_left, radius_left, a, b)
points_right = intersect_circle_line(center_right, radius_right, a, b)
if len(points_left) == 0 or len(points_right) == 0:
self.end_joint_illegal = True
return
self.b_left = Point(*closest_point_to(self.b, points_left))
self.b_right = Point(*closest_point_to(self.b, points_right))
self.check_degenerate_segment()
def arc_left(
self, arc_angle, radius=None, center=None, start_slant=None, end_slant=None,
):
if (
(radius is None and center is None) or
(radius is not None and center is not None)
):
raise TypeError('You must specify exactly one of center or radius.')
arc_angle = Angle(arc_angle)
# Create a radius vector, which is a vector from the arc center to the
# current position. Subtract to find the center, then rotate the radius
# vector to find the arc end point.
if center is None:
if arc_angle < 0:
radius = -abs(radius)
v_radius = vec.neg(vec.perp(self._vector(radius)))
center = vec.sub(self._position, v_radius)
elif radius is None:
v_radius = vec.vfrom(center, self._position)
radius = vec.mag(v_radius)
if arc_angle < 0:
radius = -radius
endpoint = vec.add(center, vec.rotate(v_radius, arc_angle.rad))
self._arc(
center,
radius,
endpoint,
arc_angle,
start_slant=start_slant,
end_slant=end_slant,
)
def arc_left(
self, arc_angle, radius=None, center=None, start_slant=None, end_slant=None,
):
if (
(radius is None and center is None)
or (radius is not None and center is not None)
):
raise TypeError('You must specify exactly one of center or radius.')
arc_angle = Angle(arc_angle)
# Create a radius vector, which is a vector from the arc center to the
# current position. Subtract to find the center, then rotate the radius
# vector to find the arc end point.
if center is None:
if arc_angle < 0:
radius = -abs(radius)
v_radius = vec.neg(vec.perp(self._vector(radius)))
center = vec.sub(self._position, v_radius)
elif radius is None:
v_radius = vec.vfrom(center, self._position)
radius = vec.mag(v_radius)
if arc_angle < 0:
radius = -radius
endpoint = vec.add(center, vec.rotate(v_radius, arc_angle.rad))
self._arc(
center,
radius,
endpoint,
arc_angle,
start_slant=start_slant,
end_slant=end_slant,
)
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 intersect_circle_line(center, radius, line_start, line_end):
"""
Find the intersection of a circle with a line.
"""
radius = abs(radius)
# First check whether the line is too far away, or if we have a
# single point of contact.
# Reference:
# http://mathworld.wolfram.com/Point-LineDistance2-Dimensional.html
r = vec.vfrom(center, line_start)
v = vec.perp(vec.vfrom(line_start, line_end))
d = vec.proj(r, v)
dist = vec.mag(d)
if float_equal(dist, radius):
# Single intersection point, because the circle and line are tangent.
point = vec.add(center, d)
return [point]
elif dist > radius:
return []
# Set up parametric equations for the line and the circle, and solve them.
# Reference:
# http://www.cs.cf.ac.uk/Dave/CM0268/PDF/circle_line_intersect_proof.pdf
xc, yc = center
x0, y0 = line_start
x1, y1 = line_end
line_x, line_y = (x1 - x0), (y1 - y0) # f, g
dx, dy = (x0 - xc), (y0 - yc)
a = line_x**2 + line_y**2
b = 2 * (line_x * dx + line_y * dy)
c = dx**2 + dy**2 - radius**2
t0, t1 = quadratic_formula(a, b, c)
return [
(
x0 + line_x * t0,
y0 + line_y * t0,
),
(
x0 + line_x * t1,
y0 + line_y * t1,
),
]
def intersect_circle_line(center, radius, line_start, line_end):
"""
Find the intersection of a circle with a line.
"""
radius = abs(radius)
# First check whether the line is too far away, or if we have a
# single point of contact.
# Reference:
# http://mathworld.wolfram.com/Point-LineDistance2-Dimensional.html
r = vec.vfrom(center, line_start)
v = vec.perp(vec.vfrom(line_start, line_end))
d = vec.proj(r, v)
dist = vec.mag(d)
if float_equal(dist, radius):
# Single intersection point, because the circle and line are tangent.
point = vec.add(center, d)
return [point]
elif dist > radius:
return []
# Set up parametric equations for the line and the circle, and solve them.
# Reference:
# http://www.cs.cf.ac.uk/Dave/CM0268/PDF/circle_line_intersect_proof.pdf
xc, yc = center
x0, y0 = line_start
x1, y1 = line_end
line_x, line_y = (x1 - x0), (y1 - y0) # f, g
dx, dy = (x0 - xc), (y0 - yc)
a = line_x**2 + line_y**2
b = 2 * (line_x * dx + line_y * dy)
c = dx**2 + dy**2 - radius**2
t0, t1 = quadratic_formula(a, b, c)
return [
(
x0 + line_x * t0,
y0 + line_y * t0,
),
(
x0 + line_x * t1,
y0 + line_y * t1,
),
]
def collide_wall(self, p):
restitution = self.restitution * p.restitution
# First, check that we haven't crossed through the wall due to
# extreme speed and low framerate.
intersection = intersect_segments(self.p1, self.p2, p.last_pos, p.pos)
if intersection:
p.pos = p.last_pos
p.rebound(self.normal, intersection, restitution)
return
# Find vectors to each endpoint of the segment.
v1 = vec.vfrom(self.p1, p.pos)
v2 = vec.vfrom(self.p2, p.pos)
# Find a perpendicular vector from the wall to p.
v_dist = vec.proj(v1, self.normal)
# Test distance from the wall.
radius2 = p.radius**2
if vec.mag2(v_dist) > radius2:
return
# Test for collision with the endpoints of the segment.
# Check whether p is too far off the end of the segment, by checking
# the sign of the vector projection, then a radius check for the
# distance from the endpoint.
if vec.dot(v1, self.tangent) < 0:
if vec.mag2(v1) <= radius2:
p.rebound(v1, self.p1, restitution)
return
if vec.dot(v2, self.tangent) > 0:
if vec.mag2(v2) <= radius2:
p.rebound(v2, self.p2, restitution)
return
# Test that p is headed toward the wall.
if vec.dot(p.velocity, v_dist) >= c.epsilon:
return
# We are definitely not off the ends of the segment, and close enough
# that we are colliding.
p.rebound(self.normal, vec.sub(p.pos, v_dist), restitution)
def collide_wall(self, p):
restitution = self.restitution * p.restitution
# First, check that we haven't crossed through the wall due to
# extreme speed and low framerate.
intersection = intersect_segments(self.p1, self.p2, p.last_pos, p.pos)
if intersection:
p.pos = p.last_pos
p.rebound(self.normal, intersection, restitution)
return
# Find vectors to each endpoint of the segment.
v1 = vec.vfrom(self.p1, p.pos)
v2 = vec.vfrom(self.p2, p.pos)
# Find a perpendicular vector from the wall to p.
v_dist = vec.proj(v1, self.normal)
# Test distance from the wall.
radius2 = p.radius**2
if vec.mag2(v_dist) > radius2:
return
# Test for collision with the endpoints of the segment.
# Check whether p is too far off the end of the segment, by checking
# the sign of the vector projection, then a radius check for the
# distance from the endpoint.
if vec.dot(v1, self.tangent) < 0:
if vec.mag2(v1) <= radius2:
p.rebound(v1, self.p1, restitution)
return
if vec.dot(v2, self.tangent) > 0:
if vec.mag2(v2) <= radius2:
p.rebound(v2, self.p2, restitution)
return
# Test that p is headed toward the wall.
if vec.dot(p.velocity, v_dist) >= c.epsilon:
return
# We are definitely not off the ends of the segment, and close enough
# that we are colliding.
p.rebound(self.normal, vec.sub(p.pos, v_dist), restitution)
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 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 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 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 collinear(*points):
"""
Determine whether the given points are collinear in the order they were
passed in.
"""
# Find vectors between successive points, in a chain.
vectors = []
for a, b in pairwise(points):
vectors.append(vec.vfrom(a, b))
# Find the angles between successive vectors in the chain. Actually we skip
# the inverse cosine calculation required to find angle, and just use ratio
# instead. The ratio is the cosine of the angle between the vectors.
for u, v in pairwise(vectors):
ratio = vec.dot(u, v) / (vec.mag(u) * vec.mag(v))
if ratio < 1.0 - epsilon:
return False
return True
def collinear(*points):
"""
Determine whether the given points are collinear in the order they were
passed in.
"""
# Find vectors between successive points, in a chain.
vectors = []
for a, b in pairwise(points):
vectors.append(vec.vfrom(a, b))
# Find the angles between successive vectors in the chain. Actually we skip
# the inverse cosine calculation required to find angle, and just use ratio
# instead. The ratio is the cosine of the angle between the vectors.
for u, v in pairwise(vectors):
ratio = vec.dot(u, v) / (vec.mag(u) * vec.mag(v))
if ratio < 1.0 - epsilon:
return False
return True
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 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 partition(points, l1, l2, s=None):
"""
Partition a set of points by a line.
The line is defined by l1, l2. The desired side of the line is given by the
point s.
If s is not given, return points to the right of the line.
If eq is True, also include points on the line.
>>> sorted(partition([(-1,0), (0,0), (1,0)], (0,1), (0,-1), (2,0)))
[(0, 0), (1, 0)]
>>> sorted(partition([(-1,0), (0,0), (1,0)], (0,1), (0,-1), (-2,0)))
[(-1, 0), (0, 0)]
>>> points = [(-2,2), (-1,0), (0,0), (1,0)]
>>> sorted(partition(points, (-1,0), (0,1), (3,0)))
[(-1, 0), (0, 0), (1, 0)]
>>> sorted(partition(points, (-1,0), (0,1), (-3,0)))
[(-2, 2), (-1, 0)]
You can omit the argument "s" if you don't care.
>>> sorted(partition([(-1,0), (0,0), (1,0)], (0,1), (0,-1)))
[(-1, 0), (0, 0)]
"""
if s is None:
s = vec.add(l1, vec.perp(vec.vfrom(l1, l2)))
if l1 == l2:
raise ValueError('l1 equals l2')
sign = sign_of(cmp_line(l1, l2, s))
if sign == 0:
raise ValueError('s is on the line l1 l2')
for p in points:
c = cmp_line(l1, l2, p)
if c == sign:
yield p
elif c == 0:
yield p
def partition(points, l1, l2, s=None):
"""
Partition a set of points by a line.
The line is defined by l1, l2. The desired side of the line is given by the
point s.
>>> partition([(-1,0), (0,0), (1,0)], (0,1), (0,-1), (2,0))[0]
{(1, 0)}
>>> partition([(-1,0), (0,0), (1,0)], (0,1), (0,-1), (-2,0))[0]
{(-1, 0)}
>>> points = [(-2,2), (-1,0), (0,0), (1,0)]
>>> sorted(partition(points, (-1,0), (0,1), (3,0))[0])
[(0, 0), (1, 0)]
>>> sorted(partition(points, (-1,0), (0,1), (-3,0))[0])
[(-2, 2)]
You can omit the argument "s" if you don't care.
>>> partition([(-1,0), (0,0), (1,0)], (0,1), (0,-1))
({(-1, 0)}, {(1, 0)})
"""
if s is None:
s = vec.add(l1, vec.perp(vec.vfrom(l1, l2)))
if l1 == l2:
raise ValueError('l1 equals l2')
sign = cmp_line(l1, l2, s)
if sign == 0:
raise ValueError('s is on the line l1 l2')
forward = set()
reverse = set()
for p in points:
c = cmp_line(l1, l2, p)
if c == sign:
forward.add(p)
elif c == -sign:
reverse.add(p)
return forward, reverse
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 __init__(self, p1, p2, restitution=c.restitution_wall):
self.p1 = p1
self.p2 = p2
self.restitution = restitution
self.tangent = vec.vfrom(self.p1, self.p2)
self.normal = vec.perp(self.tangent)
def forwardness(p):
v_p = vec.vfrom(base_middle, p)
return vec.dot(v_p, v_base),
def contains(self, point):
sign = vec.dot(
vec.vfrom(self.center, point),
self.normal,
)
return (sign >= 0)
def last_slant_width(self):
seg = self.last_segment()
return vec.mag(vec.vfrom(seg.b_left, seg.b_right))
def draw(self, pen):
#TODO: this is not very good.
def scythe_cap(pen, end):
start_heading = pen.heading
switch = False
if self.character.mirrored_x:
switch = not switch
if self.flipped:
switch = not switch
if not switch:
top = end
bottom = pen.position
else:
top = pen.position
bottom = end
# Trace the curves with a temporary pen.
temp_pen = pen.copy()
if not switch:
arc = temp_pen.arc_right
else:
arc = temp_pen.arc_left
temp_pen.move_to(top)
temp_pen.turn_to(start_heading)
outer_arcs = [
(2.4, 1.6),
(1.0, 2.8),
]
outer_points = []
for radius, distance in outer_arcs:
circumference = radius * 2 * math.pi
circle_ratio = distance / circumference
angle = circle_ratio * 360
arc(angle, radius)
outer_points.append(temp_pen.position)
outer_tip_angle = temp_pen.heading
temp_pen.move_to(bottom)
temp_pen.turn_to(start_heading)
temp_pen.move_forward(0.5)
inner_forward = temp_pen.position
temp_pen.arc_to(outer_points[-1])
inner_tip_angle = temp_pen.heading
# Draw with the real pen.
if not switch:
pen.line_to(inner_forward)
pen.arc_to(outer_points[-1])
pen.turn_to(outer_tip_angle + 180)
for p in reversed(outer_points[:-1]):
pen.arc_to(p)
pen.arc_to(top)
else:
for p in outer_points:
pen.arc_to(p)
pen.turn_to(inner_tip_angle + 180)
pen.arc_to(inner_forward)
pen.line_to(bottom)
pen.line_to_y(BOTTOM + pen.mode.width / 2)
pen.turn_to(0)
# See how far forward we have to go to make the top of the stroke
# zero-length.
temp_pen = pen.copy(paper=True)
temp_pen.line_forward(pen.mode.width, end_slant=90)
seg = temp_pen.last_segment()
extra_left = vec.mag(vec.vfrom(seg.a_left, seg.b_left))
extra_right = vec.mag(vec.vfrom(seg.a_right, seg.b_right))
extra = min(extra_left, extra_right)
dist = pen.mode.width - extra
pen.line_forward(dist, end_slant=90)
pen.last_segment().end_cap = scythe_cap
def intersect(p1, p2):
distance2 = vec.mag2(vec.vfrom(p1.pos, p2.pos))
return distance2 <= (p1.radius + p2.radius)**2
def closest_point_to(target, points):
return min(
points,
key=lambda p: vec.mag2(vec.vfrom(target, p))
)
def _vector(self):
return vec.vfrom(self.a, self.b)
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 turn_toward(self, point):
v = vec.vfrom(self._position, point)
heading = math.degrees(vec.heading(v))
self.turn_to(heading)
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 intersect(p1, p2):
distance2 = vec.mag2(vec.vfrom(p1.pos, p2.pos))
return distance2 <= (p1.radius + p2.radius) ** 2
def last_slant_width(self):
seg = self.last_segment()
return vec.mag(vec.vfrom(seg.b_left, seg.b_right))
def closest_point_to(target, points):
return min(points, key=lambda p: vec.mag2(vec.vfrom(target, p)))
def turn_toward(self, point):
v = vec.vfrom(self._position, point)
heading = math.degrees(vec.heading(v))
self.turn_to(heading)
def hook(pen, slant_angle, arc_angle, distance, adjust_inside=0, adjust_outside=0):
"""
Draw a hook shape.
Each hook has two arcs that meet at a point, with curvature in the
same direction. They are connected at the other end by a line, creating two
corners. The pen starts on the left corner, pointing toward the right
corner. The width of the pen is also used as the maximum width of the hook.
The `slant_angle` argument is 90 degrees for a hook with a "straight" base.
For slanted bases toward the inside corner, 0 < slant_angle < 90.
For slanted bases toward the outside corner, 90 < slant_angle < 180.
`arc_angle` is the arc angle of the inside arc. If `arc_angle` is negative,
then the hook curves to the right instead.
`distance` is the arc length along the inside arc.
`sharp_angle` adjusts the "sharpness" i.e. acuteness of the angle of the
hook tip. Negative angles make it "blunter".
"""
slant_angle = Angle(slant_angle)
arc_angle = Angle(arc_angle)
old_mode = pen.mode
hook_width = old_mode.width
# The pen starts at the "middle" of the hook, a point along the base that
# is in the middle of the inside and outside bounding circles.
base_heading = pen.heading
base_middle = pen.position
# Calculate the radius.
circumference = distance / (arc_angle.theta / 360)
radius = circumference / (2 * math.pi)
# Trace the inside curve to find the hook tip and some other
# important points.
temp_pen = pen.copy()
temp_pen.turn_left(slant_angle)
temp_pen.arc_left(arc_angle, radius)
center_arc = temp_pen.last_segment()
tip = center_arc.b
center = center_arc.center
# Calculate the inside and outside corner position. The outside corner is located
# along the base line, intersecting the outer circle. The outer circle is
# concentric to the inner arc's circle, but with radius larger by the
# hook_width.
v_base = vec.rotate((1, 0), base_heading.rad)
switch_corners = (
(slant_angle > 0 and arc_angle < 0) or
(slant_angle < 0 and arc_angle > 0)
)
if switch_corners:
v_base = vec.neg(v_base)
base_right = vec.add(base_middle, v_base)
points_inside = intersect_circle_line(
center, abs(center_arc.radius) - (hook_width / 2),
base_middle, base_right,
)
points_outside = intersect_circle_line(
center, abs(center_arc.radius) + (hook_width / 2),
base_middle, base_right,
)
# Take the intersection point that is the most "forward" along the base
# from the inside corner.
def forwardness(p):
v_p = vec.vfrom(base_middle, p)
return vec.dot(v_p, v_base),
inside_corner = max(points_inside, key=forwardness)
outside_corner = max(points_outside, key=forwardness)
# Adjust for having started in the middle of the hook. Move to where the
# corner of the hook is at the pen start position.
if not switch_corners:
offset = vec.vfrom(inside_corner, base_middle)
else:
offset = vec.vfrom(outside_corner, base_middle)
inside_corner = vec.add(inside_corner, offset)
outside_corner = vec.add(outside_corner, offset)
tip = vec.add(tip, offset)
center = vec.add(center, offset)
# Draw the hook.
pen.set_mode(old_mode.outliner_mode())
# Base.
pen.move_to(inside_corner)
pen.line_to(outside_corner)
# Outer arc.
pen.turn_toward(center)
if arc_angle > 0:
pen.turn_right(90 + adjust_outside)
else:
pen.turn_left(90 + adjust_outside)
pen.arc_to(tip)
# Inner arc.
pen.turn_to(center_arc.end_heading + 180)
if arc_angle > 0:
pen.turn_right(adjust_inside)
else:
pen.turn_left(adjust_inside)
pen.arc_to(inside_corner)
pen.set_mode(old_mode)
def _vector(self):
return vec.vfrom(self.a, self.b)
def __init__(self, p1, p2, restitution=c.restitution_wall):
self.p1 = p1
self.p2 = p2
self.restitution = restitution
self.tangent = vec.vfrom(self.p1, self.p2)
self.normal = vec.perp(self.tangent)
def heading(self):
return Heading.from_rad(vec.heading(vec.vfrom(self.a, self.b)))
def dist(a, b):
return vec.mag(vec.vfrom(a, b))
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 heading(self):
return Heading.from_rad(vec.heading(vec.vfrom(self.a, self.b)))
