Gaku Matsumoto / Quaternion

四元数(クォータニオン)の計算やらなんやらができます．主に演算子オーバーロードの練習で作りました．温かい目で見てやってください．

Dependents:   Hybrid_AttitudeEstimation

Quaternion.hpp@4:a914c6c3b74d, 2017-01-28 (annotated)

Committer:
Gaku0606
Date:
Sat Jan 28 21:20:28 2017 +0000
Revision:
4:a914c6c3b74d
Parent:
3:19af2a12e73a 
fff

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gaku0606 0:31fe79c6544c 1 #ifndef _QUATERNION_HPP_
Gaku0606 0:31fe79c6544c 2 #define _QUATERNION_HPP_
Gaku0606 0:31fe79c6544c 3
Gaku0606 2:7c23225b23dc 4 /**
Gaku0606 2:7c23225b23dc 5 * クォータニオンの足し，引き，掛け算などを簡単にできるようになります．
Gaku0606 2:7c23225b23dc 6 * @author Gaku MATSUMOTO
Gaku0606 2:7c23225b23dc 7 * @bref クォータニオンを使えるクラスです．
Gaku0606 0:31fe79c6544c 8 */
Gaku0606 0:31fe79c6544c 9 class Quaternion{
Gaku0606 0:31fe79c6544c 10 public:
Gaku0606 0:31fe79c6544c 11 /**
Gaku0606 0:31fe79c6544c 12 @bref Quaternionインスタンスを生成します
Gaku0606 0:31fe79c6544c 13 */
Gaku0606 0:31fe79c6544c 14 Quaternion(){
Gaku0606 0:31fe79c6544c 15 w = 1.0f;
Gaku0606 0:31fe79c6544c 16 x = 0.0f;
Gaku0606 0:31fe79c6544c 17 y = 0.0f;
Gaku0606 0:31fe79c6544c 18 z = 0.0f;
Gaku0606 0:31fe79c6544c 19 };
Gaku0606 0:31fe79c6544c 20 /**
Gaku0606 0:31fe79c6544c 21 @bref 要素を代入しながら，インスタンスを生成します．
Gaku0606 1:81bcd478f8d7 22 @param[in] _w 実部wの初期値
Gaku0606 1:81bcd478f8d7 23 @param[in] _x 虚部iの初期値
Gaku0606 1:81bcd478f8d7 24 @param[in] _y 虚部jの初期値
Gaku0606 1:81bcd478f8d7 25 @param[in] _z 虚部kの初期値
Gaku0606 0:31fe79c6544c 26 */
Gaku0606 0:31fe79c6544c 27 Quaternion(double _w, double _x, double _y, double _z){
Gaku0606 0:31fe79c6544c 28 w = _w; x = _x; y = _y; z = _z;
Gaku0606 0:31fe79c6544c 29 };
Gaku0606 0:31fe79c6544c 30
Gaku0606 0:31fe79c6544c 31 public:
Gaku0606 0:31fe79c6544c 32 double w;
Gaku0606 0:31fe79c6544c 33 double x;
Gaku0606 0:31fe79c6544c 34 double y;
Gaku0606 0:31fe79c6544c 35 double z;
Gaku0606 0:31fe79c6544c 36
Gaku0606 0:31fe79c6544c 37 public:
Gaku0606 0:31fe79c6544c 38
Gaku0606 0:31fe79c6544c 39 /**
Gaku0606 1:81bcd478f8d7 40 @bref クォータニオンの要素をコピーします．
Gaku0606 1:81bcd478f8d7 41 @note 通常の数のように代入できます
Gaku0606 0:31fe79c6544c 42 */
Gaku0606 0:31fe79c6544c 43 Quaternion operator=(Quaternion r){
Gaku0606 0:31fe79c6544c 44 w = r.w;
Gaku0606 0:31fe79c6544c 45 x = r.x;
Gaku0606 0:31fe79c6544c 46 y = r.y;
Gaku0606 0:31fe79c6544c 47 z = r.z;
Gaku0606 0:31fe79c6544c 48 return *this;
Gaku0606 0:31fe79c6544c 49 };
Gaku0606 0:31fe79c6544c 50
Gaku0606 0:31fe79c6544c 51 /**
Gaku0606 1:81bcd478f8d7 52 @bref クォータニオンを足して代入します．
Gaku0606 1:81bcd478f8d7 53 @note 通常の数のように代入できます
Gaku0606 0:31fe79c6544c 54 */
Gaku0606 0:31fe79c6544c 55 Quaternion operator+=(Quaternion r){
Gaku0606 0:31fe79c6544c 56 w += r.w;
Gaku0606 0:31fe79c6544c 57 x += r.x;
Gaku0606 0:31fe79c6544c 58 y += r.y;
Gaku0606 0:31fe79c6544c 59 z += r.z;
Gaku0606 0:31fe79c6544c 60 return *this;
Gaku0606 0:31fe79c6544c 61 };
Gaku0606 0:31fe79c6544c 62
Gaku0606 0:31fe79c6544c 63 /**
Gaku0606 1:81bcd478f8d7 64 @bref クォータニオンを引いて代入します．
Gaku0606 1:81bcd478f8d7 65 @note 通常の数のように代入できます
Gaku0606 0:31fe79c6544c 66 */
Gaku0606 0:31fe79c6544c 67 Quaternion operator-=(Quaternion r){
Gaku0606 0:31fe79c6544c 68 w -= r.w;
Gaku0606 0:31fe79c6544c 69 x -= r.x;
Gaku0606 0:31fe79c6544c 70 y -= r.y;
Gaku0606 0:31fe79c6544c 71 z -= r.z;
Gaku0606 0:31fe79c6544c 72 return *this;
Gaku0606 0:31fe79c6544c 73 };
Gaku0606 0:31fe79c6544c 74
Gaku0606 0:31fe79c6544c 75 /**
Gaku0606 0:31fe79c6544c 76 * @bref クォータニオンの掛け算をします．
Gaku0606 0:31fe79c6544c 77 * @note この際も順序は重要です．
Gaku0606 0:31fe79c6544c 78 */
Gaku0606 0:31fe79c6544c 79 Quaternion operator*=(Quaternion r){
Gaku0606 0:31fe79c6544c 80 static Quaternion QQ;
Gaku0606 0:31fe79c6544c 81 QQ.w = w*r.x - x*r.x - y*r.y - z*r.z;
Gaku0606 0:31fe79c6544c 82 QQ.x = x*r.w + w*r.x - z*r.y + y*r.z;
Gaku0606 0:31fe79c6544c 83 QQ.y = y*r.w + z*r.x + w*r.y - x*r.z;
Gaku0606 0:31fe79c6544c 84 QQ.z = z*r.w - y*r.x + x*r.y + w*r.z;
Gaku0606 0:31fe79c6544c 85 w = QQ.w;
Gaku0606 0:31fe79c6544c 86 x = QQ.x;
Gaku0606 0:31fe79c6544c 87 y = QQ.y;
Gaku0606 0:31fe79c6544c 88 z = QQ.z;
Gaku0606 0:31fe79c6544c 89 return *this;
Gaku0606 0:31fe79c6544c 90 };
Gaku0606 0:31fe79c6544c 91
Gaku0606 0:31fe79c6544c 92 /**
Gaku0606 0:31fe79c6544c 93 @bref クォータニオンの複素共役を返します．
Gaku0606 0:31fe79c6544c 94 @note 本当はアスタリスクが良かったのですが，ポインタと紛らわしいのでマイナスにしました．
Gaku0606 0:31fe79c6544c 95 */
Gaku0606 0:31fe79c6544c 96 Quaternion operator-(){
Gaku0606 0:31fe79c6544c 97 Quaternion Q;
Gaku0606 0:31fe79c6544c 98 Q.w = w;
Gaku0606 0:31fe79c6544c 99 Q.x = -x;
Gaku0606 0:31fe79c6544c 100 Q.y = -y;
Gaku0606 0:31fe79c6544c 101 Q.z = -z;
Gaku0606 0:31fe79c6544c 102 return Q;
Gaku0606 0:31fe79c6544c 103 };
Gaku0606 0:31fe79c6544c 104
Gaku0606 0:31fe79c6544c 105 /**
Gaku0606 0:31fe79c6544c 106 @bref クォータニオンを正規化して，単位クォータニオンにします．
Gaku0606 0:31fe79c6544c 107 @note 掛け算などを行うたびに実行することをお勧めします．
Gaku0606 0:31fe79c6544c 108 */
Gaku0606 0:31fe79c6544c 109 void normalize(){
Gaku0606 0:31fe79c6544c 110 double norm = sqrt(w*w + x*x + y*y + z*z);
Gaku0606 0:31fe79c6544c 111 if (norm != 0.0){
Gaku0606 0:31fe79c6544c 112 w /= norm;
Gaku0606 0:31fe79c6544c 113 x /= norm;
Gaku0606 0:31fe79c6544c 114 y /= norm;
Gaku0606 0:31fe79c6544c 115 z /= norm;
Gaku0606 0:31fe79c6544c 116 return;
Gaku0606 0:31fe79c6544c 117 }
Gaku0606 0:31fe79c6544c 118 else{
Gaku0606 0:31fe79c6544c 119 return;
Gaku0606 0:31fe79c6544c 120 }
Gaku0606 0:31fe79c6544c 121 };
Gaku0606 0:31fe79c6544c 122 };
Gaku0606 0:31fe79c6544c 123
Gaku0606 4:a914c6c3b74d 124 /**
Gaku0606 4:a914c6c3b74d 125 * @fn Quaternion operator*(Quaternion l, Quaternion r)
Gaku0606 4:a914c6c3b74d 126 * @bref クォータニオンの掛け算をします．この際，順序が重要です．
Gaku0606 4:a914c6c3b74d 127 */
Gaku0606 0:31fe79c6544c 128 Quaternion operator*(Quaternion l, Quaternion r){
Gaku0606 0:31fe79c6544c 129 static Quaternion Q;
Gaku0606 0:31fe79c6544c 130 Q.w = l.w*r.w - l.x*r.x - l.y*r.y - l.z*r.z;
Gaku0606 0:31fe79c6544c 131 Q.x = l.x*r.w + l.w*r.x - l.z*r.y + l.y*r.z;
Gaku0606 0:31fe79c6544c 132 Q.y = l.y*r.w + l.z*r.x + l.w*r.y - l.x*r.z;
Gaku0606 0:31fe79c6544c 133 Q.z = l.z*r.w - l.y*r.x + l.x*r.y + l.w*r.z;
Gaku0606 0:31fe79c6544c 134 /*double q0Dot, q1Dot, q2Dot, q3Dot;//クォータニオンの時間微分
Gaku0606 0:31fe79c6544c 135 q0Dot = -gx*qg1 - gy*qg2 - gz*qg3;
Gaku0606 0:31fe79c6544c 136 q1Dot = gx*qg0 + gz*qg2 - gy*qg3;
Gaku0606 0:31fe79c6544c 137 q2Dot = gy*qg0 - gz*qg1 + gx*qg3;
Gaku0606 0:31fe79c6544c 138 q3Dot = gz*qg0 + gy*qg1 - gx*qg2;*/
Gaku0606 0:31fe79c6544c 139 return Q;
Gaku0606 0:31fe79c6544c 140 };
Gaku0606 1:81bcd478f8d7 141
Gaku0606 4:a914c6c3b74d 142 /**
Gaku0606 4:a914c6c3b74d 143 * @fn Quaternion operator*(double s, Quaternion q)
Gaku0606 4:a914c6c3b74d 144 * @bref クォータニオンをスカラー倍します．．
Gaku0606 4:a914c6c3b74d 145 */
Gaku0606 0:31fe79c6544c 146 Quaternion operator*(double s, Quaternion q){
Gaku0606 0:31fe79c6544c 147 static Quaternion Q;
Gaku0606 0:31fe79c6544c 148 Q.w = q.w * s;
Gaku0606 0:31fe79c6544c 149 Q.x = q.x * s;
Gaku0606 0:31fe79c6544c 150 Q.y = q.y * s;
Gaku0606 0:31fe79c6544c 151 Q.z = q.z * s;
Gaku0606 0:31fe79c6544c 152 return Q;
Gaku0606 0:31fe79c6544c 153 };
Gaku0606 1:81bcd478f8d7 154
Gaku0606 4:a914c6c3b74d 155 /**
Gaku0606 4:a914c6c3b74d 156 * @fn Quaternion operator*(Quaternion q, double s)
Gaku0606 4:a914c6c3b74d 157 * @bref クォータニオンをスカラー倍します．．
Gaku0606 4:a914c6c3b74d 158 */
Gaku0606 0:31fe79c6544c 159 Quaternion operator*(Quaternion q, double s){
Gaku0606 0:31fe79c6544c 160 static Quaternion Q;
Gaku0606 0:31fe79c6544c 161 Q.w = q.w * s;
Gaku0606 0:31fe79c6544c 162 Q.x = q.x * s;
Gaku0606 0:31fe79c6544c 163 Q.y = q.y * s;
Gaku0606 0:31fe79c6544c 164 Q.z = q.z * s;
Gaku0606 0:31fe79c6544c 165 return Q;
Gaku0606 0:31fe79c6544c 166 };
Gaku0606 0:31fe79c6544c 167
Gaku0606 4:a914c6c3b74d 168 /**
Gaku0606 4:a914c6c3b74d 169 * @fn Quaternion operator+(Quaternion l, Quaternion r)
Gaku0606 4:a914c6c3b74d 170 * @bref クォータニオンの足し算をします．
Gaku0606 4:a914c6c3b74d 171 */
Gaku0606 0:31fe79c6544c 172 Quaternion operator+(Quaternion l, Quaternion r){
Gaku0606 0:31fe79c6544c 173 static Quaternion Q;
Gaku0606 0:31fe79c6544c 174 Q.w = l.w + r.w;
Gaku0606 0:31fe79c6544c 175 Q.x = l.x + r.x;
Gaku0606 0:31fe79c6544c 176 Q.y = l.y + r.y;
Gaku0606 0:31fe79c6544c 177 Q.z = l.z + r.z;
Gaku0606 0:31fe79c6544c 178 return Q;
Gaku0606 0:31fe79c6544c 179 }
Gaku0606 0:31fe79c6544c 180
Gaku0606 4:a914c6c3b74d 181 /**
Gaku0606 4:a914c6c3b74d 182 * @fn Quaternion operator-(Quaternion l, Quaternion r)
Gaku0606 4:a914c6c3b74d 183 * @bref クォータニオンの引き算をします．
Gaku0606 4:a914c6c3b74d 184 */
Gaku0606 0:31fe79c6544c 185 Quaternion operator-(Quaternion l, Quaternion r){
Gaku0606 0:31fe79c6544c 186 static Quaternion Q;
Gaku0606 0:31fe79c6544c 187 Q.w = l.w - r.w;
Gaku0606 0:31fe79c6544c 188 Q.x = l.x - r.x;
Gaku0606 0:31fe79c6544c 189 Q.y = l.y - r.y;
Gaku0606 0:31fe79c6544c 190 Q.z = l.z - r.z;
Gaku0606 0:31fe79c6544c 191 return Q;
Gaku0606 0:31fe79c6544c 192 }
Gaku0606 0:31fe79c6544c 193
Gaku0606 0:31fe79c6544c 194 #endif