/**********************************************************************
 *
 * File : rphanim.c
 *
 * Abstract : A plugin that performs hiearchical animation
 *
 * Notes    : For details on interpolating rotation with
 *            Quaternions, see p360
 *            Advanced Animation and Rendering Techniques
 *            Alan Watt and Mark Watt
 *            Addison-Wesley 1993,
 *            ISBN 0-201-54412-1
 *
 * See Also : rwsdk/plugin/anim
 *
 **********************************************************************
 *
 * This file is a product of Criterion Software Ltd.
 *
 * This file is provided as is with no warranties of any kind and is
 * provided without any obligation on Criterion Software Ltd. or
 * Canon Inc. to assist in its use or modification.
 *
 * Criterion Software Ltd. will not, under any
 * circumstances, be liable for any lost revenue or other damages arising
 * from the use of this file.
 *
 * Copyright (c) 1999 Criterion Software Ltd.
 * All Rights Reserved.
 *
 * RenderWare is a trademark of Canon Inc.
 *
 ************************************************************************/

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

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "rpplugin.h"
#include <rpdbgerr.h>
#include <rwcore.h>
#include <rpworld.h>

#include <rtquat.h>
#include <rphanim.h>

#include "stdkey.h"

#if (!defined(DOXYGEN))
static const char rcsid[] __RWUNUSED__ =
    "@@@@(#)$Id: stdkey.c,v 1.18 2001/06/06 15:47:00 mattt Exp $";
#endif /* (!defined(DOXYGEN)) */


/****************************************************************************
 Defines
 */

/****************************************************************************
 Local types
 */

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

#define _EPSILON          ((RwReal)(0.001))
#define _TOL_COS_ZERO     (((RwReal)1) - _EPSILON)
#define RwCosecMinusPiToPiMacro(result, x)      \
MACRO_START                                     \
{                                               \
    RwSinMinusPiToPiMacro(result, x);           \
    result = ((RwReal)1) / (result);            \
}                                               \
MACRO_STOP
#define ROUNDUP16(x)      (((RwUInt32)(x) + 16 - 1) & ~(16 - 1))

#define HAnimStdKeyFrameAddTogether(_pOut, _pIn1, _pIn2)               \
MACRO_START                                                     \
{                                                               \
    RtQuatMultiply(&(_pOut)->q, &(_pIn1)->q, &(_pIn2)->q);      \
    RwV3dAdd(&(_pOut)->t, &(_pIn1)->t, &(_pIn2)->t);            \
}                                                               \
MACRO_STOP

/****************************************************************************
 Local (static) globals
 */

/****************************************************************************
 Local Function Prototypes
 */

/****************************************************************************
 Functions
 */

/**
 *
 * \ingroup rphanim
 * \ref RpHAnimStdKeyFrameToMatrix
 *  converts a standard keyframe to a matrix
 *
 * \param pMatrix A pointer to the output matrix
 * \param pVoidIFrame A pointer to the input frame
 *
 * \return None
 */
void
RpHAnimStdKeyFrameToMatrix(RwMatrix * pMatrix, void * pVoidIFrame)
{

    RWAPIFUNCTION(RWSTRING("RpHAnimStdKeyFrameToMatrix"));
    RWASSERT(pMatrix);
    RWASSERT(pVoidIFrame);

    RpHAnimStdKeyFrameToMatrixMacro(pMatrix, pVoidIFrame);

    RWRETURNVOID();
}

/**
 *
 * \ingroup rphanim
 * \ref RpHAnimStdKeyFrameInterpolate
 *  interpolates between two skin frames
 * and returns the result.
 *
 * \param pVoidOut A pointer to the output frame
 * \param pVoidIn1 A pointer to the first input frame
 * \param pVoidIn2 A pointer to the second input frame
 * \param time The time to which to interpolate
 *
 * \return None
 */
