/*

-----------------------------------------------------------------------------

This source file is part of OGRE

    (Object-oriented Graphics Rendering Engine)

For the latest info, see http://www.ogre3d.org/

Copyright (c) 2000-2014 Torus Knot Software Ltd

Permission is hereby granted, free of charge, to any person obtaining a copy

of this software and associated documentation files (the "Software"), to deal

in the Software without restriction, including without limitation the rights

to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

copies of the Software, and to permit persons to whom the Software is

furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in

all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN

THE SOFTWARE.

-----------------------------------------------------------------------------

*/

#include "OgreStableHeaders.h"

// NOTE THAT THIS FILE IS BASED ON MATERIAL FROM:

// Geometric Tools, LLC

// Copyright (c) 1998-2010

// Distributed under the Boost Software License, Version 1.0.

// http://www.boost.org/LICENSE_1_0.txt

// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt

namespace Ogre {

    const Real Quaternion::msEpsilon = 1e-03;

    const Quaternion Quaternion::ZERO(0,0,0,0);

    const Quaternion Quaternion::IDENTITY(1,0,0,0);

    //-----------------------------------------------------------------------

    void Quaternion::FromRotationMatrix (const Matrix3& kRot)

    {

        // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes

        // article "Quaternion Calculus and Fast Animation".

        Real fTrace = kRot[0][0]+kRot[1][1]+kRot[2][2];

        Real fRoot;

        if ( fTrace > 0.0 )

        {

            // |w| > 1/2, may as well choose w > 1/2

            fRoot = Math::Sqrt(fTrace + 1.0f);  // 2w

            w = 0.5f*fRoot;

            fRoot = 0.5f/fRoot;  // 1/(4w)

            x = (kRot[2][1]-kRot[1][2])*fRoot;

            y = (kRot[0][2]-kRot[2][0])*fRoot;

            z = (kRot[1][0]-kRot[0][1])*fRoot;

        }

        else

        {

            // |w| <= 1/2

            static size_t s_iNext[3] = { 1, 2, 0 };

            size_t i = 0;

            if ( kRot[1][1] > kRot[0][0] )

                i = 1;

            if ( kRot[2][2] > kRot[i][i] )

                i = 2;

            size_t j = s_iNext[i];

            size_t k = s_iNext[j];

            fRoot = Math::Sqrt(kRot[i][i]-kRot[j][j]-kRot[k][k] + 1.0f);

            Real* apkQuat[3] = { &x, &y, &z };

            *apkQuat[i] = 0.5f*fRoot;

            fRoot = 0.5f/fRoot;

            w = (kRot[k][j]-kRot[j][k])*fRoot;

            *apkQuat[j] = (kRot[j][i]+kRot[i][j])*fRoot;

            *apkQuat[k] = (kRot[k][i]+kRot[i][k])*fRoot;

        }

    }

    //-----------------------------------------------------------------------

    void Quaternion::ToRotationMatrix (Matrix3& kRot) const

    {

        Real fTx  = x+x;

        Real fTy  = y+y;

        Real fTz  = z+z;

        Real fTwx = fTx*w;

        Real fTwy = fTy*w;

        Real fTwz = fTz*w;

        Real fTxx = fTx*x;

        Real fTxy = fTy*x;

        Real fTxz = fTz*x;

        Real fTyy = fTy*y;

        Real fTyz = fTz*y;

        Real fTzz = fTz*z;

        kRot[0][0] = 1.0f-(fTyy+fTzz);

        kRot[0][1] = fTxy-fTwz;

        kRot[0][2] = fTxz+fTwy;

        kRot[1][0] = fTxy+fTwz;

        kRot[1][1] = 1.0f-(fTxx+fTzz);

        kRot[1][2] = fTyz-fTwx;

        kRot[2][0] = fTxz-fTwy;

        kRot[2][1] = fTyz+fTwx;

        kRot[2][2] = 1.0f-(fTxx+fTyy);

    }

