Simple-Golf-Simulator/BasicFunc2.py at master · markccchiang/Simple-Golf-Simulator · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
from math import *

#
# physical parameters
#
g       = 9.806 # (m/sec^2)
h       = 0.001 # (sec)

#
# Other parameters
#
PI      = 3.141592653589793

#
# sub-functions
#
def cross_prod(a1, a2, a3, b1, b2, b3):
    vec_i = a2*b3 - b2*a3
    vec_j = b1*a3 - a1*b3
    vec_k = a1*b2 - b1*a2
    return vec_i, vec_j, vec_k

def unit_vec(c1, c2, c3):
    tmp_length = sqrt(c1*c1 + c2*c2 + c3*c3)
    unit_i = c1/tmp_length
    unit_j = c2/tmp_length
    unit_k = c3/tmp_length
    return unit_i, unit_j, unit_k

def vec_length(d1, d2, d3):
    return sqrt(d1*d1 + d2*d2 + d3*d3)

def minus_vec(e1, e2, e3):
    return -1*e1, -1*e2, -1*e3

def ax(v_ball_i, v_ball_j, v_ball_k, \
       v_wind_i, v_wind_j, v_wind_k, \
       w_unit_i, w_unit_j, w_unit_k, \
       C_D, C_L, rho_air, m, D):
    #
    A = (D/2)**2*PI
    #
    U_i = -1*v_ball_i + v_wind_i
    U_j = -1*v_ball_j + v_wind_j
    U_k = -1*v_ball_k + v_wind_k
    #
    abs_FD = C_D*A*rho_air*(U_i**2 + U_j**2 + U_k**2)/2
    abs_FL = C_L*A*rho_air*(U_i**2 + U_j**2 + U_k**2)/2
    #
    U_unit_i,  U_unit_j,  U_unit_k = unit_vec(U_i, U_j, U_k)
    FL_unit_i, FL_unit_j, FL_unit_k = cross_prod(U_unit_i, U_unit_j, U_unit_k, \
                                                 w_unit_i, w_unit_j, w_unit_k)
    ans = abs_FD*U_unit_i + abs_FL*FL_unit_i
    return ans/m

def ay(v_ball_i, v_ball_j, v_ball_k, \
       v_wind_i, v_wind_j, v_wind_k, \
       w_unit_i, w_unit_j, w_unit_k, \
       C_D, C_L, rho_air, m, D):
    #
    A = (D/2)**2*PI
    #
    U_i = -1*v_ball_i + v_wind_i
    U_j = -1*v_ball_j + v_wind_j
    U_k = -1*v_ball_k + v_wind_k
    #
    abs_FD = C_D*A*rho_air*(U_i**2 + U_j**2 + U_k**2)/2
    abs_FL = C_L*A*rho_air*(U_i**2 + U_j**2 + U_k**2)/2
    #
    U_unit_i,  U_unit_j,  U_unit_k = unit_vec(U_i, U_j, U_k)
    FL_unit_i, FL_unit_j, FL_unit_k = cross_prod(U_unit_i, U_unit_j, U_unit_k, \
                                                 w_unit_i, w_unit_j, w_unit_k)
    ans = abs_FD*U_unit_j + abs_FL*FL_unit_j
    return ans/m

def az(v_ball_i, v_ball_j, v_ball_k, \
       v_wind_i, v_wind_j, v_wind_k, \
       w_unit_i, w_unit_j, w_unit_k, \
       C_D, C_L, rho_air, m, D):
    #
    A = (D/2)**2*PI
    #
    U_i = -1*v_ball_i + v_wind_i
    U_j = -1*v_ball_j + v_wind_j
    U_k = -1*v_ball_k + v_wind_k
    #
    abs_FD = C_D*A*rho_air*(U_i**2 + U_j**2 + U_k**2)/2
    abs_FL = C_L*A*rho_air*(U_i**2 + U_j**2 + U_k**2)/2
    #
    U_unit_i,  U_unit_j,  U_unit_k = unit_vec(U_i, U_j, U_k)
    FL_unit_i, FL_unit_j, FL_unit_k = cross_prod(U_unit_i, U_unit_j, U_unit_k, \
                                                 w_unit_i, w_unit_j, w_unit_k)
    ans = abs_FD*U_unit_k + abs_FL*FL_unit_k - m*g
    return ans/m

