けん玉の運動シミュレーション2

#概要
前回は、玉のみの運動をシミュレーションしました。
今回は、「けん」が動く場合を入れてシミュレーションをしようと思います。

#運動方程式
後で書きます。


#シミュレーション
前回のやり方に、支点移動を追加したら一発でできました
![second.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/dc536a5b-2c9f-0110-cce2-585b04ffaf97.gif)


```python
import numpy as np
import matplotlib.pyplot as plt
import cv2			#	OpenCVライブラリ
import math


from PIL import Image, ImageDraw
def cv2pil(image):
    ''' OpenCV型 -> PIL型 '''
    new_image = image.copy()
    if new_image.ndim == 2:  # モノクロ
        pass
    elif new_image.shape[2] == 3:  # カラー
        new_image = cv2.cvtColor(new_image, cv2.COLOR_BGR2RGB)
    elif new_image.shape[2] == 4:  # 透過
        new_image = cv2.cvtColor(new_image, cv2.COLOR_BGRA2RGBA)
    new_image = Image.fromarray(new_image)
    return new_image

m=1
l=1
theta = -30*np.pi/180
T=0
dtheta = 0
g=9.81
M =10
u=0
px = 0
py = 0
dpx = 0

dt = 0.001
N = 10000

x = l*np.cos(theta) + px
y = -1*l*np.sin(theta) 
vx = -1*dtheta*l*np.sin(theta) + dpx
vy = -1*dtheta*l*np.cos(theta)

x_vec = np.array([[x],[y]])
save_x = np.array(x_vec)
v_vec = np.array([[vx],[vy]])
save_v = np.array(v_vec)
theta_vec = np.array([[dtheta],[theta]])
save_theta = np.array(theta_vec)

energy = (vx*vx+vy*vy)/2*m + m*y*g + M*dpx*dpx/2
save_energy = np.array([energy])

save_T = np.array([0])

p_vec = np.array([px])
save_p = np.array(p_vec)

for i in range(N):
    length_x = (x-px)*(x-px)+ (y-py)*(y-py) #紐の長さ計算6
    
    if length_x >= l*0.999:
    
        A=np.array([[M,0,-1*np.cos(theta)],\
                    [m,-1*m*l*np.sin(theta),np.cos(theta)],\
                    [0,-1*m*l*np.cos(theta),-1*np.sin(theta)]])
        hoge = m*(dtheta*dtheta)*l    
        B=np.array([[u],[hoge*np.cos(theta)],[-m*g-hoge*np.sin(theta)]])
        
        ans = np.linalg.solve(A,B)
        
        ddpx = ans[0][0]
        ddtheta =  ans[1][0]
        T =  ans[2][0]
    else:
        ddpx = 0
        T = -10   # 落下運動確定
        
    if T >= 0:
        
        #更新
        theta = theta + dtheta*dt
        dtheta = dtheta + ddtheta*dt
        x = l*np.cos(theta) + px
        y = -1*l*np.sin(theta) 
        vx = -1*dtheta*l*np.sin(theta) + dpx
        vy = -1*dtheta*l*np.cos(theta)
        px = px + dpx * dt
        dpx = dpx + ddpx * dt

    else:

        """
        mx = -mg に従う
        """
        #更新
        vy = vy -m*g*dt
        x = x + vx*dt
        y = y + vy*dt
        px = px + dpx * dt
        
        
        
        theta = math.atan2(-1*y,(x-px))
        if theta<0:
            theta = theta+math.pi*2
            
        """
        dtheta への　変換方法　が　思いつかない
        
        """
        e_vec = np.array([-1*np.sin(theta),-1*np.cos(theta)])
        v_vec = np.array([vx,vy])
        hoge = np.dot(e_vec,v_vec.T)
        dtheta =hoge/l
        
    #保存
    x_vec = np.array([[x],[y]])
    save_x = np.append(save_x,x_vec,1)
    theta_vec = np.array([[dtheta],[theta]])
    save_theta = np.append(save_theta,theta_vec,1)
    energy = (vx*vx+vy*vy)/2*m + m*y*g + M*dpx*dpx/2
    save_energy = np.append(save_energy,energy)
    save_T = np.append(save_T,T)
    v_vec = np.array([[vx],[vy]])
    save_v = np.append(save_v,v_vec,1)
    save_p = np.append(save_p,px)
    

#グラフ化
t=np.arange(N+1)*dt   

#fig = plt.figure(figsize=(4, 3)) #...1
fig = plt.figure(figsize=(16, 12)) #...1
ax = fig.add_subplot(611) #...2
ax2 = fig.add_subplot(612) #...
ax3 = fig.add_subplot(613) #...2
ax4 = fig.add_subplot(614) #...2
ax5 = fig.add_subplot(615) #...2
ax6 = fig.add_subplot(616) #...2

ax.plot(t,save_x[0,:],"r",label="x")
ax.plot(t,save_x[1,:],"k",label="y")

ax2.plot(t,save_theta[1,:])

ax3.plot(t,save_energy,"b",label="ene")

ax4.plot(t,save_T,"b",label="T")

ax5.plot(t,save_v[0,:],"r",label="x")
ax5.plot(t,save_v[1,:],"k",label="y")

ax6.plot(t,save_p,"k",label="px")

plt.show()

#グラフィック表示

windowName = "img" #でないとpython3ではだめ
cv2.namedWindow(windowName)

img = cv2.imread('background.png' ,cv2.IMREAD_COLOR)

height,width,channels = img.shape

cv2.line(img,(0,int(height/2)),(width,int(height/2)),(0,0,255),2)
cv2.line(img,(int(width/2),0),(int(width/2),height),(0,0,255),2)

cv2.imshow(windowName,img)    

images = []

frame_step = 10
while True:
    for i in range(int((N+1)/frame_step)):
        img2 = img.copy()
        extend = 300
        px = int(save_p[i*frame_step]*extend)
        x = int(save_x[0][i*frame_step]*extend)
        y = int(save_x[1][i*frame_step]*extend)
        cv2.circle(img2, (int(width/2)+px,int(height/2)), 10, (0, 255, 0), 4)
        cv2.circle(img2, (int(width/2)+x,int(height/2)-y), 10, (0, 0, 255), 4)
        cv2.imshow(windowName,img2)
        key = cv2.waitKey(10)
        pil_img = cv2pil(img2)
        images.append(pil_img)
        if key ==ord('q'):                 #       end 処理
            break
    
    key = cv2.waitKey(0)
    if key ==ord('q'):                 #       end 処理
        break
        
cv2.waitKey(0)
cv2.destroyAllWindows()

images[0].save('second.gif',
              save_all=True, append_images=images[1:], optimize=False, duration=40, loop=0)


```