    //-----------------------------------------------------------------------

    void Quaternion::FromAngleAxis (const Radian& rfAngle,

        const Vector3& rkAxis)

    {

        // assert:  axis[] is unit length

        //

        // The quaternion representing the rotation is

        //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

        Radian fHalfAngle ( 0.5*rfAngle );

        Real fSin = Math::Sin(fHalfAngle);

        w = Math::Cos(fHalfAngle);

        x = fSin*rkAxis.x;

        y = fSin*rkAxis.y;

        z = fSin*rkAxis.z;

    }

    //-----------------------------------------------------------------------

    void Quaternion::ToAngleAxis (Radian& rfAngle, Vector3& rkAxis) const

    {

        // The quaternion representing the rotation is

        //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

        Real fSqrLength = x*x+y*y+z*z;

        if ( fSqrLength > 0.0 )

        {

            rfAngle = 2.0*Math::ACos(w);

            Real fInvLength = Math::InvSqrt(fSqrLength);

            rkAxis.x = x*fInvLength;

            rkAxis.y = y*fInvLength;

            rkAxis.z = z*fInvLength;

        }

        else

        {

            // angle is 0 (mod 2*pi), so any axis will do

            rfAngle = Radian(0.0);

            rkAxis.x = 1.0;

            rkAxis.y = 0.0;

            rkAxis.z = 0.0;

        }

    }

    //-----------------------------------------------------------------------

    void Quaternion::FromAxes (const Vector3* akAxis)

    {

        Matrix3 kRot;

        for (size_t iCol = 0; iCol < 3; iCol++)

        {

            kRot[0][iCol] = akAxis[iCol].x;

            kRot[1][iCol] = akAxis[iCol].y;

            kRot[2][iCol] = akAxis[iCol].z;

        }

        FromRotationMatrix(kRot);

    }

    //-----------------------------------------------------------------------

    void Quaternion::FromAxes (const Vector3& xaxis, const Vector3& yaxis, const Vector3& zaxis)

    {

        Matrix3 kRot;

        kRot.FromAxes(xaxis, yaxis, zaxis);

        FromRotationMatrix(kRot);

    }

    //-----------------------------------------------------------------------

    void Quaternion::ToAxes (Vector3* akAxis) const

    {

        Matrix3 kRot;

        ToRotationMatrix(kRot);

        for (size_t iCol = 0; iCol < 3; iCol++)

        {

            akAxis[iCol].x = kRot[0][iCol];

            akAxis[iCol].y = kRot[1][iCol];

            akAxis[iCol].z = kRot[2][iCol];

        }

    }

    //-----------------------------------------------------------------------

    Vector3 Quaternion::xAxis(void) const

    {

        //Real fTx  = 2.0*x;

        Real fTy  = 2.0f*y;

        Real fTz  = 2.0f*z;

        Real fTwy = fTy*w;

        Real fTwz = fTz*w;

        Real fTxy = fTy*x;

        Real fTxz = fTz*x;

        Real fTyy = fTy*y;

        Real fTzz = fTz*z;

        return Vector3(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);

    }

    //-----------------------------------------------------------------------

    Vector3 Quaternion::yAxis(void) const

    {

        Real fTx  = 2.0f*x;

        Real fTy  = 2.0f*y;

        Real fTz  = 2.0f*z;

        Real fTwx = fTx*w;

        Real fTwz = fTz*w;

        Real fTxx = fTx*x;

        Real fTxy = fTy*x;

        Real fTyz = fTz*y;

        Real fTzz = fTz*z;

        return Vector3(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);

    }

    //-----------------------------------------------------------------------

    Vector3 Quaternion::zAxis(void) const