def RK4(tn, \
        xn, yn, zn, \
        v_xn, v_yn, v_zn, \
        v_wind_i, v_wind_j, v_wind_k, \
        w_unit_i, w_unit_j, w_unit_k, \
        C_D, C_L, rho_air, m, D):
    #---------------------------------------
    kx1 = ax(v_xn, v_yn, v_zn, v_wind_i, v_wind_j, v_wind_k, w_unit_i, w_unit_j, w_unit_k, C_D, C_L, rho_air, m, D)
    ky1 = ay(v_xn, v_yn, v_zn, v_wind_i, v_wind_j, v_wind_k, w_unit_i, w_unit_j, w_unit_k, C_D, C_L, rho_air, m, D)
    kz1 = az(v_xn, v_yn, v_zn, v_wind_i, v_wind_j, v_wind_k, w_unit_i, w_unit_j, w_unit_k, C_D, C_L, rho_air, m, D)
    #---------------------------------------
    kx2 = ax(v_xn+kx1*h/2, v_yn+ky1*h/2, v_zn+kz1*h/2, v_wind_i, v_wind_j, v_wind_k, w_unit_i, w_unit_j, w_unit_k, C_D, C_L, rho_air, m, D)
    ky2 = ay(v_xn+kx1*h/2, v_yn+ky1*h/2, v_zn+kz1*h/2, v_wind_i, v_wind_j, v_wind_k, w_unit_i, w_unit_j, w_unit_k, C_D, C_L, rho_air, m, D)
    kz2 = az(v_xn+kx1*h/2, v_yn+ky1*h/2, v_zn+kz1*h/2, v_wind_i, v_wind_j, v_wind_k, w_unit_i, w_unit_j, w_unit_k, C_D, C_L, rho_air, m, D)
    #---------------------------------------
    kx3 = ax(v_xn+kx2*h/2, v_yn+ky2*h/2, v_zn+kz2*h/2, v_wind_i, v_wind_j, v_wind_k, w_unit_i, w_unit_j, w_unit_k, C_D, C_L, rho_air, m, D)
    ky3 = ay(v_xn+kx2*h/2, v_yn+ky2*h/2, v_zn+kz2*h/2, v_wind_i, v_wind_j, v_wind_k, w_unit_i, w_unit_j, w_unit_k, C_D, C_L, rho_air, m, D)
    kz3 = az(v_xn+kx2*h/2, v_yn+ky2*h/2, v_zn+kz2*h/2, v_wind_i, v_wind_j, v_wind_k, w_unit_i, w_unit_j, w_unit_k, C_D, C_L, rho_air, m, D)
    #---------------------------------------
    kx4 = ax(v_xn+kx3*h, v_yn+ky3*h, v_zn+kz3*h, v_wind_i, v_wind_j, v_wind_k, w_unit_i, w_unit_j, w_unit_k, C_D, C_L, rho_air, m, D)
    ky4 = ay(v_xn+kx3*h, v_yn+ky3*h, v_zn+kz3*h, v_wind_i, v_wind_j, v_wind_k, w_unit_i, w_unit_j, w_unit_k, C_D, C_L, rho_air, m, D)
    kz4 = az(v_xn+kx3*h, v_yn+ky3*h, v_zn+kz3*h, v_wind_i, v_wind_j, v_wind_k, w_unit_i, w_unit_j, w_unit_k, C_D, C_L, rho_air, m, D)
    #---------------------------------------
    v_xn1 = v_xn + h*(kx1+2*kx2+2*kx3+kx4)/6
    v_yn1 = v_yn + h*(ky1+2*ky2+2*ky3+ky4)/6
    v_zn1 = v_zn + h*(kz1+2*kz2+2*kz3+kz4)/6
    #---------------------------------------
    tn1 = tn + h
    #---------------------------------------
    xn1 = xn + (v_xn+v_xn1)*h/2
    yn1 = yn + (v_yn+v_yn1)*h/2
    zn1 = zn + (v_zn+v_zn1)*h/2
    #---------------------------------------
    return tn1, v_xn1, v_yn1, v_zn1, xn1, yn1, zn1