path: root/indra/llcharacter/llkeyframewalkmotion.cpp

/**
 * @file llkeyframewalkmotion.cpp
 * @brief Implementation of LLKeyframeWalkMotion class.
 *
 * $LicenseInfo:firstyear=2001&license=viewerlgpl$
 * Second Life Viewer Source Code
 * Copyright (C) 2010, Linden Research, Inc.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation;
 * version 2.1 of the License only.
 *
 * This library 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Linden Research, Inc., 945 Battery Street, San Francisco, CA  94111  USA
 * $/LicenseInfo$
 */

//-----------------------------------------------------------------------------
// Header Files
//-----------------------------------------------------------------------------
#include "linden_common.h"

#include "llkeyframewalkmotion.h"
#include "llcharacter.h"
#include "llmath.h"
#include "m3math.h"
#include "llcriticaldamp.h"

//-----------------------------------------------------------------------------
// Macros
//-----------------------------------------------------------------------------
const F32 MAX_WALK_PLAYBACK_SPEED = 8.f;        // max m/s for which we adjust walk cycle speed

const F32 MIN_WALK_SPEED = 0.1f;                // minimum speed at which we use velocity for down foot detection
const F32 TIME_EPSILON = 0.001f;                // minumum frame time
const F32 MAX_TIME_DELTA = 2.f;                 // max two seconds a frame for calculating interpolation
F32 SPEED_ADJUST_MAX_SEC = 2.f;                 // maximum adjustment to walk animation playback speed for a second
F32 ANIM_SPEED_MAX = 1.5f;                      // absolute upper limit on animation speed
const F32 MAX_ROLL = 0.6f;
const F32 SPEED_ADJUST_TIME_CONSTANT = 0.1f;    // time constant for speed adjustment interpolation

//-----------------------------------------------------------------------------
// LLKeyframeWalkMotion()
// Class Constructor
//-----------------------------------------------------------------------------
LLKeyframeWalkMotion::LLKeyframeWalkMotion(const LLUUID &id)
:   LLKeyframeMotion(id),
    mCharacter(NULL),
    mCyclePhase(0.0f),
    mRealTimeLast(0.0f),
    mAdjTimeLast(0.0f),
    mDownFoot(0)
{}


//-----------------------------------------------------------------------------
// ~LLKeyframeWalkMotion()
// Class Destructor
//-----------------------------------------------------------------------------
LLKeyframeWalkMotion::~LLKeyframeWalkMotion()
{}


//-----------------------------------------------------------------------------
// LLKeyframeWalkMotion::onInitialize()
//-----------------------------------------------------------------------------
LLMotion::LLMotionInitStatus LLKeyframeWalkMotion::onInitialize(LLCharacter *character)
{
    mCharacter = character;

    return LLKeyframeMotion::onInitialize(character);
}

//-----------------------------------------------------------------------------
// LLKeyframeWalkMotion::onActivate()
//-----------------------------------------------------------------------------
bool LLKeyframeWalkMotion::onActivate()
{
    mRealTimeLast = 0.0f;
    mAdjTimeLast = 0.0f;

    return LLKeyframeMotion::onActivate();
}

//-----------------------------------------------------------------------------
// LLKeyframeWalkMotion::onDeactivate()
//-----------------------------------------------------------------------------
void LLKeyframeWalkMotion::onDeactivate()
{
    mCharacter->removeAnimationData("Down Foot");
    LLKeyframeMotion::onDeactivate();
}

//-----------------------------------------------------------------------------
// LLKeyframeWalkMotion::onUpdate()
//-----------------------------------------------------------------------------
bool LLKeyframeWalkMotion::onUpdate(F32 time, U8* joint_mask)
{
    LL_PROFILE_ZONE_SCOPED;
    // compute time since last update
    F32 deltaTime = time - mRealTimeLast;

    void* speed_ptr = mCharacter->getAnimationData("Walk Speed");
    F32 speed = (speed_ptr) ? *((F32 *)speed_ptr) : 1.f;

    // adjust the passage of time accordingly
    F32 adjusted_time = mAdjTimeLast + (deltaTime * speed);

    // save time for next update
    mRealTimeLast = time;
    mAdjTimeLast = adjusted_time;

    // handle wrap around
    if (adjusted_time < 0.0f)
    {
        adjusted_time = getDuration() + fmod(adjusted_time, getDuration());
    }

    // let the base class update the cycle
    return LLKeyframeMotion::onUpdate( adjusted_time, joint_mask );
}

