【第８話】サイクロイド曲線に経路が拘束された粒子の運動【ルンゲクッタで行こう！⑧】

############################################################################
#　[008]サイクロイド曲線に経路が拘束された粒子の運動
############################################################################
import os
import numpy as np
import random 
import matplotlib.pyplot as plt
import matplotlib.animation as animation
random.seed(0)

#図全体
fig = plt.figure(figsize=(15, 8))
#全体設定
plt.rcParams['font.family'] = 'Times New Roman' #フォント
plt.rcParams['font.size'] = 16 #フォントサイズ
plt.rcParams["mathtext.fontset"] = 'cm' #数式用フォント
#カラーリストの取得
colors = plt.rcParams['axes.prop_cycle'].by_key()['color']
#################################################################
## 物理系の設定
#################################################################
#質量
m = 1.0
#時間区間
t_min = 0
t_max = 20.0
#時間区間数
NT =   20000
skip =    20

#時間間隔
dt = (t_max - t_min) / NT
#座標点配列の生成
ts = np.linspace(t_min, t_max, NT + 1)
#重力加速度
g = np.array([0.0, 0.0, -10.0])

#################################################################
## 経路の設定
#################################################################
class Path:
	#コンストラクタ
	def __init__(self):
		#始点と終点のx座標
		self.x_start = -1
		self.x_end = 1
		#サイクロイド曲線パラメータr
		self.r = ( self.x_end - self.x_start ) / ( 2 * np.pi )
		#始点と終点のz座標
		self.path_z0 = 2 * self.r ; #最下点が0となるように
		#サイクロイド曲線始点の位置ベクトル
		self.start = np.array([self.x_start, 0.0, self.path_z0])

	#経路の位置ベクトルを指定する媒介変数関数
	def position( self, theta ):
		x = self.r * ( theta - np.sin(theta) ) + self.start[0]
		y = 0
		z = - self.r * ( 1 - np.cos(theta) ) + self.start[2]
		return np.array([x, y, z])

	#接線ベクトルを指定する媒介変数関数
	def tangent( self, theta ):
		A = 1.0 / np.sqrt( 2.0 )
		x = A * np.sqrt( 1.0 - np.cos(theta))
		y = 0
		z = - A * np.sqrt( 1.0 + np.cos(theta))
		if( np.sin(theta) < 0 ): z = - z
		return np.array([x, y, z])

	#曲率ベクトルを指定する媒介変数関数
	def curvature( self, theta ):
		A = - 1.0 / ( 4.0 * self.r )
		x = - A * np.sqrt( ( 1.0 + np.cos(theta) ) / ( 1.0 - np.cos(theta) ) )
		y = 0
		z = - A
		if( np.abs(x) == np.inf ):
			x = 0
			z = 0
		if( np.sin(theta) < 0 ): x = - x

		return np.array([x, y, z])
	#媒介変数の取得
	def getTheta( self, r, v ):
		#球体の相対位置ベクトル
		bar_r = r -self.start
		if( np.abs( v[0] ) > 0.1 ):
			_r = np.abs(bar_r[2]) / 2.0 * (  1.0 + (v[2]/v[0])**2 )
		else :
			_r = self.r

		if( bar_r[0] < _r * np.pi ) :
			theta = np.arccos( 1.0 + bar_r[2]/_r )
		else:
			theta = 2.0 * np.pi - np.arccos( 1.0 + bar_r[2]/_r )


		return theta

#経路の生成
path = Path()

#補正パラメータ
compensationK = 100.0;     #補正ばね弾性係数
compensationGamma = 1.0; #補正粘性抵抗係数
compensationFactor = 1;  #補正因子

