def get_ball_ball_collision_time(rvw1, rvw2, s1, s2, mu1, mu2, m1, m2, g1, g2,
R):
"""Get the time until collision between 2 balls"""
c1x, c1y = rvw1[0, 0], rvw1[0, 1]
c2x, c2y = rvw2[0, 0], rvw2[0, 1]
if s1 == pooltool.stationary or s1 == pooltool.spinning:
a1x, a1y, b1x, b1y = 0, 0, 0, 0
else:
phi1 = utils.angle(rvw1[1])
v1 = np.linalg.norm(rvw1[1])
u1 = (np.array([1, 0, 0])
if s1 == pooltool.rolling else utils.coordinate_rotation(
utils.unit_vector(get_rel_velocity(rvw1, R)), -phi1))
a1x = -1 / 2 * mu1 * g1 * (u1[0] * np.cos(phi1) - u1[1] * np.sin(phi1))
a1y = -1 / 2 * mu1 * g1 * (u1[0] * np.sin(phi1) + u1[1] * np.cos(phi1))
b1x = v1 * np.cos(phi1)
b1y = v1 * np.sin(phi1)
if s2 == pooltool.stationary or s2 == pooltool.spinning:
a2x, a2y, b2x, b2y = 0, 0, 0, 0
else:
phi2 = utils.angle(rvw2[1])
v2 = np.linalg.norm(rvw2[1])
u2 = (np.array([1, 0, 0])
if s2 == pooltool.rolling else utils.coordinate_rotation(
utils.unit_vector(get_rel_velocity(rvw2, R)), -phi2))
a2x = -1 / 2 * mu2 * g2 * (u2[0] * np.cos(phi2) - u2[1] * np.sin(phi2))
a2y = -1 / 2 * mu2 * g2 * (u2[0] * np.sin(phi2) + u2[1] * np.cos(phi2))
b2x = v2 * np.cos(phi2)
b2y = v2 * np.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
roots = np.roots([a, b, c, d, e])
roots = roots[
(abs(roots.imag) <= pooltool.tol) & \
(roots.real > pooltool.tol)
].real
return roots.min() if len(roots) else np.inf
def get_ball_rail_collision_time(rvw, s, lx, ly, l0, mu, m, g, R):
"""Get the time until collision between ball and collision"""
if s == pooltool.stationary or s == pooltool.spinning:
return np.inf
phi = utils.angle(rvw[1])
v = np.linalg.norm(rvw[1])
u = (np.array([1, 0, 0] if s ==
pooltool.rolling else utils.coordinate_rotation(
utils.unit_vector(get_rel_velocity(rvw, R)), -phi)))
ax = -1 / 2 * mu * g * (u[0] * np.cos(phi) - u[1] * np.sin(phi))
ay = -1 / 2 * 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)
roots = np.append(np.roots([A, B, C1]), np.roots([A, B, C2]))
roots = roots[
(abs(roots.imag) <= pooltool.tol) & \
(roots.real > pooltool.tol)
].real
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(r2 - r1)
t = utils.coordinate_rotation(n, np.pi / 2)
beta = utils.angle(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 evolve_roll_state(rvw, R, u_r, u_sp, g, t):
if t == 0:
return rvw
r_0, v_0, w_0, e_0 = rvw
v_0_hat = utils.unit_vector(v_0)
r = r_0 + v_0 * t - 1 / 2 * u_r * g * t**2 * v_0_hat
v = v_0 - u_r * g * t * v_0_hat
w = utils.coordinate_rotation(v / R, np.pi / 2)
e = e_0 + utils.coordinate_rotation(
(v_0 * t - 1 / 2 * u_r * g * t**2 * v_0_hat) / 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]
e[2] = temp[3, 2]
return np.array([r, v, w, e])
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(rvw[1])
rvw_B0 = utils.coordinate_rotation(rvw.T, -phi).T
# Relative velocity unit vector in ball frame
u_0 = utils.coordinate_rotation(
utils.unit_vector(get_rel_velocity(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.array([
np.array([
rvw_B0[1, 0] * t - 1 / 2 * u_s * g * t**2 * u_0[0],
-1 / 2 * u_s * g * t**2 * u_0[1], 0
]),
rvw_B0[1] - u_s * g * t * u_0,
rvw_B0[2] -
5 / 2 / R * u_s * g * t * np.cross(u_0, np.array([0, 0, 1])),
rvw_B0[3] + rvw_B0[2] * t - 1 / 2 * 5 / 2 / R * u_s * g * t**2 *
np.cross(u_0, np.array([0, 0, 1])),
])
# This transformation governs the z evolution of angular velocity
rvw_B[2, 2] = rvw_B0[2, 2]
rvw_B[3, 2] = rvw_B0[3, 2]
rvw_B = evolve_perpendicular_spin_state(rvw_B, R, u_sp, g, t)
# Rotate to table reference
rvw_T = utils.coordinate_rotation(rvw_B.T, phi).T
rvw_T[0] += rvw[0] # Add initial ball position
return rvw_T
def get_infinitesimal_quaternions(w, t, dQ_0=None):
w_norm = np.linalg.norm(w, axis=1)
w_unit = utils.unit_vector(w, handle_zero=True)
dt = np.diff(t)
theta = w_norm[1:] * dt
# Quaternion looks like m + xi + yj + zk
dQ_m = np.cos(theta / 2)[:, None]
dQ_xyz = w_unit[1:] * np.sin(theta / 2)[:, None]
dQ = np.hstack([dQ_m, dQ_xyz])
# Since the time elapsed is calculated from a difference of timestamps
# there is one less datapoint than needed. I remedy this by adding the
# identity quaternion as the first point
if dQ_0 is None:
dQ_0 = np.array([1, 0, 0, 0])
else:
dQ_0 = np.array(dQ_0)
dQ_0 = get_quat_from_vector(dQ_0)
dQ = np.vstack([dQ_0, dQ])
return dQ