// End


//-----------------------------------------------------------------------------
// LLWalkAdjustMotion()
// Class Constructor
//-----------------------------------------------------------------------------
LLWalkAdjustMotion::LLWalkAdjustMotion(const LLUUID &id) :
    LLMotion(id),
    mLastTime(0.f),
    mAnimSpeed(0.f),
    mAdjustedSpeed(0.f),
    mRelativeDir(0.f),
    mAnkleOffset(0.f)
{
    mName = "walk_adjust";
    mPelvisState = new LLJointState;
}

//-----------------------------------------------------------------------------
// LLWalkAdjustMotion::onInitialize()
//-----------------------------------------------------------------------------
LLMotion::LLMotionInitStatus LLWalkAdjustMotion::onInitialize(LLCharacter *character)
{
    mCharacter = character;
    mLeftAnkleJoint = mCharacter->getJoint("mAnkleLeft");
    mRightAnkleJoint = mCharacter->getJoint("mAnkleRight");

    mPelvisJoint = mCharacter->getJoint("mPelvis");
    mPelvisState->setJoint( mPelvisJoint );
    if ( !mPelvisJoint )
    {
        LL_WARNS() << getName() << ": Can't get pelvis joint." << LL_ENDL;
        return STATUS_FAILURE;
    }

    mPelvisState->setUsage(LLJointState::POS);
    addJointState( mPelvisState );

    return STATUS_SUCCESS;
}

//-----------------------------------------------------------------------------
// LLWalkAdjustMotion::onActivate()
//-----------------------------------------------------------------------------
bool LLWalkAdjustMotion::onActivate()
{
    mAnimSpeed = 0.f;
    mAdjustedSpeed = 0.f;
    mRelativeDir = 1.f;
    mPelvisState->setPosition(LLVector3::zero);
    // store ankle positions for next frame
    mLastLeftFootGlobalPos = mCharacter->getPosGlobalFromAgent(mLeftAnkleJoint->getWorldPosition());
    mLastLeftFootGlobalPos.mdV[VZ] = 0.0;

    mLastRightFootGlobalPos = mCharacter->getPosGlobalFromAgent(mRightAnkleJoint->getWorldPosition());
    mLastRightFootGlobalPos.mdV[VZ] = 0.0;

    F32 leftAnkleOffset = (mLeftAnkleJoint->getWorldPosition() - mCharacter->getCharacterPosition()).magVec();
    F32 rightAnkleOffset = (mRightAnkleJoint->getWorldPosition() - mCharacter->getCharacterPosition()).magVec();
    mAnkleOffset = llmax(leftAnkleOffset, rightAnkleOffset);

    return true;
}

