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