PQ_Hybrid_Electrical_Equipment_Team / Quaternion

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

Dependencies:   Vector3

Dependents:   Hybrid_main_FirstEdtion MadgwickFilter MadgwickFilter

Fork of Quaternion by Gaku Matsumoto

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?

UserRevisionLine numberNew 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 回転軸となる３次元ベクトル
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 回転軸となる３次元ベクトル
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つの３次元ベクトルを一致させるクォータニオンを計算
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 ロール，ピッチ，ヨーの順に配列に格納します．３つ以上の要素の配列を入れてください．
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