四元数(クォータニオン)の計算やらなんやらができます.主に演算子オーバーロードの練習で作りました.温かい目で見てやってください.
Dependencies: Vector3
Dependents: Hybrid_main_FirstEdtion MadgwickFilter MadgwickFilter
Fork of Quaternion by
Diff: Quaternion.hpp
- Revision:
- 7:631c068aded7
- Parent:
- 5:3c531c1f56cc
- Child:
- 8:0b4374931b0e
--- a/Quaternion.hpp Wed Jun 07 00:57:58 2017 +0000 +++ b/Quaternion.hpp Sat Dec 16 02:39:42 2017 +0000 @@ -13,29 +13,26 @@ @bref Quaternionインスタンスを生成します */ Quaternion(){ - w = 1.0; - x = 0.0; - y = 0.0; - z = 0.0; + w = 1.0f; + x = 0.0f; + y = 0.0f; + z = 0.0f; }; /** * @bref Vector3クラスからクォータニオンを作ります */ Quaternion(Vector3 vector){ - w = 0.0; - x = vector.x; - y = vector.y; - z = vector.z; + Set(vector); } /** * @bref クォータニオンを回転軸と回転角度によって初期化します。 - * param vec 回転軸となる3次元ベクトル - * param angle 回転角 [rad] + * @param vec 回転軸となる3次元ベクトル + * @param angle 回転角 [rad] */ - Quaternion(Vector3 vec, double angle){ - set(vec, angle); + Quaternion(Vector3 vec, float angle){ + Set(vec, angle); } /** @@ -45,16 +42,17 @@ @param[in] _y 虚部jの初期値 @param[in] _z 虚部kの初期値 */ - Quaternion(double _w, double _x, double _y, double _z){ + Quaternion(float _w, float _x, float _y, float _z){ w = _w; x = _x; y = _y; z = _z; }; public: - double w; - double x; - double y; - double z; + float w; + float x; + float y; + float z; + public: /** @@ -99,7 +97,7 @@ */ Quaternion operator*=(Quaternion r){ static Quaternion QQ; - QQ.w = w*r.x - x*r.x - y*r.y - z*r.z; + QQ.w = w*r.w - 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; @@ -129,8 +127,8 @@ @note ただ、クォータニオンの時間微分は正規化してはいけません */ void Normalize(){ - double norm = sqrt(w*w + x*x + y*y + z*z); - if (norm != 0.0){ + float norm = sqrt(w*w + x*x + y*y + z*z); + if (norm != 0.0f){ w /= norm; x /= norm; y /= norm; @@ -145,20 +143,20 @@ /** * @bref クォータニオンを初期化します */ - template <typename T> void set(T _w, T _x, T _y, T _z); + template <typename T> void Set(T _w, T _x, T _y, T _z); /** * @bref クォータニオンをVector3クラスで初期化します。 */ - void set(Vector3 vec); + void Set(Vector3 vec); /** * @bref クォータニオンを回転軸と回転角度によって初期化します。 * param vec 回転軸となる3次元ベクトル * param angle 回転角 [rad] */ - void set(Vector3 vec, double angle){ + void Set(Vector3 vec, float angle){ vec.Normalize(); - double halfAngle = angle /= 2.0; + float halfAngle = 0.5f * angle ; w = cos(halfAngle); x = vec.x * sin(halfAngle); @@ -223,18 +221,95 @@ @param val ロール,ピッチ,ヨーの順に配列に格納します.3つ以上の要素の配列を入れてください. @note 値は[rad]です.[degree]に変換が必要な場合は別途計算して下さい. */ - void getEulerAngle(double *val){ - double q0q0 = w * w, q1q1q2q2 = x * x - y * y, q3q3 = z * z; + void GetEulerAngle(float *val){ + float 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 ToVector3(){ + Vector3 vec3(x, y, z); + return vec3; + } + /** + * @bref 3次元ベクトルを回転します + * @param v 回転させたい3次元ベクトルのポインタ + * @note 余計なオブジェクトを作りません + */ + void Rotation(Vector3* v) { + if (v == NULL) return; + static float ww = 0.0f; + static float xx = 0.0f; + static float yy = 0.0f; + static float zz = 0.0f; + + static float vx = 0.0f, vy = 0.0f, vz = 0.0f; + static float _wx, _wy, _wz, _xy, _xz, _yz; + ww = w * w; + xx = x * x; + yy = y * y; + zz = z * z; + + _wx = w * x; + _wy = w * y; + _wz = w * z; + _xy = x * y; + _xz = x * z; + _yz = y * z; + + vx = (ww + xx - yy - zz) * v->x + 2.0f*(_xy - _wz)*v->y + 2.0f*(_xz + _wy) * v->z; + vy = 2.0f * (_xy + _wz) * v->x + (ww - xx + yy - zz) * v->y + 2.0f*(_yz - _wx)*v->z; + vz = 2.0f * (_xz - _wy) * v->x + 2.0f * (_wx + _yz)*v->y + (ww - xx - yy + zz)*v->z; + + v->x = vx; + v->y = vy; + v->z = vz; + } + + /** + * @bref 3次元ベクトルを回転します.ただし逆回転です + * @param v 回転させたい3次元ベクトルのポインタ + * @note 余計なオブジェクトを作りません + */ + void InvRotation(Vector3* v) { + if (v == NULL) return; + static float ww = 0.0f; + static float xx = 0.0f; + static float yy = 0.0f; + static float zz = 0.0f; + + static float vx = 0.0f, vy = 0.0f, vz = 0.0f; + static float _wx, _wy, _wz, _xy, _xz, _yz; + ww = w * w; + xx = x * x; + yy = y * y; + zz = z * z; + + _wx = w * x; + _wy = w * y; + _wz = w * z; + _xy = x * y; + _xz = x * z; + _yz = y * z; + + vx = (ww + xx - yy - zz) * v->x + 2.0f*(_xy + _wz)*v->y + 2.0f*(_xz - _wy) * v->z; + vy = 2.0f * (_xy - _wz) * v->x + (ww - xx + yy - zz) * v->y + 2.0f*(_yz + _wx)*v->z; + vz = 2.0f * (_xz + _wy) * v->x + 2.0f * (-_wx + _yz)*v->y + (ww - xx - yy + zz)*v->z; + + v->x = vx; + v->y = vy; + v->z = vz; + } }; void Quaternion::FromToRotation(Vector3 from, Vector3 to){ - double halfTheta = from.Angle(to) / 2.0;//回転角度 0からpi/2 + float halfTheta = 0.5f * from.Angle(to);//回転角度 0からpi/2 Vector3 axis = from * to; axis.Normalize(); @@ -246,7 +321,7 @@ -template<typename T>void Quaternion::set(T _w, T _x, T _y, T _z){ +template<typename T>void Quaternion::Set(T _w, T _x, T _y, T _z){ w = _w; x = _x; y = _y; @@ -254,7 +329,7 @@ return; } -void Quaternion::set(Vector3 vec){ +void Quaternion::Set(Vector3 vec){ w = 0.0; x = vec.x; y = vec.y; @@ -278,9 +353,9 @@ /** * @fn Quaternion operator*(double s, Quaternion q) -* @bref クォータニオンをスカラー倍します.. +* @bref クォータニオンをスカラー倍します. */ -Quaternion operator*(double s, Quaternion q){ +Quaternion operator*(float s, Quaternion q){ static Quaternion Q; Q.w = q.w * s; Q.x = q.x * s; @@ -291,9 +366,9 @@ /** * @fn Quaternion operator*(Quaternion q, double s) -* @bref クォータニオンをスカラー倍します.. +* @bref クォータニオンをスカラー倍します. */ -Quaternion operator*(Quaternion q, double s){ +Quaternion operator*(Quaternion q, float s){ static Quaternion Q; Q.w = q.w * s; Q.x = q.x * s;