回転行列 発散しない常微分方程式の軌道 再び

特定の軸を特定のベクトルへと移す回転

• 昨日までの記事で、常微分方程式が発散しないような条件を常微分方程式の階数次元空間の軌道で考えてきている
• 微分係数を指定して離散的にトラッキングすると、「理論は正しい」はずだけれども、誤差が過大になって、描図が芳しくない
• なので、軌道を決めるベクトルによる微小時間の動きを短直線ではなく、小角回転にしてみることとする
• そのためには、空間で任意になっている回転の半径ベクトルと、進行方向ベクトルとから、それに相当する回転の行列が作りたい
• ひとまずその行列を作るために次のようにする
  • 半径ベクトルとそれに直交する進行方向ベクトルを(1,0,...,0),(0,1,0,...,0)ベクトル方向に回転する(そのような行列を作る)
  • 回転させた後に、第１・第２軸が作る平面における回転(これは行列のうち、第１・２行、第１・２列以外は、単位行列の構成で、第１・２の行と列とはとなるような行列である)をさせ、もとの座標系に逆回転させることとする

# n次元空間で
# 原点を中心として、点Pが
# ベクトルVを接ベクトルとするような回転を表す行列

# デカルト座標を各座標に直す関数を使う
# デカルト座標から角座標へ
Cartesian2AngularX<-function(x){
	n<-length(x)
	if(n==1){
		return(list(r=abs(x),t=acos(sign(x))))
	}else if(n==2){
		tmpt<-0
		if(x[1]!=0){
			tmpt<-acos(x[1]/sqrt(sum(x^2)))*sign(x[2])
			if(sign(x[2])==0){
				if(x[1]>0){
					tmpt<-0
				}else if(x[1]<0){
					tmpt<-pi
				}
			}
			#tmpt<-atan(x[2]/x[1])
		}else{
			if(x[2]>0){
				tmpt<-pi/2
			}else if(x[2]<0){
				tmpt<--pi/2
			}
		}
		return(list(r=sqrt(sum(x^2)),t=tmpt))
	}
	r<-sqrt(sum(x^2))
	xst<-x/r
	#n<-length(xst)
	t<-rep(0,n-1)
	S<-C<-t
	S[n-1]<-xst[n]
	if(S[n-1]>1)S[n-1]<-1
	if(S[n-1]<(-1))S[n-1]<-(-1)
	t[n-1]<-asin(S[n-1])
	C[n-1]<-cos(t[n-1])
	cumC<-C[n-1]
	for(i in (n-2):1){
		if(cumC!=0){
			S[i]<-xst[i+1]/cumC
			if(S[i]>1)S[i]<-1
			if(S[i]<(-1))S[i]<-(-1)
			t[i]<-asin(S[i])
			C[i]<-cos(t[i])
			cumC<-cumC*C[i]
		}else{
			S[i]<-0
			t[i]<-asin(S[i])
			C[i]<-cos(t[i])
		}
		
	}
	list(r=r,t=t)
}

# (1,0,...,0)ベクトルを与えたベクトルに移す回転行列(もしくはその逆行列)を返す

VectorRotation<-function(P,inv=FALSE){ # inv=TRUEは逆行列
	n<-length(P)
	#print(n)
	if(n==1){
		return(matrix(c(1),1,1))
	}
	Q<-Cartesian2AngularX(P)

	M<-diag(n)
	M2<-M

	for(i in 1:length(Q[[2]])){
		tmp<-M
		tmp[i,i]<--sin(Q[[2]][i])
		tmp[i,i+1]<-cos(Q[[2]][i])
		tmp[i+1,i]<-cos(Q[[2]][i])
		tmp[i+1,i+1]<-sin(Q[[2]][i])
		M2<-tmp%*%M2
	}
	M2<-t(M2)[,n:1]
	
	S <- rep(1,length(P))
	S2 <- sign(M2[,1]*P)
	S[which(S2<0)] <- -1
	#S<-sign(M2[,1]*P)

	M2<-M2*S
	if(inv){
		#M2<-solve(M2)
                M2<-t(M2) # 回転行列なら逆行列は転置行列
	}
	M2
}