    {

        Real fTx  = 2.0f*x;

        Real fTy  = 2.0f*y;

        Real fTz  = 2.0f*z;

        Real fTwx = fTx*w;

        Real fTwy = fTy*w;

        Real fTxx = fTx*x;

        Real fTxz = fTz*x;

        Real fTyy = fTy*y;

        Real fTyz = fTz*y;

        return Vector3(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));

    }

    //-----------------------------------------------------------------------

    void Quaternion::ToAxes (Vector3& xaxis, Vector3& yaxis, Vector3& zaxis) const

    {

        Matrix3 kRot;

        ToRotationMatrix(kRot);

        xaxis.x = kRot[0][0];

        xaxis.y = kRot[1][0];

        xaxis.z = kRot[2][0];

        yaxis.x = kRot[0][1];

        yaxis.y = kRot[1][1];

        yaxis.z = kRot[2][1];

        zaxis.x = kRot[0][2];

        zaxis.y = kRot[1][2];

        zaxis.z = kRot[2][2];

    }

    //-----------------------------------------------------------------------

    Quaternion Quaternion::operator+ (const Quaternion& rkQ) const

    {

        return Quaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);

    }

    //-----------------------------------------------------------------------

    Quaternion Quaternion::operator- (const Quaternion& rkQ) const

    {

        return Quaternion(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);

    }

    //-----------------------------------------------------------------------

    Quaternion Quaternion::operator* (const Quaternion& rkQ) const

    {

        // NOTE:  Multiplication is not generally commutative, so in most

        // cases p*q != q*p.

        return Quaternion

        (

            w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,

            w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,

            w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,

            w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x

        );

    }

    //-----------------------------------------------------------------------

    Quaternion Quaternion::Inverse () const

    {

        Real fNorm = w*w+x*x+y*y+z*z;

        if ( fNorm > 0.0 )

        {

            Real fInvNorm = 1.0f/fNorm;

            return Quaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);

        }

        else

        {

            // return an invalid result to flag the error

            return ZERO;

        }

    }

    //-----------------------------------------------------------------------

    Quaternion Quaternion::UnitInverse () const

    {

        // assert:  'this' is unit length

        return Quaternion(w,-x,-y,-z);

    }

    //-----------------------------------------------------------------------

    Quaternion Quaternion::Exp () const

    {

        // If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then

        // exp(q) = e^w(cos(A)+sin(A)*(x*i+y*j+z*k)).  If sin(A) is near zero,

        // use exp(q) = e^w(cos(A)+(x*i+y*j+z*k)) since sin(A)/A has limit 1.

        Radian fAngle ( Math::Sqrt(x*x+y*y+z*z) );

        Real fSin = Math::Sin(fAngle);

    Real fExpW = Math::Exp(w);

        Quaternion kResult;

        kResult.w = fExpW*Math::Cos(fAngle);

        if ( Math::Abs(fAngle.valueRadians()) >= msEpsilon )

        {

            Real fCoeff = fExpW*(fSin/(fAngle.valueRadians()));

            kResult.x = fCoeff*x;

            kResult.y = fCoeff*y;

            kResult.z = fCoeff*z;

        }

        else

        {

            kResult.x = fExpW*x;

            kResult.y = fExpW*y;

            kResult.z = fExpW*z;

        }

        return kResult;

    }

    //-----------------------------------------------------------------------

    Quaternion Quaternion::Log () const

    {

        // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then

        // log(q) = (A/sin(A))*(x*i+y*j+z*k).  If sin(A) is near zero, use

        // log(q) = (x*i+y*j+z*k) since A/sin(A) has limit 1.

        Quaternion kResult;

        kResult.w = 0.0;

        if ( Math::Abs(w) < 1.0 )

        {

            // According to Neil Dantam, atan2 has the best stability.

            // http://www.neil.dantam.name/note/dantam-quaternion.pdf

            Real fNormV = Math::Sqrt(x*x + y*y + z*z);

            Radian fAngle ( Math::ATan2(fNormV, w) );

            Real fSin = Math::Sin(fAngle);

            if ( Math::Abs(fSin) >= msEpsilon )

            {

                Real fCoeff = fAngle.valueRadians()/fSin;

                kResult.x = fCoeff*x;

                kResult.y = fCoeff*y;

                kResult.z = fCoeff*z;

                return kResult;

            }

        }

        kResult.x = x;

        kResult.y = y;

        kResult.z = z;

        return kResult;

    }

    //-----------------------------------------------------------------------

    Vector3 Quaternion::operator* (const Vector3& v) const

    {

    // nVidia SDK implementation

    Vector3 uv, uuv;

    Vector3 qvec(x, y, z);

    uv = qvec.crossProduct(v);

    uuv = qvec.crossProduct(uv);

    uv *= (2.0f * w);

    uuv *= 2.0f;

    return v + uv + uuv;

    }

    //-----------------------------------------------------------------------

    Quaternion Quaternion::Slerp (Real fT, const Quaternion& rkP,

        const Quaternion& rkQ, bool shortestPath)

    {

        Real fCos = rkP.Dot(rkQ);

        Quaternion rkT;

        // Do we need to invert rotation?

        if (fCos < 0.0f && shortestPath)

        {

            fCos = -fCos;

            rkT = -rkQ;

        }

        else

        {

            rkT = rkQ;

        }

        if (Math::Abs(fCos) < 1 - msEpsilon)

        {

            // Standard case (slerp)

            Real fSin = Math::Sqrt(1 - Math::Sqr(fCos));

            Radian fAngle = Math::ATan2(fSin, fCos);

            Real fInvSin = 1.0f / fSin;

            Real fCoeff0 = Math::Sin((1.0f - fT) * fAngle) * fInvSin;

            Real fCoeff1 = Math::Sin(fT * fAngle) * fInvSin;

            return fCoeff0 * rkP + fCoeff1 * rkT;

        }

        else

        {

            // There are two situations:

            // 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear

            //    interpolation safely.

            // 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there

            //    are an infinite number of possibilities interpolation. but we haven't

            //    have method to fix this case, so just use linear interpolation here.

            Quaternion t = (1.0f - fT) * rkP + fT * rkT;

            // taking the complement requires renormalisation

            t.normalise();

            return t;

        }

    }

    //-----------------------------------------------------------------------

    Quaternion Quaternion::SlerpExtraSpins (Real fT,

        const Quaternion& rkP, const Quaternion& rkQ, int iExtraSpins)

    {

        Real fCos = rkP.Dot(rkQ);

        Radian fAngle ( Math::ACos(fCos) );

        if ( Math::Abs(fAngle.valueRadians()) < msEpsilon )

            return rkP;

        Real fSin = Math::Sin(fAngle);

        Radian fPhase ( Math::PI*iExtraSpins*fT );

        Real fInvSin = 1.0f/fSin;

        Real fCoeff0 = Math::Sin((1.0f-fT)*fAngle - fPhase)*fInvSin;

        Real fCoeff1 = Math::Sin(fT*fAngle + fPhase)*fInvSin;

        return fCoeff0*rkP + fCoeff1*rkQ;

    }

    //-----------------------------------------------------------------------

    void Quaternion::Intermediate (const Quaternion& rkQ0,

        const Quaternion& rkQ1, const Quaternion& rkQ2,

        Quaternion& rkA, Quaternion& rkB)

    {

        // assert:  q0, q1, q2 are unit quaternions

        Quaternion kQ0inv = rkQ0.UnitInverse();

        Quaternion kQ1inv = rkQ1.UnitInverse();

        Quaternion rkP0 = kQ0inv*rkQ1;

        Quaternion rkP1 = kQ1inv*rkQ2;

        Quaternion kArg = 0.25*(rkP0.Log()-rkP1.Log());

        Quaternion kMinusArg = -kArg;

        rkA = rkQ1*kArg.Exp();

        rkB = rkQ1*kMinusArg.Exp();

    }

    //-----------------------------------------------------------------------

    Quaternion Quaternion::Squad (Real fT,

        const Quaternion& rkP, const Quaternion& rkA,

        const Quaternion& rkB, const Quaternion& rkQ, bool shortestPath)

    {

        Real fSlerpT = 2.0f*fT*(1.0f-fT);

        Quaternion kSlerpP = Slerp(fT, rkP, rkQ, shortestPath);

        Quaternion kSlerpQ = Slerp(fT, rkA, rkB);

        return Slerp(fSlerpT, kSlerpP ,kSlerpQ);

    }

    //-----------------------------------------------------------------------

  Radian Quaternion::getRoll(bool reprojectAxis) const

  {

    if (reprojectAxis)

    {

      // roll = atan2(localx.y, localx.x)

      // pick parts of xAxis() implementation that we need

//      Real fTx  = 2.0*x;

      Real fTy  = 2.0f*y;

      Real fTz  = 2.0f*z;

      Real fTwz = fTz*w;

      Real fTxy = fTy*x;

      Real fTyy = fTy*y;

      Real fTzz = fTz*z;

      // Vector3(1.0-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);

      return Radian(Math::ATan2(fTxy+fTwz, 1.0f-(fTyy+fTzz)));

    }

    else

    {

      return Radian(Math::ATan2(2*(x*y + w*z), w*w + x*x - y*y - z*z));

    }

  }

    //-----------------------------------------------------------------------

  Radian Quaternion::getPitch(bool reprojectAxis) const

  {

    if (reprojectAxis)

    {

      // pitch = atan2(localy.z, localy.y)

      // pick parts of yAxis() implementation that we need

      Real fTx  = 2.0f*x;

//      Real fTy  = 2.0f*y;

      Real fTz  = 2.0f*z;

      Real fTwx = fTx*w;

      Real fTxx = fTx*x;

      Real fTyz = fTz*y;

      Real fTzz = fTz*z;

      // Vector3(fTxy-fTwz, 1.0-(fTxx+fTzz), fTyz+fTwx);

      return Radian(Math::ATan2(fTyz+fTwx, 1.0f-(fTxx+fTzz)));

    }

    else

    {

      // internal version

      return Radian(Math::ATan2(2*(y*z + w*x), w*w - x*x - y*y + z*z));

    }

  }

    //-----------------------------------------------------------------------

  Radian Quaternion::getYaw(bool reprojectAxis) const

  {

    if (reprojectAxis)

    {

      // yaw = atan2(localz.x, localz.z)

      // pick parts of zAxis() implementation that we need

      Real fTx  = 2.0f*x;

      Real fTy  = 2.0f*y;

      Real fTz  = 2.0f*z;

      Real fTwy = fTy*w;

      Real fTxx = fTx*x;

      Real fTxz = fTz*x;

      Real fTyy = fTy*y;

      // Vector3(fTxz+fTwy, fTyz-fTwx, 1.0-(fTxx+fTyy));

      return Radian(Math::ATan2(fTxz+fTwy, 1.0f-(fTxx+fTyy)));

    }

    else

    {

      // internal version

      return Radian(Math::ASin(-2*(x*z - w*y)));

    }

  }

    //-----------------------------------------------------------------------

    Quaternion Quaternion::nlerp(Real fT, const Quaternion& rkP,

        const Quaternion& rkQ, bool shortestPath)

    {

    Quaternion result;

        Real fCos = rkP.Dot(rkQ);

    if (fCos < 0.0f && shortestPath)

    {

      result = rkP + fT * ((-rkQ) - rkP);

    }

    else

    {

      result = rkP + fT * (rkQ - rkP);

    }

        result.normalise();

        return result;

    }

    std::ostream& operator<<(std::ostream& o, const Quaternion& q)

    {

        return o << "Quaternion(" << q.w << ", " << q.x << ", " << q.y << ", " << q.z << ")";

    }

}