////////////////////////////////////////////////////////
//  四元数クラス ソースファイル
//  Version 1.0 , 2008/3/30 , by Li Cheng
////////////////////////////////////////////////////////
#include "quaternion.h"

////////////////////////////////////////////////////////
// 3次元ベクトルクラス
////////////////////////////////////////////////////////

// ***** コンストラクタ
vector_3D::vector_3D()
{
	x = 0;
	y = 0;
	z = 0;
}
vector_3D::vector_3D( double x_, double y_, double z_ )
{
	x = x_;
	y = y_;
	z = z_;
}

// ***** 演算子
// 代入 =
void vector_3D::operator =  (vector_3D V)
{
	x = V.x;
	y = V.y;
	z = V.z;
}
// 等号 ==
bool vector_3D::operator == (vector_3D V)
{
	return ( x == V.x && y == V.y && z == V.z );
}
// 加算 +
vector_3D vector_3D::operator +  (vector_3D V)
{
	return vector_3D(x + V.x, y + V.y, z + V.z);
}
// 減算 -
vector_3D vector_3D::operator -  (vector_3D V)
{
	return vector_3D(x - V.x, y - V.y, z - V.z);
}
// 係数倍 *
vector_3D vector_3D::operator *  (double k)
{
	return vector_3D( k*x, k*y, k*z );
}

// ***** スカラー積とベクトル積
// スカラー積
double DotP(vector_3D a, vector_3D b)
{
	return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
}

// ベクトル積
vector_3D CrossP(vector_3D a, vector_3D b){
	vector_3D v;
	v.x = (a.y * b.z) - (a.z * b.y);
	v.y = (a.z * b.x) - (a.x * b.z);
	v.z = (a.x * b.y) - (a.y * b.x);
	return v;
}

////////////////////////////////////////////////////////
// 四元数クラス
////////////////////////////////////////////////////////

// ***** コンストラクタ
quaternion::quaternion()
{
	w = 0;
	x = 0;
	y = 0;
	z = 0;
}
quaternion::quaternion(double w_, double x_, double y_, double z_)
{
	w = w_;
	x = x_;
	y = y_;
	z = z_;
}
quaternion::quaternion(double w_, vector_3D V_)
{
	w = w_;
	x = V_.x;
	y = V_.y;
	z = V_.z;
}

// ***** 演算子
// 代入 =
void quaternion::operator = (quaternion Q)
{
	w = Q.w;
	x = Q.x;
	y = Q.y;
	z = Q.z;
}
// 等号 ==
bool quaternion::operator == (quaternion Q)
{
	return ( w == Q.w && x == Q.x && y == Q.y && z == Q.z );
}
// 加算 +
quaternion quaternion::operator + (quaternion Q)
{
	return quaternion(w + Q.w, x + Q.x, y + Q.y, z + Q.z);
}
// 減算 -
quaternion quaternion::operator - (quaternion Q)
{
	return quaternion(w - Q.w, x - Q.x, y - Q.y, z - Q.z);
}
// 乗算 *
quaternion quaternion::operator * (quaternion Q)
{
	// q1 = w1 + V1
	double    w1 = w;
	vector_3D V1(x,y,z);
	
	// q2 = w2 + V2
	double    w2 = Q.w;
	vector_3D V2(Q.x,Q.y,Q.z);
	
	// q1 * q2 = (w1*w2 - V1・V2) + (w1*V2 + w2*V1 + V1×V2)
	double    wx = w1*w2 - DotP(V1,V2);
	vector_3D Vx = V2*w1 + V1*w2 + CrossP(V1,V2);
	return quaternion(wx, Vx);
}
// 係数倍 *
quaternion quaternion::operator * (double k)
{
	return quaternion( k*w, k*x, k*y, k*z );
}

// ***** 四元共役
quaternion quaternion::cq ()
{
	return quaternion( w, -x, -y, -z );
}

// ***** 実部の虚部の設定/取得
// 実部の設定
void quaternion::SetReal( double w_ )
{
	w = w_;
}
// 虚部の設定
void quaternion::SetImage( double x_, double y_, double z_ )
{
	x = x_;
	y = y_;
	z = z_;
}
void quaternion::SetImage( vector_3D V_ )
{
	x = V_.x;
	y = V_.y;
	z = V_.z;
}
// 実部の取得
double quaternion::Real()
{
	return w;
}
// 虚部の取得
vector_3D quaternion::Image()
{
	return vector_3D(x,y,z);
}
