2011-02-06

進行方向が巡回するとき２

R 多次元回転曲線フルネ=セレ

Moving frameを回転する回転行列とフルネ=セレの行列の関係を考える
フレネ=セレの行列は
- $\frac{d\mathbf{e}}{ds} = M_F \mathbf{e}$
一方、Moving frameを回転させる行列は
- $\mathbf{e(s)}=R^s \mathbf{e}$
- ここで、回転行列 $R$ を特異値に分解し $R=VSV^{*}$ とすると
- $\mathbf{e(s)}=V S^{s} V^{*} \mathbf{e}$ となる
- $s$ を十分小さくすれば、回転行列 $R$ の処理を繰り返しても曲線が描ける
このような行列は次のようにして作る
$R=(r_{ij})$
第i曲率を $k_i$ としたときに、そのサインとコサインをそれぞれ $C_i,S_i$ とする
のとき
- $r_{11}=C_1$
- $r_{12}=-S_1$
- $r_{ij\not = 1,2}=0$
のとき
- $j=1$ のとき $r_{ij}= (\prod_{p=j}^{i-1} S_i)\times C_{i}$
- [tex:1
- $j=i$ のとき $r_{ij}=r_{ii}=C_{i-1}C_i$
- $j=i+1$ のとき $r_{ij}=r{i i+1}=-S_i$
- $j>i+1$ のとき $r_{ij}=0$
のとき
- $j=1$ のとき $r_{ij}= (\prod_{p=j}^{i-1} S_i)$
- [tex:1
- $j=i$ のとき $r_{ij}=r_{ii}=C_{i-1}$
Ｒで確かめてみよう

n<-5
# Rotと曲率Kの関係は

# 1からn-1次の曲率
Ksin<-runif(n-1)

# フレネ=セレ行列を作る
MakeFrenetSerret<-function(K){
	n<-length(K)+1
	M<-matrix(0,n,n)
	for(i in 1:length(Ksin)){
		M[i,i+1]<--Ksin[i]
		M[i+1,i]<-Ksin[i]
	}
	M
}
M<-MakeFrenetSerret(Ksin)

E.outK<-eigen(M)
VK<-E.outK[[2]]
V2K<-solve(VK)
SK<-diag(E.outK[[1]])

# Moving Frameを回転させる行列Mkを作る
# 回転行列１操作あたりに生じる変化は
# Mk%*%e - e
# この第i番ベクトルに関する変化がei+1と作る内積が
# 第i曲率
# そのような回転行列を作る

MakeMatRotatingMF<-function(K){
	Ksin<-K
	Kt<-asin(Ksin)
	Kcos<-cos(Kt)

	Mk<-matrix(0,n,n)
	Mk[1,1]<-Kcos[1]
	Mk[1,2]<--Ksin[1]
	for(i in 2:n){
		Mk[i,1:(i-1)]<-Mk[i-1,1:(i-1)]*Ksin[i-1]/Kcos[i-1]
		Mk[i,i]<-Kcos[i-1]
		if(i<n){
			Mk[i,1:i]<-Mk[i,1:i]*Kcos[i]
			Mk[i,i+1]<--Ksin[i]
		}
	}
	Mk
}

Diff<-MakeMatRotatingMF(Ksin)

E.out<-eigen(Diff)

V<-E.out[[2]]
V2<-solve(V)
S<-diag(E.out[[1]])

# 動かしてみる

Niter<-1000
# 差分単位
dt<-0.001

# 回転行列による変化記録
xs<-matrix(0,Niter,n)
xs[1,]<-runif(n)
# フレネ=セレによる変化記録
xsK<-xs

T<-runif(Niter-1)
#T<-sort(T)

# 初期値設定
# Moving Frame
dhk<-NormalBase(n)
dhkK<-dhk
# 初速度はMoving Frameの第１ベクトル
v<-dhk[1,]
vK<-dhkK[1,]

for(i in 2:Niter){
	# 次点の座標は、現在位置に速度x刻み幅
	xs[i,]<-xs[i-1,]+v*dt
	xsK[i,]<-xsK[i-1,]+vK*dt
	# Moving frameを動かす
	# 回転行列はその行列の特異値分解を使う
	#dhk<-(V%*%S^(dt)%*%V2) %*% dhk
	dhk<-(V%*%diag(E.out[[1]]^(dt))%*%V2) %*% dhk
	# フレネ=セレはMoving Frameにかけて出した微分量を使う
	dhkK<-M%*%dhkK*dt+ dhkK
	# 単位ベクトル補正をして、速度を更新する
	ll<-apply(dhk^2,1,sum)
	dhk<-((dhk)/sqrt(ll))
	v<-dhk[1,]
	llK<-apply(dhkK^2,1,sum)
	dhkK<-((dhkK)/sqrt(llK))
	vK<-dhkK[1,]
	# 両方法間の速度の違いをプリントアウトしたい？
	#print(v)
	#print(vK)
	#print("---")
}



library(rgl)
plot3d(xs[,1],xs[,2],xs[,3],col=rainbow(Niter),main="Moving Frame Rotating Matrix")
open3d()
plot3d(xsK[,1],xsK[,2],xsK[,3],col=rainbow(Niter),main="Frenet-Serret")
open3d()
# ２方法を合わせて
plot3d(c(xs[,1],xsK[,1]),c(xs[,2],xsK[,2]),c(xs[,3],xsK[,3]),col=c(rep(1,Niter),rep(2,Niter)),main="Black=Rotation,Red=Frenet-Serret")

本当は、回転行列があって、その回転のs微分が $M_F$ に一致するように回転行列を作ればよいのかと思うのだが…
以下はその試行錯誤(で、まだ成功していない)
フレネ=セレの行列は
- $\frac{d\mathbf{e}}{ds} = M_F \mathbf{e}$
一方、Moving frameを回転させる行列は
- $\mathbf{e(s)}=R^s \mathbf{e(0)}$
- ここで、回転行列 $R$ を特異値に分解し $R=VSV^{*}$ とすると
- $\mathbf{e(s)}=V S^{s} V^{*} \mathbf{e}$ となる
- $S$ は対角行列で、その成分 $s_j=e^{i w_j s}=cos(w_j s+\theta)+i sin(w_j s+\theta)$ である
- $\frac{d s_j}{ds}=w_j (-sin(w_j s+\theta) +i cos(w_j s+\theta))$
- $\frac{d s_j}{ds} = w_j e^{i w_j s +\theta+ \frac{pi}{2}}$
- $(\frac{d s_j}{ds})$ を対角成分とする行列を $T$ とし
- $\frac{d R}{ds}=V T V^{*}$ とする
なので
- $M_F$ を特異値分解し $T$ を求め
- の対角成分を満足するようにする
  - $w_j = ||t_j||$
  - $\theta=b$
- $S$ は対角行列で、その成分 $s_j=e^{i w_j s}=cos(w_j s)+i sin(w_j s)$ である
- $\frac{d s_j}{ds}=w_j (-sin(w_j s) + cos(w_j s))$
- $\frac{d s_j}{ds} = w_j e^{i w_j s + \frac{pi}{2}}$
- 1からn-1次の曲率を与える
$k_i=\frac{d e_i}{ds} e_{i+1}$ という関係(第i番目のmoving frame ベクトルの時間変化との第i+1番目のmoving frameベクトルとの内積が第i番目の曲率)であるから…
以下のソースは動くけれど、思ったようには動いていない。

# Rotと曲率Kの関係は

Ksin<-runif(n-1)

# フレネ=セレ行列を作る
MakeFrenetSerret<-function(K){
	n<-length(K)+1
	M<-matrix(0,n,n)
	for(i in 1:length(Ksin)){
		M[i,i+1]<--Ksin[i]
		M[i+1,i]<-Ksin[i]
	}
	M
}
M<-MakeFrenetSerret(Ksin)

E.outK<-eigen(M)
VK<-E.outK[[2]]
V2K<-solve(VK)
SK<-diag(E.outK[[1]])

# 単位ベクトルを行とする回転行列を作る
# 回転行列なので、正規直交基底を成す
# 行ベクトル同士は直交する
# 
Kt<-asin(Ksin)

Kcos<-cos(Kt)

Mk<-matrix(0,n,n)
Mk[1,1]<-Kcos[1]
Mk[1,2]<--Ksin[1]
for(i in 2:n){
	Mk[i,1:(i-1)]<-Mk[i-1,1:(i-1)]*Ksin[i-1]/Kcos[i-1]
	Mk[i,i]<-Kcos[i-1]
	if(i<n){
		Mk[i,1:i]<-Mk[i,1:i]*Kcos[i]
		Mk[i,i+1]<--Ksin[i]
	}
	
	#Mk[i,i-1]<--Ksin[i-1]*Kcos[i]
	#Mk[i,i]<-Kcos[i-1]*Kcos[i]
	#Mk[i,i+1]<-Ksin[i]
}

Diff<-M
t(Diff)%*%Diff

E.out<-eigen(Diff)

V<-E.out[[2]]
V2<-solve(V)
S<-diag(E.out[[1]])

Reals<-Re(E.out[[1]])
Imags<-Im(E.out[[1]])
Ar<-Arg(E.out[[1]])
Mods<-Mod(E.out[[1]])

#RotsS<-diag(complex(real=cos(Mods*((Ar-pi/2)%%(2*pi))),imaginary=sin(Mods*((Ar-pi/2)%%(2*pi)))))
RotsS<-diag(complex(real=cos(Mods),imaginary=sin(Mods)))

Rot<-V%*%RotsS%*%V2




Niter<-1000
xs<-matrix(0,Niter,n)
xs[1,]<-runif(n)
xsK<-xs
dt<-0.01

T<-runif(Niter-1)
#T<-sort(T)

dhk<-NormalBase(n)
v<-dhk[1,]
dhkK<-dhk
vK<-dhkK[1,]
for(i in 2:Niter){
	#xs[i,]<-xs[i-1,]+v*dt*T[i-1]
	#xs[i,]<-xs[i-1,]+v*dt
	#xs[i,]<-(V%*%RotsS^(dt*(i-1))%*%V2)%*%xs[1,]
	xs[i,]<-(V%*%diag(E.out[[1]]^(dt*(i-1)))%*%V2)%*%xs[1,]
	xsK[i,]<-xsK[i-1,]+vK*dt
	
	#dhk<-(V%*%S^(dt)%*%V2) %*% dhk
	dhk<-(V%*%RotsS^(dt)%*%V2) %*% dhk
	dhk<-Re(dhk)
	#dhk<-Rot%*%dhk 
	dhkK<-M%*%dhkK*dt+ dhkK
	# 単位ベクトル補正
	ll<-apply(dhk^2,1,sum)
	dhk<-((dhk)/sqrt(ll))
	v<-dhk[1,]
	llK<-apply(dhkK^2,1,sum)
	dhkK<-((dhkK)/sqrt(llK))
	vK<-dhkK[1,]
	print(v)
	print(vK)
	print("---")
	
}



library(rgl)
plot3d(xs[,1],xs[,2],xs[,3],col=rainbow(Niter))
open3d()
plot3d(xsK[,1],xsK[,2],xsK[,3],col=rainbow(Niter))
open3d()

plot3d(c(xs[,1],xsK[,1]),c(xs[,2],xsK[,2]),c(xs[,3],xsK[,3]),col=c(rep(1,Niter),rep(2,Niter)))

matplot(xs,type="l")