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 回転軸となる３次元ベクトル
-	*  param   angle 回転角 [rad]
+	*  @param   vec 回転軸となる３次元ベクトル
+	*  @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 回転軸となる３次元ベクトル
 	*  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 ロール，ピッチ，ヨーの順に配列に格納します．３つ以上の要素の配列を入れてください．
 	@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;