四元数(クォータニオン)の計算やらなんやらができます.主に演算子オーバーロードの練習で作りました.温かい目で見てやってください.
Dependencies: Vector3
Dependents: Hybrid_main_FirstEdtion MadgwickFilter MadgwickFilter
Fork of Quaternion by
Quaternion.hpp@2:7c23225b23dc, 2017-01-28 (annotated)
- Committer:
- Gaku0606
- Date:
- Sat Jan 28 21:14:26 2017 +0000
- Revision:
- 2:7c23225b23dc
- Parent:
- 1:81bcd478f8d7
- Child:
- 3:19af2a12e73a
jjj
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 | 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 | 0:31fe79c6544c | 124 | /** |
Gaku0606 | 0:31fe79c6544c | 125 | @bref クォータニオンの掛け算をします.この際,順序が重要です. |
Gaku0606 | 0:31fe79c6544c | 126 | */ |
Gaku0606 | 0:31fe79c6544c | 127 | Quaternion operator*(Quaternion l, Quaternion r){ |
Gaku0606 | 0:31fe79c6544c | 128 | static Quaternion Q; |
Gaku0606 | 0:31fe79c6544c | 129 | Q.w = l.w*r.w - l.x*r.x - l.y*r.y - l.z*r.z; |
Gaku0606 | 0:31fe79c6544c | 130 | Q.x = l.x*r.w + l.w*r.x - l.z*r.y + l.y*r.z; |
Gaku0606 | 0:31fe79c6544c | 131 | Q.y = l.y*r.w + l.z*r.x + l.w*r.y - l.x*r.z; |
Gaku0606 | 0:31fe79c6544c | 132 | Q.z = l.z*r.w - l.y*r.x + l.x*r.y + l.w*r.z; |
Gaku0606 | 0:31fe79c6544c | 133 | /*double q0Dot, q1Dot, q2Dot, q3Dot;//クォータニオンの時間微分 |
Gaku0606 | 0:31fe79c6544c | 134 | q0Dot = -gx*qg1 - gy*qg2 - gz*qg3; |
Gaku0606 | 0:31fe79c6544c | 135 | q1Dot = gx*qg0 + gz*qg2 - gy*qg3; |
Gaku0606 | 0:31fe79c6544c | 136 | q2Dot = gy*qg0 - gz*qg1 + gx*qg3; |
Gaku0606 | 0:31fe79c6544c | 137 | q3Dot = gz*qg0 + gy*qg1 - gx*qg2;*/ |
Gaku0606 | 0:31fe79c6544c | 138 | return Q; |
Gaku0606 | 0:31fe79c6544c | 139 | }; |
Gaku0606 | 1:81bcd478f8d7 | 140 | |
Gaku0606 | 1:81bcd478f8d7 | 141 | /** @bref クォータニオンをスカラー倍します..*/ |
Gaku0606 | 0:31fe79c6544c | 142 | Quaternion operator*(double s, Quaternion q){ |
Gaku0606 | 0:31fe79c6544c | 143 | static Quaternion Q; |
Gaku0606 | 0:31fe79c6544c | 144 | Q.w = q.w * s; |
Gaku0606 | 0:31fe79c6544c | 145 | Q.x = q.x * s; |
Gaku0606 | 0:31fe79c6544c | 146 | Q.y = q.y * s; |
Gaku0606 | 0:31fe79c6544c | 147 | Q.z = q.z * s; |
Gaku0606 | 0:31fe79c6544c | 148 | return Q; |
Gaku0606 | 0:31fe79c6544c | 149 | }; |
Gaku0606 | 1:81bcd478f8d7 | 150 | |
Gaku0606 | 1:81bcd478f8d7 | 151 | /** @bref クォータニオンをスカラー倍します..*/ |
Gaku0606 | 0:31fe79c6544c | 152 | Quaternion operator*(Quaternion q, double s){ |
Gaku0606 | 0:31fe79c6544c | 153 | static Quaternion Q; |
Gaku0606 | 0:31fe79c6544c | 154 | Q.w = q.w * s; |
Gaku0606 | 0:31fe79c6544c | 155 | Q.x = q.x * s; |
Gaku0606 | 0:31fe79c6544c | 156 | Q.y = q.y * s; |
Gaku0606 | 0:31fe79c6544c | 157 | Q.z = q.z * s; |
Gaku0606 | 0:31fe79c6544c | 158 | return Q; |
Gaku0606 | 0:31fe79c6544c | 159 | }; |
Gaku0606 | 0:31fe79c6544c | 160 | |
Gaku0606 | 1:81bcd478f8d7 | 161 | /** @bref クォータニオンの足し算をします.*/ |
Gaku0606 | 0:31fe79c6544c | 162 | Quaternion operator+(Quaternion l, Quaternion r){ |
Gaku0606 | 0:31fe79c6544c | 163 | static Quaternion Q; |
Gaku0606 | 0:31fe79c6544c | 164 | Q.w = l.w + r.w; |
Gaku0606 | 0:31fe79c6544c | 165 | Q.x = l.x + r.x; |
Gaku0606 | 0:31fe79c6544c | 166 | Q.y = l.y + r.y; |
Gaku0606 | 0:31fe79c6544c | 167 | Q.z = l.z + r.z; |
Gaku0606 | 0:31fe79c6544c | 168 | return Q; |
Gaku0606 | 0:31fe79c6544c | 169 | } |
Gaku0606 | 0:31fe79c6544c | 170 | |
Gaku0606 | 1:81bcd478f8d7 | 171 | /** @bref クォータニオンの引き算をします.*/ |
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 | 0:31fe79c6544c | 181 | #endif |