CWE-ou-minkata/Sources/Plasma/PubUtilLib/plAnimation/plMatrixChannel.cpp

/*==LICENSE==*

CyanWorlds.com Engine - MMOG client, server and tools
Copyright (C) 2011  Cyan Worlds, Inc.

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

Additional permissions under GNU GPL version 3 section 7

If you modify this Program, or any covered work, by linking or
combining it with any of RAD Game Tools Bink SDK, Autodesk 3ds Max SDK,
NVIDIA PhysX SDK, Microsoft DirectX SDK, OpenSSL library, Independent
JPEG Group JPEG library, Microsoft Windows Media SDK, or Apple QuickTime SDK
(or a modified version of those libraries),
containing parts covered by the terms of the Bink SDK EULA, 3ds Max EULA,
PhysX SDK EULA, DirectX SDK EULA, OpenSSL and SSLeay licenses, IJG
JPEG Library README, Windows Media SDK EULA, or QuickTime SDK EULA, the
licensors of this Program grant you additional
permission to convey the resulting work. Corresponding Source for a
non-source form of such a combination shall include the source code for
the parts of OpenSSL and IJG JPEG Library used as well as that of the covered
work.

You can contact Cyan Worlds, Inc. by email legal@cyan.com
 or by snail mail at:
      Cyan Worlds, Inc.
      14617 N Newport Hwy
      Mead, WA   99021

*==LICENSE==*/

/////////////////////////////////////////////////////////////////////////////////////////
//
// INCLUDES
//
/////////////////////////////////////////////////////////////////////////////////////////

// havok (must be first)
//#include <hkmath/quaternion.h>
//#include <hkmath/quaternion.h>

#include <cmath>

// singular
#include "plMatrixChannel.h"

// local
#include "plQuatChannel.h"
#include "plPointChannel.h"

// global
#include "hsResMgr.h"
#include "plProfile.h"
#include "hsTimer.h"

// other
#include "pnSceneObject/plDrawInterface.h"
#include "pnSceneObject/plSimulationInterface.h"
#include "pnSceneObject/plCoordinateInterface.h"
#include "pnSceneObject/plAudioInterface.h"
#include "plInterp/plController.h"
#include "plInterp/plAnimTimeConvert.h"
#include "plInterp/hsInterp.h"
#include "plTransform/hsAffineParts.h"

/////////////////////////////////////////////////////////////////////////////////////////
//
// PROFILING GIBBLIES
//
/////////////////////////////////////////////////////////////////////////////////////////

plProfile_Extern(AffineValue);
plProfile_Extern(AffineInterp);
plProfile_Extern(AffineBlend);
plProfile_Extern(AffineCompose);
plProfile_Extern(MatrixApplicator);

/////////////////////////////////////////////////////////////////////////////////////////
//
// plMatrixChannel
//
/////////////////////////////////////////////////////////////////////////////////////////

// ctor --------------------------
// -----
plMatrixChannel::plMatrixChannel()
: plAGChannel()
{
}

// dtor ---------------------------
// -----
plMatrixChannel::~plMatrixChannel()
{
}

// Value --------------------------------------------------------
// ------
const hsMatrix44 & plMatrixChannel::Value(double time, bool peek)
{
    return fResult;
}

// AffineValue -----------------------------------------------------------
// ------------
const hsAffineParts & plMatrixChannel::AffineValue(double time, bool peek)
{
    return fAP;
}

// Value --------------------------------------------------------------
// ------
void plMatrixChannel::Value(hsMatrix44 &matrix, double time, bool peek)
{
    matrix = Value(time);
}

// MakeCombine -----------------------------------------------
// ------------
plAGChannel * plMatrixChannel::MakeCombine(plAGChannel *other)
{
    return nil;
}

