CWE-ou-minkata/Sources/Plasma/PubUtilLib/plAvatar/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/>.

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>

// 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 = TRACKED_NEW plMatrixBlend(this, matChanB, channelBias, blendPriority);
    }
    return result;
}

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

// MakeTimeScale --------------------------------------------------------
// --------------
plAGChannel * plMatrixChannel::MakeTimeScale(plScalarChannel *timeSource)
{
    return TRACKED_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);
}

/////////////////////////////////////////////////////////////////////////////////////////
//
// 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 ----------------------------
// ------------
hsBool 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, fTimeSource->Value(time, true));
    fChannelIn->Dump(indent + 1, optimized, time);
    
}


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

// ctor ----------------------
// -----
plMatrixBlend::plMatrixBlend()
: fChannelA(nil),
  fChannelB(nil),
  fChannelBias(nil)
{
}

// 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 plMatrixBlend::GetPriority() {
    return fPriority;
}

hsBool plMatrixBlend::IsStoppedAt(double time)
{
    hsScalar 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 hsScalar &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);
    hsScalar 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, 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((hsScalar)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((hsScalar)time, &fAP, cache);
    plProfile_EndTiming(AffineInterp);
    return fAP;
}

// MakeCacheChannel ------------------------------------------------------------
// -----------------
plAGChannel *plMatrixControllerChannel::MakeCacheChannel(plAnimTimeConvert *atc)
{
    plControllerCacheInfo *cache = fController->CreateCache();
    cache->SetATC(atc);
    return TRACKED_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);
}

// 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 hsScalar 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
hsBool plMatrixDelayedCorrectionApplicator::CanBlend(plAGApplicator *app)
{
    plMatrixChannelApplicator *matChannelApp = plMatrixChannelApplicator::ConvertNoRef(app);

    if( plMatrixChannelApplicator::ConvertNoRef(app) )
    {
        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;

                hsScalar blend = (hsScalar)((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 -----------------------------------------------
// ---------
hsBool plMatrixDifferenceApp::CanBlend(plAGApplicator *app)
{
    plMatrixChannelApplicator *matChannelApp = plMatrixChannelApplicator::ConvertNoRef(app);

    if( plMatrixChannelApplicator::ConvertNoRef(app) )
    {
        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 && 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()
{

}