四元数(クォータニオン)の計算やらなんやらができます.主に演算子オーバーロードの練習で作りました.温かい目で見てやってください.
Dependencies: Vector3
Dependents: Hybrid_main_FirstEdtion MadgwickFilter MadgwickFilter
Fork of Quaternion by
Diff: Quaternion.hpp
- Revision:
- 0:31fe79c6544c
- Child:
- 1:81bcd478f8d7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Quaternion.hpp Sat Jan 28 19:53:10 2017 +0000 @@ -0,0 +1,178 @@ +#ifndef _QUATERNION_HPP_ +#define _QUATERNION_HPP_ + +/** + @fn Quaternion.hpp + @author Gaku MATSUMOTO + @bref クォータニオンを使えるクラスです. +*/ + +class Quaternion{ +public: + /** + @bref Quaternionインスタンスを生成します + */ + Quaternion(){ + w = 1.0f; + x = 0.0f; + y = 0.0f; + z = 0.0f; + }; + /** + @bref 要素を代入しながら,インスタンスを生成します. + */ + 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 クォータニオンの要素をコピーします. + */ + Quaternion operator=(Quaternion r){ + w = r.w; + x = r.x; + y = r.y; + z = r.z; + return *this; + }; + + /** + @bref クォータニオンを足して代入します. + */ + Quaternion operator+=(Quaternion r){ + w += r.w; + x += r.x; + y += r.y; + z += r.z; + return *this; + }; + + /** + @bref クォータニオンを引いて代入します. + */ + 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 掛け算などを行うたびに実行することをお勧めします. + */ + 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 クォータニオンの掛け算をします.この際,順序が重要です. +*/ +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; + /*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; +}; +/** +@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; +}; +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 \ No newline at end of file