四元数(クォータニオン)の計算やらなんやらができます.主に演算子オーバーロードの練習で作りました.温かい目で見てやってください.
Dependencies: Vector3
Dependents: Hybrid_main_FirstEdtion MadgwickFilter MadgwickFilter
Fork of Quaternion by
Quaternion.hpp
- Committer:
- Gaku0606
- Date:
- 2017-10-25
- Revision:
- 6:0c20582087b1
- Parent:
- 5:3c531c1f56cc
File content as of revision 6:0c20582087b1:
#ifndef _QUATERNION_HPP_ #define _QUATERNION_HPP_ #include "Vector3.hpp" /** * クォータニオンの足し,引き,掛け算などを簡単にできるようになります. * @author Gaku MATSUMOTO * @bref クォータニオンを使えるクラスです. */ class Quaternion{ public: /** @bref Quaternionインスタンスを生成します */ Quaternion(){ w = 1.0; x = 0.0; y = 0.0; z = 0.0; }; /** * @bref Vector3クラスからクォータニオンを作ります */ Quaternion(Vector3 vector){ set(vector); } /** * @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; }; public: double w; double x; double y; double z; public: /** @bref クォータニオンの要素をコピーします. @note 通常の数のように代入できます */ Quaternion operator=(Quaternion r){ w = r.w; x = r.x; y = r.y; z = r.z; return *this; }; /** @bref クォータニオンを足して代入します. @note 通常の数のように代入できます */ Quaternion operator+=(Quaternion r){ w += r.w; x += r.x; y += r.y; z += r.z; return *this; }; /** @bref クォータニオンを引いて代入します. @note 通常の数のように代入できます */ Quaternion operator-=(Quaternion r){ w -= r.w; x -= r.x; y -= r.y; z -= r.z; return *this; }; /** * @bref クォータニオンの掛け算をします. * @note この際も順序は重要です. */ Quaternion operator*=(Quaternion r){ static Quaternion QQ; QQ.w = w*r.x - x*r.x - y*r.y - z*r.z; QQ.x = x*r.w + w*r.x - z*r.y + y*r.z; QQ.y = y*r.w + z*r.x + w*r.y - x*r.z; QQ.z = z*r.w - y*r.x + x*r.y + w*r.z; w = QQ.w; x = QQ.x; y = QQ.y; z = QQ.z; return *this; }; /** @bref クォータニオンの複素共役を返します. @note 本当はアスタリスクが良かったのですが,ポインタと紛らわしいのでマイナスにしました. */ Quaternion operator-(){ Quaternion Q; Q.w = w; Q.x = -x; Q.y = -y; Q.z = -z; return Q; }; /** @bref クォータニオンを正規化して,単位クォータニオンにします. @note 掛け算などを行うたびに実行することをお勧めします. @note ただ、クォータニオンの時間微分は正規化してはいけません */ void Normalize(){ double norm = sqrt(w*w + x*x + y*y + z*z); if (norm != 0.0){ w /= norm; x /= norm; y /= norm; z /= norm; return; } else{ 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)); } /** * @bref クォータニオンをVector3クラスに変換します * @note クォータニオンのx,y,z成分を持ったベクトルを作ります */ Vector3 makeVector3(){ Vector3 vec3(x, y, z); return vec3; } }; 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 クォータニオンの掛け算をします.この際,順序が重要です. */ Quaternion operator*(Quaternion l, Quaternion r){ static Quaternion Q; Q.w = l.w*r.w - l.x*r.x - l.y*r.y - l.z*r.z; 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; return Q; }; /** * @fn Quaternion operator*(double s, Quaternion q) * @bref クォータニオンをスカラー倍します.. */ Quaternion operator*(double s, Quaternion q){ static Quaternion Q; Q.w = q.w * s; Q.x = q.x * s; Q.y = q.y * s; Q.z = q.z * s; return Q; }; /** * @fn Quaternion operator*(Quaternion q, double s) * @bref クォータニオンをスカラー倍します.. */ Quaternion operator*(Quaternion q, double s){ static Quaternion Q; Q.w = q.w * s; Q.x = q.x * s; Q.y = q.y * s; Q.z = q.z * s; return Q; }; /** @bref クォータニオンの足し算をします. */ Quaternion operator+(Quaternion l, Quaternion r){ static Quaternion Q; Q.w = l.w + r.w; Q.x = l.x + r.x; Q.y = l.y + r.y; Q.z = l.z + r.z; return Q; } /** @bref クォータニオンの引き算をします. */ Quaternion operator-(Quaternion l, Quaternion r){ static Quaternion Q; Q.w = l.w - r.w; Q.x = l.x - r.x; Q.y = l.y - r.y; Q.z = l.z - r.z; return Q; } #endif