/* 
 * Functionality for Slerps/Spherical Linear Interpolations 
 *
 * Data Structures for Slerps/Spherical Linear Interpolations
 * See also GemsIII/quatspin.c in
 * http://www.acm.org/pubs/tog/GraphicsGems/gemsiii.zip
 *
 *
 * Copyright (c) Criterion Software Limited
 */

/** 
 * \ingroup rtslerp
 * \page rtslerpoverview RtSlerp Toolkit Overview
 *
 * The RtSlerp Toolkit provides support for interpolating rotations between 
 * two matrices.
 *
 * After creating an RtSlerp object, the developer then specifies the start 
 * and end matrices for the interpolator to work on. 
 *
 * Once set, the developer can then specify an interpolation value and obtain 
 * a new matrix generated by interpolating between the supplied start and end 
 * matrices.
 *
 */

/***************************************************************************
 *                                                                         *
 * Module  : rtslerp.c                                                     *
 *                                                                         *
 * Purpose : Spherical, linear interpolations between matrices,            *
 * and between quaternions.                                                *
 *                                                                         *
 **************************************************************************/

/****************************************************************************
 Includes
 */

#include <stdlib.h>
#include <math.h>

#include "rpplugin.h"
#include "rpdbgerr.h"
#include "rtslerp.h"

#if (!defined(DOXYGEN))
static const char   rcsid[] __RWUNUSED__ =
    "@@(#)$Id: rtslerp.c,v 1.31 2001/03/02 12:00:40 johns Exp $";
#endif /* (!defined(DOXYGEN)) */

/****************************************************************************
 Local Defines
 */

#define RwIEEECosecAcutefMacro(result, x)       \
MACRO_START                                     \
{                                               \
    RwSinMinusPiToPiMacro(result, x);            \
    result = ((RwReal)1) / (result);            \
}                                               \
MACRO_STOP

#if (defined(RWDEBUG) && defined(RWVERBOSE))
#define SLERPMESSAGE(_x)     RWMESSAGE(_x)
#endif /* (defined(RWDEBUG) && defined(RWVERBOSE)) */

#if (!defined(SLERPMESSAGE))
#define SLERPMESSAGE(_x)       /* do nothing */
#endif /* (!defined(SLERPMESSAGE)) */

/****************************************************************************
 Public Globals
 */

#if (defined(RWDEBUG))
long                rtSlerpStackDepth = 0;
#endif /* (defined(RWDEBUG)) */

/****************************************************************************
 Local Types
 */

/****************************************************************************
 Local (Static) Prototypes
 */

/****************************************************************************
 Defines
 */
#define _EPSILON          ((RwReal)(0.00001))
#define _TOL_CACHE        ((RwReal)0.01)
#define _TOL_COS_PI       (_EPSILON - ((RwReal)1))
#define _TOL_COS_ZERO     (((RwReal)1) - _EPSILON)

/****************************************************************************
 Globals (across program)
 */

/****************************************************************************
 Local (static) Globals
 */

/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

                     Slerp and lerp functions

 !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */

/**
 * \ingroup rtslerp
 * \ref RtSlerpCreate is used to create a new spherical linear interpolator, 
 * or SLERP, for interpolating rotation matrices.  The matrix reference mask 
 * specifies whether the start and end matrices are created internally or are 
 * referenced externally; this effects how the SLERP is initialized.
 *
 * The include file rtslerp.h and the library file rtslerp.lib are required to 
 * use this function.
 *
 * \param nMatRefMask  flags specifying whether to copy or reference the matrices.
 * \return pointer to Slerp data-structure on success; NULL pointer otherwise
 *
 * \see RtSlerpInitialize
 * \see RtSlerpDestroy
 * \see RtSlerpGetMatrix
 * \see RtSlerpSetLerp
 */
