四元数(クォータニオン)の計算やらなんやらができます.主に演算子オーバーロードの練習で作りました.温かい目で見てやってください.
Dependencies: Vector3
Dependents: Hybrid_main_FirstEdtion MadgwickFilter MadgwickFilter
Fork of Quaternion by
Diff: Quaternion.hpp
- Revision:
- 5:3c531c1f56cc
- Parent:
- 4:a914c6c3b74d
- Child:
- 6:0c20582087b1
- Child:
- 7:631c068aded7
diff -r a914c6c3b74d -r 3c531c1f56cc Quaternion.hpp --- a/Quaternion.hpp Sat Jan 28 21:20:28 2017 +0000 +++ b/Quaternion.hpp Wed Jun 07 00:57:58 2017 +0000 @@ -1,28 +1,49 @@ #ifndef _QUATERNION_HPP_ #define _QUATERNION_HPP_ - -/** +#include "Vector3.hpp" +/** * クォータニオンの足し,引き,掛け算などを簡単にできるようになります. * @author Gaku MATSUMOTO * @bref クォータニオンを使えるクラスです. */ + class Quaternion{ public: /** @bref Quaternionインスタンスを生成します */ Quaternion(){ - w = 1.0f; - x = 0.0f; - y = 0.0f; - z = 0.0f; + w = 1.0; + x = 0.0; + y = 0.0; + z = 0.0; }; + /** - @bref 要素を代入しながら,インスタンスを生成します. - @param[in] _w 実部wの初期値 - @param[in] _x 虚部iの初期値 - @param[in] _y 虚部jの初期値 - @param[in] _z 虚部kの初期値 + * @bref Vector3クラスからクォータニオンを作ります + */ + Quaternion(Vector3 vector){ + w = 0.0; + x = vector.x; + y = vector.y; + z = vector.z; + } + + /** + * @bref クォータニオンを回転軸と回転角度によって初期化します。 + * param vec 回転軸となる3次元ベクトル + * param angle 回転角 [rad] + */ + Quaternion(Vector3 vec, double angle){ + set(vec, angle); + } + + /** + @bref 要素を代入しながら,インスタンスを生成します. + @param[in] _w 実部wの初期値 + @param[in] _x 虚部iの初期値 + @param[in] _y 虚部jの初期値 + @param[in] _z 虚部kの初期値 */ Quaternion(double _w, double _x, double _y, double _z){ w = _w; x = _x; y = _y; z = _z; @@ -37,8 +58,8 @@ public: /** - @bref クォータニオンの要素をコピーします. - @note 通常の数のように代入できます + @bref クォータニオンの要素をコピーします. + @note 通常の数のように代入できます */ Quaternion operator=(Quaternion r){ w = r.w; @@ -49,8 +70,8 @@ }; /** - @bref クォータニオンを足して代入します. - @note 通常の数のように代入できます + @bref クォータニオンを足して代入します. + @note 通常の数のように代入できます */ Quaternion operator+=(Quaternion r){ w += r.w; @@ -61,8 +82,8 @@ }; /** - @bref クォータニオンを引いて代入します. - @note 通常の数のように代入できます + @bref クォータニオンを引いて代入します. + @note 通常の数のように代入できます */ Quaternion operator-=(Quaternion r){ w -= r.w; @@ -105,8 +126,9 @@ /** @bref クォータニオンを正規化して,単位クォータニオンにします. @note 掛け算などを行うたびに実行することをお勧めします. + @note ただ、クォータニオンの時間微分は正規化してはいけません */ - void normalize(){ + void Normalize(){ double norm = sqrt(w*w + x*x + y*y + z*z); if (norm != 0.0){ w /= norm; @@ -119,9 +141,128 @@ return; } }; + + /** + * @bref クォータニオンを初期化します + */ + template <typename T> void set(T _w, T _x, T _y, T _z); + /** + * @bref クォータニオンをVector3クラスで初期化します。 + */ + void set(Vector3 vec); + + /** + * @bref クォータニオンを回転軸と回転角度によって初期化します。 + * param vec 回転軸となる3次元ベクトル + * param angle 回転角 [rad] + */ + void set(Vector3 vec, double angle){ + vec.Normalize(); + double halfAngle = angle /= 2.0; + + w = cos(halfAngle); + x = vec.x * sin(halfAngle); + y = vec.y * sin(halfAngle); + z = vec.z * sin(halfAngle); + } + + /** + * @bref クォータニオンの各要素に配列のようにアクセスします + */ + double q(int i){ + double ans = 0.0; + switch (i){ + case 1: + ans = w; + break; + case 2: + ans = x; + break; + case 3: + ans = y; + break; + case 4: + ans = z; + break; + } + return ans; + } + + /** + * @bref クォータニオンのノルムを計算します + */ + double Norm(){ + return fabs(w*w + x*x + y*y + z*z); + } + + + /** クォータニオンとクォータニオンを比較して等しければtrue 等しくなければfalse*/ + bool operator==(Quaternion Q){ + if (w == Q.w && x == Q.x && y == Q.y && z == Q.z){ + return true; + } + return false; + } + /** クォータニオンとクォータニオンを比較して等しくなければtrue 等しければfalse*/ + bool operator!=(Quaternion Q){ + if (w == Q.w && x == Q.x && y == Q.y && z == Q.z){ + return false; + } + return true; + } + + /** + * @bref 2つの3次元ベクトルを一致させるクォータニオンを計算 + * @param from 始点となるベクトルのインスタンス + * @param to 終点となるベクトルのインスタンス + */ + void FromToRotation(Vector3 from, Vector3 to); + + /** + @bref オイラー角で姿勢を取得します. + @param val ロール,ピッチ,ヨーの順に配列に格納します.3つ以上の要素の配列を入れてください. + @note 値は[rad]です.[degree]に変換が必要な場合は別途計算して下さい. + */ + void getEulerAngle(double *val){ + double q0q0 = w * w, q1q1q2q2 = x * x - y * y, q3q3 = z * z; + val[0] = (atan2(2.0f * (w * x + y * z), q0q0 - q1q1q2q2 + q3q3)); + val[1] = (-asin(2.0f * (x * z - w * y))); + val[2] = (atan2(2.0f * (x * y + w * z), q0q0 + q1q1q2q2 - q3q3)); + } + + }; -/** +void Quaternion::FromToRotation(Vector3 from, Vector3 to){ + double halfTheta = from.Angle(to) / 2.0;//回転角度 0からpi/2 + Vector3 axis = from * to; + axis.Normalize(); + + w = cos(halfTheta); + x = axis.x * sin(halfTheta); + y = axis.y * sin(halfTheta); + z = axis.z * sin(halfTheta); +} + + + +template<typename T>void Quaternion::set(T _w, T _x, T _y, T _z){ + w = _w; + x = _x; + y = _y; + z = _z; + return; +} + +void Quaternion::set(Vector3 vec){ + w = 0.0; + x = vec.x; + y = vec.y; + z = vec.z; + return; +} + +/** * @fn Quaternion operator*(Quaternion l, Quaternion r) * @bref クォータニオンの掛け算をします.この際,順序が重要です. */ @@ -131,15 +272,11 @@ Q.x = l.x*r.w + l.w*r.x - l.z*r.y + l.y*r.z; Q.y = l.y*r.w + l.z*r.x + l.w*r.y - l.x*r.z; Q.z = l.z*r.w - l.y*r.x + l.x*r.y + l.w*r.z; - /*double q0Dot, q1Dot, q2Dot, q3Dot;//クォータニオンの時間微分 - q0Dot = -gx*qg1 - gy*qg2 - gz*qg3; - q1Dot = gx*qg0 + gz*qg2 - gy*qg3; - q2Dot = gy*qg0 - gz*qg1 + gx*qg3; - q3Dot = gz*qg0 + gy*qg1 - gx*qg2;*/ + return Q; }; -/** +/** * @fn Quaternion operator*(double s, Quaternion q) * @bref クォータニオンをスカラー倍します.. */ @@ -165,9 +302,9 @@ return Q; }; + /** -* @fn Quaternion operator+(Quaternion l, Quaternion r) -* @bref クォータニオンの足し算をします. + @bref クォータニオンの足し算をします. */ Quaternion operator+(Quaternion l, Quaternion r){ static Quaternion Q; @@ -179,8 +316,7 @@ } /** -* @fn Quaternion operator-(Quaternion l, Quaternion r) -* @bref クォータニオンの引き算をします. + @bref クォータニオンの引き算をします. */ Quaternion operator-(Quaternion l, Quaternion r){ static Quaternion Q;