// MakeBlend ---------------------------------------------------
// ----------
plAGChannel * plMatrixChannel::MakeBlend(plAGChannel * channelB,
                                         plScalarChannel * channelBias,
                                         int blendPriority)
{
    plMatrixChannel * matChanB = plMatrixChannel::ConvertNoRef(channelB);
    plAGChannel * result = this;        // if the blend fails, we keep our position in the graph

    if (matChanB)
    {
        result = new plMatrixBlend(this, matChanB, channelBias, blendPriority);
    }
    return result;
}

// MakeZeroState -----------------------------
// --------------
plAGChannel * plMatrixChannel::MakeZeroState()
{
    return new plMatrixConstant(Value(0));
}

// MakeTimeScale --------------------------------------------------------
// --------------
plAGChannel * plMatrixChannel::MakeTimeScale(plScalarChannel *timeSource)
{
    return new plMatrixTimeScale(this, timeSource);
}

// Dump -------------------------------------------
// -----
void plMatrixChannel::Dump(int indent, bool optimized, double time)
{
    std::string indentStr;
    for(int i = 0; i < indent; i++)
    {
        indentStr += "- ";
    }
    hsStatusMessageF("%s matChan<%s>", indentStr.c_str(), fName.c_str());
}

/////////////////////////////////////////////////////////////////////////////////////////
//
// plMatrixConstant
//
/////////////////////////////////////////////////////////////////////////////////////////

// ctor ----------------------------
// -----
plMatrixConstant::plMatrixConstant()
: plMatrixChannel()
{
}

plMatrixConstant::~plMatrixConstant()
{
}

plMatrixConstant::plMatrixConstant(const hsMatrix44 &value)
{
    Set(value);
}

void plMatrixConstant::Set(const hsMatrix44 &value)
{
    fResult = value;

    gemAffineParts gemParts1;
    decomp_affine(value.fMap, &gemParts1); 
    AP_SET(fAP, gemParts1);
}

void plMatrixConstant::Write(hsStream *stream, hsResMgr *mgr)
{
    plMatrixChannel::Write(stream, mgr);
    fAP.Write(stream);
}

void plMatrixConstant::Read(hsStream *stream, hsResMgr *mgr)
{
    plMatrixChannel::Read(stream, mgr);
    fAP.Read(stream);
    fAP.ComposeMatrix(&fResult);
}

/////////////////////////////////////////////////////////////////////////////////////////
//
// plMatrixTimeScale
//
// Insert into the graph when you need to change the speed or direction of time
// Also serves as a handy instancing node, since it just passes its data through.
//
/////////////////////////////////////////////////////////////////////////////////////////

// ctor ------------------------------
// -----
plMatrixTimeScale::plMatrixTimeScale()
: plMatrixChannel(), fTimeSource(nil), fChannelIn(nil)
{
}

// ctor ------------------------------------------------------------------------
// -----
plMatrixTimeScale::plMatrixTimeScale(plMatrixChannel *channel,
                                     plScalarChannel *timeSource)
: fChannelIn(channel),
  fTimeSource(timeSource)
{
}

// dtor -------------------------------
// -----
plMatrixTimeScale::~plMatrixTimeScale()
{
}

// IsStoppedAt ----------------------------
// ------------
bool plMatrixTimeScale::IsStoppedAt(double time)
{
    return fTimeSource->IsStoppedAt(time);
}

// Value ----------------------------------------------------------
// ------
const hsMatrix44 & plMatrixTimeScale::Value(double time, bool peek)
{
    fResult = fChannelIn->Value(fTimeSource->Value(time, peek), peek);

    return fResult;
}

const hsAffineParts & plMatrixTimeScale::AffineValue(double time, bool peek)
{
    fAP = fChannelIn->AffineValue(fTimeSource->Value(time, peek), peek);
    return fAP;
}

// Detach ----------------------------------------------------
// -------
plAGChannel * plMatrixTimeScale::Detach(plAGChannel * detach)
{
    plAGChannel *result = this;

    // HAVE to recurse on the incoming channel in case there are cycles;
    // even if we're detaching this node it might also be further upstream
    fChannelIn = plMatrixChannel::ConvertNoRef(fChannelIn->Detach(detach));

    // If you delete a timescale, it is not replaced with its upstream node;
    // it's just gone.
    if(!fChannelIn || detach == this)
        result = nil;

    if(result != this)
        delete this;

    return result;
}

