PQ_Hybrid_Electrical_Equipment_Team
四元数(クォータニオン)の計算やらなんやらができます．主に演算子オーバーロードの練習で作りました．温かい目で見てやってください．
Dependencies: Vector3
Dependents: Hybrid_main_FirstEdtion MadgwickFilter MadgwickFilter
Fork of
Quaternion
by 
Gaku Matsumoto
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 |
--- 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 ロール，ピッチ，ヨーの順に配列に格納します．３つ以上の要素の配列を入れてください．
+ @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 クォータニオンの足し算をします．
--- 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