四元数(クォータニオン)の計算やらなんやらができます.主に演算子オーバーロードの練習で作りました.温かい目で見てやってください.
Dependencies: Vector3
Dependents: Hybrid_main_FirstEdtion MadgwickFilter MadgwickFilter
Fork of Quaternion by
Revision 8:0b4374931b0e, committed 2018-02-25
- Comitter:
- Gaku0606
- Date:
- Sun Feb 25 19:44:47 2018 +0000
- Parent:
- 7:631c068aded7
- Commit message:
- double -> float;
Changed in this revision
Quaternion.hpp | Show annotated file Show diff for this revision Revisions of this file |
Vector3.lib | Show annotated file Show diff for this revision Revisions of this file |
diff -r 631c068aded7 -r 0b4374931b0e Quaternion.hpp --- a/Quaternion.hpp Sat Dec 16 02:39:42 2017 +0000 +++ b/Quaternion.hpp Sun Feb 25 19:44:47 2018 +0000 @@ -1,6 +1,6 @@ #ifndef _QUATERNION_HPP_ #define _QUATERNION_HPP_ -#include "Vector3.hpp" +#include "Vector3/Vector3.hpp" /** * クォータニオンの足し,引き,掛け算などを簡単にできるようになります. * @author Gaku MATSUMOTO @@ -158,17 +158,17 @@ vec.Normalize(); float halfAngle = 0.5f * angle ; - w = cos(halfAngle); - x = vec.x * sin(halfAngle); - y = vec.y * sin(halfAngle); - z = vec.z * sin(halfAngle); + w = cosf(halfAngle); + x = vec.x * sinf(halfAngle); + y = vec.y * sinf(halfAngle); + z = vec.z * sinf(halfAngle); } /** * @bref クォータニオンの各要素に配列のようにアクセスします */ - double q(int i){ - double ans = 0.0; + float q(int i){ + float ans = 0.0; switch (i){ case 1: ans = w; @@ -189,8 +189,8 @@ /** * @bref クォータニオンのノルムを計算します */ - double Norm(){ - return fabs(w*w + x*x + y*y + z*z); + float Norm(){ + return fabsf(w*w + x*x + y*y + z*z); } @@ -223,9 +223,21 @@ */ 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)); + val[0] = (atan2f(2.0f * (w * x + y * z), q0q0 - q1q1q2q2 + q3q3)); + val[1] = (-asinf(2.0f * (x * z - w * y))); + val[2] = (atan2f(2.0f * (x * y + w * z), q0q0 + q1q1q2q2 - q3q3)); + } + + /** + @bref オイラー角で姿勢を取得します. + @param val ロール,ピッチ,ヨーの順に配列に格納します.3つ以上の要素の配列を入れてください. + @note 値は[rad]です.[degree]に変換が必要な場合は別途計算して下さい. + */ + void GetEulerAngle(Vector3 *v) { + float q0q0 = w * w, q1q1q2q2 = x * x - y * y, q3q3 = z * z; + v->x = (atan2f(2.0f * (w * x + y * z), q0q0 - q1q1q2q2 + q3q3)); + v->y = (-asinf(2.0f * (x * z - w * y))); + v->z = (atan2f(2.0f * (x * y + w * z), q0q0 + q1q1q2q2 - q3q3)); } /** @@ -250,7 +262,7 @@ static float zz = 0.0f; static float vx = 0.0f, vy = 0.0f, vz = 0.0f; - static float _wx, _wy, _wz, _xy, _xz, _yz; + static float _wx, _wy, _wz, _xy, _zx, _yz; ww = w * w; xx = x * x; yy = y * y; @@ -260,12 +272,12 @@ _wy = w * y; _wz = w * z; _xy = x * y; - _xz = x * z; + _zx = z * x; _yz = y * z; - vx = (ww + xx - yy - zz) * v->x + 2.0f*(_xy - _wz)*v->y + 2.0f*(_xz + _wy) * v->z; + vx = (ww + xx - yy - zz) * v->x + 2.0f*(_xy - _wz)*v->y + 2.0f*(_zx + _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; + vz = 2.0f * (_zx - _wy) * v->x + 2.0f * (_wx + _yz)*v->y + (ww - xx - yy + zz)*v->z; v->x = vx; v->y = vy; @@ -377,6 +389,37 @@ return Q; }; +/** +*/ +Vector3 operator*(Quaternion q, Vector3 v) { + + static Vector3 ans; + 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, _zx, _yz; + ww = q.w * q.w; + xx = q.x * q.x; + yy = q.y * q.y; + zz = q.z * q.z; + + _wx = q.w * q.x; + _wy = q.w * q.y; + _wz = q.w * q.z; + _xy = q.x * q.y; + _zx = q.z * q.x; + _yz = q.y * q.z; + + ans.x = (ww + xx - yy - zz) * v.x + 2.0f*(_xy - _wz)*v.y + 2.0f*(_zx + _wy) * v.z; + ans.y = 2.0f * (_xy + _wz) * v.x + (ww - xx + yy - zz) * v.y + 2.0f*(_yz - _wx)*v.z; + ans.z = 2.0f * (_zx - _wy) * v.x + 2.0f * (_wx + _yz)*v.y + (ww - xx - yy + zz)*v.z; + + return ans; +} + /** @bref クォータニオンの足し算をします.
diff -r 631c068aded7 -r 0b4374931b0e Vector3.lib --- a/Vector3.lib Sat Dec 16 02:39:42 2017 +0000 +++ b/Vector3.lib Sun Feb 25 19:44:47 2018 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/users/Gaku0606/code/Vector3/#3d5d3689521c +https://developer.mbed.org/users/Gaku0606/code/Vector3/#d68f06f4d554