// Dump ---------------------------------------------
// -----
void plMatrixTimeScale::Dump(int indent, bool optimized, double time)
{
    std::string indentStr;
    for(int i = 0; i < indent; i++)
    {
        indentStr += "- ";
    }
    hsStatusMessageF("%s matTimeScale <%s> at time <%f>", indentStr.c_str(), fName.c_str(), fTimeSource->Value(time, true));
    fChannelIn->Dump(indent + 1, optimized, time);

}


/////////////////////////////////////////////////////////////////////////////////////////
//
// plMatrixBlend
//
/////////////////////////////////////////////////////////////////////////////////////////

// ctor ----------------------
// -----
plMatrixBlend::plMatrixBlend()
: fChannelA(nullptr),
  fOptimizedA(nullptr),
  fChannelB(nullptr),
  fOptimizedB(nullptr),
  fChannelBias(nullptr),
  fPriority(0)
{
}

// ctor ----------------------------------------------------------------------------
// -----
plMatrixBlend::plMatrixBlend(plMatrixChannel * channelA, plMatrixChannel * channelB,
                             plScalarChannel * channelBias, int priority)
: fChannelA(channelA),
  fOptimizedA(channelA),
  fChannelB(channelB),
  fOptimizedB(channelB),
  fChannelBias(channelBias),
  fPriority(priority)
{
}

// dtor -----------------------
// -----
plMatrixBlend::~plMatrixBlend()
{
    fChannelA = nil;
    fChannelB = nil;
    fChannelBias = nil;
}

// MakeBlend --------------------------------------------------
// ----------
plAGChannel * plMatrixBlend::MakeBlend(plAGChannel *newChannel,
                                       plScalarChannel *channelBias,
                                       int blendPriority)
{
    plMatrixChannel * newMatChan = plMatrixChannel::ConvertNoRef(newChannel);
    plAGChannel *result = this;
    int effectiveBlendPriority = (blendPriority == -1 ? fPriority : blendPriority);

    if(newMatChan)
    {
        if(effectiveBlendPriority >= fPriority)
        {
            // if the new channel has higher priority, just do it.
            result = plMatrixChannel::MakeBlend(newMatChan, channelBias, effectiveBlendPriority);
        } else {
            // we're higher priority: pass to our upstream channel
            fChannelA = plMatrixChannel::ConvertNoRef(fChannelA->MakeBlend(newChannel, channelBias, blendPriority));
            hsAssert(fChannelA, "MakeBlend returned non-matrix channel.");
            // ask our upstream channel to do the blend: it can't be atop us
            // this request will get recursively delegated until the priorities work.
        }
    }
    return result;
}

uint16_t plMatrixBlend::GetPriority() {
    return fPriority;
}

bool plMatrixBlend::IsStoppedAt(double time)
{
    float blend = fChannelBias->Value(time);
    if (blend == 0)
        return fChannelA->IsStoppedAt(time);
    if (blend == 1)
        return fChannelB->IsStoppedAt(time);

    return (fChannelA->IsStoppedAt(time) && fChannelB->IsStoppedAt(time));
}

// Value ------------------------------------------------------
// ------
const hsMatrix44 & plMatrixBlend::Value(double time, bool peek)
{
    const hsAffineParts &parts = AffineValue(time, peek);

    plProfile_BeginTiming(AffineCompose);
    parts.ComposeMatrix(&fResult);
    plProfile_EndTiming(AffineCompose);
    return fResult;
}

