def get_ball_ball_collision_coeffs_fast(rvw1, rvw2, s1, s2, mu1, mu2, m1, m2, g1, g2, R):
"""Get the quartic coeffs required to determine the ball-ball collision time (just-in-time compiled)
Notes
=====
- Speed comparison in pooltool/tests/speed/get_ball_ball_collision_coeffs.py
"""
c1x, c1y = rvw1[0, 0], rvw1[0, 1]
c2x, c2y = rvw2[0, 0], rvw2[0, 1]
if s1 == const.spinning or s1 == const.pocketed or s1 == const.stationary:
a1x, a1y, b1x, b1y = 0, 0, 0, 0
else:
phi1 = utils.angle_fast(rvw1[1])
v1 = np.linalg.norm(rvw1[1])
u1 = (np.array([1,0,0], dtype=np.float64)
if s1 == const.rolling
else utils.coordinate_rotation_fast(utils.unit_vector_fast(utils.get_rel_velocity_fast(rvw1, R)), -phi1))
K1 = -0.5*mu1*g1
cos_phi1 = np.cos(phi1)
sin_phi1 = np.sin(phi1)
a1x = K1*(u1[0]*cos_phi1 - u1[1]*sin_phi1)
a1y = K1*(u1[0]*sin_phi1 + u1[1]*cos_phi1)
b1x = v1*cos_phi1
b1y = v1*sin_phi1
if s2 == const.spinning or s2 == const.pocketed or s2 == const.stationary:
a2x, a2y, b2x, b2y = 0., 0., 0., 0.
else:
phi2 = utils.angle_fast(rvw2[1])
v2 = np.linalg.norm(rvw2[1])
u2 = (np.array([1,0,0], dtype=np.float64)
if s2 == const.rolling
else utils.coordinate_rotation_fast(utils.unit_vector_fast(utils.get_rel_velocity_fast(rvw2, R)), -phi2))
K2 = -0.5*mu2*g2
cos_phi2 = np.cos(phi2)
sin_phi2 = np.sin(phi2)
a2x = K2*(u2[0]*cos_phi2 - u2[1]*sin_phi2)
a2y = K2*(u2[0]*sin_phi2 + u2[1]*cos_phi2)
b2x = v2*cos_phi2
b2y = v2*sin_phi2
Ax, Ay = a2x-a1x, a2y-a1y
Bx, By = b2x-b1x, b2y-b1y
Cx, Cy = c2x-c1x, c2y-c1y
a = Ax**2 + Ay**2
b = 2*Ax*Bx + 2*Ay*By
c = Bx**2 + 2*Ax*Cx + 2*Ay*Cy + By**2
d = 2*Bx*Cx + 2*By*Cy
e = Cx**2 + Cy**2 - 4*R**2
return a, b, c, d, e
def skip_ball_ball_collision(rvw1, rvw2, s1, s2, R1, R2):
if (s1 == const.spinning or s1 == const.pocketed or s1 == const.stationary) and \
(s2 == const.spinning or s2 == const.pocketed or s2 == const.stationary):
# Neither balls are moving. No collision.
return True
if s1 == const.pocketed or s2 == const.pocketed:
# One of the balls is pocketed
return True
if s1 == const.rolling and s2 == const.rolling:
# Both balls are rolling (straight line trajectories). Here I am checking whether both dot
# products face away from the line connecting the two balls. If so, they are guaranteed not
# to collide
r12 = rvw2[0] - rvw1[0]
dot1 = r12[0]*rvw1[1,0] + r12[1]*rvw1[1,1] + r12[2]*rvw1[1,2]
if dot1 <= 0:
dot2 = r12[0]*rvw2[1,0] + r12[1]*rvw2[1,1] + r12[2]*rvw2[1,2]
if dot2 >= 0:
return True
if s1 == const.rolling and (s2 == const.spinning or s2 == const.stationary):
# ball1 is rolling, which guarantees a straight-line trajectory. Some assumptions can be
# made based on this fact
r12 = rvw2[0] - rvw1[0]
# ball2 is not moving, so we can pinpoint the range of angles ball1 must be headed
# in for a collision
d = np.linalg.norm(r12)
unit_d = r12/d
unit_v = utils.unit_vector_fast(rvw1[1])
# Angles are in radians
# Calculate forwards and backwards angles, e.g. 10 and 350, take the min
angle = np.arccos(np.dot(unit_d, unit_v))
max_hit_angle = 0.5*np.pi - math.acos((R1+R2)/d)
if angle > max_hit_angle:
return True
if s2 == const.rolling and (s1 == const.spinning or s1 == const.stationary):
# ball2 is rolling, which guarantees a straight-line trajectory. Some assumptions can be
# made based on this fact
r21 = rvw1[0] - rvw2[0]
# ball1 is not moving, so we can pinpoint the range of angles ball2 must be headed
# in for a collision
d = np.linalg.norm(r21)
unit_d = r21/d
unit_v = utils.unit_vector_fast(rvw2[1])
# Angles are in radians
# Calculate forwards and backwards angles, e.g. 10 and 350, take the min
angle = np.arccos(np.dot(unit_d, unit_v))
max_hit_angle = 0.5*np.pi - math.acos((R1+R2)/d)
if angle > max_hit_angle:
return True
return False
def get_ball_ball_collision_coeffs(rvw1, rvw2, s1, s2, mu1, mu2, m1, m2, g1, g2, R):
"""Get the quartic coeffs required to determine the ball-ball collision time"""
c1x, c1y = rvw1[0, 0], rvw1[0, 1]
c2x, c2y = rvw2[0, 0], rvw2[0, 1]
if s1 in const.nontranslating:
a1x, a1y, b1x, b1y = 0, 0, 0, 0
else:
phi1 = utils.angle_fast(rvw1[1])
v1 = np.linalg.norm(rvw1[1])
u1 = (np.array([1,0,0])
if s1 == const.rolling
else utils.coordinate_rotation_fast(utils.unit_vector_fast(utils.get_rel_velocity_fast(rvw1, R)), -phi1))
K1 = -0.5*mu1*g1
cos_phi1 = np.cos(phi1)
sin_phi1 = np.sin(phi1)
a1x = K1*(u1[0]*cos_phi1 - u1[1]*sin_phi1)
a1y = K1*(u1[0]*sin_phi1 + u1[1]*cos_phi1)
b1x = v1*cos_phi1
b1y = v1*sin_phi1
if s2 in const.nontranslating:
a2x, a2y, b2x, b2y = 0, 0, 0, 0
else:
phi2 = utils.angle_fast(rvw2[1])
v2 = np.linalg.norm(rvw2[1])
u2 = (np.array([1,0,0])
if s2 == const.rolling
else utils.coordinate_rotation_fast(utils.unit_vector_fast(utils.get_rel_velocity_fast(rvw2, R)), -phi2))
K2 = -0.5*mu2*g2
cos_phi2 = np.cos(phi2)
sin_phi2 = np.sin(phi2)
a2x = K2*(u2[0]*cos_phi2 - u2[1]*sin_phi2)
a2y = K2*(u2[0]*sin_phi2 + u2[1]*cos_phi2)
b2x = v2*cos_phi2
b2y = v2*sin_phi2
Ax, Ay = a2x-a1x, a2y-a1y
Bx, By = b2x-b1x, b2y-b1y
Cx, Cy = c2x-c1x, c2y-c1y
a = Ax**2 + Ay**2
b = 2*Ax*Bx + 2*Ay*By
c = Bx**2 + 2*Ax*Cx + 2*Ay*Cy + By**2
d = 2*Bx*Cx + 2*By*Cy
e = Cx**2 + Cy**2 - 4*R**2
return a, b, c, d, e
def process_cushion_collision(self, entry):
cushion = self.get_cushion(entry)
cushion_height = cushion.p1[2]
# Point where cue center contacts collision plane
Px, Py, Pz = entry.getSurfacePoint(self.avoid_nodes['scene'])
# The tip of the cue stick
Ex, Ey, Ez = self.avoid_nodes['cue_stick_model'].getPos(
self.avoid_nodes['scene'])
# Center ofthe cueing ball
Bx, By, Bz = self.avoid_nodes['cue_stick_focus'].getPos(
self.avoid_nodes['scene'])
# The desired point where cue contacts collision plane, excluding cue width
Dx, Dy, Dz = Px, Py, cushion_height
# Center of aim
v = np.array([Ex - Px, Ey - Py, Ez - Pz])
u = utils.unit_vector_fast(
v) * self.avoid_nodes['cue_stick_model'].getX()
Fx, Fy, Fz = Ex + u[0], Ey + u[1], Ez + u[2]
min_theta = np.arctan2(Dz - Fz, np.sqrt((Dx - Fx)**2 + (Dy - Fy)**2))
# Correct for cue's cylindrical radius at collision point
# distance from cue tip (E) to desired collision point (D)
l = np.sqrt((Dx - Ex)**2 + (Dy - Ey)**2 + (Dz - Ez)**2)
cue_radius = self.get_cue_radius(l)
min_theta += np.arctan2(cue_radius, l)
return max(0, min_theta) * 180 / np.pi
def evolve_slide_state(rvw, R, m, u_s, u_sp, g, t):
if t == 0:
return rvw
# Angle of initial velocity in table frame
phi = utils.angle_fast(rvw[1])
rvw_B0 = utils.coordinate_rotation_fast(rvw.T, -phi).T
# Relative velocity unit vector in ball frame
u_0 = utils.coordinate_rotation_fast(utils.unit_vector_fast(utils.get_rel_velocity_fast(rvw, R)), -phi)
# Calculate quantities according to the ball frame. NOTE w_B in this code block
# is only accurate of the x and y evolution of angular velocity. z evolution of
# angular velocity is done in the next block
rvw_B = np.empty((3,3), dtype=np.float64)
rvw_B[0,0] = rvw_B0[1,0]*t - 0.5*u_s*g*t**2 * u_0[0]
rvw_B[0,1] = -0.5*u_s*g*t**2 * u_0[1]
rvw_B[0,2] = 0
rvw_B[1,:] = rvw_B0[1] - u_s*g*t*u_0
rvw_B[2,:] = rvw_B0[2] - 5/2/R*u_s*g*t * utils.cross_fast(u_0, np.array([0,0,1], dtype=np.float64))
# This transformation governs the z evolution of angular velocity
rvw_B[2, 2] = rvw_B0[2, 2]
rvw_B = evolve_perpendicular_spin_state(rvw_B, R, u_sp, g, t)
# Rotate to table reference
rvw_T = utils.coordinate_rotation_fast(rvw_B.T, phi).T
rvw_T[0] += rvw[0] # Add initial ball position
return rvw_T
def get_ball_pocket_collision_coeffs_fast(rvw, s, a, b, r, mu, m, g, R):
"""Get the quartic coeffs required to determine the ball-pocket collision time (just-in-time compiled)
Notes
=====
- Speed comparison in pooltool/tests/speed/get_ball_circular_cushion_collision_coeffs.py
"""
if s == const.spinning or s == const.pocketed or s == const.stationary:
return np.inf, np.inf, np.inf, np.inf, np.inf
phi = utils.angle_fast(rvw[1])
v = np.linalg.norm(rvw[1])
u = (np.array([1,0,0], dtype=np.float64)
if s == const.rolling
else utils.coordinate_rotation_fast(utils.unit_vector_fast(utils.get_rel_velocity_fast(rvw, R)), -phi))
K = -0.5*mu*g
cos_phi = np.cos(phi)
sin_phi = np.sin(phi)
ax = K*(u[0]*cos_phi - u[1]*sin_phi)
ay = K*(u[0]*sin_phi + u[1]*cos_phi)
bx, by = v*cos_phi, v*sin_phi
cx, cy = rvw[0, 0], rvw[0, 1]
A = 0.5 * (ax**2 + ay**2)
B = ax*bx + ay*by
C = ax*(cx-a) + ay*(cy-b) + 0.5*(bx**2 + by**2)
D = bx*(cx-a) + by*(cy-b)
E = 0.5*(a**2 + b**2 + cx**2 + cy**2 - r**2) - (cx*a + cy*b)
return A, B, C, D, E
def process_ball_collision(self, entry):
min_theta = 0
ball = self.get_ball(entry)
if ball == self.shots.active.cue.cueing_ball:
return 0
scene = render.find('scene')
# Radius of transect
n = np.array(entry.get_surface_normal(render.find('scene')))
phi = ((self.avoid_nodes['cue_stick_focus'].getH() + 180) %
360) * np.pi / 180
c = np.array([np.cos(phi), np.sin(phi), 0])
gamma = np.arccos(np.dot(n, c))
AB = (ball.R + self.shots.active.cue.tip_radius) * np.cos(gamma)
# Center of blocking ball transect
Ax, Ay, _ = entry.getSurfacePoint(scene)
Ax -= (AB + self.shots.active.cue.tip_radius) * np.cos(phi)
Ay -= (AB + self.shots.active.cue.tip_radius) * np.sin(phi)
Az = ball.R
# Center of aim, leveled to ball height
Cx, Cy, Cz = self.avoid_nodes['cue_stick_focus'].getPos(scene)
axR = -self.avoid_nodes['cue_stick'].getY()
Cx += -axR * np.sin(phi)
Cy += axR * np.cos(phi)
AC = np.sqrt((Ax - Cx)**2 + (Ay - Cy)**2 + (Az - Cz)**2)
BC = np.sqrt(AC**2 - AB**2)
min_theta_no_english = np.arcsin(AB / AC)
# Cue tip point, no top/bottom english
m = self.avoid_nodes['cue_stick_model'].getX()
u = utils.unit_vector_fast(
np.array(
[-np.cos(phi), -np.sin(phi),
np.sin(min_theta_no_english)]))
Ex, Ey, Ez = Cx + m * u[0], Cy + m * u[1], Cz + m * u[2]
# Point where cue contacts blocking ball, no top/bottom english
Bx, By, Bz = Cx + BC * u[0], Cy + BC * u[1], Cz + BC * u[2]
# Extra angle due to top/bottom english
BE = np.sqrt((Bx - Ex)**2 + (By - Ey)**2 + (Bz - Ez)**2)
bxR = self.avoid_nodes['cue_stick'].getZ()
beta = -np.arctan2(bxR, BE)
if beta < 0:
beta += 10 * np.pi / 180 * (np.exp(bxR / BE)**2 - 1)
min_theta = min_theta_no_english + beta
return max(0, min_theta) * 180 / np.pi
def aim_at_pos(self, pos):
"""Set phi to aim at a 3D position
Parameters
==========
pos : array-like
A length-3 iterable specifying the x, y, z coordinates of the position to be aimed at
"""
direction = utils.angle_fast(
utils.unit_vector_fast(np.array(pos) - self.cueing_ball.rvw[0]))
self.set_state(phi=direction * 180 / np.pi)
def get_ball_linear_cushion_collision_time(rvw, s, lx, ly, l0, p1, p2, mu, m, g, R):
"""Get the time until collision between ball and linear cushion segment"""
if s in const.nontranslating:
return np.inf
phi = utils.angle_fast(rvw[1])
v = np.linalg.norm(rvw[1])
u = (np.array([1,0,0]
if s == const.rolling
else utils.coordinate_rotation_fast(utils.unit_vector_fast(utils.get_rel_velocity_fast(rvw, R)), -phi)))
cos_phi = np.cos(phi)
ax = -0.5*mu*g*(u[0]*np.cos(phi) - u[1]*np.sin(phi))
ay = -0.5*mu*g*(u[0]*np.sin(phi) + u[1]*np.cos(phi))
bx, by = v*np.cos(phi), v*np.sin(phi)
cx, cy = rvw[0, 0], rvw[0, 1]
A = lx*ax + ly*ay
B = lx*bx + ly*by
C1 = l0 + lx*cx + ly*cy + R*np.sqrt(lx**2 + ly**2)
C2 = l0 + lx*cx + ly*cy - R*np.sqrt(lx**2 + ly**2)
root1, root2 = utils.quadratic_fast(A,B,C1)
root3, root4 = utils.quadratic_fast(A,B,C2)
roots = np.array([
root1,
root2,
root3,
root4,
])
roots = roots[
(abs(roots.imag) <= const.tol) & \
(roots.real > const.tol)
].real
# All roots beyond this point are real and positive
for i, root in enumerate(roots):
rvw_dtau, _ = evolve_state_motion(s, rvw, R, m, mu, 1, mu, g, root)
s_score = - np.dot(p1 - rvw_dtau[0], p2 - p1) / np.dot(p2 - p1, p2 - p1)
if not (0 <= s_score <= 1):
roots[i] = np.inf
return roots.min() if len(roots) else np.inf
def resolve_ball_ball_collision(rvw1, rvw2):
"""FIXME Instantaneous, elastic, equal mass collision"""
r1, r2 = rvw1[0], rvw2[0]
v1, v2 = rvw1[1], rvw2[1]
v_rel = v1 - v2
v_mag = np.linalg.norm(v_rel)
n = utils.unit_vector_fast(r2 - r1)
t = utils.coordinate_rotation_fast(n, np.pi/2)
beta = utils.angle_fast(v_rel, n)
rvw1[1] = t * v_mag*np.sin(beta) + v2
rvw2[1] = n * v_mag*np.cos(beta) + v2
return rvw1, rvw2
def __init__(self, cushion_id, p1, p2, direction=2):
"""A linear cushion segment defined by the line between points p1 and p2
Parameters
==========
p1 : tuple
A length-3 tuple that defines a 3D point in space where the cushion segment starts
p2 : tuple
A length-3 tuple that defines a 3D point in space where the cushion segment ends
direction: 0 or 1, default 2
For most table geometries, the playing surface only exists on one side of the cushion,
so collisions only need to be checked for one direction. This direction can be specified
with either 0 or 1. To determine whether 0 or 1 should be used, please experiment
(FIXME: determine the rule). By default, both collision directions are checked, which
can be specified explicitly by passing 2, however this makes collision checks twice as
slow for event-based shot evolution algorithms.
"""
self.id = cushion_id
self.p1 = np.array(p1, dtype=np.float64)
self.p2 = np.array(p2, dtype=np.float64)
p1x, p1y, p1z = self.p1
p2x, p2y, p2z = self.p2
if p1z != p2z:
raise ValueError(f"LinearCushionSegment with id '{self.id}' has points p1 and p2 with different cushion heights (h)")
self.height = p1z
if (p2x - p1x) == 0:
self.lx = 1
self.ly = 0
self.l0 = -p1x
else:
self.lx = - (p2y - p1y) / (p2x - p1x)
self.ly = 1
self.l0 = (p2y - p1y) / (p2x - p1x) * p1x - p1y
self.normal = utils.unit_vector_fast(np.array([self.lx, self.ly, 0]))
if direction not in {0, 1, 2}:
raise ConfigError("LinearCushionSegment :: `direction` must be 0, 1, or 2.")
self.direction = direction
def get_ball_pocket_collision_coeffs(rvw, s, a, b, r, mu, m, g, R):
"""Get the quartic coeffs required to determine the ball-pocket collision time
Parameters
==========
a : float
The x-coordinate of the pocket's center
b : float
The y-coordinate of the pocket's center
r : float
The radius of the pocket's center
mu : float
The rolling or sliding coefficient of friction. Should match the value of s
"""
if s in const.nontranslating:
return np.inf, np.inf, np.inf, np.inf, np.inf
phi = utils.angle_fast(rvw[1])
v = np.linalg.norm(rvw[1])
u = (np.array([1,0,0])
if s == const.rolling
else utils.coordinate_rotation_fast(utils.unit_vector_fast(utils.get_rel_velocity_fast(rvw, R)), -phi))
K = -0.5*mu*g
cos_phi = np.cos(phi)
sin_phi = np.sin(phi)
ax = K*(u[0]*cos_phi - u[1]*sin_phi)
ay = K*(u[0]*sin_phi + u[1]*cos_phi)
bx, by = v*cos_phi, v*sin_phi
cx, cy = rvw[0, 0], rvw[0, 1]
A = 0.5 * (ax**2 + ay**2)
B = ax*bx + ay*by
C = ax*(cx-a) + ay*(cy-b) + 0.5*(bx**2 + by**2)
D = bx*(cx-a) + by*(cy-b)
E = 0.5*(a**2 + b**2 + cx**2 + cy**2 - r**2) - (cx*a + cy*b)
return A, B, C, D, E
def skip_ball_linear_cushion_collision(rvw, s, u_r, g, R, p1, p2, normal):
if (s == const.spinning or s == const.pocketed or s == const.stationary):
# Ball isn't moving. No collision.
return True
if s == const.rolling:
# Since the ball is rolling, it is a straight line trajectory. The strategy here is to
# see whether the trajectory of the ball is going to intersect with either of the collisions
# defined by a linear cushion segment. Let r1 be the position of the ball and r2 be the
# final position of the ball (rolling to a stop). Let p11 and p21 be the intersection points
# of the first intersection line, and let p12 and p22 be the intersection points of the
# second. This code uses orientation to determine if If r1 -> r2 intersects p11 -> p21 or
# p12 -> p22
p11 = p1 + R*normal
p12 = p1 - R*normal
p21 = p2 + R*normal
p22 = p2 - R*normal
t = np.linalg.norm(rvw[1]) / (u_r*g)
v_0_hat = utils.unit_vector_fast(rvw[1])
r1 = rvw[0]
r2 = r1 + rvw[1] * t - 0.5*u_r*g*t**2 * v_0_hat
o1 = utils.orientation(r1, r2, p11)
o2 = utils.orientation(r1, r2, p21)
o3 = utils.orientation(p11, p21, r1)
o4 = utils.orientation(p11, p21, r2)
# Whether or not trajectory intersects with first intersection line
int1 = ((o1 != o2) and (o3 != o4))
o1 = utils.orientation(r1, r2, p12)
o2 = utils.orientation(r1, r2, p22)
o3 = utils.orientation(p12, p22, r1)
o4 = utils.orientation(p12, p22, r2)
# Whether or not trajectory intersects with first intersection line
int2 = ((o1 != o2) and (o3 != o4))
if not int1 and not int2:
return True
return False
def evolve_roll_state(rvw, R, u_r, u_sp, g, t):
if t == 0:
return rvw
r_0, v_0, w_0 = rvw
v_0_hat = utils.unit_vector_fast(v_0)
r = r_0 + v_0 * t - 0.5*u_r*g*t**2 * v_0_hat
v = v_0 - u_r*g*t * v_0_hat
w = utils.coordinate_rotation_fast(v/R, np.pi/2)
# Independently evolve the z spin
temp = evolve_perpendicular_spin_state(rvw, R, u_sp, g, t)
w[2] = temp[2, 2]
new_rvw = np.empty((3,3), dtype=np.float64)
new_rvw[0,:] = r
new_rvw[1,:] = v
new_rvw[2,:] = w
return new_rvw
def get_normal(self, rvw):
normal = utils.unit_vector_fast(rvw[0,:] - self.center)
normal[2] = 0 # remove z-component
return normal
def get_ball_linear_cushion_collision_time_fast(rvw, s, lx, ly, l0, p1, p2, direction, mu, m, g, R):
"""Get the time until collision between ball and linear cushion segment (just-in-time compiled)
Notes
=====
- Speed comparison in pooltool/tests/speed/get_ball_circular_cushion_collision_coeffs.py
"""
if s == const.spinning or s == const.pocketed or s == const.stationary:
return np.inf
phi = utils.angle_fast(rvw[1])
v = np.linalg.norm(rvw[1])
u = (np.array([1,0,0], dtype=np.float64)
if s == const.rolling
else utils.coordinate_rotation_fast(utils.unit_vector_fast(utils.get_rel_velocity_fast(rvw, R)), -phi))
K = -0.5*mu*g
cos_phi = np.cos(phi)
sin_phi = np.sin(phi)
ax = K*(u[0]*cos_phi - u[1]*sin_phi)
ay = K*(u[0]*sin_phi + u[1]*cos_phi)
bx, by = v*cos_phi, v*sin_phi
cx, cy = rvw[0, 0], rvw[0, 1]
A = lx*ax + ly*ay
B = lx*bx + ly*by
if direction == 0:
C = l0 + lx * cx + ly * cy + R * np.sqrt(lx ** 2 + ly ** 2)
root1, root2 = utils.quadratic_fast(A,B,C)
roots = [root1, root2]
elif direction == 1:
C = l0 + lx * cx + ly * cy - R * np.sqrt(lx ** 2 + ly ** 2)
root1, root2 = utils.quadratic_fast(A,B,C)
roots = [root1, root2]
else:
C1 = l0 + lx * cx + ly * cy + R * np.sqrt(lx ** 2 + ly ** 2)
C2 = l0 + lx * cx + ly * cy - R * np.sqrt(lx ** 2 + ly ** 2)
root1, root2 = utils.quadratic_fast(A,B,C1)
root3, root4 = utils.quadratic_fast(A,B,C2)
roots = [root1, root2, root3, root4]
min_time = np.inf
for root in roots:
if np.abs(root.imag) > const.tol:
continue
if root.real <= const.tol:
continue
rvw_dtau, _ = evolve_state_motion(s, rvw, R, m, mu, 1, mu, g, root)
s_score = - np.dot(p1 - rvw_dtau[0], p2 - p1) / np.dot(p2 - p1, p2 - p1)
if not (0 <= s_score <= 1):
continue
if root.real < min_time:
min_time = root.real
return min_time