PQ_Hybrid_Electrical_Equipment_Team / Quaternion

四元数(クォータニオン)の計算やらなんやらができます．主に演算子オーバーロードの練習で作りました．温かい目で見てやってください．

Dependencies:   Vector3

Dependents:   Hybrid_main_FirstEdtion MadgwickFilter MadgwickFilter

Fork of Quaternion by Gaku Matsumoto

Quaternion.hpp@8:0b4374931b0e, 2018-02-25 (annotated)

Committer:
Gaku0606
Date:
Sun Feb 25 19:44:47 2018 +0000
Revision:
8:0b4374931b0e
Parent:
7:631c068aded7 
double -> float;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gaku0606 0:31fe79c6544c 1 #ifndef _QUATERNION_HPP_
Gaku0606 0:31fe79c6544c 2 #define _QUATERNION_HPP_
Gaku0606 8:0b4374931b0e 3 #include "Vector3/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 回転軸となる３次元ベクトル
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 回転軸となる３次元ベクトル
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 8:0b4374931b0e 161 w = cosf(halfAngle);
Gaku0606 8:0b4374931b0e 162 x = vec.x * sinf(halfAngle);
Gaku0606 8:0b4374931b0e 163 y = vec.y * sinf(halfAngle);
Gaku0606 8:0b4374931b0e 164 z = vec.z * sinf(halfAngle);
Gaku0606 5:3c531c1f56cc 165 }
Gaku0606 5:3c531c1f56cc 166
Gaku0606 5:3c531c1f56cc 167 /**
Gaku0606 5:3c531c1f56cc 168 * @bref クォータニオンの各要素に配列のようにアクセスします
Gaku0606 5:3c531c1f56cc 169 */
Gaku0606 8:0b4374931b0e 170 float q(int i){
Gaku0606 8:0b4374931b0e 171 float 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 8:0b4374931b0e 192 float Norm(){
Gaku0606 8:0b4374931b0e 193 return fabsf(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つの３次元ベクトルを一致させるクォータニオンを計算
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 ロール，ピッチ，ヨーの順に配列に格納します．３つ以上の要素の配列を入れてください．
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 8:0b4374931b0e 226 val[0] = (atan2f(2.0f * (w * x + y * z), q0q0 - q1q1q2q2 + q3q3));
Gaku0606 8:0b4374931b0e 227 val[1] = (-asinf(2.0f * (x * z - w * y)));
Gaku0606 8:0b4374931b0e 228 val[2] = (atan2f(2.0f * (x * y + w * z), q0q0 + q1q1q2q2 - q3q3));
Gaku0606 8:0b4374931b0e 229 }
Gaku0606 8:0b4374931b0e 230
Gaku0606 8:0b4374931b0e 231 /**
Gaku0606 8:0b4374931b0e 232 @bref オイラー角で姿勢を取得します．
Gaku0606 8:0b4374931b0e 233 @param val ロール，ピッチ，ヨーの順に配列に格納します．３つ以上の要素の配列を入れてください．
Gaku0606 8:0b4374931b0e 234 @note 値は[rad]です．[degree]に変換が必要な場合は別途計算して下さい．
Gaku0606 8:0b4374931b0e 235 */
Gaku0606 8:0b4374931b0e 236 void GetEulerAngle(Vector3 *v) {
Gaku0606 8:0b4374931b0e 237 float q0q0 = w * w, q1q1q2q2 = x * x - y * y, q3q3 = z * z;
Gaku0606 8:0b4374931b0e 238 v->x = (atan2f(2.0f * (w * x + y * z), q0q0 - q1q1q2q2 + q3q3));
Gaku0606 8:0b4374931b0e 239 v->y = (-asinf(2.0f * (x * z - w * y)));
Gaku0606 8:0b4374931b0e 240 v->z = (atan2f(2.0f * (x * y + w * z), q0q0 + q1q1q2q2 - q3q3));
Gaku0606 5:3c531c1f56cc 241 }
Gaku0606 5:3c531c1f56cc 242
Gaku0606 7:631c068aded7 243 /**
Gaku0606 7:631c068aded7 244 * @bref クォータニオンをVector3クラスに変換します
Gaku0606 7:631c068aded7 245 * @note クォータニオンのx,y,z成分を持ったベクトルを作ります
Gaku0606 7:631c068aded7 246 */
Gaku0606 7:631c068aded7 247 Vector3 ToVector3(){
Gaku0606 7:631c068aded7 248 Vector3 vec3(x, y, z);
Gaku0606 7:631c068aded7 249 return vec3;
Gaku0606 7:631c068aded7 250 }
Gaku0606 5:3c531c1f56cc 251
Gaku0606 7:631c068aded7 252 /**
Gaku0606 7:631c068aded7 253 * @bref 3次元ベクトルを回転します
Gaku0606 7:631c068aded7 254 * @param v 回転させたい3次元ベクトルのポインタ
Gaku0606 7:631c068aded7 255 * @note 余計なオブジェクトを作りません
Gaku0606 7:631c068aded7 256 */
Gaku0606 7:631c068aded7 257 void Rotation(Vector3* v) {
Gaku0606 7:631c068aded7 258 if (v == NULL) return;
Gaku0606 7:631c068aded7 259 static float ww = 0.0f;
Gaku0606 7:631c068aded7 260 static float xx = 0.0f;
Gaku0606 7:631c068aded7 261 static float yy = 0.0f;
Gaku0606 7:631c068aded7 262 static float zz = 0.0f;
Gaku0606 7:631c068aded7 263
Gaku0606 7:631c068aded7 264 static float vx = 0.0f, vy = 0.0f, vz = 0.0f;
Gaku0606 8:0b4374931b0e 265 static float _wx, _wy, _wz, _xy, _zx, _yz;
Gaku0606 7:631c068aded7 266 ww = w * w;
Gaku0606 7:631c068aded7 267 xx = x * x;
Gaku0606 7:631c068aded7 268 yy = y * y;
Gaku0606 7:631c068aded7 269 zz = z * z;
Gaku0606 7:631c068aded7 270
Gaku0606 7:631c068aded7 271 _wx = w * x;
Gaku0606 7:631c068aded7 272 _wy = w * y;
Gaku0606 7:631c068aded7 273 _wz = w * z;
Gaku0606 7:631c068aded7 274 _xy = x * y;
Gaku0606 8:0b4374931b0e 275 _zx = z * x;
Gaku0606 7:631c068aded7 276 _yz = y * z;
Gaku0606 7:631c068aded7 277
Gaku0606 8:0b4374931b0e 278 vx = (ww + xx - yy - zz) * v->x + 2.0f*(_xy - _wz)*v->y + 2.0f*(_zx + _wy) * v->z;
Gaku0606 7:631c068aded7 279 vy = 2.0f * (_xy + _wz) * v->x + (ww - xx + yy - zz) * v->y + 2.0f*(_yz - _wx)*v->z;
Gaku0606 8:0b4374931b0e 280 vz = 2.0f * (_zx - _wy) * v->x + 2.0f * (_wx + _yz)*v->y + (ww - xx - yy + zz)*v->z;
Gaku0606 7:631c068aded7 281
Gaku0606 7:631c068aded7 282 v->x = vx;
Gaku0606 7:631c068aded7 283 v->y = vy;
Gaku0606 7:631c068aded7 284 v->z = vz;
Gaku0606 7:631c068aded7 285 }
Gaku0606 7:631c068aded7 286
Gaku0606 7:631c068aded7 287 /**
Gaku0606 7:631c068aded7 288 * @bref 3次元ベクトルを回転します.ただし逆回転です
Gaku0606 7:631c068aded7 289 * @param v 回転させたい3次元ベクトルのポインタ
Gaku0606 7:631c068aded7 290 * @note 余計なオブジェクトを作りません
Gaku0606 7:631c068aded7 291 */
Gaku0606 7:631c068aded7 292 void InvRotation(Vector3* v) {
Gaku0606 7:631c068aded7 293 if (v == NULL) return;
Gaku0606 7:631c068aded7 294 static float ww = 0.0f;
Gaku0606 7:631c068aded7 295 static float xx = 0.0f;
Gaku0606 7:631c068aded7 296 static float yy = 0.0f;
Gaku0606 7:631c068aded7 297 static float zz = 0.0f;
Gaku0606 7:631c068aded7 298
Gaku0606 7:631c068aded7 299 static float vx = 0.0f, vy = 0.0f, vz = 0.0f;
Gaku0606 7:631c068aded7 300 static float _wx, _wy, _wz, _xy, _xz, _yz;
Gaku0606 7:631c068aded7 301 ww = w * w;
Gaku0606 7:631c068aded7 302 xx = x * x;
Gaku0606 7:631c068aded7 303 yy = y * y;
Gaku0606 7:631c068aded7 304 zz = z * z;
Gaku0606 7:631c068aded7 305
Gaku0606 7:631c068aded7 306 _wx = w * x;
Gaku0606 7:631c068aded7 307 _wy = w * y;
Gaku0606 7:631c068aded7 308 _wz = w * z;
Gaku0606 7:631c068aded7 309 _xy = x * y;
Gaku0606 7:631c068aded7 310 _xz = x * z;
Gaku0606 7:631c068aded7 311 _yz = y * z;
Gaku0606 7:631c068aded7 312
Gaku0606 7:631c068aded7 313 vx = (ww + xx - yy - zz) * v->x + 2.0f*(_xy + _wz)*v->y + 2.0f*(_xz - _wy) * v->z;
Gaku0606 7:631c068aded7 314 vy = 2.0f * (_xy - _wz) * v->x + (ww - xx + yy - zz) * v->y + 2.0f*(_yz + _wx)*v->z;
Gaku0606 7:631c068aded7 315 vz = 2.0f * (_xz + _wy) * v->x + 2.0f * (-_wx + _yz)*v->y + (ww - xx - yy + zz)*v->z;
Gaku0606 7:631c068aded7 316
Gaku0606 7:631c068aded7 317 v->x = vx;
Gaku0606 7:631c068aded7 318 v->y = vy;
Gaku0606 7:631c068aded7 319 v->z = vz;
Gaku0606 7:631c068aded7 320 }
Gaku0606 0:31fe79c6544c 321 };
Gaku0606 0:31fe79c6544c 322
Gaku0606 5:3c531c1f56cc 323 void Quaternion::FromToRotation(Vector3 from, Vector3 to){
Gaku0606 7:631c068aded7 324 float halfTheta = 0.5f * from.Angle(to);//回転角度 0からpi/2
Gaku0606 5:3c531c1f56cc 325 Vector3 axis = from * to;
Gaku0606 5:3c531c1f56cc 326 axis.Normalize();
Gaku0606 5:3c531c1f56cc 327
Gaku0606 5:3c531c1f56cc 328 w = cos(halfTheta);
Gaku0606 5:3c531c1f56cc 329 x = axis.x * sin(halfTheta);
Gaku0606 5:3c531c1f56cc 330 y = axis.y * sin(halfTheta);
Gaku0606 5:3c531c1f56cc 331 z = axis.z * sin(halfTheta);
Gaku0606 5:3c531c1f56cc 332 }
Gaku0606 5:3c531c1f56cc 333
Gaku0606 5:3c531c1f56cc 334
Gaku0606 5:3c531c1f56cc 335
Gaku0606 7:631c068aded7 336 template<typename T>void Quaternion::Set(T _w, T _x, T _y, T _z){
Gaku0606 5:3c531c1f56cc 337 w = _w;
Gaku0606 5:3c531c1f56cc 338 x = _x;
Gaku0606 5:3c531c1f56cc 339 y = _y;
Gaku0606 5:3c531c1f56cc 340 z = _z;
Gaku0606 5:3c531c1f56cc 341 return;
Gaku0606 5:3c531c1f56cc 342 }
Gaku0606 5:3c531c1f56cc 343
Gaku0606 7:631c068aded7 344 void Quaternion::Set(Vector3 vec){
Gaku0606 5:3c531c1f56cc 345 w = 0.0;
Gaku0606 5:3c531c1f56cc 346 x = vec.x;
Gaku0606 5:3c531c1f56cc 347 y = vec.y;
Gaku0606 5:3c531c1f56cc 348 z = vec.z;
Gaku0606 5:3c531c1f56cc 349 return;
Gaku0606 5:3c531c1f56cc 350 }
Gaku0606 5:3c531c1f56cc 351
Gaku0606 5:3c531c1f56cc 352 /**
Gaku0606 4:a914c6c3b74d 353 * @fn Quaternion operator*(Quaternion l, Quaternion r)
Gaku0606 4:a914c6c3b74d 354 * @bref クォータニオンの掛け算をします．この際，順序が重要です．
Gaku0606 4:a914c6c3b74d 355 */
Gaku0606 0:31fe79c6544c 356 Quaternion operator*(Quaternion l, Quaternion r){
Gaku0606 0:31fe79c6544c 357 static Quaternion Q;
Gaku0606 0:31fe79c6544c 358 Q.w = l.w*r.w - l.x*r.x - l.y*r.y - l.z*r.z;
Gaku0606 0:31fe79c6544c 359 Q.x = l.x*r.w + l.w*r.x - l.z*r.y + l.y*r.z;
Gaku0606 0:31fe79c6544c 360 Q.y = l.y*r.w + l.z*r.x + l.w*r.y - l.x*r.z;
Gaku0606 0:31fe79c6544c 361 Q.z = l.z*r.w - l.y*r.x + l.x*r.y + l.w*r.z;
Gaku0606 5:3c531c1f56cc 362
Gaku0606 0:31fe79c6544c 363 return Q;
Gaku0606 0:31fe79c6544c 364 };
Gaku0606 1:81bcd478f8d7 365
Gaku0606 5:3c531c1f56cc 366 /**
Gaku0606 4:a914c6c3b74d 367 * @fn Quaternion operator*(double s, Quaternion q)
Gaku0606 7:631c068aded7 368 * @bref クォータニオンをスカラー倍します．
Gaku0606 4:a914c6c3b74d 369 */
Gaku0606 7:631c068aded7 370 Quaternion operator*(float s, Quaternion q){
Gaku0606 0:31fe79c6544c 371 static Quaternion Q;
Gaku0606 0:31fe79c6544c 372 Q.w = q.w * s;
Gaku0606 0:31fe79c6544c 373 Q.x = q.x * s;
Gaku0606 0:31fe79c6544c 374 Q.y = q.y * s;
Gaku0606 0:31fe79c6544c 375 Q.z = q.z * s;
Gaku0606 0:31fe79c6544c 376 return Q;
Gaku0606 0:31fe79c6544c 377 };
Gaku0606 1:81bcd478f8d7 378
Gaku0606 4:a914c6c3b74d 379 /**
Gaku0606 4:a914c6c3b74d 380 * @fn Quaternion operator*(Quaternion q, double s)
Gaku0606 7:631c068aded7 381 * @bref クォータニオンをスカラー倍します．
Gaku0606 4:a914c6c3b74d 382 */
Gaku0606 7:631c068aded7 383 Quaternion operator*(Quaternion q, float s){
Gaku0606 0:31fe79c6544c 384 static Quaternion Q;
Gaku0606 0:31fe79c6544c 385 Q.w = q.w * s;
Gaku0606 0:31fe79c6544c 386 Q.x = q.x * s;
Gaku0606 0:31fe79c6544c 387 Q.y = q.y * s;
Gaku0606 0:31fe79c6544c 388 Q.z = q.z * s;
Gaku0606 0:31fe79c6544c 389 return Q;
Gaku0606 0:31fe79c6544c 390 };
Gaku0606 0:31fe79c6544c 391
Gaku0606 8:0b4374931b0e 392 /**
Gaku0606 8:0b4374931b0e 393 */
Gaku0606 8:0b4374931b0e 394 Vector3 operator*(Quaternion q, Vector3 v) {
Gaku0606 8:0b4374931b0e 395
Gaku0606 8:0b4374931b0e 396 static Vector3 ans;
Gaku0606 8:0b4374931b0e 397 static float ww = 0.0f;
Gaku0606 8:0b4374931b0e 398 static float xx = 0.0f;
Gaku0606 8:0b4374931b0e 399 static float yy = 0.0f;
Gaku0606 8:0b4374931b0e 400 static float zz = 0.0f;
Gaku0606 8:0b4374931b0e 401
Gaku0606 8:0b4374931b0e 402 //static float vx = 0.0f, vy = 0.0f, vz = 0.0f;
Gaku0606 8:0b4374931b0e 403 static float _wx, _wy, _wz, _xy, _zx, _yz;
Gaku0606 8:0b4374931b0e 404 ww = q.w * q.w;
Gaku0606 8:0b4374931b0e 405 xx = q.x * q.x;
Gaku0606 8:0b4374931b0e 406 yy = q.y * q.y;
Gaku0606 8:0b4374931b0e 407 zz = q.z * q.z;
Gaku0606 8:0b4374931b0e 408
Gaku0606 8:0b4374931b0e 409 _wx = q.w * q.x;
Gaku0606 8:0b4374931b0e 410 _wy = q.w * q.y;
Gaku0606 8:0b4374931b0e 411 _wz = q.w * q.z;
Gaku0606 8:0b4374931b0e 412 _xy = q.x * q.y;
Gaku0606 8:0b4374931b0e 413 _zx = q.z * q.x;
Gaku0606 8:0b4374931b0e 414 _yz = q.y * q.z;
Gaku0606 8:0b4374931b0e 415
Gaku0606 8:0b4374931b0e 416 ans.x = (ww + xx - yy - zz) * v.x + 2.0f*(_xy - _wz)*v.y + 2.0f*(_zx + _wy) * v.z;
Gaku0606 8:0b4374931b0e 417 ans.y = 2.0f * (_xy + _wz) * v.x + (ww - xx + yy - zz) * v.y + 2.0f*(_yz - _wx)*v.z;
Gaku0606 8:0b4374931b0e 418 ans.z = 2.0f * (_zx - _wy) * v.x + 2.0f * (_wx + _yz)*v.y + (ww - xx - yy + zz)*v.z;
Gaku0606 8:0b4374931b0e 419
Gaku0606 8:0b4374931b0e 420 return ans;
Gaku0606 8:0b4374931b0e 421 }
Gaku0606 8:0b4374931b0e 422
Gaku0606 5:3c531c1f56cc 423
Gaku0606 4:a914c6c3b74d 424 /**
Gaku0606 5:3c531c1f56cc 425 @bref クォータニオンの足し算をします．
Gaku0606 4:a914c6c3b74d 426 */
Gaku0606 0:31fe79c6544c 427 Quaternion operator+(Quaternion l, Quaternion r){
Gaku0606 0:31fe79c6544c 428 static Quaternion Q;
Gaku0606 0:31fe79c6544c 429 Q.w = l.w + r.w;
Gaku0606 0:31fe79c6544c 430 Q.x = l.x + r.x;
Gaku0606 0:31fe79c6544c 431 Q.y = l.y + r.y;
Gaku0606 0:31fe79c6544c 432 Q.z = l.z + r.z;
Gaku0606 0:31fe79c6544c 433 return Q;
Gaku0606 0:31fe79c6544c 434 }
Gaku0606 0:31fe79c6544c 435
Gaku0606 4:a914c6c3b74d 436 /**
Gaku0606 5:3c531c1f56cc 437 @bref クォータニオンの引き算をします．
Gaku0606 4:a914c6c3b74d 438 */
Gaku0606 0:31fe79c6544c 439 Quaternion operator-(Quaternion l, Quaternion r){
Gaku0606 0:31fe79c6544c 440 static Quaternion Q;
Gaku0606 0:31fe79c6544c 441 Q.w = l.w - r.w;
Gaku0606 0:31fe79c6544c 442 Q.x = l.x - r.x;
Gaku0606 0:31fe79c6544c 443 Q.y = l.y - r.y;
Gaku0606 0:31fe79c6544c 444 Q.z = l.z - r.z;
Gaku0606 0:31fe79c6544c 445 return Q;
Gaku0606 0:31fe79c6544c 446 }
Gaku0606 0:31fe79c6544c 447
Gaku0606 0:31fe79c6544c 448 #endif