// AffineValue ---------------------------------------------------------
// ------------
const hsAffineParts & plMatrixBlend::AffineValue(double time, bool peek)
{
    const float &blend = fChannelBias->Value(time);
    if(blend == 0) {
        return fOptimizedA->AffineValue(time, peek);
    } else {
        if(blend == 1) {
            return fOptimizedB->AffineValue(time, peek);
        } else {
            const hsAffineParts &apA = fChannelA->AffineValue(time, peek);
            const hsAffineParts &apB = fChannelB->AffineValue(time, peek);

            plProfile_BeginTiming(AffineBlend);
            hsInterp::LinInterp(&apA, &apB, blend, &fAP);
            plProfile_EndTiming(AffineBlend);
        }
    }
    return fAP;
}

// Detach ----------------------------------------------
// -------
plAGChannel * plMatrixBlend::Detach(plAGChannel *remove)
{
    plAGChannel *result = this;

    // it's possible that the incoming channel could reside down *all* of our
    // branches (it's a graph, not a tree,) so we always pass down all limbs
    fChannelBias = plScalarChannel::ConvertNoRef(fChannelBias->Detach(remove));
    fChannelA = plMatrixChannel::ConvertNoRef(fChannelA->Detach(remove));
    fChannelB = plMatrixChannel::ConvertNoRef(fChannelB->Detach(remove));

    if(!fChannelBias)
        result = fChannelA;
    else if(fChannelA && !fChannelB)
        result = fChannelA;
    else if(fChannelB && !fChannelA)
        result = fChannelB;
    else if(!fChannelA && !fChannelB)
        result = nil;

    if(result != this)
        delete this;

    return result;
}

// Optimize -------------------------------------
// ---------
plAGChannel *plMatrixBlend::Optimize(double time)
{
    fOptimizedA = (plMatrixChannel *)fChannelA->Optimize(time);
    fOptimizedB = (plMatrixChannel *)fChannelB->Optimize(time);
    float blend = fChannelBias->Value(time);
    if(blend == 0.0f)
        return fOptimizedA;
    if(blend == 1.0f)
        return fOptimizedB;
    else
        return this;
}

// Dump -----------------------------------------
// -----
void plMatrixBlend::Dump(int indent, bool optimized, double time)
{
    std::string indentStr;
    for(int i = 0; i < indent; i++)
    {
        indentStr += "- ";
    }
    hsStatusMessageF("%s matBlend<%s>, bias:<%f>", indentStr.c_str(), fName.c_str(), fChannelBias->Value(time, true));
    if(optimized)
    {
        fOptimizedB->Dump(indent + 1, optimized, time);
        fOptimizedA->Dump(indent + 1, optimized, time);
    } else {
        fChannelB->Dump(indent + 1, optimized, time);
        fChannelA->Dump(indent + 1, optimized, time);
    }
}

/////////////////////////////////////////////////////////////////////////////////////////
//
// plMatrixControllerChannel
//
/////////////////////////////////////////////////////////////////////////////////////////

// ctor ----------------------------------------------
// -----
plMatrixControllerChannel::plMatrixControllerChannel()
: plMatrixChannel(), fController(nil)
{
}

// ctor ---------------------------------------------------------------
// -----
plMatrixControllerChannel::plMatrixControllerChannel(plController *controller,
                                                     hsAffineParts *parts)
: fController(controller)
{
    fAP = *parts;
}

// dtor -----------------------------------------------
// -----
plMatrixControllerChannel::~plMatrixControllerChannel()
{
    if(fController) {
        delete fController;
        fController = nil;
    }
}

// Value ------------------------------------------------------------------
// ------
const hsMatrix44 & plMatrixControllerChannel::Value(double time, bool peek)
{
    return Value(time, peek, nil);
}

// Value ------------------------------------------------------------------
// ------
const hsMatrix44 & plMatrixControllerChannel::Value(double time, bool peek,
                                                    plControllerCacheInfo *cache)
{
    plProfile_BeginTiming(AffineInterp);
    fController->Interp((float)time, &fAP, cache);
    plProfile_EndTiming(AffineInterp);

    plProfile_BeginTiming(AffineCompose);
    fAP.ComposeMatrix(&fResult);
    plProfile_EndTiming(AffineCompose);     
    return fResult;
}