void
RpHAnimStdKeyFrameInterpolate(void * pVoidOut, void * pVoidIn1,
                              void * pVoidIn2, RwReal time)
{
    RpHAnimStdKeyFrame * pOut = (RpHAnimStdKeyFrame *)pVoidOut;
    RpHAnimStdKeyFrame * pIn1 = (RpHAnimStdKeyFrame *)pVoidIn1;
    RpHAnimStdKeyFrame * pIn2 = (RpHAnimStdKeyFrame *)pVoidIn2;

    /* Compute dot product
     * (equal to cosine of the angle between quaternions)
     */
    RwReal              fCosTheta = (RwV3dDotProduct(&pIn1->q.imag,
                                                     &pIn2->q.imag) +
                                     pIn1->q.real * pIn2->q.real);
    RwReal              fAlpha = ((time - pIn1->time) /

                                  (pIn2->time - pIn1->time));
    RwReal              fBeta;
    RwBool              bObtuseTheta;
    RwBool              bNearlyZeroTheta;

    RWAPIFUNCTION(RWSTRING("RpHAnimStdKeyFrameInterpolate"));
    RWASSERT(pOut);
    RWASSERT(pIn1);
    RWASSERT(pIn2);
    RWASSERT(pIn1->time <= time);
    RWASSERT(pIn2->time >= time);

    /* Linearly interpolate positions */
    RpHAnimStdKeyFrameTransInterpolate(&pOut->t, &pIn1->t, fAlpha, &pIn2->t);
    pOut->time = time;

    /* Check angle to see if quaternions are in opposite hemispheres */
    bObtuseTheta = (fCosTheta < ((RwReal) 0));

    if (bObtuseTheta)
    {
        /* If so, flip one of the quaterions */
        fCosTheta = -fCosTheta;
        RwV3dNegate(&pIn2->q.imag, &pIn2->q.imag);
        pIn2->q.real = -pIn2->q.real;
    }

    /* Set factors to do linear interpolation, as a special case where the */
    /* quaternions are close together. */
    fBeta = ((RwReal) 1) - fAlpha;

    /* If the quaternions aren't close, proceed with spherical interpolation */
    bNearlyZeroTheta = (fCosTheta >= _TOL_COS_ZERO);

    if (!bNearlyZeroTheta)
    {
        /* Quad trig functions disabled by default */
        /* owing to accuracy issues */

        RwReal        fTheta;
        RwReal        fCosecTheta;

        RwIEEEACosfMacro(fTheta, fCosTheta);
        RwCosecMinusPiToPiMacro(fCosecTheta, fTheta);

        fBeta *=  fTheta;
        RwSinMinusPiToPiMacro(fBeta, fBeta);
        fBeta *=  fCosecTheta;

        fAlpha *=  fTheta;
        RwSinMinusPiToPiMacro(fAlpha, fAlpha);
        fAlpha *=  fCosecTheta;

    }

    /* Do the interpolation */
    pOut->q.imag.x = fBeta * pIn1->q.imag.x + fAlpha * pIn2->q.imag.x;
    pOut->q.imag.y = fBeta * pIn1->q.imag.y + fAlpha * pIn2->q.imag.y;
    pOut->q.imag.z = fBeta * pIn1->q.imag.z + fAlpha * pIn2->q.imag.z;
    pOut->q.real = fBeta * pIn1->q.real + fAlpha * pIn2->q.real;

#if(0)
    /* Assert no worse than 5% error in length^2 of
     * spherically interpolated quaternion  */

    RWASSERT(bNearlyZeroTheta ||
             (((0.95) < RtQuatModulusSquaredMacro(&pOut->q)) &&
              (RtQuatModulusSquaredMacro(&pOut->q) < (1 / 0.95))));
#endif /* (0) */

    RWRETURNVOID();
}

/**
 * \ingroup rphanim
 * \ref RpHAnimStdKeyFrameBlend
 *  Blends between two RpHAnimFrames using a given
 * blend factor.
 *
 * \param pVoidOut A pointer to the output frame.
 * \param pVoidIn1 A pointer to the first input frame.
 * \param pVoidIn2 A pointer to the second input frame.
 * \param fAlpha The blending factor.
 *
 * \return None
 */