//-----------------------------------------------------------------------------
// LLWalkAdjustMotion::onUpdate()
//-----------------------------------------------------------------------------
bool LLWalkAdjustMotion::onUpdate(F32 time, U8* joint_mask)
{
    LL_PROFILE_ZONE_SCOPED;
    // delta_time is guaranteed to be non zero
    F32 delta_time = llclamp(time - mLastTime, TIME_EPSILON, MAX_TIME_DELTA);
    mLastTime = time;

    // find the avatar motion vector in the XY plane
    LLVector3 avatar_velocity = mCharacter->getCharacterVelocity() * mCharacter->getTimeDilation();
    avatar_velocity.mV[VZ] = 0.f;

    F32 speed = llclamp(avatar_velocity.magVec(), 0.f, MAX_WALK_PLAYBACK_SPEED);

    // grab avatar->world transforms
    LLQuaternion avatar_to_world_rot = mCharacter->getRootJoint()->getWorldRotation();

    LLQuaternion world_to_avatar_rot(avatar_to_world_rot);
    world_to_avatar_rot.conjugate();

    LLVector3 foot_slip_vector;

    // find foot drift along velocity vector
    if (speed > MIN_WALK_SPEED)
    {   // walking/running

        // calculate world-space foot drift
        // use global coordinates to seamlessly handle region crossings
        LLVector3d leftFootGlobalPosition = mCharacter->getPosGlobalFromAgent(mLeftAnkleJoint->getWorldPosition());
        leftFootGlobalPosition.mdV[VZ] = 0.0;
        LLVector3 leftFootDelta(leftFootGlobalPosition - mLastLeftFootGlobalPos);
        mLastLeftFootGlobalPos = leftFootGlobalPosition;

        LLVector3d rightFootGlobalPosition = mCharacter->getPosGlobalFromAgent(mRightAnkleJoint->getWorldPosition());
        rightFootGlobalPosition.mdV[VZ] = 0.0;
        LLVector3 rightFootDelta(rightFootGlobalPosition - mLastRightFootGlobalPos);
        mLastRightFootGlobalPos = rightFootGlobalPosition;

        // get foot drift along avatar direction of motion
        F32 left_foot_slip_amt = leftFootDelta * avatar_velocity;
        F32 right_foot_slip_amt = rightFootDelta * avatar_velocity;

        // if right foot is pushing back faster than left foot...
        if (right_foot_slip_amt < left_foot_slip_amt)
        {   //...use it to calculate optimal animation speed
            foot_slip_vector = rightFootDelta;
        }
        else
        {   // otherwise use the left foot
            foot_slip_vector = leftFootDelta;
        }

        // calculate ideal pelvis offset so that foot is glued to ground and damp towards it
        // this will soak up transient slippage
        //
        // FIXME: this interacts poorly with speed adjustment
        // mPelvisOffset compensates for foot drift by moving the avatar pelvis in the opposite
        // direction of the drift, up to a certain limited distance
        // but this will cause the animation playback rate calculation below to
        // kick in too slowly and sometimes start playing the animation in reverse.

        //mPelvisOffset -= PELVIS_COMPENSATION_WIEGHT * (foot_slip_vector * world_to_avatar_rot);//lerp(LLVector3::zero, -1.f * (foot_slip_vector * world_to_avatar_rot), LLSmoothInterpolation::getInterpolant(0.1f));

        ////F32 drift_comp_max = DRIFT_COMP_MAX_TOTAL * (llclamp(speed, 0.f, DRIFT_COMP_MAX_SPEED) / DRIFT_COMP_MAX_SPEED);
        //F32 drift_comp_max = DRIFT_COMP_MAX_TOTAL;

        //// clamp pelvis offset to a 90 degree arc behind the nominal position
        //// NB: this is an ADDITIVE amount that is accumulated every frame, so clamping it alone won't do the trick
        //// must clamp with absolute position of pelvis in mind
        //LLVector3 currentPelvisPos = mPelvisState->getJoint()->getPosition();
        //mPelvisOffset.mV[VX] = llclamp( mPelvisOffset.mV[VX], -drift_comp_max, drift_comp_max );
        //mPelvisOffset.mV[VY] = llclamp( mPelvisOffset.mV[VY], -drift_comp_max, drift_comp_max );
        //mPelvisOffset.mV[VZ] = 0.f;
        //
        //mLastRightFootGlobalPos += LLVector3d(mPelvisOffset * avatar_to_world_rot);
        //mLastLeftFootGlobalPos += LLVector3d(mPelvisOffset * avatar_to_world_rot);

        //foot_slip_vector -= mPelvisOffset;

        LLVector3 avatar_movement_dir = avatar_velocity;
        avatar_movement_dir.normalize();

        // planted foot speed is avatar velocity - foot slip amount along avatar movement direction
        F32 foot_speed = speed - ((foot_slip_vector * avatar_movement_dir) / delta_time);

        // multiply animation playback rate so that foot speed matches avatar speed
        F32 min_speed_multiplier = clamp_rescale(speed, 0.f, 1.f, 0.f, 0.1f);
        F32 desired_speed_multiplier = llclamp(speed / foot_speed, min_speed_multiplier, ANIM_SPEED_MAX);

        // blend towards new speed adjustment value
        F32 new_speed_adjust = LLSmoothInterpolation::lerp(mAdjustedSpeed, desired_speed_multiplier, SPEED_ADJUST_TIME_CONSTANT);

        // limit that rate at which the speed adjustment changes
        F32 speedDelta = llclamp(new_speed_adjust - mAdjustedSpeed, -SPEED_ADJUST_MAX_SEC * delta_time, SPEED_ADJUST_MAX_SEC * delta_time);
        mAdjustedSpeed += speedDelta;

        // modulate speed by dot products of facing and velocity
        // so that if we are moving sideways, we slow down the animation
        // and if we're moving backward, we walk backward
        // do this at the end to be more responsive to direction changes instead of in the above speed calculations
        F32 directional_factor = (avatar_movement_dir * world_to_avatar_rot).mV[VX];

        mAnimSpeed = mAdjustedSpeed * directional_factor;
    }
    else
    {   // standing/turning

        // damp out speed adjustment to 0
        mAnimSpeed = LLSmoothInterpolation::lerp(mAnimSpeed, 1.f, 0.2f);
        //mPelvisOffset = lerp(mPelvisOffset, LLVector3::zero, LLSmoothInterpolation::getInterpolant(0.2f));
    }

    // broadcast walk speed change
    mCharacter->setAnimationData("Walk Speed", &mAnimSpeed);

    // set position
    // need to update *some* joint to keep this animation active
    mPelvisState->setPosition(mPelvisOffset);

    return true;
}

