///////////////////////////////////////////////////////////////////////////////
//  CDualQuaternion.cpp

#include "CDualQuaternion.h"
#include "S3D.h"
#include "SMatrix.h"

namespace math3d {

CDualQuaternion::CDualQuaternion() :
	m_RealPart(0.0f, 0.0f, 0.0f, 1.0f),
	m_DualPart(0.0f, 0.0f, 0.0f, 0.0f)
{
}

CDualQuaternion::CDualQuaternion(const SQuaternion& RealPart, const SQuaternion& DualPart) :
	m_RealPart(RealPart),
	m_DualPart(DualPart)
{
}

CDualQuaternion::CDualQuaternion(const SQuaternion& Rotate, const S3D& Move) :
	m_RealPart(Rotate),
	m_DualPart(
		0.5f * (Move.fX * Rotate.fW + Move.fY * Rotate.fZ - Move.fZ * Rotate.fY),
		0.5f * (-Move.fX * Rotate.fZ + Move.fY * Rotate.fW + Move.fZ * Rotate.fX),
		0.5f * (Move.fX * Rotate.fY - Move.fY * Rotate.fX + Move.fZ * Rotate.fW),
		-0.5f * (Move.fX * Rotate.fX + Move.fY * Rotate.fY + Move.fZ * Rotate.fZ))
{
}

CDualQuaternion::~CDualQuaternion()
{
}

CDualQuaternion CDualQuaternion::operator+(const CDualQuaternion& Obj) const
{
	return CDualQuaternion(m_RealPart + Obj.m_RealPart, m_DualPart * Obj.m_DualPart);
}

CDualQuaternion CDualQuaternion::operator*(const CDualQuaternion& Obj) const
{
	return CDualQuaternion(
		Obj.m_RealPart * m_RealPart,
		Obj.m_RealPart * m_DualPart + Obj.m_DualPart * m_RealPart);
}

CDualQuaternion& CDualQuaternion::operator*=(const CDualQuaternion& Obj)
{
	*this = *this * Obj;

	return *this;
}

//
CDualQuaternion CDualQuaternion::GetInverse() const
{
	const SQuaternion InvReal = m_RealPart.GetInverse();

	const float rd = -2.0f * (m_RealPart.fX * m_DualPart.fX + m_RealPart.fY * m_DualPart.fY +
		m_RealPart.fZ * m_DualPart.fZ + m_RealPart.fW * m_DualPart.fW);

	return CDualQuaternion(m_RealPart.GetInverse(), m_DualPart.GetInverse() +
		SQuaternion(InvReal.fX * rd, InvReal.fY * rd, InvReal.fZ * rd, InvReal.fW * rd));
}

// 実部
const SQuaternion& CDualQuaternion::GetRealPart() const
{
	return m_RealPart;
}

// 非実部
const SQuaternion& CDualQuaternion::GetDualPart() const
{
	return m_DualPart;
}

// 行列
SMatrix CDualQuaternion::GetMatrix() const
{
	const float fXX = m_RealPart.fX * m_RealPart.fX;
	const float fYY = m_RealPart.fY * m_RealPart.fY;
	const float fZZ = m_RealPart.fZ * m_RealPart.fZ;

	const float fXY = m_RealPart.fX * m_RealPart.fY;
	const float fXZ = m_RealPart.fX * m_RealPart.fZ;
	const float fXW = m_RealPart.fX * m_RealPart.fW;
	const float fYZ = m_RealPart.fY * m_RealPart.fZ;
	const float fYW = m_RealPart.fY * m_RealPart.fW;
	const float fZW = m_RealPart.fZ * m_RealPart.fW;

	return SMatrix(
		1.0f - 2.0f * (fYY + fZZ),		// 00
		2.0f * (fXY + fZW),		// 10
		2.0f * (fXZ - fYW),		// 20
		0.0f,

		2.0f * (fXY - fZW),		// 01
		1.0f - 2.0f * (fZZ + fXX),		// 11
		2.0f * (fYZ + fXW),		// 21
		0.0f,

		2.0f * (fXZ + fYW),		// 02
		2.0f * (fYZ - fXW),		// 12
		1.0f - 2.0f * (fYY + fXX),		// 22
		0.0f,

		2.0f * (-m_DualPart.fW * m_RealPart.fX + m_RealPart.fW * m_DualPart.fX - m_DualPart.fY * m_RealPart.fZ + m_RealPart.fY * m_DualPart.fZ),
		2.0f * (-m_DualPart.fW * m_RealPart.fY + m_RealPart.fZ * m_DualPart.fX - m_DualPart.fZ * m_RealPart.fX + m_RealPart.fW * m_DualPart.fY),
		2.0f * (-m_DualPart.fW * m_RealPart.fZ + m_RealPart.fX * m_DualPart.fY + m_DualPart.fZ * m_RealPart.fW - m_RealPart.fY * m_DualPart.fX),
		1.0f);							// 33
}

}