#粒子に加わる力
def F (self, t, r_t, v_t ):
	#外場（重力）の計算
	fe = m * g

	#経路自体の運動
	v0 = np.array([0.0, 0.0, 0.0])
	a0 = np.array([0.0, 0.0, 0.0])

	#相対速度
	bar_v = v_t - v0

	#媒介変数の取得
	theta = path.getTheta ( r_t, bar_v )

	#媒介変数に対する位置ベクトル、接線ベクトル、曲率ベクトルの計算
	position = path.position( theta )
	tangent = path.tangent( theta )
	curvature = path.curvature( theta )

	#微係数dl/dtとd^2l/dt^2を計算
	dl_dt = np.dot( bar_v, tangent )
	d2l_dt2 = np.dot( fe, tangent ) / m - np.dot( a0, tangent) 

	#加速度を計算
	a = a0 + d2l_dt2 * tangent + dl_dt * dl_dt * curvature
	a_length = np.sqrt(np.sum( a**2 ))

	#補正倍率
	ratio = a_length * compensationFactor
	#ズレ位置ベクトル
	bar_r = r_t - position
	#補正ばね弾性力
	fk = -compensationK * ratio * bar_r
	#法線ベクトル
	curvature_length = np.sqrt(np.sum( curvature**2 ))
	n1 = curvature / curvature_length
	n2 = np.cross(n1, tangent )
	#補正粘性抵抗力
	fgamma1 = -compensationGamma * np.dot( bar_v, n1 ) * ratio * n1 
	fgamma2 = -compensationGamma * np.dot( bar_v, n2 ) * ratio * n2
	#経路補正力を加える
	a += fk + fgamma1 + fgamma2

	return m * a

######################################
#　４次のルンゲ・クッタ　クラス
######################################
class RK4:
	#コンストラクタ
	def __init__(self, m, dt = 0.01, r0 = np.array([0.0, 0.0, 0.0]), v0 = np.array([0.0, 0.0, 0.0]) ):
		self.m = m
		#現時刻の位置と速度
		self.r = r0.copy()
		self.v = v0.copy()
		#１ステップ前の時刻の位置と速度
		self._r = r0.copy()
		self._v = v0.copy()
		#差分
		self.dr = np.array([0.0, 0.0, 0.0])
		self.dv = np.array([0.0, 0.0, 0.0])
		#時間間隔
		self.dt = dt
		#内部
		self.__v1 = np.array([0.0, 0.0, 0.0])
		self.__v2 = np.array([0.0, 0.0, 0.0])
		self.__v3 = np.array([0.0, 0.0, 0.0])
		self.__v4 = np.array([0.0, 0.0, 0.0])
		self.__a1 = np.array([0.0, 0.0, 0.0])
		self.__a2 = np.array([0.0, 0.0, 0.0])
		self.__a3 = np.array([0.0, 0.0, 0.0])
		self.__a4 = np.array([0.0, 0.0, 0.0])

	#速度を与えるメソッド
	def V(self, t, r, v):
		return v.copy()
	##########################################################
	#加速度を与えるメソッド
	def A(self, t, r_t, v_t):
		return F(self, t, r_t, v_t ) / self.m
	##########################################################

	#時間発展を計算するメソッド
	def timeEvolution(self, t):
		#１段目
		self.__v1 = self.V(
			t, 
			self.r, 
			self.v
		)
		self.__a1 = self.A(
			t, 
			self.r, 
			self.v
		)
		#２段目
		self.__v2 = self.V(
			t + self.dt / 2.0, 
			self.r + self.__v1 * self.dt / 2.0, 
			self.v + self.__a1 * self.dt / 2.0
		)
		self.__a2 = self.A(
			t + self.dt / 2.0, 
			self.r + self.__v1 * self.dt / 2.0, 
			self.v + self.__a1 * self.dt / 2.0			
		)
		#３段目
		self.__v3 = self.V(
			t + self.dt / 2.0, 
			self.r + self.__v2 * self.dt / 2.0, 
			self.v + self.__a2 * self.dt / 2.0					
		)
		self.__a3 = self.A(
			t + self.dt / 2.0, 
			self.r + self.__v2 * self.dt / 2.0, 
			self.v + self.__a2 * self.dt / 2.0			
		)
		#４段目
		self.__v4 = self.V(
			t + self.dt, 
			self.r + self.__v3 * self.dt, 
			self.v + self.__a3 * self.dt					
		)
		self.__a4 = self.A(
			t + self.dt, 
			self.r + self.__v3 * self.dt, 
			self.v + self.__a3 * self.dt			
		)
		#差分の計算
		self.dr = self.dt * ( self.__v1 + 2.0 * self.__v2 + 2.0 * self.__v3 + self.__v4 ) / 6.0 
		self.dv = self.dt * ( self.__a1 + 2.0 * self.__a2 + 2.0 * self.__a3 + self.__a4 ) / 6.0 