RtSlerp            *
RtSlerpCreate(RwInt32 nMatRefMask)
{
    RtSlerp            *spSlerp;

    RWAPIFUNCTION(RWSTRING("RtSlerpCreate"));

    /* Allocate slerp */
    spSlerp = (RtSlerp *) RwMalloc(sizeof(RtSlerp));
    if (!spSlerp)
    {
        RWERROR((E_RW_NOMEM, sizeof(RtSlerp)));
        RWRETURN((RtSlerp *) NULL);
    }

    spSlerp->matRefMask = nMatRefMask;

    /* Is the start matrix stored by reference or should we create one? */
    if (spSlerp->matRefMask & rtSLERPREFSTARTMAT)
    {
        spSlerp->startMat = (RwMatrix *) NULL;
    }
    else
    {
        spSlerp->startMat = RwMatrixCreate();
    }

    /* Is the end matrix stored by reference or should we create one? */
    if (spSlerp->matRefMask & rtSLERPREFENDMAT)
    {
        spSlerp->endMat = (RwMatrix *) NULL;
    }
    else
    {
        spSlerp->endMat = RwMatrixCreate();
    }

    RWRETURN(spSlerp);
}

/**
 * \ingroup rtslerp
 * \ref RtSlerpInitialize initializes a RtSlerp struct with two matrices
 * 
 * \param spSlerp  the target RtSlerp to initialize
 * \param mpMat1  the initial matrix
 * \param mpMat2  the final matrix
 *
 * \return pointer to Slerp data-structure on success; NULL pointer otherwise
 *
 * \see RtSlerpCreate
 * \see RtSlerpDestroy
 * \see RtSlerpGetMatrix
 * \see RtSlerpSetLerp
 */
RtSlerp            *
RtSlerpInitialize(RtSlerp * spSlerp,
                  RwMatrix * mpMat1, RwMatrix * mpMat2)
{
    RwV3d               vCentre;
    RwMatrix            mScratch;

    RWAPIFUNCTION(RWSTRING("RtSlerpInitialize"));
    RWASSERT(spSlerp);
    RWASSERT(mpMat1);
    RWASSERT(mpMat2);

    RwMatrixSetIdentity(&mScratch);

    /* Get matrices */
    if (spSlerp->matRefMask & rtSLERPREFSTARTMAT)
    {
        spSlerp->startMat = mpMat1;
    }
    else
    {
        RwMatrixCopy(spSlerp->startMat, mpMat1);
    }

    if (spSlerp->matRefMask & rtSLERPREFENDMAT)
    {
        spSlerp->endMat = mpMat2;
    }
    else
    {
        RwMatrixCopy(spSlerp->endMat, mpMat2);
    }

    /* Create the transform matrix between the two matrices */
    RwMatrixInvert(&mScratch, spSlerp->startMat);
    RwMatrixTransform(&mScratch, spSlerp->endMat, rwCOMBINEPOSTCONCAT);

    /* Now pull out the axis and angle from this matrix */
    RwMatrixQueryRotate(&mScratch, &spSlerp->axis, &spSlerp->angle,
                        &vCentre);

    /* If the angle is small then just lerp by default */
    spSlerp->useLerp = (spSlerp->angle < (RwReal) (2.0));

    /* Return the results */
    RWRETURN(spSlerp);
}

/**
 * \ingroup rtslerp
 * \ref RtSlerpDestroy frees resources used by a RtSlerp data-structure,
 * including any  matrices which it got by copying (as opposed to referencing)
 *
 * \param spSlerp  pointer to the data-structure
 *
 * \see RtSlerpCreate
 * \see RtSlerpInitialize
 * \see RtSlerpGetMatrix
 * \see RtSlerpSetLerp
 */
void
RtSlerpDestroy(RtSlerp * spSlerp)
{
    RWAPIFUNCTION(RWSTRING("RtSlerpDestroy"));

    /* Assert that we have a pointer */
    RWASSERT(spSlerp);

    /* Destroy matrices */
    if (!(spSlerp->matRefMask & rtSLERPREFSTARTMAT))
    {
        RwMatrixDestroy(spSlerp->startMat);
    }
    if (!(spSlerp->matRefMask & rtSLERPREFENDMAT))
    {
        RwMatrixDestroy(spSlerp->endMat);
    }

    RwFree(spSlerp);

    RWRETURNVOID();
}

/**
 * \ingroup rtslerp
 * \ref RtSlerpGetMatrix is used to interpolate between the start and 
 * end matrices of the specified SLERP using the given interpolation value.  
 * An interpolation value of zero will produce the start matrix while a value 
 * of one gives the end matrix.
 *
 * The include file rtslerp.h and the library file rtslerp.lib are required 
 * to use this function.
 * 
 * \param spSlerp  pointer to an input slerp data-structure
 * \param mpResultMat  pointer to an output matrix
 * \param nDelta  value between 0.0 and 1.0 where
 *        \li 0.0    denotes the initial  matrix and
 *        \li 1.0    denotes the final matrix
 *
 * \return pointer to the matrix on success; NULL pointer otherwise
 *
 * \see RtSlerpCreate
 * \see RtSlerpInitialize
 * \see RtSlerpDestroy
 * \see RtSlerpSetLerp
 */
