!4×4の実行列によるクォータニオン(四元数)の計算
DIM ZZ(4,4),T1(4,4),T2(4,4) !作業用
!行列の演算ルーチン FUNCTION tr(A(,)) !行列Aのトレース LET t=0 !対角成分の和 FOR m=1 TO MIN(UBOUND(A,1),UBOUND(A,2)) LET t=t+A(m,m) NEXT m LET tr=t END FUNCTION
!クォータニオン(四元数)の演算ルーチン
DIM i(4,4) !虚数単位 DATA 0,-1,0, 0 DATA 1, 0,0, 0 DATA 0, 0,0,-1 DATA 0, 0,1, 0 MAT READ i DIM j(4,4) !虚数単位 DATA 0, 0,-1,0 DATA 0, 0, 0,1 DATA 1, 0, 0,0 DATA 0,-1, 0,0 MAT READ j DIM k(4,4) !虚数単位 DATA 0,0, 0,-1 DATA 0,0,-1, 0 DATA 0,1, 0, 0 DATA 1,0, 0, 0 MAT READ k
!関数 SUB QuatZER(Z(,)) !零 ※q=0 MAT Z=ZER END SUB SUB QuatIDN(Z(,)) !単位四元数 ※q=1 MAT Z=IDN END SUB SUB QuatRe(Z(,),ReZ(,)) !qの実部 Re(q) MAT ZZ=TRN(Z) MAT T1=Z+ZZ MAT ReZ=(1/2)*T1 !ReZ=(Z+Z')/2 END SUB SUB QuatIm(Z(,),ImZ(,)) !qの虚部 Im(q) MAT ZZ=TRN(Z) MAT T1=Z-ZZ MAT ImZ=(1/2)*T1 !ImZ=(Z-Z')/2 END SUB SUB QuatConj(Z(,), ZZ(,)) !共役四元数 ※q'=w-x*i-y*j-z*k、i,j,kは虚数単位 MAT ZZ=TRN(Z) END SUB SUB QuatABS(Z(,), AbsZ(,)) !絶対値|q| ※ノルム MAT ZZ=TRN(Z) MAT T1=Z*ZZ !q*q'=|q|^2 MAT AbsZ=( SQR( tr(T1)/4 ) )*IDN !!!別解 MAT AbsZ=( DET(Z)^(1/4) )*IDN END SUB SUB QuatINV(Z(,), iZ(,)) !逆四元数q^-1 MAT iZ=INV(Z) END SUB
|