// AffineValue ---------------------------------------------------------------------
// ------------
const hsAffineParts & plMatrixControllerChannel::AffineValue(double time, bool peek)
{
    return AffineValue(time, peek, nil);
}

// AffineValue ---------------------------------------------------------------------
// ------------
const hsAffineParts & plMatrixControllerChannel::AffineValue(double time, bool peek,
                                                             plControllerCacheInfo *cache)
{
    plProfile_BeginTiming(AffineInterp);
    fController->Interp((float)time, &fAP, cache);
    plProfile_EndTiming(AffineInterp);
    return fAP;
}

// MakeCacheChannel ------------------------------------------------------------
// -----------------
plAGChannel *plMatrixControllerChannel::MakeCacheChannel(plAnimTimeConvert *atc)
{
    plControllerCacheInfo *cache = fController->CreateCache();
    cache->SetATC(atc);
    return new plMatrixControllerCacheChannel(this, cache);
}

void plMatrixControllerChannel::Dump(int indent, bool optimized, double time)
{
    std::string indentStr;
    for(int i = 0; i < indent; i++)
    {
        indentStr += "- ";
    }
    hsStatusMessageF("%s MatController<%s>", indentStr.c_str(), fName.c_str());
}

// Write -------------------------------------------------------------
// ------
void plMatrixControllerChannel::Write(hsStream *stream, hsResMgr *mgr)
{
    plMatrixChannel::Write(stream, mgr);

    hsAssert(fController, "Trying to write plMatrixControllerChannel with nil controller. File will not be importable.");
    mgr->WriteCreatable(stream, fController);

    fAP.Write(stream);
}

// Read -------------------------------------------------------------
// ------
void plMatrixControllerChannel::Read(hsStream *stream, hsResMgr *mgr)
{
    plMatrixChannel::Read(stream, mgr);

    fController = plController::ConvertNoRef(mgr->ReadCreatable(stream));

    fAP.Read(stream);
}

/////////////////////////////////
// PLMATRIXCONTROLLERCACHECHANNEL
/////////////////////////////////

// CTOR
plMatrixControllerCacheChannel::plMatrixControllerCacheChannel()
: plMatrixChannel(), fControllerChannel(nil), fCache(nil)
{
}

// CTOR(name, controller)
plMatrixControllerCacheChannel::plMatrixControllerCacheChannel(plMatrixControllerChannel *controller, plControllerCacheInfo *cache)
: fControllerChannel(controller), fCache(cache)
{
}

// ~DTOR()
plMatrixControllerCacheChannel::~plMatrixControllerCacheChannel()
{
    delete fCache;
    fControllerChannel = nil;
}

// VALUE(time)
const hsMatrix44 & plMatrixControllerCacheChannel::Value(double time, bool peek)
{
    return fControllerChannel->Value(time, peek, fCache);
}

const hsAffineParts & plMatrixControllerCacheChannel::AffineValue(double time, bool peek)
{
    return fControllerChannel->AffineValue(time, peek, fCache);
}

// DETACH
plAGChannel * plMatrixControllerCacheChannel::Detach(plAGChannel * detach)
{
    plAGChannel *result = this;

    fControllerChannel =
        plMatrixControllerChannel::ConvertNoRef(fControllerChannel->Detach(detach));

    if(detach == this)
        result = fControllerChannel;

    if(!fControllerChannel)
        result = nil;

    if(result != this)
        delete this;

    return result;
}

/////////////////////
// PLQUATPOINTCOMBINE
/////////////////////

// CTOR
plQuatPointCombine::plQuatPointCombine()
: fQuatChannel(nil), fPointChannel(nil)
{
}

// CTOR
plQuatPointCombine::plQuatPointCombine(plQuatChannel *quatChannel, plPointChannel *pointChannel)
: fQuatChannel(quatChannel),
  fPointChannel(pointChannel)
{
}

// DTOR
plQuatPointCombine::~plQuatPointCombine()
{
    if(fQuatChannel) {
    //XXX   delete fQuatChannel;
        fQuatChannel = nil;
    }
    if(fPointChannel) {
    //XXX   delete fPointChannel;
        fPointChannel = nil;
    }
}