RwMatrix           *
RtSlerpGetMatrix(RtSlerp * spSlerp,
                 RwMatrix * mpResultMat, RwReal nDelta)
{
    RWAPIFUNCTION(RWSTRING("RtSlerpGetMatrix"));
    RWASSERT(spSlerp);
    RWASSERT(mpResultMat);

    /* Cap and floor the delta */
    /* If we are at one end the solution is easy */
    if (nDelta <= (RwReal) (0))
    {
        nDelta = (RwReal) (0);
        if (mpResultMat != spSlerp->startMat)
        {
            RwMatrixCopy(mpResultMat, spSlerp->startMat);
        }
        RWRETURN(mpResultMat);
    }
    else if (nDelta >= (RwReal) (1))
    {
        nDelta = (RwReal) (1);
        if (mpResultMat != spSlerp->endMat)
        {
            RwMatrixCopy(mpResultMat, spSlerp->endMat);
        }
        RWRETURN(mpResultMat);
    }

    /* Do the lerp if we are, else... */
    if (spSlerp->useLerp)
    {
        /* Get the lerp matrix */
        RwMatrix            mLerpMat;

        RwMatrixSetIdentity(&mLerpMat);

        RwV3dSub(&mLerpMat.right, &spSlerp->endMat->right,
                 &spSlerp->startMat->right);
        RwV3dSub(&mLerpMat.up, &spSlerp->endMat->up,
                 &spSlerp->startMat->up);
        RwV3dSub(&mLerpMat.at, &spSlerp->endMat->at,
                 &spSlerp->startMat->at);
        RwV3dSub(&mLerpMat.pos, &spSlerp->endMat->pos,
                 &spSlerp->startMat->pos);

        /* Do lerp */
        RwV3dScale(&mLerpMat.right, &mLerpMat.right, nDelta);
        RwV3dScale(&mLerpMat.up, &mLerpMat.up, nDelta);
        RwV3dScale(&mLerpMat.at, &mLerpMat.at, nDelta);
        RwV3dScale(&mLerpMat.pos, &mLerpMat.pos, nDelta);

        RwV3dAdd(&mpResultMat->right, &spSlerp->startMat->right,
                 &mLerpMat.right);
        RwV3dAdd(&mpResultMat->up, &spSlerp->startMat->up,
                 &mLerpMat.up);
        RwV3dAdd(&mpResultMat->at, &spSlerp->startMat->at,
                 &mLerpMat.at);
        RwV3dAdd(&mpResultMat->pos, &spSlerp->startMat->pos,
                 &mLerpMat.pos);

        RwV3dNormalize(&mpResultMat->right, &mpResultMat->right);
        RwV3dNormalize(&mpResultMat->up, &mpResultMat->up);
        RwV3dNormalize(&mpResultMat->at, &mpResultMat->at);
    }
    else
    {
        RwV3d               vStartPos;
        RwV3d               vEndPos;

        vStartPos = spSlerp->startMat->pos;
        vEndPos = spSlerp->endMat->pos;

        /* Remove the translation for now */
        RwMatrixCopy(mpResultMat, spSlerp->startMat);
        mpResultMat->pos.x = (RwReal) (0);
        mpResultMat->pos.y = (RwReal) (0);
        mpResultMat->pos.z = (RwReal) (0);

        /* Rotate the new matrix */
        RwMatrixRotate(mpResultMat, &spSlerp->axis,
                       ((spSlerp->angle) * (nDelta)),
                       rwCOMBINEPOSTCONCAT);

        /* Do linear interpolation on position */
        RwV3dSub(&mpResultMat->pos, &vEndPos, &vStartPos);
        RwV3dScale(&mpResultMat->pos, &mpResultMat->pos, nDelta);
        RwV3dAdd(&mpResultMat->pos, &mpResultMat->pos, &vStartPos);
    }

    /* Return it */
    RWRETURN(mpResultMat);
}

