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