Diff: Quaternion.hpp

Revision:

5:3c531c1f56cc

Parent:

4:a914c6c3b74d

Child:

6:0c20582087b1

Child:

7:631c068aded7

--- a/Quaternion.hpp	Sat Jan 28 21:20:28 2017 +0000
+++ b/Quaternion.hpp	Wed Jun 07 00:57:58 2017 +0000
@@ -1,28 +1,49 @@
 #ifndef _QUATERNION_HPP_
 #define _QUATERNION_HPP_
-
-/** 
+#include "Vector3.hpp"
+/**
 * クォータニオンの足し，引き，掛け算などを簡単にできるようになります．
 * @author  Gaku MATSUMOTO
 * @bref  クォータニオンを使えるクラスです．
 */
+
 class Quaternion{
 public:
 	/**
 		@bref	Quaternionインスタンスを生成します
 	*/
 	Quaternion(){
-		w = 1.0f;
-		x = 0.0f;
-		y = 0.0f;
-		z = 0.0f;
+		w = 1.0;
+		x = 0.0;
+		y = 0.0;
+		z = 0.0;
 	};
+
 	/**
-		@bref	要素を代入しながら，インスタンスを生成します．
-		@param[in]  _w  実部wの初期値
-		@param[in]  _x  虚部iの初期値
-		@param[in]  _y  虚部jの初期値
-		@param[in]  _z  虚部kの初期値
+	*  @bref Vector3クラスからクォータニオンを作ります
+	*/
+	Quaternion(Vector3 vector){
+		w = 0.0;
+		x = vector.x;
+		y = vector.y;
+		z = vector.z;
+	}
+
+	/**
+	*  @bref   クォータニオンを回転軸と回転角度によって初期化します。
+	*  param   vec 回転軸となる３次元ベクトル
+	*  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;
@@ -37,8 +58,8 @@
 public:
 
 	/**
-		@bref  クォータニオンの要素をコピーします．
-		@note  通常の数のように代入できます 
+	@bref  クォータニオンの要素をコピーします．
+	@note  通常の数のように代入できます
 	*/
 	Quaternion operator=(Quaternion r){
 		w = r.w;
@@ -49,8 +70,8 @@
 	};
 
 	/**
-		@bref  クォータニオンを足して代入します．
-		@note  通常の数のように代入できます 
+	@bref  クォータニオンを足して代入します．
+	@note  通常の数のように代入できます
 	*/
 	Quaternion operator+=(Quaternion r){
 		w += r.w;
@@ -61,8 +82,8 @@
 	};
 
 	/**
-		@bref  クォータニオンを引いて代入します．
-		@note  通常の数のように代入できます 
+	@bref  クォータニオンを引いて代入します．
+	@note  通常の数のように代入できます
 	*/
 	Quaternion operator-=(Quaternion r){
 		w -= r.w;
@@ -105,8 +126,9 @@
 	/**
 		@bref	クォータニオンを正規化して，単位クォータニオンにします．
 		@note	掛け算などを行うたびに実行することをお勧めします．
+		@note   ただ、クォータニオンの時間微分は正規化してはいけません
 	*/
-	void normalize(){
+	void Normalize(){
 		double norm = sqrt(w*w + x*x + y*y + z*z);
 		if (norm != 0.0){
 			w /= norm;
@@ -119,9 +141,128 @@
 			return;
 		}
 	};
+
+	/**
+	*  @bref   クォータニオンを初期化します
+	*/
+	template <typename T> void set(T _w, T _x, T _y, T _z);
+	/**
+	*  @bref   クォータニオンをVector3クラスで初期化します。
+	*/
+	void set(Vector3 vec);
+
+	/**
+	*  @bref   クォータニオンを回転軸と回転角度によって初期化します。
+	*  param   vec 回転軸となる３次元ベクトル
+	*  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つの３次元ベクトルを一致させるクォータニオンを計算
+	*  @param from 始点となるベクトルのインスタンス
+	*  @param to   終点となるベクトルのインスタンス
+	*/
+	void FromToRotation(Vector3 from, Vector3 to);
+
+	/**
+	@bref   オイラー角で姿勢を取得します．
+	@param  val ロール，ピッチ，ヨーの順に配列に格納します．３つ以上の要素の配列を入れてください．
+	@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));
+	}
+
+
 };
 
-/** 
+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 クォータニオンの掛け算をします．この際，順序が重要です．
 */
@@ -131,15 +272,11 @@
 	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;
 };
 
-/** 
+/**
 * @fn Quaternion operator*(double s, Quaternion q)
 * @bref クォータニオンをスカラー倍します．．
 */
@@ -165,9 +302,9 @@
 	return Q;
 };
 
+
 /**
-* @fn Quaternion operator+(Quaternion l, Quaternion r)
-* @bref クォータニオンの足し算をします．
+	@bref	クォータニオンの足し算をします．
 */
 Quaternion operator+(Quaternion l, Quaternion r){
 	static Quaternion Q;
@@ -179,8 +316,7 @@
 }
 
 /**
-* @fn Quaternion operator-(Quaternion l, Quaternion r)
-* @bref クォータニオンの引き算をします．
+	@bref	クォータニオンの引き算をします．
 */
 Quaternion operator-(Quaternion l, Quaternion r){
 	static Quaternion Q;