!2×2の複素行列によるクォータニオン(四元数)の計算
OPTION ARITHMETIC COMPLEX
DIM ZZ(2,2),T1(2,2),T2(2,2) !作業用
!行列の演算ルーチン
!関数 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 SUB MatrixConj(A(,), AA(,)) !複素共役行列 ~A IF UBOUND(A,1)<>UBOUND(AA,1) OR UBOUND(A,2)<>UBOUND(AA,2) THEN PRINT "行または列の大きさが違います。" STOP END IF FOR m=1 TO UBOUND(A,1) FOR n=1 TO UBOUND(A,2) LET AA(m,n)=conj(A(m,n)) NEXT n NEXT m END SUB SUB MatrixConjTRN(A(,), AA(,)) !複素共役転置行列 A* DIM T9(100,100) MAT T9=ZER(UBOUND(A,1),UBOUND(A,2)) CALL MatrixConj(A,T9) IF UBOUND(A,1)<>UBOUND(AA,2) OR UBOUND(A,2)<>UBOUND(AA,1) THEN PRINT "行または列の大きさが違います。" STOP END IF MAT AA=TRN(T9) END SUB
!クォータニオン(四元数)の演算ルーチン
DIM i(2,2) !虚数単位 DATA 0,-1 DATA 1, 0 MAT READ i
!関数 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) CALL MatrixConjTRN(Z,ZZ) MAT T1=Z+ZZ MAT ReZ=(1/2)*T1 !ReZ=(Z+Z')/2 END SUB SUB QuatIm(Z(,),ImZ(,)) !qの虚部 Im(q) CALL MatrixConjTRN(Z,ZZ) 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は虚数単位 CALL MatrixConjTRN(Z,ZZ) END SUB SUB QuatABS(Z(,), AbsZ(,)) !絶対値|q| ※ノルム CALL MatrixConjTRN(Z,ZZ) MAT T1=Z*ZZ !q*q'=|q|^2 MAT AbsZ=( SQR( tr(T1)/2 ) )*IDN !!!別解 MAT AbsZ=( SQR( DET(Z) ) )*IDN END SUB SUB QuatINV(Z(,), iZ(,)) !逆四元数q^-1 MAT iZ=INV(Z) END SUB
|