File content as of revision 5:3c531c1f56cc:

#ifndef _QUATERNION_HPP_
#define _QUATERNION_HPP_
#include "Vector3.hpp"
/**
* クォータニオンの足し，引き，掛け算などを簡単にできるようになります．
* @author  Gaku MATSUMOTO
* @bref  クォータニオンを使えるクラスです．
*/

class Quaternion{
public:
	/**
		@bref	Quaternionインスタンスを生成します
	*/
	Quaternion(){
		w = 1.0;
		x = 0.0;
		y = 0.0;
		z = 0.0;
	};

	/**
	*  @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;
	};

public:
	double w;
	double x;
	double y;
	double z;

public:

	/**
	@bref  クォータニオンの要素をコピーします．
	@note  通常の数のように代入できます
	*/
	Quaternion operator=(Quaternion r){
		w = r.w;
		x = r.x;
		y = r.y;
		z = r.z;
		return *this;
	};

	/**
	@bref  クォータニオンを足して代入します．
	@note  通常の数のように代入できます
	*/
	Quaternion operator+=(Quaternion r){
		w += r.w;
		x += r.x;
		y += r.y;
		z += r.z;
		return *this;
	};

	/**
	@bref  クォータニオンを引いて代入します．
	@note  通常の数のように代入できます
	*/
	Quaternion operator-=(Quaternion r){
		w -= r.w;
		x -= r.x;
		y -= r.y;
		z -= r.z;
		return *this;
	};

	/**
	*	@bref	クォータニオンの掛け算をします．
	*	@note	この際も順序は重要です．
	*/
	Quaternion operator*=(Quaternion r){
		static Quaternion QQ;
		QQ.w = w*r.x - 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;
		w = QQ.w;
		x = QQ.x;
		y = QQ.y;
		z = QQ.z;
		return *this;
	};

	/**
		@bref	クォータニオンの複素共役を返します．
		@note	本当はアスタリスクが良かったのですが，ポインタと紛らわしいのでマイナスにしました．
	*/
	Quaternion operator-(){
		Quaternion Q;
		Q.w = w;
		Q.x = -x;
		Q.y = -y;
		Q.z = -z;
		return Q;
	};

	/**
		@bref	クォータニオンを正規化して，単位クォータニオンにします．
		@note	掛け算などを行うたびに実行することをお勧めします．
		@note   ただ、クォータニオンの時間微分は正規化してはいけません
	*/
	void Normalize(){
		double norm = sqrt(w*w + x*x + y*y + z*z);
		if (norm != 0.0){
			w /= norm;
			x /= norm;
			y /= norm;
			z /= norm;
			return;
		}
		else{
			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 クォータニオンの掛け算をします．この際，順序が重要です．
*/
Quaternion operator*(Quaternion l, Quaternion r){
	static Quaternion Q;
	Q.w = l.w*r.w - l.x*r.x - l.y*r.y - l.z*r.z;
	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;
	
	return Q;
};

/**
* @fn Quaternion operator*(double s, Quaternion q)
* @bref クォータニオンをスカラー倍します．．
*/
Quaternion operator*(double s, Quaternion q){
	static Quaternion Q;
	Q.w = q.w * s;
	Q.x = q.x * s;
	Q.y = q.y * s;
	Q.z = q.z * s;
	return Q;
};

/**
* @fn Quaternion operator*(Quaternion q, double s)
* @bref クォータニオンをスカラー倍します．．
*/
Quaternion operator*(Quaternion q, double s){
	static Quaternion Q;
	Q.w = q.w * s;
	Q.x = q.x * s;
	Q.y = q.y * s;
	Q.z = q.z * s;
	return Q;
};


/**
	@bref	クォータニオンの足し算をします．
*/
Quaternion operator+(Quaternion l, Quaternion r){
	static Quaternion Q;
	Q.w = l.w + r.w;
	Q.x = l.x + r.x;
	Q.y = l.y + r.y;
	Q.z = l.z + r.z;
	return Q;
}

/**
	@bref	クォータニオンの引き算をします．
*/
Quaternion operator-(Quaternion l, Quaternion r){
	static Quaternion Q;
	Q.w = l.w - r.w;
	Q.x = l.x - r.x;
	Q.y = l.y - r.y;
	Q.z = l.z - r.z;
	return Q;
}

#endif