def collision_aw_aw(xy0, vxy0, w0, xy1, vxy1, w1):
th_col = dc.calc_th(xy0, xy1)
v0, th0 = dc.calc_v(vxy0), dc.calc_th(vxy0)
v0n = v0 * math.cos(th0 - th_col)
v0t = v0 * math.sin(th0 - th_col)
v1, th1 = dc.calc_v(vxy1), dc.calc_th(vxy1)
v1n = v1 * math.cos(th1 - th_col)
v1t = v1 * math.sin(th1 - th_col)
vn, vt = v0n - v1n, v0t - v1t
v_threshold = 1
ni_m = vn
if abs(vn) <= v_threshold:
ni_m *= 0.5
maxFriction = FRICTION_STONES * ni_m
oldTangentLambda = vt / 6.0
ti_m = max(-maxFriction, min(oldTangentLambda, maxFriction))
dw = ti_m * 2.0 / dc.STONE_RADIUS
# update
awv1 = np.hypot(v1n + ni_m, v1t + ti_m)
awth1 = np.arctan2(v1n + ni_m, v1t + ti_m) + th_col
aww1 = w1 + dw
awvxy1 = (awv1 * math.sin(awth1), awv1 * math.cos(awth1))
awv0 = np.hypot(v0n - ni_m, v0t - ti_m)
awth0 = np.arctan2(v0n - ni_m, v0t - ti_m) + th_col
aww0 = w0 + dw
awvxy0 = (awv0 * math.sin(awth0), awv0 * math.cos(awth0))
return awvxy0, aww0, awvxy1, aww1
def collision_aw_asl(xy, vxy, w, asxy):
th_col = dc.calc_th(xy, asxy)
v, th = dc.calc_v(vxy), dc.calc_th(vxy)
# relative velocity(+)
vt = v * math.sin(th - th_col) # tangent velocity
vn = max(0.0, v * math.cos(th - th_col)) # normal velocity
v_threshold = 1
ni_m = vn
if vn <= v_threshold:
ni_m *= 0.5
maxFriction = FRICTION_STONES * ni_m
oldTangentLambda = vt / 6.0
ti_m = max(-maxFriction, min(oldTangentLambda, maxFriction))
dw = ti_m * 2.0 / dc.STONE_RADIUS
# update
asv = np.hypot(ni_m, ti_m)
asth = np.arctan2(ni_m, ti_m) + th_col
asw = dw
asvxy = (asv * math.sin(asth), asv * math.cos(asth))
awv = np.hypot(vn - ni_m, vt - ti_m)
awth = np.arctan2(vn - ni_m, vt - ti_m) + th_col
aww = w + dw
awvxy = (awv * math.sin(awth), awv * math.cos(awth))
return awvxy, aww, asvxy, asw
def calc_next_vtheta(vxy, w):
v = dc.calc_v(vxy)
normalized_vxy = vxy / v
if w > 0:
angle = -math.pi / 2
else:
angle = math.pi / 2
dvxy = np.matmul(rotationMatrix(angle), normalized_vxy) * FRICTION_RINK
return (vxy[0] + dvxy[0], vxy[1] + dvxy[1]), w
def step_xy_vxy_by_xy_vxywt(xy, vxy, w, t):
V = dc.calc_v(vxy)
R = calc_r_by_v(V)
Theta = calc_theta_by_r(R)
v = V - FRICTION_RINK * t
r = calc_r_by_v(v)
#print(V, v, r)
theta = calc_theta_by_r(r)
print(r, Theta - theta)
l2 = (R ** 2) + (r ** 2) - 2 * R * r * math.cos(Theta - theta)
l = math.sqrt(l2)
#print("l =", l)
#cos_beta = ((l ** 2) + (R ** 2) - (r ** 2)) / (2 * l * R)
#beta = math.acos(cos_beta)
sin_beta = r / l * math.sin(Theta - theta)
beta = math.asin(sin_beta)
#print("beta =", beta)
dtheta = Theta - theta
alpha = ALPHA
if w < 0:
dtheta = -dtheta
alpha = -alpha
beta = -beta
#print("ovtheta =", dc.calc_th(vxy), "dtheta =", dtheta)
ntheta = dc.calc_th(vxy) + alpha - beta
nvtheta = dc.calc_th(vxy) + dtheta
#print("ntheta =", ntheta, "nvtheta =", nvtheta)
nxy = (xy[0] + l * math.sin(ntheta), xy[1] + l * math.cos(ntheta))
nvxy = (v * math.sin(nvtheta), v * math.cos(nvtheta))
return nxy, nvxy
def calc_dxy_by_vxys(vxy, spin):
return DR_V_R * dc.calc_v(vxy) * np.matmul(MAT_ALPHA[spin], vxy)
def deliver_by_xy_vxyw(stones, index, oxy, vxy, w):
gxy = calc_xy_by_xy_vxyw(oxy, vxy, w)
if not len(stones):
# no asleep stones
return [(index, gxy)]
asleep = copy.deepcopy(stones)
awake = [(index, oxy, vxy, w, gxy)]
t = 0
while len(awake):
print(t, awake, asleep)
# awake itself
dt_self = []
for aw in awake:
st = calc_t_by_v(dc.calc_v(aw[2]))
dt_self.append(st)
# awake - awake
dt_awake_min = float('inf')
for i, aw0 in enumerate(awake):
for j, aw1 in enumerate(awake[:i]):
if is_collisious_aw_aw(aw0[1], aw0[2], aw1[1], aw1[2]):
dt = calc_dt_by_aw_aw(aw0[1], aw0[2], aw0[3], aw0[4],
aw1[1], aw1[2], aw1[3], aw1[4])
if dt < dt_awake_min:
dt_awake_min = dt
pair_awake = (i, j)
# awake - asleep
dt_asleep_min = float('inf')
for i, aw in enumerate(awake):
for j, asl in enumerate(asleep):
if is_collisious_aw_as(aw[1], aw[1], asl[1]):
dt = calc_dt_by_aw_asl(aw[1], aw[2], aw[3], aw[4], asl[1])
if dt < dt_asleep_min:
dt_asleep_min= dt
pair_asleep = (i, j)
if dt_awake_min <= dt_asleep_min:
if dt_awake_min < DT_COLLISION:
# awake - awake collision
aw0, aw1 = awake[pair_awake[0]], awake[pair_awake[1]]
awvxy0, aww0, awvxy1, aww1 = collision_aw_aw(aw0[1], aw0[2], aw0[3],
aw1[1], aw1[2], aw1[3])
awake[pair_awake[0]] = (aw0[0], aw0[1], awvxy0, aww0, calc_xy_by_xy_vxyw(aw0[1], awvxy0, aww0))
awake[pair_awake[1]] = (aw1[0], aw1[1], awvxy1, aww1, calc_xy_by_xy_vxyw(aw1[1], awvxy1, aww1))
continue
dt_all = dt_awake_min
else:
if dt_asleep_min < DT_COLLISION:
# awake - asleep collision
aw, asl = awake[pair_asleep[0]], asleep[pair_asleep[1]]
awvxy, aww, asvxy, asw = collision_aw_asl(aw[1], aw[2], aw[3], asl[1])
awake[pair_asleep[0]] = (aw[0], aw[1], awvxy, aww, calc_xy_by_xy_vxyw(aw[1], awvxy, aww))
awake.append((asl[0], asl[1], asvxy, asw, calc_xy_by_xy_vxyw(asl[1], asvxy, asw)))
asleep.pop(pair_asleep[1])
continue
dt_all = dt_asleep_min
print("dt_all = %f" % dt_all)
next_awake = []
# stop
for i, st in enumerate(dt_self):
if st <= dt_all:
aw = awake[i]
asleep.append((aw[0], aw[4]))
else:
next_awake.append(aw)
print(next_awake)
# move
for i, aw in enumerate(next_awake):
nxy, nvxy = step_xy_vxy_by_xy_vxywt(aw[1], aw[2], aw[3], dt_all)
next_awake[i] = (aw[0], nxy, nvxy, aw[3], aw[4])
awake = next_awake
t += dt
return asleep
def calc_dt_by_aw_aw(xy0, vxy0, w0, gxy0, xy1, vxy1, w1, gxy1):
r = dc.calc_r(xy0, xy1) - 2 * dc.STONE_RADIUS
return r / (dc.calc_v(vxy0) + dc.calc_v(vxy1))
def calc_dt_by_aw_asl(xy, vxy, w, gxy, asxy):
gt = calc_t_by_v(dc.calc_v(vxy))
return gt - calc_nt_by_aw_asl(xy, vxy, w, gxy, asxy)
print(np.array([[((calc_r_by_lv(l, v) - calc_r_lb_by_lv(l, v)) / (calc_r_ub_by_lv(l, v) - calc_r_lb_by_lv(l, v)))
for l in range(28, 38, 2)] for v in range(30, 33)]))
print(np.array([[1 - (calc_r_lb_by_lv(l, v) / calc_r_by_lv(l, v))
for l in range(28, 38, 2)] for v in range(30, 33)]))
print(np.array([[(calc_r_ub_by_lv(l, v) / calc_r_by_lv(l, v)) - 1
for l in range(28, 38, 2)] for v in range(30, 33)]))
print("step function")
gxy = dc.XY_TEE
w = dc.W_SPIN
xy = dc.XY_THROW
vxy = calc_vxy_by_xyw(xy, gxy, w)
t = calc_t_by_v(dc.calc_v(vxy))
x = [float(i) for i in range(2, 100)]
print(vxy)
steps = 0
while t > 0.0001:
print(t, xy, vxy)
dt = t * (1 - 1 / x[steps])
xy, vxy = step_xy_vxy_by_xy_vxywt(xy, vxy, w, dt)
t -= dt
steps += 1
print(xy, vxy)
w = -dc.W_SPIN
xy = dc.XY_THROW
