つづき
!関数 ※ベクトル系 SUB QuatDOT(Z1(,),Z2(,), Z(,)) !内積(q1,q2)=w1*w2+x1*x2+y1*y2+z1*z2 MAT ZZ=TRN(Z2) MAT T1=Z1*ZZ MAT Z=( tr(T1)/4 )*IDN !tr(q1*q2')/4 END SUB SUB QuatNRM(Z(,), nZ(,)) !正規化 |q|=1 MAT ZZ=TRN(Z) MAT T1=Z*ZZ !q*q'=|q|^2 MAT nZ=( 1/SQR( tr(T1)/4 ) )*Z !!!別解 MAT nZ=( 1/DET(Z)^(1/4) )*Z END SUB SUB QuatCROSS(Z1(,),Z2(,), Z(,)) !外積q1×q2 ※ベクトル(x,y,z)=[0,x,y,z] MAT Z=Z1*Z2 !w=0より、q1*q2=-(q1,q2)+q1×q2 CALL QuatDOT(Z1,Z2,T1) MAT Z=Z+T1 END SUB
!表記 SUB dispQuat(s$,q(,)) !行列表記の四元数を表示する ※q=[w,x,y,z] PRINT s$;"[ "; PRINT q(1,1);q(2,1);q(3,1);q(4,1); !1列目 PRINT "]" END SUB SUB Quat(w,x,y,z, q(,)) !四元数を行列表記する ※q=w+x*i+y*j+z*k、i,j,kは虚数単位 MAT q=w*IDN !実行列 MAT T1=x*i MAT q=q+T1 MAT T1=y*j MAT q=q+T1 MAT T1=z*k MAT q=q+T1 END SUB !---------- ここまでがサブルーチン
!q1,q2を複素行列で表現する DIM q1(4,4) !q1=3+2i-j-2k CALL Quat(3,2,-1,-2, q1) CALL dispQuat("q1=",q1) MAT PRINT q1; !debug debug debug
DIM q2(4,4) !q2=-2-4i+j-3k CALL Quat(-2,-4,1,-3, q2) CALL dispQuat("q2=",q2) MAT PRINT q2;
|