#ifndef __JBXL_CPP_QUATERNION_H_ #define __JBXL_CPP_QUATERNION_H_ /** @brief 回転・クォータニオンライブラリヘッダ @file Rotation.h @author Fumi.Iseki (C) */ #include "Matrix++.h" #include "Vector.h" // namespace jbxl { ///////////////////////////////////////////////////////////////////////////////////////////////////////////// // // クォータニオン // class DllExport Quaternion; // Q -> E Vector Quaternion2EulerXYZ(Quaternion qut, Vector* vct=NULL); Vector Quaternion2EulerZYX(Quaternion qut, Vector* vct=NULL); Vector Quaternion2EulerXZY(Quaternion qut, Vector* vct=NULL); Vector Quaternion2EulerYZX(Quaternion qut, Vector* vct=NULL); Vector Quaternion2EulerYXZ(Quaternion qut, Vector* vct=NULL); Vector Quaternion2EulerZXY(Quaternion qut, Vector* vct=NULL); //////////////////////////////////////////////////////////////////////////////////////////////// /** class DllExport Quaternion クォータニオンの定義 */ class DllExport Quaternion { public: double s; double x; double y; double z; double n; ///< ノルム double c; ///< 信頼度 public: Quaternion(void) { init();} Quaternion(double S, double X, double Y, double Z, double N=0.0, double C=1.0) { set(S, X, Y, Z, N, C);} Quaternion(double S, Vector v) { setRotation(S, v);} virtual ~Quaternion(void) {} double norm(void) { n = sqrt(s*s+x*x+y*y+z*z); return n;} void normalize(void); void set(double S, double X, double Y, double Z, double N=0.0, double C=1.0); void init(double C=1.0) { s = n = 1.0; x = y = z = 0.0; c = C;} Vector getVector() { Vector v(x, y, z); return v;} double getScalar() { return s;} double getAngle() { return 2.0*acos(s);} ///< 0～2π double getMathAngle() { double t=2.0*acos(s); if(t>PI)t-=PI2; return t;} ///< -π～π Matrix getRotMatrix(); void setRotation(double e, Vector v); void setRotation(double e, double X, double Y, double Z, double N=0.0) { Vector v(X, Y, Z, N); setRotation(e, v);} // 注：eの成分は演算順 void setEulerXYZ(Vector e); // void setEulerZYX(Vector e); // void setEulerXZY(Vector e); // void setEulerYZX(Vector e); // void setEulerYXZ(Vector e); // void setEulerZXY(Vector e); // Vector getEulerXYZ(Vector* vt=NULL) { return Quaternion2EulerXYZ(*this, vt);} Vector getEulerZYX(Vector* vt=NULL) { return Quaternion2EulerZYX(*this, vt);} Vector getEulerXZY(Vector* vt=NULL) { return Quaternion2EulerXZY(*this, vt);} Vector getEulerYZX(Vector* vt=NULL) { return Quaternion2EulerYZX(*this, vt);} Vector getEulerYXZ(Vector* vt=NULL) { return Quaternion2EulerYXZ(*this, vt);} Vector getEulerZXY(Vector* vt=NULL) { return Quaternion2EulerZXY(*this, vt);} Vector execRotation(Vector v); }; ///////////////////////////////////////////////////////////////////////////////////////////////////////////// // Quaternion オペレータ inline bool operator == (const Quaternion q1, const Quaternion q2) { return (q1.s==q2.s && q1.x==q2.x && q1.y==q2.y && q1.z==q2.z); } ///< ~ 共役 inline Quaternion operator ~ (const Quaternion a) { return Quaternion(a.s, -a.x, -a.y, -a.z, a.n, a.c); } // inline Quaternion operator - (const Quaternion a) { return Quaternion(-a.s, -a.x, -a.y, -a.z, a.n, a.c); } inline Quaternion operator + (const Quaternion a, const Quaternion b) { return Quaternion(a.s+b.s, a.x+b.x, a.y+b.y, a.z+b.z, 0.0, Min(a.c, b.c)); } inline Quaternion operator - (const Quaternion a, const Quaternion b) { return Quaternion(a.s-b.s, a.x-b.x, a.y-b.y, a.z-b.z, 0.0, Min(a.c, b.c)); } template inline Quaternion operator * (const R d, const Quaternion a) { return Quaternion(d*a.s, d*a.x, d*a.y, d*a.z, d*a.n, a.c); } template inline Quaternion operator * (const Quaternion a, const R d) { return Quaternion(a.s*d, a.x*d, a.y*d, a.z*d, a.n*d, a.c); } template inline Quaternion operator / (const Quaternion a, const R d) { return Quaternion(a.s/d, a.x/d, a.y/d, a.z/d, a.n/d, a.c); } // * inline Quaternion operator * (const Quaternion a, const Quaternion b) { Quaternion quat; if (a.c<0.0 || b.c<0.0) quat.init(-1.0); else quat.set(a.s*b.s-a.x*b.x-a.y*b.y-a.z*b.z, a.s*b.x+a.x*b.s+a.y*b.z-a.z*b.y, a.s*b.y+a.y*b.s+a.z*b.x-a.x*b.z, a.s*b.z+a.z*b.s+a.x*b.y-a.y*b.x, 1.0, Min(a.c, b.c)); return quat; } // * inline Quaternion operator * (const Quaternion q, const Vector v) { Quaternion quat; if (q.c<0.0 || v.c<0.0) quat.init(-1.0); else quat.set(-q.x*v.x-q.y*v.y-q.z*v.z, q.s*v.x+q.y*v.z-q.z*v.y, q.s*v.y+q.z*v.x-q.x*v.z, q.s*v.z+q.x*v.y-q.y*v.x, v.n, Min(q.c, v.c)); return quat; } // * inline Quaternion operator * (const Vector v, const Quaternion q) { Quaternion quat; if (q.c<0.0 || v.c<0.0) quat.init(-1.0); else quat.set(-v.x*q.x-v.y*q.y-v.z*q.z, v.x*q.s+v.y*q.z-v.z*q.y, v.y*q.s+v.z*q.x-v.x*q.z, v.z*q.s+v.x*q.y-v.y*q.x, v.n, Min(v.c, q.c)); return quat; } ///////////////////////////////////////////////////////////////////////////////////////////////////////////// // // 回転行列，オイラー角，クォータニオン // // 注：オイラー角ベクトルの成分は演算順に指定する． // 例 YZX回転なら e.x:Y回転，e.y:Z回転, e.z:X回転 // // E -> R Matrix EulerXYZ2RotMatrix(Vector eul); Matrix EulerZYX2RotMatrix(Vector eul); Matrix EulerXZY2RotMatrix(Vector eul); Matrix EulerYZX2RotMatrix(Vector eul); Matrix EulerYXZ2RotMatrix(Vector eul); Matrix EulerZXY2RotMatrix(Vector eul); // R -> E Vector RotMatrix2EulerXYZ(Matrix mtx, Vector* vct=NULL); Vector RotMatrix2EulerZYX(Matrix mtx, Vector* vct=NULL); Vector RotMatrix2EulerXZY(Matrix mtx, Vector* vct=NULL); Vector RotMatrix2EulerYZX(Matrix mtx, Vector* vct=NULL); Vector RotMatrix2EulerYXZ(Matrix mtx, Vector* vct=NULL); Vector RotMatrix2EulerZXY(Matrix mtx, Vector* vct=NULL); Vector RotMatrixElements2EulerXYZ(double m11, double m12, double m13, double m21, double m31, double m32, double m33, Vector* vct=NULL); Vector RotMatrixElements2EulerZYX(double m11, double m12, double m13, double m21, double m23, double m31, double m33, Vector* vct=NULL); Vector RotMatrixElements2EulerXZY(double m11, double m12, double m13, double m21, double m22, double m23, double m31, Vector* vct=NULL); Vector RotMatrixElements2EulerYZX(double m11, double m12, double m13, double m21, double m22, double m31, double m32, Vector* vct=NULL); Vector RotMatrixElements2EulerYXZ(double m12, double m21, double m22, double m23, double m31, double m32, double m33, Vector* vct=NULL); Vector RotMatrixElements2EulerZXY(double m12, double m13, double m21, double m22, double m23, double m32, double m33, Vector* vct=NULL); // Q -> E /* Vector Quaternion2EulerXYZ(Quaternion qut, Vector* vct=NULL); Vector Quaternion2EulerZYX(Quaternion qut, Vector* vct=NULL); Vector Quaternion2EulerXZY(Quaternion qut, Vector* vct=NULL); Vector Quaternion2EulerYZX(Quaternion qut, Vector* vct=NULL); Vector Quaternion2EulerYXZ(Quaternion qut, Vector* vct=NULL); Vector Quaternion2EulerZXY(Quaternion qut, Vector* vct=NULL); */ // R -> Q Quaternion RotMatrix2Quaternion(Matrix mtx); // Q -> R #define Quaternion2RotMatrix(q) (q).getRotMatrix() // E -> Q （注：eの成分は演算順） inline Quaternion EulerXYZ2Quaternion(Vector e) { Quaternion q; q.setEulerXYZ(e); return q;} inline Quaternion EulerZYX2Quaternion(Vector e) { Quaternion q; q.setEulerZYX(e); return q;} inline Quaternion EulerXZY2Quaternion(Vector e) { Quaternion q; q.setEulerXZY(e); return q;} inline Quaternion EulerYZX2Quaternion(Vector e) { Quaternion q; q.setEulerYZX(e); return q;} inline Quaternion EulerYXZ2Quaternion(Vector e) { Quaternion q; q.setEulerYXZ(e); return q;} inline Quaternion EulerZXY2Quaternion(Vector e) { Quaternion q; q.setEulerZXY(e); return q;} ///////////////////////////////////////////////////////////////////////////////////////////////////////////// // // ベクトルの回転，クォータニオン // #define vectorRotation(v, q) (q).execRotation((v)) Quaternion V2VQuaternion(Vector a, Vector b); Quaternion PPPQuaternion(Vector a, Vector b, Vector c); Quaternion VPPQuaternion(Vector a, Vector b, Vector c); Quaternion PPVQuaternion(Vector a, Vector b, Vector c); Quaternion SlerpQuaternion(Quaternion a, Quaternion b, double t); } // namespace #endif