void
RpHAnimStdKeyFrameBlend(void * pVoidOut,
                        void * pVoidIn1,
                        void * pVoidIn2,
                        RwReal fAlpha)
{
    RpHAnimStdKeyFrame * pOut = (RpHAnimStdKeyFrame *)pVoidOut;
    RpHAnimStdKeyFrame * pIn1 = (RpHAnimStdKeyFrame *)pVoidIn1;
    RpHAnimStdKeyFrame * pIn2 = (RpHAnimStdKeyFrame *)pVoidIn2;

    RwReal              fBeta, fTheta;
    /* Compute dot product
     * (equal to cosine of the angle between quaternions)
     */
    RwReal              fCosTheta =
        RwV3dDotProduct(&pIn1->q.imag, &pIn2->q.imag) +
        pIn1->q.real * pIn2->q.real;
    RwBool              bObtuseTheta;
    RwBool              bNearlyZeroTheta;

    RWAPIFUNCTION(RWSTRING("RpHAnimStdKeyFrameBlend"));
    RWASSERT(pOut);
    RWASSERT(pIn1);
    RWASSERT(pIn2);

    /* Linearly interpolate positions */
    RpHAnimStdKeyFrameTransInterpolate(&pOut->t, &pIn1->t, fAlpha, &pIn2->t);

    /* Check angle to see if quaternions are in opposite hemispheres */
    bObtuseTheta = (fCosTheta < ((RwReal) 0));

    if (bObtuseTheta)
    {
        /* If so, flip one of the quaterions */
        fCosTheta = -fCosTheta;
        RwV3dNegate(&pIn2->q.imag, &pIn2->q.imag);
        pIn2->q.real = -pIn2->q.real;
    }

    /* Set factors to do linear interpolation, as a special case where the */
    /* quaternions are close together. */
    fBeta = 1.0f - fAlpha;

    /* If the quaternions aren't close, proceed with spherical interpolation */
    bNearlyZeroTheta = (fCosTheta >= _TOL_COS_ZERO);

    if (!bNearlyZeroTheta)
    {
        RwReal              fCosecTheta;

        RwIEEEACosfMacro(fTheta, fCosTheta);
        RwCosecMinusPiToPiMacro(fCosecTheta, fTheta);

        fBeta *=  fTheta;
        RwSinMinusPiToPiMacro(fBeta, fBeta);
        fBeta *=  fCosecTheta;

        fAlpha *=  fTheta;
        RwSinMinusPiToPiMacro(fAlpha, fAlpha);
        fAlpha *=  fCosecTheta;
    }

    /* Do the interpolation */
    pOut->q.imag.x = fBeta * pIn1->q.imag.x + fAlpha * pIn2->q.imag.x;
    pOut->q.imag.y = fBeta * pIn1->q.imag.y + fAlpha * pIn2->q.imag.y;
    pOut->q.imag.z = fBeta * pIn1->q.imag.z + fAlpha * pIn2->q.imag.z;
    pOut->q.real = fBeta * pIn1->q.real + fAlpha * pIn2->q.real;

    RWRETURNVOID();
}

/**
 *
 * \ingroup rphanim
 * \ref RpHAnimStdKeyFrameStreamRead
 *
 * Reads a hierarchical animation from a stream.
 *
 * \param stream A pointer to the stream to be read from.
 * \param pAnimation A pointer to the animation into which to read the data
 *
 * \return A pointer to the animation, or NULL if an error occurs
 *
 */
RpHAnimAnimation *
RpHAnimStdKeyFrameStreamRead(RwStream * stream, RpHAnimAnimation *pAnimation)
{
    RwInt32             i;
    RwInt32             temp;
    RpHAnimStdKeyFrame  *pFrames;

    RWAPIFUNCTION(RWSTRING("RpHAnimStdKeyFrameStreamRead"));
    RWASSERT(stream);

    pAnimation->pFrames = RwMalloc(sizeof(RpHAnimStdKeyFrame) *
                                 pAnimation->numFrames);

    if (!pAnimation->pFrames)
    {
        RWRETURN((RpHAnimAnimation *)NULL);
    }

    pFrames = (RpHAnimStdKeyFrame *)pAnimation->pFrames;

    for (i = 0; i < pAnimation->numFrames; i++)
    {
        if (!RwStreamReadReal
            (stream, (RwReal *) & (pFrames[i].time),
             sizeof(RwReal) * 8))
        {
            RWRETURN((RpHAnimAnimation *)NULL);
        }

        if (!RwStreamReadInt
            (stream, (RwInt32 *) & temp, sizeof(RwInt32)))
        {
            RWRETURN((RpHAnimAnimation *)NULL);
        }

        pFrames[i].prevFrame =
            &pFrames[temp / sizeof(RpHAnimStdKeyFrame)];
    }

    RWRETURN(pAnimation);
}

/**
 *
 * \ingroup rphanim
 * \ref RpHAnimStdKeyFrameStreamWrite
 *
 *
 * Writes a hierarchical animation to a stream.
 *
 * \param pAnimation A pointer to the animation to be written.
 * \param stream A pointer to the stream to be written to.
 *
 * \return TRUE on success, or FALSE if an error occurs.
 *
 */
