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