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