Quaternion.hpp

00001 #ifndef _QUATERNION_HPP_
00002 #define _QUATERNION_HPP_
00003 
00004 /** 
00005 * クォータニオンの足し，引き，掛け算などを簡単にできるようになります．
00006 * @author  Gaku MATSUMOTO
00007 * @bref  クォータニオンを使えるクラスです．
00008 */
00009 class Quaternion{
00010 public:
00011     /**
00012         @bref   Quaternionインスタンスを生成します
00013     */
00014     Quaternion(){
00015         w = 1.0f;
00016         x = 0.0f;
00017         y = 0.0f;
00018         z = 0.0f;
00019     };
00020     /**
00021         @bref   要素を代入しながら，インスタンスを生成します．
00022         @param[in]  _w  実部wの初期値
00023         @param[in]  _x  虚部iの初期値
00024         @param[in]  _y  虚部jの初期値
00025         @param[in]  _z  虚部kの初期値
00026     */
00027     Quaternion(double _w, double _x, double _y, double _z){
00028         w = _w; x = _x; y = _y; z = _z;
00029     };
00030 
00031 public:
00032     double w;
00033     double x;
00034     double y;
00035     double z;
00036 
00037 public:
00038 
00039     /**
00040         @bref  クォータニオンの要素をコピーします．
00041         @note  通常の数のように代入できます 
00042     */
00043     Quaternion operator=(Quaternion r){
00044         w = r.w;
00045         x = r.x;
00046         y = r.y;
00047         z = r.z;
00048         return *this;
00049     };
00050 
00051     /**
00052         @bref  クォータニオンを足して代入します．
00053         @note  通常の数のように代入できます 
00054     */
00055     Quaternion operator+=(Quaternion r){
00056         w += r.w;
00057         x += r.x;
00058         y += r.y;
00059         z += r.z;
00060         return *this;
00061     };
00062 
00063     /**
00064         @bref  クォータニオンを引いて代入します．
00065         @note  通常の数のように代入できます 
00066     */
00067     Quaternion operator-=(Quaternion r){
00068         w -= r.w;
00069         x -= r.x;
00070         y -= r.y;
00071         z -= r.z;
00072         return *this;
00073     };
00074 
00075     /**
00076     *   @bref   クォータニオンの掛け算をします．
00077     *   @note   この際も順序は重要です．
00078     */
00079     Quaternion operator*=(Quaternion r){
00080         static Quaternion QQ;
00081         QQ.w = w*r.x - x*r.x - y*r.y - z*r.z;
00082         QQ.x = x*r.w + w*r.x - z*r.y + y*r.z;
00083         QQ.y = y*r.w + z*r.x + w*r.y - x*r.z;
00084         QQ.z = z*r.w - y*r.x + x*r.y + w*r.z;
00085         w = QQ.w;
00086         x = QQ.x;
00087         y = QQ.y;
00088         z = QQ.z;
00089         return *this;
00090     };
00091 
00092     /**
00093         @bref   クォータニオンの複素共役を返します．
00094         @note   本当はアスタリスクが良かったのですが，ポインタと紛らわしいのでマイナスにしました．
00095     */
00096     Quaternion operator-(){
00097         Quaternion Q;
00098         Q.w = w;
00099         Q.x = -x;
00100         Q.y = -y;
00101         Q.z = -z;
00102         return Q;
00103     };
00104 
00105     /**
00106         @bref   クォータニオンを正規化して，単位クォータニオンにします．
00107         @note   掛け算などを行うたびに実行することをお勧めします．
00108     */
00109     void normalize(){
00110         double norm = sqrt(w*w + x*x + y*y + z*z);
00111         if (norm != 0.0){
00112             w /= norm;
00113             x /= norm;
00114             y /= norm;
00115             z /= norm;
00116             return;
00117         }
00118         else{
00119             return;
00120         }
00121     };
00122 };
00123 
00124 /** 
00125 * @fn Quaternion operator*(Quaternion l, Quaternion r)
00126 * @bref クォータニオンの掛け算をします．この際，順序が重要です．
00127 */
00128 Quaternion operator*(Quaternion l, Quaternion r){
00129     static Quaternion Q;
00130     Q.w = l.w*r.w - l.x*r.x - l.y*r.y - l.z*r.z;
00131     Q.x = l.x*r.w + l.w*r.x - l.z*r.y + l.y*r.z;
00132     Q.y = l.y*r.w + l.z*r.x + l.w*r.y - l.x*r.z;
00133     Q.z = l.z*r.w - l.y*r.x + l.x*r.y + l.w*r.z;
00134     /*double q0Dot, q1Dot, q2Dot, q3Dot;//クォータニオンの時間微分
00135     q0Dot =        -gx*qg1 - gy*qg2 - gz*qg3;
00136     q1Dot = gx*qg0         + gz*qg2 - gy*qg3;
00137     q2Dot = gy*qg0 - gz*qg1         + gx*qg3;
00138     q3Dot = gz*qg0 + gy*qg1 - gx*qg2;*/
00139     return Q;
00140 };
00141 
00142 /** 
00143 * @fn Quaternion operator*(double s, Quaternion q)
00144 * @bref クォータニオンをスカラー倍します．．
00145 */
00146 Quaternion operator*(double s, Quaternion q){
00147     static Quaternion Q;
00148     Q.w = q.w * s;
00149     Q.x = q.x * s;
00150     Q.y = q.y * s;
00151     Q.z = q.z * s;
00152     return Q;
00153 };
00154 
00155 /**
00156 * @fn Quaternion operator*(Quaternion q, double s)
00157 * @bref クォータニオンをスカラー倍します．．
00158 */
00159 Quaternion operator*(Quaternion q, double s){
00160     static Quaternion Q;
00161     Q.w = q.w * s;
00162     Q.x = q.x * s;
00163     Q.y = q.y * s;
00164     Q.z = q.z * s;
00165     return Q;
00166 };
00167 
00168 /**
00169 * @fn Quaternion operator+(Quaternion l, Quaternion r)
00170 * @bref クォータニオンの足し算をします．
00171 */
00172 Quaternion operator+(Quaternion l, Quaternion r){
00173     static Quaternion Q;
00174     Q.w = l.w + r.w;
00175     Q.x = l.x + r.x;
00176     Q.y = l.y + r.y;
00177     Q.z = l.z + r.z;
00178     return Q;
00179 }
00180 
00181 /**
00182 * @fn Quaternion operator-(Quaternion l, Quaternion r)
00183 * @bref クォータニオンの引き算をします．
00184 */
00185 Quaternion operator-(Quaternion l, Quaternion r){
00186     static Quaternion Q;
00187     Q.w = l.w - r.w;
00188     Q.x = l.x - r.x;
00189     Q.y = l.y - r.y;
00190     Q.z = l.z - r.z;
00191     return Q;
00192 }
00193 
00194 #endif