#ifndef __QUATERNION_CLASS_IMPL_DEFINE__
#define __QUATERNION_CLASS_IMPL_DEFINE__
#include "Quaternion.h"
Quaternion::Quaternion()
{
    w = 0.0f;
    x = 0.0f;
    y = 0.0f;
    z = 0.0f;
}

Quaternion::Quaternion(double w, double x, double y, double z) :w(w), x(x), y(y), z(z)
{

}
// 转换为单位四元数
VOID Quaternion::ToIdentityQuaternion()
{
    w = 1.0f;
    x = 0.0f;
    y = 0.0f;
    z = 0.0f;
}

// 绕任意轴旋转
VOID Quaternion::RotationAboutAnyAxis(VectorInformation& rotateAxis, double angle)
{
    assert(fabs(rotateAxis.SquareVectorDescription() - 1.0f) < 0.01f);

    double halfAngle = HALF_VALUE(angle);
    // 计算sin半角
    double sinHalfAngle = sin(halfAngle);

    w = cos(halfAngle);
    x = rotateAxis.GetX() * sinHalfAngle;
    y = rotateAxis.GetY() * sinHalfAngle;
    z = rotateAxis.GetZ() * sinHalfAngle;
}
double Quaternion::SquareQuaternion()
{
    return sqrt(w * w + x * x + y * y + z * z);
}
double SafeAcos(double angle)
{
    if (angle < -1.0f)
    {
        return PI;
    }
    else if (angle > 1.0F)
    {
        return 0.0f;
    }
    return acos(angle);
}
// 根据四元数计算旋转角
double Quaternion::GetRotationAngle()
{
    double halfAngle = SafeAcos(w);
    return halfAngle * 2.0f;
}
// 根据四元数计算旋转轴
VectorInformation Quaternion::GetRotationAxis()
{
    // 计算sin半角
    double sinHalfAngle = 1.0f - w * w;
    double sqrtHalfAngle = sqrt(sinHalfAngle);
    double nx = x * sqrtHalfAngle;
    double ny = y * sqrtHalfAngle;
    double nz = z * sqrtHalfAngle;
    return VectorInformation(nx, ny, nz);
}

VOID Quaternion::normalize()
{
    double sqrtQuat =this->SquareQuaternion();
    if (sqrtQuat>0)
    {
        // 正常的四元数
        w /= sqrtQuat;
        x /= sqrtQuat;
        y /= sqrtQuat;
        z /= sqrtQuat;
    }
    else
    {
        // 非正常的四元数,断言+转换为单位四元数
        assert(false);
        ToIdentityQuaternion();
    }

}

// 绕xyz轴旋转
VOID Quaternion::RotationAroundXYZAxis(ROTATE_TYPE rotateType, double angle)
{
    double halfAngle = HALF_VALUE(angle);
    // w固定为cos半角
    w = cos(halfAngle);

    switch (rotateType)
    {
    case ROTATE_TYPE::ROTATE_X: {
        x = sin(halfAngle);
        y = z = 0.0f;
        break;
    }
    case ROTATE_TYPE::ROTATE_Y: {
        y = sin(halfAngle);
        x = z = 0.0f;
        break;
    }
    case ROTATE_TYPE::ROTATE_Z: {
        z = sin(halfAngle);
        x = y = 0.0f;
        break;
    }
    default:
        assert(false);
        break;
    }
}

Quaternion operator*(Quaternion& q1, Quaternion& q2)
{
    double w1 = q1.GetW();
    double x1 = q1.GetX();
    double y1 = q1.GetY();
    double z1 = q1.GetZ();
    double w2 = q2.GetW();
    double x2 = q2.GetX();
    double y2 = q2.GetY();
    double z2 = q2.GetZ();

    double newW = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2;
    double newX = w1 * x2 + x1 * w2 + z1 * y2 - y1 * z2;
    double newY = w1 * y2 + y1 * w2 + x1 * z2 - z1 * x2;
    double newZ = w1 * z2 + z1 * w2 + y1 * x2 - x1 * y2;
    return Quaternion(newW, newX, newY, newZ);
}

// 计算四元数的点乘
double QuaternionPointMultiplication(Quaternion& q1, Quaternion& q2)
{
    double w1 = q1.GetW();
    double x1 = q1.GetX();
    double y1 = q1.GetY();
    double z1 = q1.GetZ();
    double w2 = q2.GetW();
    double x2 = q2.GetX();
    double y2 = q2.GetY();
    double z2 = q2.GetZ();
    return w1 * w2+ x1* x2+ y1* y2+ z1* z2;
}

// 计算四元数的共轭
Quaternion QuaternionConjugate(Quaternion& q)
{
    return Quaternion(q.GetW(), -q.GetX(), -q.GetY(), -q.GetZ());
}

Quaternion PowerOfQuaternion(Quaternion& q, double exponent)
{
    if (fabs(q.GetW()) > 0.999999f) {
        return q;
    }
    double alpha1 = acos(q.GetW());
    double alpha2 = exponent * alpha1;

    double m = sin(alpha2) / sin(alpha1);
    double NewW = cos(alpha2);
    double NewX = q.GetX() * m;
    double NewY = q.GetY() * m;
    double NewZ = q.GetZ() * m;
    return Quaternion(NewW, NewX, NewY, NewZ);
}







#endif // !__QUATERNION_CLASS_IMPL_DEFINE__