// VALUE(time)
const hsMatrix44 & plQuatPointCombine::Value(double time)
{
    if(fQuatChannel)
    {
        const hsQuat &quat = fQuatChannel->Value(time);
        quat.MakeMatrix(&fResult);
    } else {
        fResult.Reset();
    }
    if(fPointChannel)
    {
        const hsPoint3 &point = fPointChannel->Value(time);
        fResult.SetTranslate(&point);
    }
    return fResult;
}

const hsAffineParts & plQuatPointCombine::AffineValue(double time)
{
    // XXX Lame hack to get things to compile for now. 
    // Will fix when we actually start using this channel type.
    gemAffineParts gemParts1;
    decomp_affine(Value(time).fMap, &gemParts1); 
    AP_SET(fAP, gemParts1);

    return fAP;
}

// DETACH
plAGChannel * plQuatPointCombine::Detach(plAGChannel *channel)
{
    hsAssert(this != channel, "Can't detach combiners or blenders directly. Detach sub-channels instead.");
    if(this != channel)
    {
        // *** check the types on the replacement channels to make sure they're compatible
        fQuatChannel = (plQuatChannel *)fQuatChannel->Detach(channel);
        fPointChannel = (plPointChannel *)fPointChannel->Detach(channel);
    }
    return this;
}

///////////////////////////////////////////////////////////////////////////////////////////
//
// PLMATRIXCHANNELAPPLICATOR
//
///////////////////////////////////////////////////////////////////////////////////////////

// IAPPLY
void plMatrixChannelApplicator::IApply(const plAGModifier *mod, double time)
{
    if(fChannel)
    {
        plMatrixChannel *matChan = plMatrixChannel::ConvertNoRef(fChannel);

        if(matChan)
        {
            hsMatrix44 inverse;

            plProfile_BeginTiming(AffineValue);
            const hsAffineParts &ap = matChan->AffineValue(time);
            plProfile_EndTiming(AffineValue);
            
            hsMatrix44 result;

            plProfile_BeginTiming(AffineCompose);
            ap.ComposeMatrix(&result);
            ap.ComposeInverseMatrix(&inverse);
            //result.GetInverse(&inverse);
            plProfile_EndTiming(AffineCompose);

            plProfile_BeginTiming(MatrixApplicator);
            plCoordinateInterface *CI = IGetCI(mod);
            CI->SetLocalToParent(result, inverse);
            plProfile_EndTiming(MatrixApplicator);  
        }
    }
}

///////////////////////////////////////////////////////////////////////////////////////////
//
// plMatrixDelayedCorrectionApplicator
//
///////////////////////////////////////////////////////////////////////////////////////////

const float plMatrixDelayedCorrectionApplicator::fDelayLength = 1.f; // seconds

void plMatrixDelayedCorrectionApplicator::SetCorrection(hsMatrix44 &cor)
{
    if (fIgnoreNextCorrection)
    {
        // We want the first correction we get from an avatar to be
        // instantaneous, otherwise they float over from (0, 0, 0).
        fIgnoreNextCorrection = false;
        return;
    }

    // decomp_affine seems to always give us the smaller angle quaternion,
    // which looks right visually when we interp. If certain cases become
    // visually annoying, we can check and adjust things here.
    gemAffineParts gemParts1;
    decomp_affine(cor.fMap, &gemParts1); 
    AP_SET(fCorAP, gemParts1);

    fDelayStart = hsTimer::GetSysSeconds();
}


// CANBLEND
bool plMatrixDelayedCorrectionApplicator::CanBlend(plAGApplicator *app)
{
    plMatrixChannelApplicator *matChannelApp = plMatrixChannelApplicator::ConvertNoRef(app);
    if (matChannelApp)
        return true;
    return false;
}