#################################################################
## 計算開始
#################################################################
#初期条件
particles =[]
for i in range(20):
	#球体の初期位置
	theta0 = np.pi
	#初速度
	x0 = path.r * ( theta0 - np.sin(theta0) ) + path.start[0]
	y0 = 0
	z0 = - path.r * ( 1.0 - np.cos(theta0)) + path.start[2]
	#初期条件
	r0 = np.array([ x0, y0, z0 ])
	v0 = np.array([ 0.170 * (i+1), 0.0, 0.0 ])

	particles.append(  RK4( m, dt, r0, v0 ) )

####################################
## 描画用関数
####################################
#アニメーション作成用
ims=[]

#円座標点配列の生成
thetas = np.linspace(0, 2.0*np.pi, 200)
p = path.position( thetas )
cirsx = p[0]
cirsy = p[2]

def plot( t ):
	#各コマを描画
	img = plt.plot(cirsx, cirsy, color = "black", linestyle='dotted', linewidth = 1 )
	i = 0
	for particle in particles:
		img += plt.plot([particle.r[0]], [particle.r[2]], color = colors[i], marker = 'o', markersize = 20, linestyle='solid', linewidth = 0 )
		i += 1
		if (i>9): i = 0

	time = plt.text(0.8, 0.717, "t = " +  str("{:.2f}".format(t)) + r"$[{\rm s}]$" , ha='left', va='center', fontsize=18)
	#テキストをグラフに追加
	img.append( time )
	ims.append( img )
	E = 1.0/2.0 * m * np.sum( particles[0].v**2 ) - m * np.dot( g, particles[0].r )
	print( "t = {:.4f}".format(t)," E =",  E )

####################################
## 各時刻における運動方程式の計算
####################################
for tn in range(len(ts)):
	t = ts[tn]
	if( tn%skip == 0 ): 
		plot(t)

	#ルンゲ・クッタ法による時間発展
	for particle in particles:
		particle.timeEvolution( t )
	#位置と速度を更新
	for particle in particles:
		particle.r += particle.dr
		particle.v += particle.dv

#################################################################
## 分子運動アニメーション
#################################################################
plt.title( u"サイクロイド曲線経路に拘束された球体の運動", fontsize=20, fontname="Yu Gothic", fontweight=1000)
plt.xlabel(r"$x\,[{\rm m}]$" , fontsize=20, fontname="Yu Gothic", fontweight=1000)
plt.ylabel(r"$z\,[{\rm m}]$" , fontsize=20, fontname="Yu Gothic", fontweight=1000)
#罫線の描画
plt.grid(which = "major", axis = "x", alpha = 0.8, linestyle = "-", linewidth = 1)
plt.grid(which = "major", axis = "y", alpha = 0.8, linestyle = "-", linewidth = 1)

#描画範囲を設定
plt.xlim([-1.1, 1.1])
plt.ylim([-0.1, 0.7])
plt.subplots_adjust(left = 0.12, right = 0.97, bottom = 0.10, top = 0.90)

#アニメーションの生成
ani = animation.ArtistAnimation(fig, ims, interval=10)
#ani.save("output.html", writer=animation.HTMLWriter())
#ani.save("output.gif", writer="imagemagick")
ani.save("output.mp4", writer="ffmpeg", dpi=300)

#グラフの表示
plt.show()