# ２つのベクトルを取って
# その第１ベクトルを(1,0,...,0)に写し
# 第２ベクトルを(1,0,...,0)と(0,1,0,...,0)とが張る面に移す
# P,Vを第１・２軸平面に移す行列はinv=TRUE
# その逆はinv=FALSE
XYRotation<-function(P,V,inv=TRUE){
	# Pを(1,0,...,0)軸に回転する行列
	R1<-VectorRotation(P,inv=TRUE)
	# VそR1で回転して
	V2<-R1%*%V
	# その第１軸以外の成分を取り出して、この部分の回転を求める
	tmpV<-V2[2:length(V)]
	tmpR2<-VectorRotation(tmpV,inv=TRUE)
	R2<-diag(length(V))
	R2[2:n,2:n]<-tmpR2
	ret<-R2%*%R1
	#if(!inv)ret<-solve(ret)
 # 回転行列なら逆行列は転置行列
        if(!inv)ret<-t(ret)
	ret
}

# ２つの直交する単位ベクトルを適当に作る

n<-5 # 次元

P<-rnorm(n)
#P<-P/sqrt(sum(P^2))
V<-rnorm(n)

V[n]<--sum(P[1:(n-1)]*V[1:(n-1)])/P[n]

#V<-V/sqrt(sum(V^2))
P
V
# 直交確認
sum(P*V)

R<-XYRotation(P,V)
P2<-R%*%P
V2<-R%*%V
P2
V2

速度ベクトルの動きを回転行列にしてシミュレーション

• 前の記事を用いて、離散的取扱いのために「飛び出さ」ないようにしてシミュレーションしてみた
• 飛び出さないよう、高速になると、１ステップの時間を短くしている(スローモーション化)ので、本当は「さーっ」と通り過ぎるだろうあたりで収束してしまっているようなプロットになっているけれども。。。

n<-3

Niter<-1000
dx<-exp(1)/1000

Ys<-matrix(0,Niter,n+1)

loop<-TRUE
while(loop){
	Ys[1,]<-c(rnorm(n),0)*0.001
	if(sum(Ys[1,1:(n-2)]*Ys[1,2:(n-1)])<0)loop<-FALSE
}
#Ys[1,]<-c(-1,1,0.5,0)
shogen<-sign(Ys[1,])
TmpFx<-function(ys,Equal=FALSE,a=0.1){
	tmp<-0
	ret<-0
	for(i in 1:(length(ys)-1)){
		tmp<-tmp+ys[i]*ys[i+1]
	}
	ret<--tmp/ys[length(ys)]
	if(!Equal){
		ret<-ret+sign(ys[length(ys)])*a
	}
	ret
}


Ys[1,n+1]<-TmpFx(Ys[1,1:n])
alpha<-0.01
for(i in 2:Niter){
	#V<-c(Ys[i-1,2:(n+1)],0)
	V<-Ys[i-1,2:(n+1)]
	#V<-Vs*dx
	P<-Ys[i-1,1:n]
	R<-XYRotation(P,V)
	Rinv<-solve(R)
	Vnorm<-sqrt(sum(V^2))
	Pnorm<-sqrt(sum(P^2))
	theta<-atan(Vnorm/Pnorm)
	# 半径が大きく、移動ベクトルも大きいと
	# １ステップの移動距離が大きくなりすぎるので
	# １ステップあたりの移動距離を等しくする
	theta<-theta/Vnorm*dx
	#print(theta)
	tmpRot<-diag(n)
	tmpRot[1,1]<-cos(theta)
	tmpRot[1,2]<--sin(theta)
	tmpRot[2,1]<-sin(theta)
	tmpRot[2,2]<-cos(theta)
	
	tmpP<-R%*%P
	tmpP<-tmpRot%*%tmpP
	tmpP<-Rinv%*%tmpP
	Ys[i,1:n]<-tmpP
	
	#Vst<-V/Vnorm
	#Ys[i,]<-Ys[i-1,]+dx*Vst
	Ys[i,n+1]<-TmpFx(Ys[i,1:n],Equal=TRUE)
	#zeroq<-sum(Vst[1:n]*Ys[i-1,1:n])
	#print(zeroq)
	#if(abs(zeroq)>0.0001)print(zeroq)
	tmpshogen<-sign(Ys[i,])
	#if(sum((shogen-tmpshogen)^2)!=0){
	#	print("chage shogen")
	#	tmpshogen2<-abs(shogen-tmpshogen)*(-0.5)+1
	#	tmpshogen3<-(tmpshogen2-1)*(-1)
	#	Ys[i,]<-Ys[i,]*tmpshogen2+c(rep(alpha,n)*tmpshogen3[1:n],0)
	#}
	
}
#matplot(Ys[,1:n],type="l")

plot(as.data.frame(Ys))

library(rgl)
plot3d(Ys[,1:3],col=rainbow(1000),xlim=range(Ys[,1:3]),ylim=range(Ys[,1:3]),zlim=range(Ys[,1:3]))

matplot(Ys[,1:n],type="l")