/**
 * \ingroup rtslerp
 * \ref RtSlerpSetLerp is used to toggle the use of spherical and non-spherical 
 * linear interpolation on the specified SLERP.
 *
 * The include file rtslerp.h and the library file rtslerp.lib are required to use 
 * this function.
 *
 * \param spSlerp  slerp to alter.
 * \param useLerp  boolean flag to use linear rather than spherical interpolation
 *        \li TRUE    means always use lerps
 *        \li FALSE   means use slerps where possible
 * 
 * \return pointer to Slerp data-structure on success; NULL pointer otherwise
 *
 * \see RtSlerpCreate
 * \see RtSlerpInitialize
 * \see RtSlerpDestroy
 * \see RtSlerpGetMatrix
 */
RtSlerp            *
RtSlerpSetLerp(RtSlerp * spSlerp, RwBool useLerp)
{
    RWAPIFUNCTION(RWSTRING("RtSlerpSetLerp"));
    RWASSERT(spSlerp);

    spSlerp->useLerp = useLerp;
    RWRETURN(spSlerp);
}

/**
 * \ingroup rtslerp
 * \ref RtQuatSetupSlerpCache sets up some cached values for 
 * Quaternion slerping.
 * 
 * \param qpFrom  initial quaternion
 * \param qpTo  final quaternion,
 * \param sCache  target slerp cache. 
 *
 * \see RtQuatConvertFromMatrix
 * \see RtQuatSetupSlerpArgandCache
 * \see RtQuatSlerp
 * \see RtQuatSlerpArgand
 */
void
RtQuatSetupSlerpCache(RtQuat * qpFrom,
                      RtQuat * qpTo, RtQuatSlerpCache * sCache)
{
    RwReal              cosOm;
    RwBool              obtuseOm;

    RWAPIFUNCTION(RWSTRING("RtQuatSetupSlerpCache"));

#if (0)
    SLERPMESSAGE(("[ [%f %f %f] %f] == qpFrom",
                  qpFrom->imag.x, qpFrom->imag.y,
                  qpFrom->imag.z, qpFrom->real));
    SLERPMESSAGE(("[ [%f %f %f] %f] == qpTo",
                  qpTo->imag.x, qpTo->imag.y, qpTo->imag.z,
                  qpTo->real));
#endif /* (0) */

    RtQuatAssign(&sCache->raFrom, qpFrom);

    cosOm =
        RwV3dDotProduct(&qpFrom->imag, &qpTo->imag) +
        (qpFrom->real * qpTo->real);

    obtuseOm = (cosOm < ((RwReal) 0));
    /* SLERPMESSAGE(("obtuseOm %d cosOm %f", obtuseOm, cosOm)); */

    if (obtuseOm)
    {
        cosOm = (cosOm < ((RwReal) - 1)) ? ((RwReal) 1) : -cosOm;
        RtQuatNegate(&sCache->raTo, qpTo);
    }
    else
    {
        cosOm = (cosOm > ((RwReal) 1)) ? ((RwReal) 1) : cosOm;
        RtQuatAssign(&sCache->raTo, qpTo);
    }

    RwIEEEACosfMacro(sCache->omega, cosOm);
    RWASSERT((0 <= sCache->omega) && (sCache->omega <= rwPIOVER2));

    sCache->nearlyZeroOm = (cosOm >= _TOL_COS_ZERO);

    if (!sCache->nearlyZeroOm)
    {
        const RwReal        omega = sCache->omega;
        RwReal              cosecOm;

        RwIEEECosecAcutefMacro(cosecOm, omega);

        /* Optimization: Pre-multiply by cosec */

        RtQuatScale(&sCache->raFrom, &sCache->raFrom, cosecOm);
        RtQuatScale(&sCache->raTo, &sCache->raTo, cosecOm);
    }

    RWRETURNVOID();
}

/**
 * \ingroup rtslerp
 * \ref RtQuatSetupSlerpArgandCache sets up some cached values for 
 * Argand Quaternion slerping.
 * 
 * \param qpFrom  initial quaternion
 * \param qpTo  final quaternion,
 * \param sArgandCache  target slerp cache. 
 *
 * \see RtQuatConvertFromMatrix
 * \see RtQuatSetupSlerpCache
 * \see RtQuatSlerp
 * \see RtQuatSlerpArgand
 */