// IAPPLY
void plMatrixDelayedCorrectionApplicator::IApply(const plAGModifier *mod, double time)
{
    if(fChannel)
    {
        if(fEnabled)
        {
            plMatrixChannel *matChan = plMatrixChannel::ConvertNoRef(fChannel);
            hsAssert(matChan, "Invalid channel given to plMatrixChannelApplicator");

            plProfile_BeginTiming(MatrixApplicator);
            plCoordinateInterface *CI = IGetCI(mod);            
            const hsMatrix44 &animResult = matChan->Value(time);
            hsMatrix44 localResult;
            hsMatrix44 localInverse;

            if (time < fDelayStart + fDelayLength)
            {
                hsAffineParts identAP;
                identAP.Reset();
                hsAffineParts interpAP;
                hsMatrix44 interpResult;

                float blend = (float)((time - fDelayStart) / fDelayLength);
                hsInterp::LinInterp(&fCorAP, &identAP, blend, &interpAP);
                interpAP.ComposeMatrix(&interpResult);
                
                localResult = interpResult * animResult;
                localResult.GetInverse(&localInverse);
                CI->SetLocalToParent(localResult, localInverse);
            }
            else
            {
                animResult.GetInverse(&localInverse);
                CI->SetLocalToParent(animResult, localInverse);     
            }
            plProfile_EndTiming(MatrixApplicator);
        }
    }
}

///////////////////////////////////////////////////////////////////////////////////////////
//
// PLMATRIXDIFFERENCEAPPLICATOR
//
///////////////////////////////////////////////////////////////////////////////////////////

// Reset -------------------------------------
// ------
void plMatrixDifferenceApp::Reset(double time)
{
    hsAssert(fChannel,"Missing input channel when resetting.");
    if(fChannel)
    {
        plMatrixChannel *matChan = plMatrixChannel::ConvertNoRef(fChannel);
        hsAssert(matChan, "Invalid channel given to plMatrixChannelApplicator");
        if(matChan)
        {
            hsMatrix44 L2A, A2L;
            const hsAffineParts &ap = matChan->AffineValue(time);
            ap.ComposeMatrix(&L2A);             // what comes out of AffineValue is a local-to-animation
            ap.ComposeInverseMatrix(&A2L);

            fLastA2L = A2L;
            fLastL2A = L2A;
        }
    }
}

// CanBlend -----------------------------------------------
// ---------
bool plMatrixDifferenceApp::CanBlend(plAGApplicator *app)
{
    plMatrixChannelApplicator *matChannelApp = plMatrixChannelApplicator::ConvertNoRef(app);
    if (matChannelApp)
        return true;
    return false;
}

// *** move this somewhere real
bool CompareMatrices2(const hsMatrix44 &matA, const hsMatrix44 &matB, float tolerance)
{
    bool c00 = fabs(matA.fMap[0][0] - matB.fMap[0][0]) < tolerance;
    bool c01 = fabs(matA.fMap[0][1] - matB.fMap[0][1]) < tolerance;
    bool c02 = fabs(matA.fMap[0][2] - matB.fMap[0][2]) < tolerance;
    bool c03 = fabs(matA.fMap[0][3] - matB.fMap[0][3]) < tolerance;

    bool c10 = fabs(matA.fMap[1][0] - matB.fMap[1][0]) < tolerance;
    bool c11 = fabs(matA.fMap[1][1] - matB.fMap[1][1]) < tolerance;
    bool c12 = fabs(matA.fMap[1][2] - matB.fMap[1][2]) < tolerance;
    bool c13 = fabs(matA.fMap[1][3] - matB.fMap[1][3]) < tolerance;

    bool c20 = fabs(matA.fMap[2][0] - matB.fMap[2][0]) < tolerance;
    bool c21 = fabs(matA.fMap[2][1] - matB.fMap[2][1]) < tolerance;
    bool c22 = fabs(matA.fMap[2][2] - matB.fMap[2][2]) < tolerance;
    bool c23 = fabs(matA.fMap[2][3] - matB.fMap[2][3]) < tolerance;

    bool c30 = fabs(matA.fMap[3][0] - matB.fMap[3][0]) < tolerance;
    bool c31 = fabs(matA.fMap[3][1] - matB.fMap[3][1]) < tolerance;
    bool c32 = fabs(matA.fMap[3][2] - matB.fMap[3][2]) < tolerance;
    bool c33 = fabs(matA.fMap[3][3] - matB.fMap[3][3]) < tolerance;

    return c00 && c01 && c02 && c03 && c10 && c11 && c12 && c13 && c20 && c21 && c22 && c23 && c30 && c31 && c32 && c33;
}

