四元数(クォータニオン)の計算やらなんやらができます.主に演算子オーバーロードの練習で作りました.温かい目で見てやってください.
Dependencies: Vector3
Dependents: Hybrid_main_FirstEdtion MadgwickFilter MadgwickFilter
Fork of Quaternion by
Quaternion.hpp@6:0c20582087b1, 2017-10-25 (annotated)
- Committer:
- Gaku0606
- Date:
- Wed Oct 25 08:08:06 2017 +0000
- Revision:
- 6:0c20582087b1
- Parent:
- 5:3c531c1f56cc
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 | 5:3c531c1f56cc | 3 | #include "Vector3.hpp" |
Gaku0606 | 5:3c531c1f56cc | 4 | /** |
Gaku0606 | 2:7c23225b23dc | 5 | * クォータニオンの足し,引き,掛け算などを簡単にできるようになります. |
Gaku0606 | 2:7c23225b23dc | 6 | * @author Gaku MATSUMOTO |
Gaku0606 | 2:7c23225b23dc | 7 | * @bref クォータニオンを使えるクラスです. |
Gaku0606 | 0:31fe79c6544c | 8 | */ |
Gaku0606 | 5:3c531c1f56cc | 9 | |
Gaku0606 | 0:31fe79c6544c | 10 | class Quaternion{ |
Gaku0606 | 0:31fe79c6544c | 11 | public: |
Gaku0606 | 0:31fe79c6544c | 12 | /** |
Gaku0606 | 0:31fe79c6544c | 13 | @bref Quaternionインスタンスを生成します |
Gaku0606 | 0:31fe79c6544c | 14 | */ |
Gaku0606 | 0:31fe79c6544c | 15 | Quaternion(){ |
Gaku0606 | 5:3c531c1f56cc | 16 | w = 1.0; |
Gaku0606 | 5:3c531c1f56cc | 17 | x = 0.0; |
Gaku0606 | 5:3c531c1f56cc | 18 | y = 0.0; |
Gaku0606 | 5:3c531c1f56cc | 19 | z = 0.0; |
Gaku0606 | 0:31fe79c6544c | 20 | }; |
Gaku0606 | 5:3c531c1f56cc | 21 | |
Gaku0606 | 0:31fe79c6544c | 22 | /** |
Gaku0606 | 5:3c531c1f56cc | 23 | * @bref Vector3クラスからクォータニオンを作ります |
Gaku0606 | 5:3c531c1f56cc | 24 | */ |
Gaku0606 | 5:3c531c1f56cc | 25 | Quaternion(Vector3 vector){ |
Gaku0606 | 6:0c20582087b1 | 26 | set(vector); |
Gaku0606 | 5:3c531c1f56cc | 27 | } |
Gaku0606 | 5:3c531c1f56cc | 28 | |
Gaku0606 | 5:3c531c1f56cc | 29 | /** |
Gaku0606 | 5:3c531c1f56cc | 30 | * @bref クォータニオンを回転軸と回転角度によって初期化します。 |
Gaku0606 | 6:0c20582087b1 | 31 | * @param vec 回転軸となる3次元ベクトル |
Gaku0606 | 6:0c20582087b1 | 32 | * @param angle 回転角 [rad] |
Gaku0606 | 5:3c531c1f56cc | 33 | */ |
Gaku0606 | 5:3c531c1f56cc | 34 | Quaternion(Vector3 vec, double angle){ |
Gaku0606 | 5:3c531c1f56cc | 35 | set(vec, angle); |
Gaku0606 | 5:3c531c1f56cc | 36 | } |
Gaku0606 | 5:3c531c1f56cc | 37 | |
Gaku0606 | 5:3c531c1f56cc | 38 | /** |
Gaku0606 | 5:3c531c1f56cc | 39 | @bref 要素を代入しながら,インスタンスを生成します. |
Gaku0606 | 5:3c531c1f56cc | 40 | @param[in] _w 実部wの初期値 |
Gaku0606 | 5:3c531c1f56cc | 41 | @param[in] _x 虚部iの初期値 |
Gaku0606 | 5:3c531c1f56cc | 42 | @param[in] _y 虚部jの初期値 |
Gaku0606 | 5:3c531c1f56cc | 43 | @param[in] _z 虚部kの初期値 |
Gaku0606 | 0:31fe79c6544c | 44 | */ |
Gaku0606 | 0:31fe79c6544c | 45 | Quaternion(double _w, double _x, double _y, double _z){ |
Gaku0606 | 0:31fe79c6544c | 46 | w = _w; x = _x; y = _y; z = _z; |
Gaku0606 | 0:31fe79c6544c | 47 | }; |
Gaku0606 | 0:31fe79c6544c | 48 | |
Gaku0606 | 0:31fe79c6544c | 49 | public: |
Gaku0606 | 0:31fe79c6544c | 50 | double w; |
Gaku0606 | 0:31fe79c6544c | 51 | double x; |
Gaku0606 | 0:31fe79c6544c | 52 | double y; |
Gaku0606 | 0:31fe79c6544c | 53 | double z; |
Gaku0606 | 0:31fe79c6544c | 54 | |
Gaku0606 | 0:31fe79c6544c | 55 | public: |
Gaku0606 | 0:31fe79c6544c | 56 | |
Gaku0606 | 0:31fe79c6544c | 57 | /** |
Gaku0606 | 5:3c531c1f56cc | 58 | @bref クォータニオンの要素をコピーします. |
Gaku0606 | 5:3c531c1f56cc | 59 | @note 通常の数のように代入できます |
Gaku0606 | 0:31fe79c6544c | 60 | */ |
Gaku0606 | 0:31fe79c6544c | 61 | Quaternion operator=(Quaternion r){ |
Gaku0606 | 0:31fe79c6544c | 62 | w = r.w; |
Gaku0606 | 0:31fe79c6544c | 63 | x = r.x; |
Gaku0606 | 0:31fe79c6544c | 64 | y = r.y; |
Gaku0606 | 0:31fe79c6544c | 65 | z = r.z; |
Gaku0606 | 0:31fe79c6544c | 66 | return *this; |
Gaku0606 | 0:31fe79c6544c | 67 | }; |
Gaku0606 | 0:31fe79c6544c | 68 | |
Gaku0606 | 0:31fe79c6544c | 69 | /** |
Gaku0606 | 5:3c531c1f56cc | 70 | @bref クォータニオンを足して代入します. |
Gaku0606 | 5:3c531c1f56cc | 71 | @note 通常の数のように代入できます |
Gaku0606 | 0:31fe79c6544c | 72 | */ |
Gaku0606 | 0:31fe79c6544c | 73 | Quaternion operator+=(Quaternion r){ |
Gaku0606 | 0:31fe79c6544c | 74 | w += r.w; |
Gaku0606 | 0:31fe79c6544c | 75 | x += r.x; |
Gaku0606 | 0:31fe79c6544c | 76 | y += r.y; |
Gaku0606 | 0:31fe79c6544c | 77 | z += r.z; |
Gaku0606 | 0:31fe79c6544c | 78 | return *this; |
Gaku0606 | 0:31fe79c6544c | 79 | }; |
Gaku0606 | 0:31fe79c6544c | 80 | |
Gaku0606 | 0:31fe79c6544c | 81 | /** |
Gaku0606 | 5:3c531c1f56cc | 82 | @bref クォータニオンを引いて代入します. |
Gaku0606 | 5:3c531c1f56cc | 83 | @note 通常の数のように代入できます |
Gaku0606 | 0:31fe79c6544c | 84 | */ |
Gaku0606 | 0:31fe79c6544c | 85 | Quaternion operator-=(Quaternion r){ |
Gaku0606 | 0:31fe79c6544c | 86 | w -= r.w; |
Gaku0606 | 0:31fe79c6544c | 87 | x -= r.x; |
Gaku0606 | 0:31fe79c6544c | 88 | y -= r.y; |
Gaku0606 | 0:31fe79c6544c | 89 | z -= r.z; |
Gaku0606 | 0:31fe79c6544c | 90 | return *this; |
Gaku0606 | 0:31fe79c6544c | 91 | }; |
Gaku0606 | 0:31fe79c6544c | 92 | |
Gaku0606 | 0:31fe79c6544c | 93 | /** |
Gaku0606 | 0:31fe79c6544c | 94 | * @bref クォータニオンの掛け算をします. |
Gaku0606 | 0:31fe79c6544c | 95 | * @note この際も順序は重要です. |
Gaku0606 | 0:31fe79c6544c | 96 | */ |
Gaku0606 | 0:31fe79c6544c | 97 | Quaternion operator*=(Quaternion r){ |
Gaku0606 | 0:31fe79c6544c | 98 | static Quaternion QQ; |
Gaku0606 | 0:31fe79c6544c | 99 | QQ.w = w*r.x - x*r.x - y*r.y - z*r.z; |
Gaku0606 | 0:31fe79c6544c | 100 | QQ.x = x*r.w + w*r.x - z*r.y + y*r.z; |
Gaku0606 | 0:31fe79c6544c | 101 | QQ.y = y*r.w + z*r.x + w*r.y - x*r.z; |
Gaku0606 | 0:31fe79c6544c | 102 | QQ.z = z*r.w - y*r.x + x*r.y + w*r.z; |
Gaku0606 | 0:31fe79c6544c | 103 | w = QQ.w; |
Gaku0606 | 0:31fe79c6544c | 104 | x = QQ.x; |
Gaku0606 | 0:31fe79c6544c | 105 | y = QQ.y; |
Gaku0606 | 0:31fe79c6544c | 106 | z = QQ.z; |
Gaku0606 | 0:31fe79c6544c | 107 | return *this; |
Gaku0606 | 0:31fe79c6544c | 108 | }; |
Gaku0606 | 0:31fe79c6544c | 109 | |
Gaku0606 | 0:31fe79c6544c | 110 | /** |
Gaku0606 | 0:31fe79c6544c | 111 | @bref クォータニオンの複素共役を返します. |
Gaku0606 | 0:31fe79c6544c | 112 | @note 本当はアスタリスクが良かったのですが,ポインタと紛らわしいのでマイナスにしました. |
Gaku0606 | 0:31fe79c6544c | 113 | */ |
Gaku0606 | 0:31fe79c6544c | 114 | Quaternion operator-(){ |
Gaku0606 | 0:31fe79c6544c | 115 | Quaternion Q; |
Gaku0606 | 0:31fe79c6544c | 116 | Q.w = w; |
Gaku0606 | 0:31fe79c6544c | 117 | Q.x = -x; |
Gaku0606 | 0:31fe79c6544c | 118 | Q.y = -y; |
Gaku0606 | 0:31fe79c6544c | 119 | Q.z = -z; |
Gaku0606 | 0:31fe79c6544c | 120 | return Q; |
Gaku0606 | 0:31fe79c6544c | 121 | }; |
Gaku0606 | 0:31fe79c6544c | 122 | |
Gaku0606 | 0:31fe79c6544c | 123 | /** |
Gaku0606 | 0:31fe79c6544c | 124 | @bref クォータニオンを正規化して,単位クォータニオンにします. |
Gaku0606 | 0:31fe79c6544c | 125 | @note 掛け算などを行うたびに実行することをお勧めします. |
Gaku0606 | 5:3c531c1f56cc | 126 | @note ただ、クォータニオンの時間微分は正規化してはいけません |
Gaku0606 | 0:31fe79c6544c | 127 | */ |
Gaku0606 | 5:3c531c1f56cc | 128 | void Normalize(){ |
Gaku0606 | 0:31fe79c6544c | 129 | double norm = sqrt(w*w + x*x + y*y + z*z); |
Gaku0606 | 0:31fe79c6544c | 130 | if (norm != 0.0){ |
Gaku0606 | 0:31fe79c6544c | 131 | w /= norm; |
Gaku0606 | 0:31fe79c6544c | 132 | x /= norm; |
Gaku0606 | 0:31fe79c6544c | 133 | y /= norm; |
Gaku0606 | 0:31fe79c6544c | 134 | z /= norm; |
Gaku0606 | 0:31fe79c6544c | 135 | return; |
Gaku0606 | 0:31fe79c6544c | 136 | } |
Gaku0606 | 0:31fe79c6544c | 137 | else{ |
Gaku0606 | 0:31fe79c6544c | 138 | return; |
Gaku0606 | 0:31fe79c6544c | 139 | } |
Gaku0606 | 0:31fe79c6544c | 140 | }; |
Gaku0606 | 5:3c531c1f56cc | 141 | |
Gaku0606 | 5:3c531c1f56cc | 142 | /** |
Gaku0606 | 5:3c531c1f56cc | 143 | * @bref クォータニオンを初期化します |
Gaku0606 | 5:3c531c1f56cc | 144 | */ |
Gaku0606 | 5:3c531c1f56cc | 145 | template <typename T> void set(T _w, T _x, T _y, T _z); |
Gaku0606 | 5:3c531c1f56cc | 146 | /** |
Gaku0606 | 5:3c531c1f56cc | 147 | * @bref クォータニオンをVector3クラスで初期化します。 |
Gaku0606 | 5:3c531c1f56cc | 148 | */ |
Gaku0606 | 5:3c531c1f56cc | 149 | void set(Vector3 vec); |
Gaku0606 | 5:3c531c1f56cc | 150 | |
Gaku0606 | 5:3c531c1f56cc | 151 | /** |
Gaku0606 | 5:3c531c1f56cc | 152 | * @bref クォータニオンを回転軸と回転角度によって初期化します。 |
Gaku0606 | 5:3c531c1f56cc | 153 | * param vec 回転軸となる3次元ベクトル |
Gaku0606 | 5:3c531c1f56cc | 154 | * param angle 回転角 [rad] |
Gaku0606 | 5:3c531c1f56cc | 155 | */ |
Gaku0606 | 5:3c531c1f56cc | 156 | void set(Vector3 vec, double angle){ |
Gaku0606 | 5:3c531c1f56cc | 157 | vec.Normalize(); |
Gaku0606 | 5:3c531c1f56cc | 158 | double halfAngle = angle /= 2.0; |
Gaku0606 | 5:3c531c1f56cc | 159 | |
Gaku0606 | 5:3c531c1f56cc | 160 | w = cos(halfAngle); |
Gaku0606 | 5:3c531c1f56cc | 161 | x = vec.x * sin(halfAngle); |
Gaku0606 | 5:3c531c1f56cc | 162 | y = vec.y * sin(halfAngle); |
Gaku0606 | 5:3c531c1f56cc | 163 | z = vec.z * sin(halfAngle); |
Gaku0606 | 5:3c531c1f56cc | 164 | } |
Gaku0606 | 5:3c531c1f56cc | 165 | |
Gaku0606 | 5:3c531c1f56cc | 166 | /** |
Gaku0606 | 5:3c531c1f56cc | 167 | * @bref クォータニオンの各要素に配列のようにアクセスします |
Gaku0606 | 5:3c531c1f56cc | 168 | */ |
Gaku0606 | 5:3c531c1f56cc | 169 | double q(int i){ |
Gaku0606 | 5:3c531c1f56cc | 170 | double ans = 0.0; |
Gaku0606 | 5:3c531c1f56cc | 171 | switch (i){ |
Gaku0606 | 5:3c531c1f56cc | 172 | case 1: |
Gaku0606 | 5:3c531c1f56cc | 173 | ans = w; |
Gaku0606 | 5:3c531c1f56cc | 174 | break; |
Gaku0606 | 5:3c531c1f56cc | 175 | case 2: |
Gaku0606 | 5:3c531c1f56cc | 176 | ans = x; |
Gaku0606 | 5:3c531c1f56cc | 177 | break; |
Gaku0606 | 5:3c531c1f56cc | 178 | case 3: |
Gaku0606 | 5:3c531c1f56cc | 179 | ans = y; |
Gaku0606 | 5:3c531c1f56cc | 180 | break; |
Gaku0606 | 5:3c531c1f56cc | 181 | case 4: |
Gaku0606 | 5:3c531c1f56cc | 182 | ans = z; |
Gaku0606 | 5:3c531c1f56cc | 183 | break; |
Gaku0606 | 5:3c531c1f56cc | 184 | } |
Gaku0606 | 5:3c531c1f56cc | 185 | return ans; |
Gaku0606 | 5:3c531c1f56cc | 186 | } |
Gaku0606 | 5:3c531c1f56cc | 187 | |
Gaku0606 | 5:3c531c1f56cc | 188 | /** |
Gaku0606 | 5:3c531c1f56cc | 189 | * @bref クォータニオンのノルムを計算します |
Gaku0606 | 5:3c531c1f56cc | 190 | */ |
Gaku0606 | 5:3c531c1f56cc | 191 | double Norm(){ |
Gaku0606 | 5:3c531c1f56cc | 192 | return fabs(w*w + x*x + y*y + z*z); |
Gaku0606 | 5:3c531c1f56cc | 193 | } |
Gaku0606 | 5:3c531c1f56cc | 194 | |
Gaku0606 | 5:3c531c1f56cc | 195 | |
Gaku0606 | 5:3c531c1f56cc | 196 | /** クォータニオンとクォータニオンを比較して等しければtrue 等しくなければfalse*/ |
Gaku0606 | 5:3c531c1f56cc | 197 | bool operator==(Quaternion Q){ |
Gaku0606 | 5:3c531c1f56cc | 198 | if (w == Q.w && x == Q.x && y == Q.y && z == Q.z){ |
Gaku0606 | 5:3c531c1f56cc | 199 | return true; |
Gaku0606 | 5:3c531c1f56cc | 200 | } |
Gaku0606 | 5:3c531c1f56cc | 201 | return false; |
Gaku0606 | 5:3c531c1f56cc | 202 | } |
Gaku0606 | 5:3c531c1f56cc | 203 | /** クォータニオンとクォータニオンを比較して等しくなければtrue 等しければfalse*/ |
Gaku0606 | 5:3c531c1f56cc | 204 | bool operator!=(Quaternion Q){ |
Gaku0606 | 5:3c531c1f56cc | 205 | if (w == Q.w && x == Q.x && y == Q.y && z == Q.z){ |
Gaku0606 | 5:3c531c1f56cc | 206 | return false; |
Gaku0606 | 5:3c531c1f56cc | 207 | } |
Gaku0606 | 5:3c531c1f56cc | 208 | return true; |
Gaku0606 | 5:3c531c1f56cc | 209 | } |
Gaku0606 | 5:3c531c1f56cc | 210 | |
Gaku0606 | 5:3c531c1f56cc | 211 | /** |
Gaku0606 | 5:3c531c1f56cc | 212 | * @bref 2つの3次元ベクトルを一致させるクォータニオンを計算 |
Gaku0606 | 5:3c531c1f56cc | 213 | * @param from 始点となるベクトルのインスタンス |
Gaku0606 | 5:3c531c1f56cc | 214 | * @param to 終点となるベクトルのインスタンス |
Gaku0606 | 5:3c531c1f56cc | 215 | */ |
Gaku0606 | 5:3c531c1f56cc | 216 | void FromToRotation(Vector3 from, Vector3 to); |
Gaku0606 | 5:3c531c1f56cc | 217 | |
Gaku0606 | 5:3c531c1f56cc | 218 | /** |
Gaku0606 | 5:3c531c1f56cc | 219 | @bref オイラー角で姿勢を取得します. |
Gaku0606 | 5:3c531c1f56cc | 220 | @param val ロール,ピッチ,ヨーの順に配列に格納します.3つ以上の要素の配列を入れてください. |
Gaku0606 | 5:3c531c1f56cc | 221 | @note 値は[rad]です.[degree]に変換が必要な場合は別途計算して下さい. |
Gaku0606 | 5:3c531c1f56cc | 222 | */ |
Gaku0606 | 5:3c531c1f56cc | 223 | void getEulerAngle(double *val){ |
Gaku0606 | 5:3c531c1f56cc | 224 | double q0q0 = w * w, q1q1q2q2 = x * x - y * y, q3q3 = z * z; |
Gaku0606 | 5:3c531c1f56cc | 225 | val[0] = (atan2(2.0f * (w * x + y * z), q0q0 - q1q1q2q2 + q3q3)); |
Gaku0606 | 5:3c531c1f56cc | 226 | val[1] = (-asin(2.0f * (x * z - w * y))); |
Gaku0606 | 5:3c531c1f56cc | 227 | val[2] = (atan2(2.0f * (x * y + w * z), q0q0 + q1q1q2q2 - q3q3)); |
Gaku0606 | 5:3c531c1f56cc | 228 | } |
Gaku0606 | 5:3c531c1f56cc | 229 | |
Gaku0606 | 6:0c20582087b1 | 230 | /** |
Gaku0606 | 6:0c20582087b1 | 231 | * @bref クォータニオンをVector3クラスに変換します |
Gaku0606 | 6:0c20582087b1 | 232 | * @note クォータニオンのx,y,z成分を持ったベクトルを作ります |
Gaku0606 | 6:0c20582087b1 | 233 | */ |
Gaku0606 | 6:0c20582087b1 | 234 | Vector3 makeVector3(){ |
Gaku0606 | 6:0c20582087b1 | 235 | Vector3 vec3(x, y, z); |
Gaku0606 | 6:0c20582087b1 | 236 | return vec3; |
Gaku0606 | 6:0c20582087b1 | 237 | } |
Gaku0606 | 5:3c531c1f56cc | 238 | |
Gaku0606 | 0:31fe79c6544c | 239 | }; |
Gaku0606 | 0:31fe79c6544c | 240 | |
Gaku0606 | 5:3c531c1f56cc | 241 | void Quaternion::FromToRotation(Vector3 from, Vector3 to){ |
Gaku0606 | 5:3c531c1f56cc | 242 | double halfTheta = from.Angle(to) / 2.0;//回転角度 0からpi/2 |
Gaku0606 | 5:3c531c1f56cc | 243 | Vector3 axis = from * to; |
Gaku0606 | 5:3c531c1f56cc | 244 | axis.Normalize(); |
Gaku0606 | 5:3c531c1f56cc | 245 | |
Gaku0606 | 5:3c531c1f56cc | 246 | w = cos(halfTheta); |
Gaku0606 | 5:3c531c1f56cc | 247 | x = axis.x * sin(halfTheta); |
Gaku0606 | 5:3c531c1f56cc | 248 | y = axis.y * sin(halfTheta); |
Gaku0606 | 5:3c531c1f56cc | 249 | z = axis.z * sin(halfTheta); |
Gaku0606 | 5:3c531c1f56cc | 250 | } |
Gaku0606 | 5:3c531c1f56cc | 251 | |
Gaku0606 | 5:3c531c1f56cc | 252 | |
Gaku0606 | 5:3c531c1f56cc | 253 | |
Gaku0606 | 5:3c531c1f56cc | 254 | template<typename T>void Quaternion::set(T _w, T _x, T _y, T _z){ |
Gaku0606 | 5:3c531c1f56cc | 255 | w = _w; |
Gaku0606 | 5:3c531c1f56cc | 256 | x = _x; |
Gaku0606 | 5:3c531c1f56cc | 257 | y = _y; |
Gaku0606 | 5:3c531c1f56cc | 258 | z = _z; |
Gaku0606 | 5:3c531c1f56cc | 259 | return; |
Gaku0606 | 5:3c531c1f56cc | 260 | } |
Gaku0606 | 5:3c531c1f56cc | 261 | |
Gaku0606 | 5:3c531c1f56cc | 262 | void Quaternion::set(Vector3 vec){ |
Gaku0606 | 5:3c531c1f56cc | 263 | w = 0.0; |
Gaku0606 | 5:3c531c1f56cc | 264 | x = vec.x; |
Gaku0606 | 5:3c531c1f56cc | 265 | y = vec.y; |
Gaku0606 | 5:3c531c1f56cc | 266 | z = vec.z; |
Gaku0606 | 5:3c531c1f56cc | 267 | return; |
Gaku0606 | 5:3c531c1f56cc | 268 | } |
Gaku0606 | 5:3c531c1f56cc | 269 | |
Gaku0606 | 5:3c531c1f56cc | 270 | /** |
Gaku0606 | 4:a914c6c3b74d | 271 | * @fn Quaternion operator*(Quaternion l, Quaternion r) |
Gaku0606 | 4:a914c6c3b74d | 272 | * @bref クォータニオンの掛け算をします.この際,順序が重要です. |
Gaku0606 | 4:a914c6c3b74d | 273 | */ |
Gaku0606 | 0:31fe79c6544c | 274 | Quaternion operator*(Quaternion l, Quaternion r){ |
Gaku0606 | 0:31fe79c6544c | 275 | static Quaternion Q; |
Gaku0606 | 0:31fe79c6544c | 276 | Q.w = l.w*r.w - l.x*r.x - l.y*r.y - l.z*r.z; |
Gaku0606 | 0:31fe79c6544c | 277 | Q.x = l.x*r.w + l.w*r.x - l.z*r.y + l.y*r.z; |
Gaku0606 | 0:31fe79c6544c | 278 | Q.y = l.y*r.w + l.z*r.x + l.w*r.y - l.x*r.z; |
Gaku0606 | 0:31fe79c6544c | 279 | Q.z = l.z*r.w - l.y*r.x + l.x*r.y + l.w*r.z; |
Gaku0606 | 5:3c531c1f56cc | 280 | |
Gaku0606 | 0:31fe79c6544c | 281 | return Q; |
Gaku0606 | 0:31fe79c6544c | 282 | }; |
Gaku0606 | 1:81bcd478f8d7 | 283 | |
Gaku0606 | 5:3c531c1f56cc | 284 | /** |
Gaku0606 | 4:a914c6c3b74d | 285 | * @fn Quaternion operator*(double s, Quaternion q) |
Gaku0606 | 4:a914c6c3b74d | 286 | * @bref クォータニオンをスカラー倍します.. |
Gaku0606 | 4:a914c6c3b74d | 287 | */ |
Gaku0606 | 0:31fe79c6544c | 288 | Quaternion operator*(double s, Quaternion q){ |
Gaku0606 | 0:31fe79c6544c | 289 | static Quaternion Q; |
Gaku0606 | 0:31fe79c6544c | 290 | Q.w = q.w * s; |
Gaku0606 | 0:31fe79c6544c | 291 | Q.x = q.x * s; |
Gaku0606 | 0:31fe79c6544c | 292 | Q.y = q.y * s; |
Gaku0606 | 0:31fe79c6544c | 293 | Q.z = q.z * s; |
Gaku0606 | 0:31fe79c6544c | 294 | return Q; |
Gaku0606 | 0:31fe79c6544c | 295 | }; |
Gaku0606 | 1:81bcd478f8d7 | 296 | |
Gaku0606 | 4:a914c6c3b74d | 297 | /** |
Gaku0606 | 4:a914c6c3b74d | 298 | * @fn Quaternion operator*(Quaternion q, double s) |
Gaku0606 | 4:a914c6c3b74d | 299 | * @bref クォータニオンをスカラー倍します.. |
Gaku0606 | 4:a914c6c3b74d | 300 | */ |
Gaku0606 | 0:31fe79c6544c | 301 | Quaternion operator*(Quaternion q, double s){ |
Gaku0606 | 0:31fe79c6544c | 302 | static Quaternion Q; |
Gaku0606 | 0:31fe79c6544c | 303 | Q.w = q.w * s; |
Gaku0606 | 0:31fe79c6544c | 304 | Q.x = q.x * s; |
Gaku0606 | 0:31fe79c6544c | 305 | Q.y = q.y * s; |
Gaku0606 | 0:31fe79c6544c | 306 | Q.z = q.z * s; |
Gaku0606 | 0:31fe79c6544c | 307 | return Q; |
Gaku0606 | 0:31fe79c6544c | 308 | }; |
Gaku0606 | 0:31fe79c6544c | 309 | |
Gaku0606 | 5:3c531c1f56cc | 310 | |
Gaku0606 | 4:a914c6c3b74d | 311 | /** |
Gaku0606 | 5:3c531c1f56cc | 312 | @bref クォータニオンの足し算をします. |
Gaku0606 | 4:a914c6c3b74d | 313 | */ |
Gaku0606 | 0:31fe79c6544c | 314 | Quaternion operator+(Quaternion l, Quaternion r){ |
Gaku0606 | 0:31fe79c6544c | 315 | static Quaternion Q; |
Gaku0606 | 0:31fe79c6544c | 316 | Q.w = l.w + r.w; |
Gaku0606 | 0:31fe79c6544c | 317 | Q.x = l.x + r.x; |
Gaku0606 | 0:31fe79c6544c | 318 | Q.y = l.y + r.y; |
Gaku0606 | 0:31fe79c6544c | 319 | Q.z = l.z + r.z; |
Gaku0606 | 0:31fe79c6544c | 320 | return Q; |
Gaku0606 | 0:31fe79c6544c | 321 | } |
Gaku0606 | 0:31fe79c6544c | 322 | |
Gaku0606 | 4:a914c6c3b74d | 323 | /** |
Gaku0606 | 5:3c531c1f56cc | 324 | @bref クォータニオンの引き算をします. |
Gaku0606 | 4:a914c6c3b74d | 325 | */ |
Gaku0606 | 0:31fe79c6544c | 326 | Quaternion operator-(Quaternion l, Quaternion r){ |
Gaku0606 | 0:31fe79c6544c | 327 | static Quaternion Q; |
Gaku0606 | 0:31fe79c6544c | 328 | Q.w = l.w - r.w; |
Gaku0606 | 0:31fe79c6544c | 329 | Q.x = l.x - r.x; |
Gaku0606 | 0:31fe79c6544c | 330 | Q.y = l.y - r.y; |
Gaku0606 | 0:31fe79c6544c | 331 | Q.z = l.z - r.z; |
Gaku0606 | 0:31fe79c6544c | 332 | return Q; |
Gaku0606 | 0:31fe79c6544c | 333 | } |
Gaku0606 | 0:31fe79c6544c | 334 | |
Gaku0606 | 0:31fe79c6544c | 335 | #endif |