void
RtQuatSetupSlerpArgandCache(RtQuat * qpFrom,
                            RtQuat * qpTo,
                            RtQuatSlerpArgandCache * sArgandCache)
{
    RwReal              cosOm;
    RwBool              obtuseOm;
    RtQuat              raTo;

    RWAPIFUNCTION(RWSTRING("RtQuatSetupSlerpArgandCache"));

#if (0)
    SLERPMESSAGE(("[ [%f %f %f] %f] == qpFrom",
                  qpFrom->imag.x, qpFrom->imag.y,
                  qpFrom->imag.z, qpFrom->real));
    SLERPMESSAGE(("[ [%f %f %f] %f] == qpTo",
                  qpTo->imag.x, qpTo->imag.y, qpTo->imag.z,
                  qpTo->real));
#endif /* (0) */

    cosOm =
        RwV3dDotProduct(&qpFrom->imag, &qpTo->imag) +
        (qpFrom->real * qpTo->real);

    obtuseOm = (cosOm < ((RwReal) 0));

    /* SLERPMESSAGE(("obtuseOm %d cosOm %f", obtuseOm, cosOm)); */

    if (obtuseOm)
    {
        RtQuatNegate(&raTo, qpTo);
    }
    else
    {
        RtQuatAssign(&raTo, qpTo);
    }

    RtQuatUnitLog(&sArgandCache->logTo, &raTo);
    RtQuatUnitLog(&sArgandCache->logBase, qpFrom);
    RtQuatSub(&sArgandCache->logBase,
              &sArgandCache->logTo, &sArgandCache->logBase);

    RWRETURNVOID();
}

#if ( defined(RWDEBUG) || defined(RWSUPPRESSINLINE) )

/**
 * \ingroup rtslerp
 * \ref RtQuatSlerp spherically interpolates between two quaternions.
 *
 * \param qpResult  target quaternion
 * \param qpFrom  initial quaternion
 * \param qpTo  final quaternion
 * \param rT  parameter for interplotion where
 *        \li 0.0     denotes the initial quaterion
 *        \li 1.0     denotes the final quaternion
 * \param sCache  as initialized with \ref RtQuatSetupSlerpCache
 *
 * \see RtQuatSetupSlerpCache
 * \see RtQuatSetupSlerpArgandCache
 * \see RtQuatSlerpArgand
 */
void
RtQuatSlerp(RtQuat * qpResult,
            RtQuat * qpFrom,
            RtQuat * qpTo, RwReal rT, RtQuatSlerpCache * sCache)
{

    RWAPIFUNCTION(RWSTRING("RtQuatSlerp"));

    /* SLERPMESSAGE(("%p == sCache; %f == rT", sCache, rT)); */

    RWASSERT(qpResult && qpFrom && qpTo);
    RWASSERT(sCache);
    RWASSERT((0 <= sCache->omega) && (sCache->omega <= rwPIOVER2));

    RtQuatSlerpMacro(qpResult, qpFrom, qpTo, rT, sCache);

    RWRETURNVOID();
}

/**
 * \ingroup rtslerp
 * \ref RtQuatSlerpArgand spherically interpolates between two 
 * quaternions in an Argand style
 *
 * \param qpResult  target quaternion
 * \param qpFrom  initial quaternion
 * \param qpTo  final quaternion
 * \param rT  parameter for interplotion where
 *        \li 0.0       denotes the initial quaterion
 *        \li 1.0       denotes the final quaternion
 * \param sArgandCache  as initialized with \ref RtQuatSetupSlerpArgandCache
 *
 * \see RtQuatSetupSlerpCache
 * \see RtQuatSetupSlerpArgandCache
 * \see RtQuatSlerp
 */
void
RtQuatSlerpArgand(RtQuat * qpResult,
                  RtQuat * qpFrom,
                  RtQuat * qpTo,
                  RwReal rT, RtQuatSlerpArgandCache * sArgandCache)
{
    RWAPIFUNCTION(RWSTRING("RtQuatSlerpArgand"));

    /* SLERPMESSAGE(("%p == sArgandCache %f == rT", sArgandCache, rT)); */

    RWASSERT(qpResult && qpFrom && qpTo);

    RtQuatSlerpArgandMacro(qpResult, qpFrom, qpTo, rT, sArgandCache);

    RWRETURNVOID();
}

#endif /* ( defined(RWDEBUG) || defined(RWSUPPRESSINLINE) ) */
