mgcikjoint.cpp

3D Game Engine Design Source Code非常棒

// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// RESTRICTED USE SOURCE CODE
// http://www.magic-software.com/License/restricted.pdf

#include "MgcIKJoint.h"
#include "MgcIKGoal.h"
#include "MgcNode.h"
#include "MgcMatrix3.h"

MgcImplementRTTI(MgcIKJoint,MgcObject);
MgcImplementStream(MgcIKJoint);

//----------------------------------------------------------------------------
MgcIKJoint::MgcIKJoint (MgcSpatial* pkObject)
{
    m_pkObject = pkObject;

    int i;
    for (i = 0; i < 3; i++)
    {
        m_abAllowTrn[i] = false;
        m_afMinTrn[i] = -MgcMath::INFINITY;
        m_afMaxTrn[i] = +MgcMath::INFINITY;
        m_afTensionTrn[i] = 0.0;
        m_afOriginTrn[i] = 0.0;
        m_afDampTrn[i] = 1.0;

        m_abAllowRot[i] = false;
        m_afMinRot[i] = -MgcMath::PI;
        m_afMaxRot[i] = +MgcMath::PI;
        m_afTensionRot[i] = 0.0;
        m_afOriginRot[i] = 0.0;
        m_afDampRot[i] = 1.0;
    }
}
//----------------------------------------------------------------------------
void MgcIKJoint::UpdateTransforms ()
{
    float fWorldScale;
    MgcMatrix3 kWorldRotate;
    MgcVector3 kWorldTranslate;

    MgcNode* pkParent = m_pkObject->GetParent();
    if ( pkParent )
    {
        fWorldScale = pkParent->WorldScale()*m_pkObject->Scale();
        kWorldRotate = pkParent->WorldRotate()*m_pkObject->Rotate();
        kWorldTranslate = pkParent->WorldTranslate() +
            pkParent->WorldScale()*(pkParent->WorldRotate() *
            m_pkObject->Translate());
    }
    else
    {
        fWorldScale = m_pkObject->Scale();
        kWorldRotate = m_pkObject->Rotate();
        kWorldTranslate = m_pkObject->Translate();
    }

    m_pkObject->WorldScale() = fWorldScale;
    m_pkObject->WorldRotate() = kWorldRotate;
    m_pkObject->WorldTranslate() = kWorldTranslate;
}
//----------------------------------------------------------------------------
MgcVector3 MgcIKJoint::GetAxis (int i)
{
    MgcNode* pkParent = m_pkObject->GetParent();
    if ( pkParent )
        return pkParent->WorldRotate().GetColumn(i);
    else
        return MgcVector3::UNIT_X;
}
//----------------------------------------------------------------------------
void MgcIKJoint::Update (unsigned int uiGoalQuantity, MgcIKGoal** apkGoal)
{
    const MgcReal fEpsilon = 1e-06;

    MgcVector3 kAxis;
    MgcReal fNumer, fDenom, fWeight;
    unsigned int uiG;
    MgcIKGoal* pkGoal;
    MgcReal afTrnDelta[3], afRotDelta[3];

    // update translations
    int i;
    for (i = 0; i < 3; i++)
    {
        if ( m_abAllowTrn[i] )
        {
            kAxis = GetAxis(i);
            fNumer = 0.0;
            fDenom = 0.0;

            for (uiG = 0; uiG < uiGoalQuantity; uiG++)
            {
                pkGoal = apkGoal[uiG];
                fWeight = pkGoal->Weight();

                MgcVector3 kGmE = pkGoal->GetPosition() -
                    pkGoal->GetEffectorPosition();
                fNumer += fWeight*kAxis.Dot(kGmE);
                fDenom += fWeight;
            }

            if ( MgcMath::Abs(fDenom) > fEpsilon )
                afTrnDelta[i] = fNumer/fDenom;
            else
                afTrnDelta[i] = 0.0;
        }
        else
        {
            afTrnDelta[i] = 0.0;
        }
    }

    // update rotations
    for (i = 0; i < 3; i++)
    {
        if ( m_abAllowRot[i] )
        {
            kAxis = GetAxis(i);
            fNumer = 0.0;
            fDenom = 0.0;

            for (uiG = 0; uiG < uiGoalQuantity; uiG++)
            {
                pkGoal = apkGoal[uiG];
                fWeight = pkGoal->Weight();

                MgcVector3 kEmI = pkGoal->GetEffectorPosition() -
                    m_pkObject->WorldTranslate();
                MgcVector3 kGmI = pkGoal->GetPosition() -
                    m_pkObject->WorldTranslate();
                MgcVector3 kAxEmI = kAxis.Cross(kEmI);
                MgcVector3 kAxAxEmI = kAxis.Cross(kAxEmI);
                fNumer += kGmI.Dot(kAxEmI)*fWeight;
                fDenom += kGmI.Dot(kAxAxEmI)*fWeight;
            }

            if ( MgcMath::Abs(fDenom) > fEpsilon )
                afRotDelta[i] = MgcMath::ATan2(fNumer,fDenom);
            else
                afRotDelta[i] = 0.0;
        }
        else
        {
            afRotDelta[i] = 0.0;
        }
    }

    // satisfy constraints and compute final local transforms
    ApplyConstraints(afTrnDelta,afRotDelta);
}
//----------------------------------------------------------------------------
void MgcIKJoint::ApplyConstraints (MgcReal afTrnDelta[3],
    MgcReal afRotDelta[3])
{
    int i;
    MgcReal fDiff;

    // translation constraints
    bool bHasTranslation = false;
    for (i = 0; i < 3; i++)
    {
        if ( m_abAllowTrn[i] )
        {
            bHasTranslation = true;

            // apply spring physics
            if ( m_afTensionTrn[i] > 0.0 )
            {
                fDiff = m_afOriginTrn[i] - m_pkObject->Translate()[i];
                afTrnDelta[i] += m_afTensionTrn[i]*fDiff;
            }

            if ( m_afDampTrn[i] < 1.0 )
                afTrnDelta[i] *= m_afDampTrn[i];

            // clamp
            if ( afTrnDelta[i] < m_afMinTrn[i] )
                afTrnDelta[i] = m_afMinTrn[i];
            else if ( afTrnDelta[i] > m_afMaxTrn[i] )
                afTrnDelta[i] = m_afMaxTrn[i];
        }
    }

    // rotation constraints
    bool bHasRotation = false;
    for (i = 0; i < 3; i++)
    {
        if ( m_abAllowRot[i] )
        {
            bHasRotation = true;

            // apply spring physics
            if ( m_afTensionRot[i] > 0.0 )
            {
                fDiff = m_afOriginRot[i] -
                    m_pkObject->Rotate().ExtractAngle(i);
                afRotDelta[i] += m_afTensionRot[i]*fDiff;
            }

            if ( m_afDampRot[i] < 1.0 )
                afRotDelta[i] *= m_afDampRot[i];

            // clamp
            if ( afRotDelta[i] < m_afMinRot[i] )
                afRotDelta[i] = m_afMinRot[i];
            else if ( afRotDelta[i] > m_afMaxRot[i] )
                afRotDelta[i] = m_afMaxRot[i];
        }
    }

    // create new local translation
    if ( bHasTranslation )
    {
        m_pkObject->Translate() = MgcVector3(afTrnDelta[0],afTrnDelta[1],
            afTrnDelta[2]);
    }

    // create new local rotation
    if ( bHasRotation )
    {
        m_pkObject->Rotate().FromEulerAnglesZYX(afRotDelta[2],afRotDelta[1],
            afRotDelta[0]);
    }
}
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------
// streaming
//----------------------------------------------------------------------------
MgcObject* MgcIKJoint::Factory (MgcStream& rkStream)
{
    MgcIKJoint* pkObject = new MgcIKJoint;
    MgcStream::Link* pkLink = new MgcStream::Link(pkObject);
    pkObject->Load(rkStream,pkLink);
    return pkObject;
}
//----------------------------------------------------------------------------
void MgcIKJoint::Load (MgcStream& rkStream, MgcStream::Link* pkLink)
{
    MgcObject::Load(rkStream,pkLink);
}
//----------------------------------------------------------------------------
void MgcIKJoint::Link (MgcStream& rkStream, MgcStream::Link* pkLink)
{
    MgcObject::Link(rkStream,pkLink);
}
//----------------------------------------------------------------------------
bool MgcIKJoint::Register (MgcStream& rkStream)
{
    return MgcObject::Register(rkStream);
}
//----------------------------------------------------------------------------
void MgcIKJoint::Save (MgcStream& rkStream)
{
    MgcObject::Save(rkStream);
}
//----------------------------------------------------------------------------