RwBool
RpHAnimStdKeyFrameStreamWrite(RpHAnimAnimation * pAnimation,
                              RwStream * stream)
{
    RwInt32             i;
    RwInt32             temp;
    RpHAnimStdKeyFrame        *pFrames;

    RWAPIFUNCTION(RWSTRING("RpHAnimStdKeyFrameStreamWrite"));
    RWASSERT(pAnimation);
    RWASSERT(stream);

    pFrames = (RpHAnimStdKeyFrame *)pAnimation->pFrames;

    for (i = 0; i < pAnimation->numFrames; i++)
    {
        if (!RwStreamWriteReal
            (stream, (RwReal *) & (pFrames[i].time),
             sizeof(RwReal) * 8))
        {
            RWRETURN(FALSE);
        }

        temp =
            (RwInt32) (pFrames[i].prevFrame) -
            (RwInt32) (pFrames);

        if (!RwStreamWriteInt
            (stream, (RwInt32 *) & temp, sizeof(RwInt32)))
        {
            RWRETURN(FALSE);
        }
    }

    RWRETURN(TRUE);
}

/**
 *
 * \ingroup rphanim
 * \ref RpHAnimStdKeyFrameStreamGetSize
 *
 * Calculates the size of a hierarchical animation keyframes when written to a stream.
 *
 * \param pAnimation A pointer to the hierarchical animation.
 *
 * \return Size of the hierarchical animation keyframes in bytes.
 *
 */
RwInt32
RpHAnimStdKeyFrameStreamGetSize(RpHAnimAnimation * pAnimation)
{
    RwInt32             i;
    RwInt32             size = 0;

    RWAPIFUNCTION(RWSTRING("RpHAnimStdKeyFrameStreamGetSize"));

    RWASSERT(pAnimation);

    for (i = 0; i < pAnimation->numFrames; i++)
    {
        size += sizeof(RwReal) * 8 + sizeof(RwInt32);
    }

    RWRETURN(size);
}

/**
 *
 * \ingroup rphanim
 * \ref RpHAnimStdKeyFrameMulRecip
 *
 * Multiplies one keyframe by the reciprocal of another (in place).
 *
 * \param pVoidFrame A void pointer to the keyframe to modify.
 * \param pVoidStart A void pointer to the start keyframe to multiply by.
 *
 * \return None
 *
 */
void
RpHAnimStdKeyFrameMulRecip(void *pVoidFrame, void *pVoidStart)
{
    RpHAnimStdKeyFrame * pAnimFrame = (RpHAnimStdKeyFrame *)pVoidFrame;
    RpHAnimStdKeyFrame * pStartFrame = (RpHAnimStdKeyFrame *)pVoidStart;
    RtQuat              qInverse, qFrame;
    RWAPIFUNCTION(RWSTRING("RpHAnimStdKeyFrameMulRecip"));
    RWASSERT(pVoidFrame);
    RWASSERT(pVoidStart);

    /* Get inverse of hierarchy frame */
    RtQuatReciprocal(&qInverse, &pStartFrame->q);

    /* Transform animation frame by inverse of hierarchy frame */
    qFrame = pAnimFrame->q;
    RtQuatMultiply(&pAnimFrame->q, &qInverse, &qFrame);
    RwV3dSub(&pAnimFrame->t, &pAnimFrame->t, &pStartFrame->t);

    RWRETURNVOID();

}

/**
 *
 * \ingroup rphanim
 * \ref RpHAnimStdKeyFrameAdd
 *
 * Adds two keyframes together.
 *
 * \param pVoidOut A void pointer to the output keyframe.
 * \param pVoidIn1 A void pointer to the first keyframe to add.
 * \param pVoidIn2 A void pointer to the second keyframe to add.
 *
 * \return None
 *
 */
void
RpHAnimStdKeyFrameAdd(void * pVoidOut, void * pVoidIn1,
                      void * pVoidIn2)
{
    RpHAnimStdKeyFrame * pOut = (RpHAnimStdKeyFrame *)pVoidOut;
    RpHAnimStdKeyFrame * pIn1 = (RpHAnimStdKeyFrame *)pVoidIn1;
    RpHAnimStdKeyFrame * pIn2 = (RpHAnimStdKeyFrame *)pVoidIn2;

    RWAPIFUNCTION(RWSTRING("RpHAnimStdKeyFrameAdd"));
    RWASSERT(pVoidOut);
    RWASSERT(pVoidIn1);
    RWASSERT(pVoidIn2);

    HAnimStdKeyFrameAddTogether(pOut, pIn1, pIn2);

    RWRETURNVOID();
}