// IAPPLY
void plMatrixDifferenceApp::IApply(const plAGModifier *mod, double time)
{
    plMatrixChannel *matChan = plMatrixChannel::ConvertNoRef(fChannel);
    hsAssert(matChan, "Invalid channel given to plMatrixChannelApplicator");
    hsMatrix44 L2A, A2L;

    const hsAffineParts &ap = matChan->AffineValue(time);

    plProfile_BeginTiming(AffineCompose);
    ap.ComposeMatrix(&L2A);             // what comes out of AffineValue is a local-to-animation
    ap.ComposeInverseMatrix(&A2L);
    plProfile_EndTiming(AffineCompose);

    plProfile_BeginTiming(MatrixApplicator);    
    if(fNew)                    // if it's new, there's no previous frame to diff against;
    {                           // cache the current and don't do anything
        fLastA2L = A2L;
        fLastL2A = L2A;

        fNew = false;
    } else {
        if( ! CompareMatrices2(fLastA2L, A2L, .0001f) && ! CompareMatrices2(fLastL2A, L2A, .0001f))
        {
            plCoordinateInterface *CI = IGetCI(mod);

            hsMatrix44 l2p = CI->GetLocalToParent();
            hsMatrix44 p2l = CI->GetParentToLocal();

            hsMatrix44 prev2Cur = fLastA2L * L2A;   // previous to current in local space
            hsMatrix44 cur2Prev = A2L * fLastL2A;   // current to previous in local space

            hsMatrix44 newL2P = l2p * prev2Cur;
            hsMatrix44 newP2L = cur2Prev * p2l;

            CI->SetLocalToParent(newL2P, newP2L);
            CI->FlushTransform();

            fLastL2A = L2A;
            fLastA2L = A2L;
        }
    }
    plProfile_EndTiming(MatrixApplicator);  
}

///////////////////////////////////////////////////////////////////////////////////////////
//
// PLIK2APPLICATOR
//
///////////////////////////////////////////////////////////////////////////////////////////

/** A two-bone IK applicator.
    */
class plIK2Applicator : public plMatrixChannelApplicator
{
public:

    // The latest time we were asked to evaluate. We won't actually do the evaluation
    // until the other bone is asked as well.
    double GetUpdateTime();

    void SetIsEndEffector(bool status);
    bool GetIsEndEffector();

    void SetTarget(hsPoint3 &worldPoint);

private:
    virtual void IApply(const plAGModifier *mod, double time);
    void ISolve();
    // The other bone involved in the IK solution
    plIK2Applicator *fOtherBone;
    // The latest time we were asked to evaluate. We won't actually run our
    // process until the other guy is asked to evaluate the same time.
    double  fUpdateTime;

    hsPoint3 fTarget;
    bool fIsEndEffector;
};

// GetUpdateTime ---------------
double plIK2Applicator::GetUpdateTime()
{
    return fUpdateTime;
}

void plIK2Applicator::SetIsEndEffector(bool status)
{
    fIsEndEffector = status;
}

bool plIK2Applicator::GetIsEndEffector()
{
    return fIsEndEffector;
}

void plIK2Applicator::IApply(const plAGModifier *mod, double time)
{
    fUpdateTime = time;
    if(time == fOtherBone->GetUpdateTime())
    {
        // we're both up-to-date: go ahead and solve
        ISolve();
    }
}

void plIK2Applicator::ISolve()
{

}