//-----------------------------------------------------------------------------
// LLWalkAdjustMotion::onDeactivate()
//-----------------------------------------------------------------------------
void LLWalkAdjustMotion::onDeactivate()
{
    mCharacter->removeAnimationData("Walk Speed");
}

//-----------------------------------------------------------------------------
// LLFlyAdjustMotion::LLFlyAdjustMotion()
//-----------------------------------------------------------------------------
LLFlyAdjustMotion::LLFlyAdjustMotion(const LLUUID &id)
    : LLMotion(id),
      mRoll(0.f)
{
    mName = "fly_adjust";

    mPelvisState = new LLJointState;
}

//-----------------------------------------------------------------------------
// LLFlyAdjustMotion::onInitialize()
//-----------------------------------------------------------------------------
LLMotion::LLMotionInitStatus LLFlyAdjustMotion::onInitialize(LLCharacter *character)
{
    mCharacter = character;

    LLJoint* pelvisJoint = mCharacter->getJoint("mPelvis");
    mPelvisState->setJoint( pelvisJoint );
    if ( !pelvisJoint )
    {
        LL_WARNS() << getName() << ": Can't get pelvis joint." << LL_ENDL;
        return STATUS_FAILURE;
    }

    mPelvisState->setUsage(LLJointState::POS | LLJointState::ROT);
    addJointState( mPelvisState );

    return STATUS_SUCCESS;
}

//-----------------------------------------------------------------------------
// LLFlyAdjustMotion::onActivate()
//-----------------------------------------------------------------------------
bool LLFlyAdjustMotion::onActivate()
{
    mPelvisState->setPosition(LLVector3::zero);
    mPelvisState->setRotation(LLQuaternion::DEFAULT);
    mRoll = 0.f;
    return true;
}

//-----------------------------------------------------------------------------
// LLFlyAdjustMotion::onUpdate()
//-----------------------------------------------------------------------------
bool LLFlyAdjustMotion::onUpdate(F32 time, U8* joint_mask)
{
    LL_PROFILE_ZONE_SCOPED;
    LLVector3 ang_vel = mCharacter->getCharacterAngularVelocity() * mCharacter->getTimeDilation();
    F32 speed = mCharacter->getCharacterVelocity().magVec();

    F32 roll_factor = clamp_rescale(speed, 7.f, 15.f, 0.f, -MAX_ROLL);
    F32 target_roll = llclamp(ang_vel.mV[VZ], -4.f, 4.f) * roll_factor;

    // roll is critically damped interpolation between current roll and angular velocity-derived target roll
    mRoll = LLSmoothInterpolation::lerp(mRoll, target_roll, U32Milliseconds(100));

    LLQuaternion roll(mRoll, LLVector3(0.f, 0.f, 1.f));
    mPelvisState->setRotation(roll);

    return true;
}