# sakaguchi.py
# 坂口玲仁, ボウリングの数理と剛体の運動のシミュレーション, 2025/2/28

import math
import numpy as np
from scipy.integrate import odeint # 常分方程式をとくライブラリ
import matplotlib.pyplot as plt

#初期パラメータ
g=9.81 # 重力加速度[m/s^2]
u_k=0.12 # 動摩擦係数
u_s=0.2 # 静止摩擦係数
R=0.1085 # ボールの半径[m]
m =7 # 質量[kg]
v0 =8 # 初速[m/s]
θ =0 # 投球角度

# 角速度ベクトル
wx = -7.76
wy = 28.98
wz = 0.0

# 回転軸に対する物体の抵抗を表す慣性乗積(主慣性モーメント)
Ix = 0.033
Iy = 0.033
Iz = 0.033

import math

# ボールの摩擦力を計算し、滑る場合と転がる場合を分ける
def compute_friction_forces(X,μ_k,μ_s,g,R,m):
    vx, vy, wx, wy = X[2], X[3], X[4], X[5]

    # 滑っているか転がっているかの判定
    slip_x = vx - R * wy # x方向の滑り
    slip_y = vy + R * wx # y方向の滑り
    slip_magnitude = math.sqrt(slip_x**2 + slip_y**2) # 滑りの大きさ
    
    if slip_magnitude > 1e-5: # 滑っている場合(動摩擦)
        friction = np.array([
            -μ_k * g * (slip_x / slip_magnitude),
            -μ_k * g * (slip_y / slip_magnitude),
            0
        ])
    else: # 転がっている場合(静止摩擦)
        friction = np.array([
            - μ_s * g * slip_x,
            - μ_s * g * slip_y,
            0
        ])

    return friction

# ボウリングのボールの運動を記述する微分方程式
def func(X, t, μ_k, μ_s, g, R, m, Ix, Iy, Iz):
    # 現在の状態変数を取得
    x,y,vx, vy, ωx, ωy, ωz=X[:7]

    # 摩擦力の計算
    friction = compute_friction_forces(X, μ_k, μ_s, g, R, m)

    # 角運動量ベクトルの計算
    L = np.array([Ix * ωx, Iy * ωy, Iz * ωz])

    # 剛体の回転運動における角加速度の計算
    dL_dt = np.cross(L, np.array([ωx, ωy, ωz]))
    + np.array([-R * friction[1], R * friction[0], 0])
    dw_dt = np.array([dL_dt[0] / Ix, dL_dt[1] / Iy, dL_dt[2] / Iz])

    # 慣性主軸ベクトルの取得
    i = np.array([X[7], X[8], X[9]])
    j = np.array([X[10], X[11], X[12]])
    k = np.array([X[13], X[14], X[15]])
    # 慣性主軸の更新(角速度ベクトルとの外積)
    di_dt = np.cross(np.array([ωx, ωy, ωz]), i)
    dj_dt = np.cross(np.array([ωx, ωy, ωz]), j)
    dk_dt = np.cross(np.array([ωx, ωy, ωz]), k)

    # 速度の更新
    dv_dt = friction[:2]
    return [vx, vy, dv_dt[0], dv_dt[1], dw_dt[0], dw_dt[1], dw_dt[2],
            di_dt[0], di_dt[1], di_dt[2], dj_dt[0], dj_dt[1], dj_dt[2],
            dk_dt[0], dk_dt[1], dk_dt[2]]

X0 = [0, 0, v0*math.cos(math.radians(θ)), v0*math.sin(math.radians(θ)),
      wx, wy, wz, 1,0,0, 0,1,0, 0,0,1] # ボールの初期値
#X = [x, y, v*cos 0, v*sin 0, wx, wy, wz, ]
t= np.arange(0,10,0.01) # tの範囲を設定

X = odeint(func, X0, t, args=(u_k, u_s, g, R, m, Ix, Iy, Iz))

# ボウリングピンを配置
def plot_bowling_pins():
    pin_coordinates = np.array([
        [18.29, 0],
        [18.554, 0.1524],
        [18.554, -0.1524],
        [18.818, 0.3048],
        [18.818, -0.3048],
        [18.818, 0],
        [19.082, 0.1524],
        [19.082, -0.1524],
        [19.082, 0.4572],
        [19.082, -0.4572],
    ])

    plt.figure(figsize=(20.29, 1.066))
    plt.scatter(pin_coordinates[:,0], pin_coordinates[:,1],
                marker='o', color='black', s=30)

plot_bowling_pins()

#x,yを描写
plt.plot(X[:,0], X[:,1])

plt.xlim(0, 20.29)
plt.ylim(-0.533, 0.533)
plt.xlabel('x', fontsize=10)
plt.ylabel('y', fontsize=10)

plt.title('Bowling Ball Motion')
plt.show()