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

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

# 2つのベクトルを取って
# その第1ベクトルを(1,0,...,0)に写し
# 第2ベクトルを(1,0,...,0)と(0,1,0,...,0)とが張る面に移す
# P,Vを第1・2軸平面に移す行列はinv=TRUE
# その逆はinv=FALSE
XYRotation<-function(P,V,inv=TRUE){
	# Pを(1,0,...,0)軸に回転する行列
	R1<-VectorRotation(P,inv=TRUE)
	# VそR1で回転して
	V2<-R1%*%V
	# その第1軸以外の成分を取り出して、この部分の回転を求める
	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
}

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

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