// 四元数演算プログラム例 #include "stdafx.h" #include "string.h" #include "math.h" typedef struct{// 四元数を示す構造体 int r; double a, b, c, d;// r : 球面上の集合のとき r = 1, 一般の四元数のとき r = 0 }Quater; Quater quater(int r, double a, double b, double c,double d){//四元数 Quater q; q.r = r; q.a = a; q.b = b; q.c = c; q.d = d; return q;} Quater quater(double a, double b, double c,double d){//一般の四元数設定 Quater q; q.r = 0; q.a = a; q.b = b; q.c = c; q.d = d; return q;} Quater quater(double r){//球面上の集合の設定 Quater q; q.r = 1; q.a = 0; q.b = r; q.c = 0; q.d = 0; return q;} int QtoStr(Quater q,char s[]){//四元数を文字列に変換 char A[128];char C[4]=" + "; if(q.r==1){//球面集合のとき strcpy(s,"実部 "); sprintf(A,"%f",q.a);strcat(s,A); strcat(s, ", 虚部球面集合半径 "); sprintf(A,"%f",q.b);strcat(s,A); } else{//一般の四元数のとき sprintf(s,"%f",q.a);C[1]=((q.b<0)?'-':'+');strcat(s,C); sprintf(A,"%f",abs(q.b)); strcat(s,A); strcat(s," i"); C[1]=((q.c<0)?'-':'+') ; strcat(s,C); sprintf(A,"%f",abs(q.c)); strcat(s,A); strcat(s," j"); C[1]=((q.d<0)?'-':'+') ; strcat(s,C); sprintf(A,"%f",abs(q.d)); strcat(s,A); strcat(s," k"); } return strlen(s); } double Qnorm2(Quater q){//ノルムの2乗 return q.a * q.a + q.b * q.b + q.c * q.c + q.d * q.d; } double Qnorm(Quater q){ return sqrt(Qnorm2(q));}//ノルム Quater Qconj(Quater q){ return quater(q.r, q.a, -q.b, -q.c, -q.d);}//共役四元数 Quater Qsqrt(Quater x){//平方根 double b=x.b*x.b+x.c*x.c+x.d*x.d; //虚部のノルムの2乗 if(b<1E-64){ if(x.a < 0) return quater(abs(x.a));//負の実数のとき return quater(sqrt(x.a),0,0,0); //正の実数のとき } //実数以外のとき、通常の四元数とする。 double a = sqrt((x.a+sqrt(x.a*x.a+b))/2), a2 = a * 2; return quater(a, x.b / a2, x.c / a2, x.d / a2); } Quater Qpow2(Quater x){//四元数の平方 return quater(x.a * x.a - x.b * x.b - x.c * x.c - x.d * x.d, 2 * x.a * x.b, 2 * x.a * x.c, 2 * x.a * x.d); } Quater Qadd(Quater x, Quater y){//加算 int r=((x.r != y.r)? 0: x.r); return quater(r, x.a + y.a, x.b + y.b, x.c + y.c, x.d + y.d); } Quater Qsub(Quater x, Quater y){//減算 int r=((x.r != y.r)? 0: x.r); return quater(r, x.a - y.a, x.b - y.b, x.c - y.c, x.d - y.d); } int IsReal(Quater x){// 実数かどうかの判定 return(abs(x.b)<1E-32 && abs(x.c)<1E-32 && abs(x.d)<1E-32); } Quater Qmlt(Quater x, Quater y){//乗算 if(IsReal(x))// x が実数のとき return quater(y.r, x.a * y.a, x.a * y.b, x.a * y.c, x.a * y.d); if(IsReal(y))// y が実数のとき return quater(x.r, y.a * x.a, y.a * x.b, y.a * x.c, y.a * x.d); return quater(x.a * y.a - x.b * y.b - x.c * y.c - x.d * y.d,//一般の四元数 x.a * y.b + x.b * y.a + x.c * y.d - x.d * y.c, x.a * y.c + x.c * y.a + x.d * y.b - x.b * y.d, x.a * y.d + x.d * y.a + x.b * y.c - x.c * y.b); } Quater Qinv(Quater q){//四元数の逆数 double n2 = Qnorm2(q); if(abs(n2) < 1E-64){ printf("\n*** Zero Divided ***\n"); return quater(0,0,0,0);} return quater(q.r, q.a/ n2, -q.b / n2, -q.c / n2, -q.d / n2); } Quater Qdiv(Quater p, Quater q){return Qmlt(p, Qinv(q));}// p * q^(-1) 除算 Quater QdivB(Quater p, Quater q){return Qmlt(Qinv(p), q);}// p^(-1) * q Quater Qversor(Quater q){// 単位四元数 double n=Qnorm(q); if(abs(n) < 1E-32){ printf("\n*** Zero Divide ***\n");return quater(1,0,0,0);} return quater(q.r, q.a / n, q.b / n, q.c / n, q.d / n); } Quater Qvector(Quater q){ return quater(q.r, 0, q.b, q.c, q.d);}// ベクトル部分のみ Quater QrotVector(Quater q, double angle){//回転ベクトル設定 Quater V = Qmlt(Qversor(Qvector(q)), quater(sin(angle/2),0,0,0)); return quater(cos(angle/2), V.b, V.c, V.d); } Quater Qrotation(Quater A, Quater axis, double angle){//軸(axis)周りの回転 Quater q=QrotVector(axis,angle); return Qmlt(q, Qmlt(A, Qconj(q))); } Quater Qexp(Quater q){//指数 Quater V=Qvector(q); double nV=Qnorm(V), cosV=cos(nV), SV=0; if(abs(nV)>1E-32) SV = sin(nV) / nV; double EA = exp(q.a), ES = EA * SV; return quater(cosV * EA, V.b * ES, V.c * ES, V.d * ES); } Quater Qlog(Quater q){//対数 Quater V=Qvector(q); double nV=Qnorm(V); if(abs(nV)<1E-32) return quater(log(q.a),0,0,0); double qq=Qnorm(q), TH=acos(q.a/qq)/nV; return quater(log(qq), V.b * TH, V.c * TH, V.d * TH); } Quater Qpow(Quater q, Quater p){return Qexp(Qmlt(p,Qlog(q)));}// べき乗 int _tmain(int argc, _TCHAR* argv[]) { double Pi=3.14159265358979; char str[1024];Quater q, q2, qr, qr2; q=quater(5) ; QtoStr(q, str); printf("\n 球面集合  %s",str); q=quater(1,2,3,4); QtoStr(q, str); printf("\n 文字列変換 %s",str); qr = Qsqrt(q); QtoStr(qr, str); printf("\n sqr %s",str); qr2=Qpow2(qr); QtoStr(qr2, str); printf("\n pow2 %s",str); q2=quater(2,3,4,5); qr2=Qadd(q, q2); QtoStr(qr2, str); printf("\n add %s",str); qr2=Qsub(q, q2); QtoStr(qr2, str); printf("\n sub %s",str); qr2=Qmlt(q, q2); QtoStr(qr2, str); printf("\n mlt %s",str); qr =Qdiv(qr2, q2); QtoStr(qr, str); printf("\n div %s",str); qr =QdivB(q, qr2); QtoStr(qr, str); printf("\n divB %s",str); qr2=Qrotation(q, q2, Pi/6); QtoStr(qr2, str); printf("\n rot %s",str); qr =Qrotation(qr2, q2, -Pi/6); QtoStr(qr, str); printf("\n reverce rot %s",str); Quater qe=quater(1 ,0.4, 0.5, 0.3); QtoStr(qe, str); printf("\n exp data %s",str); qr2=Qexp(qe); QtoStr(qr2, str); printf("\n exp %s",str); qr =Qlog(qr2); QtoStr(qr, str); printf("\n log %s",str); qr2=Qpow(q, qe); QtoStr(qr2, str); printf("\n pow %s",str); qr =Qpow(qr2, Qinv(qe)); QtoStr(qr, str); printf("\n inv pow %s",str); getchar(); return 0; }