diff options
Diffstat (limited to 'indra/llcharacter/llkeyframestandmotion.cpp')
| -rw-r--r-- | indra/llcharacter/llkeyframestandmotion.cpp | 320 |
1 files changed, 320 insertions, 0 deletions
diff --git a/indra/llcharacter/llkeyframestandmotion.cpp b/indra/llcharacter/llkeyframestandmotion.cpp new file mode 100644 index 0000000000..6810fb450f --- /dev/null +++ b/indra/llcharacter/llkeyframestandmotion.cpp @@ -0,0 +1,320 @@ +/** + * @file llkeyframestandmotion.cpp + * @brief Implementation of LLKeyframeStandMotion class. + * + * Copyright (c) 2001-$CurrentYear$, Linden Research, Inc. + * $License$ + */ + +//----------------------------------------------------------------------------- +// Header Files +//----------------------------------------------------------------------------- +#include "linden_common.h" + +#include "llkeyframestandmotion.h" +#include "llcharacter.h" + +//----------------------------------------------------------------------------- +// Macros and consts +//----------------------------------------------------------------------------- +#define GO_TO_KEY_POSE 1 +#define MIN_TRACK_SPEED 0.01f +const F32 ROTATION_THRESHOLD = 0.6f; +const F32 POSITION_THRESHOLD = 0.1f; + +//----------------------------------------------------------------------------- +// LLKeyframeStandMotion() +// Class Constructor +//----------------------------------------------------------------------------- +LLKeyframeStandMotion::LLKeyframeStandMotion(const LLUUID &id) : LLKeyframeMotion(id) +{ + mFlipFeet = FALSE; + mCharacter = NULL; + + // create kinematic hierarchy + mPelvisJoint.addChild( &mHipLeftJoint ); + mHipLeftJoint.addChild( &mKneeLeftJoint ); + mKneeLeftJoint.addChild( &mAnkleLeftJoint ); + mPelvisJoint.addChild( &mHipRightJoint ); + mHipRightJoint.addChild( &mKneeRightJoint ); + mKneeRightJoint.addChild( &mAnkleRightJoint ); + + mPelvisState = NULL; + + mHipLeftState = NULL; + mKneeLeftState = NULL; + mAnkleLeftState = NULL; + + mHipRightState = NULL; + mKneeRightState = NULL; + mAnkleRightState = NULL; + + mTrackAnkles = TRUE; + + mFrameNum = 0; +} + + +//----------------------------------------------------------------------------- +// ~LLKeyframeStandMotion() +// Class Destructor +//----------------------------------------------------------------------------- +LLKeyframeStandMotion::~LLKeyframeStandMotion() +{ +} + + +//----------------------------------------------------------------------------- +// LLKeyframeStandMotion::onInitialize() +//----------------------------------------------------------------------------- +LLMotion::LLMotionInitStatus LLKeyframeStandMotion::onInitialize(LLCharacter *character) +{ + // save character pointer for later use + mCharacter = character; + + mFlipFeet = FALSE; + + // load keyframe data, setup pose and joint states + LLMotion::LLMotionInitStatus status = LLKeyframeMotion::onInitialize(character); + if ( status == STATUS_FAILURE ) + { + return status; + } + + // find the necessary joint states + LLPose *pose = getPose(); + mPelvisState = pose->findJointState("mPelvis"); + + mHipLeftState = pose->findJointState("mHipLeft"); + mKneeLeftState = pose->findJointState("mKneeLeft"); + mAnkleLeftState = pose->findJointState("mAnkleLeft"); + + mHipRightState = pose->findJointState("mHipRight"); + mKneeRightState = pose->findJointState("mKneeRight"); + mAnkleRightState = pose->findJointState("mAnkleRight"); + + if ( !mPelvisState || + !mHipLeftState || + !mKneeLeftState || + !mAnkleLeftState || + !mHipRightState || + !mKneeRightState || + !mAnkleRightState ) + { + llinfos << getName() << ": Can't find necessary joint states" << llendl; + return STATUS_FAILURE; + } + + return STATUS_SUCCESS; +} + +//----------------------------------------------------------------------------- +// LLKeyframeStandMotion::onActivate() +//----------------------------------------------------------------------------- +BOOL LLKeyframeStandMotion::onActivate() +{ + //------------------------------------------------------------------------- + // setup the IK solvers + //------------------------------------------------------------------------- + mIKLeft.setPoleVector( LLVector3(1.0f, 0.0f, 0.0f)); + mIKRight.setPoleVector( LLVector3(1.0f, 0.0f, 0.0f)); + mIKLeft.setBAxis( LLVector3(0.05f, 1.0f, 0.0f)); + mIKRight.setBAxis( LLVector3(-0.05f, 1.0f, 0.0f)); + + mLastGoodPelvisRotation.loadIdentity(); + mLastGoodPosition.clearVec(); + + mFrameNum = 0; + + return LLKeyframeMotion::onActivate(); +} + +//----------------------------------------------------------------------------- +// LLKeyframeStandMotion::onDeactivate() +//----------------------------------------------------------------------------- +void LLKeyframeStandMotion::onDeactivate() +{ + LLKeyframeMotion::onDeactivate(); +} + +//----------------------------------------------------------------------------- +// LLKeyframeStandMotion::onUpdate() +//----------------------------------------------------------------------------- +BOOL LLKeyframeStandMotion::onUpdate(F32 time, U8* joint_mask) +{ + //------------------------------------------------------------------------- + // let the base class update the cycle + //------------------------------------------------------------------------- + BOOL status = LLKeyframeMotion::onUpdate(time, joint_mask); + if (!status) + { + return FALSE; + } + + LLVector3 root_world_pos = mPelvisState->getJoint()->getParent()->getWorldPosition(); + + // have we received a valid world position for this avatar? + if (root_world_pos.isExactlyZero()) + { + return TRUE; + } + + //------------------------------------------------------------------------- + // Stop tracking (start locking) ankles once ease in is done. + // Setting this here ensures we track until we get valid foot position. + //------------------------------------------------------------------------- + if (dot(mPelvisState->getJoint()->getWorldRotation(), mLastGoodPelvisRotation) < ROTATION_THRESHOLD) + { + mLastGoodPelvisRotation = mPelvisState->getJoint()->getWorldRotation(); + mLastGoodPelvisRotation.normQuat(); + mTrackAnkles = TRUE; + } + else if ((mCharacter->getCharacterPosition() - mLastGoodPosition).magVecSquared() > POSITION_THRESHOLD) + { + mLastGoodPosition = mCharacter->getCharacterPosition(); + mTrackAnkles = TRUE; + } + else if (mPose.getWeight() < 1.f) + { + mTrackAnkles = TRUE; + } + + + //------------------------------------------------------------------------- + // propagate joint positions to internal versions + //------------------------------------------------------------------------- + mPelvisJoint.setPosition( + root_world_pos + + mPelvisState->getPosition() ); + + mHipLeftJoint.setPosition( mHipLeftState->getJoint()->getPosition() ); + mKneeLeftJoint.setPosition( mKneeLeftState->getJoint()->getPosition() ); + mAnkleLeftJoint.setPosition( mAnkleLeftState->getJoint()->getPosition() ); + + mHipLeftJoint.setScale( mHipLeftState->getJoint()->getScale() ); + mKneeLeftJoint.setScale( mKneeLeftState->getJoint()->getScale() ); + mAnkleLeftJoint.setScale( mAnkleLeftState->getJoint()->getScale() ); + + mHipRightJoint.setPosition( mHipRightState->getJoint()->getPosition() ); + mKneeRightJoint.setPosition( mKneeRightState->getJoint()->getPosition() ); + mAnkleRightJoint.setPosition( mAnkleRightState->getJoint()->getPosition() ); + + mHipRightJoint.setScale( mHipRightState->getJoint()->getScale() ); + mKneeRightJoint.setScale( mKneeRightState->getJoint()->getScale() ); + mAnkleRightJoint.setScale( mAnkleRightState->getJoint()->getScale() ); + //------------------------------------------------------------------------- + // propagate joint rotations to internal versions + //------------------------------------------------------------------------- + mPelvisJoint.setRotation( mPelvisState->getJoint()->getWorldRotation() ); + +#if GO_TO_KEY_POSE + mHipLeftJoint.setRotation( mHipLeftState->getRotation() ); + mKneeLeftJoint.setRotation( mKneeLeftState->getRotation() ); + mAnkleLeftJoint.setRotation( mAnkleLeftState->getRotation() ); + + mHipRightJoint.setRotation( mHipRightState->getRotation() ); + mKneeRightJoint.setRotation( mKneeRightState->getRotation() ); + mAnkleRightJoint.setRotation( mAnkleRightState->getRotation() ); +#else + mHipLeftJoint.setRotation( mHipLeftState->getJoint()->getRotation() ); + mKneeLeftJoint.setRotation( mKneeLeftState->getJoint()->getRotation() ); + mAnkleLeftJoint.setRotation( mAnkleLeftState->getJoint()->getRotation() ); + + mHipRightJoint.setRotation( mHipRightState->getJoint()->getRotation() ); + mKneeRightJoint.setRotation( mKneeRightState->getJoint()->getRotation() ); + mAnkleRightJoint.setRotation( mAnkleRightState->getJoint()->getRotation() ); +#endif + + // need to wait for underlying keyframe motion to affect the skeleton + if (mFrameNum == 2) + { + mIKLeft.setupJoints( &mHipLeftJoint, &mKneeLeftJoint, &mAnkleLeftJoint, &mTargetLeft ); + mIKRight.setupJoints( &mHipRightJoint, &mKneeRightJoint, &mAnkleRightJoint, &mTargetRight ); + } + else if (mFrameNum < 2) + { + mFrameNum++; + return TRUE; + } + + mFrameNum++; + + //------------------------------------------------------------------------- + // compute target position by projecting ankles to the ground + //------------------------------------------------------------------------- + if ( mTrackAnkles ) + { + mCharacter->getGround( mAnkleLeftJoint.getWorldPosition(), mPositionLeft, mNormalLeft); + mCharacter->getGround( mAnkleRightJoint.getWorldPosition(), mPositionRight, mNormalRight); + + mTargetLeft.setPosition( mPositionLeft ); + mTargetRight.setPosition( mPositionRight ); + } + + //------------------------------------------------------------------------- + // update solvers + //------------------------------------------------------------------------- + mIKLeft.solve(); + mIKRight.solve(); + + //------------------------------------------------------------------------- + // make ankle rotation conform to the ground + //------------------------------------------------------------------------- + if ( mTrackAnkles ) + { + LLVector4 dirLeft4 = mAnkleLeftJoint.getWorldMatrix().getFwdRow4(); + LLVector4 dirRight4 = mAnkleRightJoint.getWorldMatrix().getFwdRow4(); + LLVector3 dirLeft = vec4to3( dirLeft4 ); + LLVector3 dirRight = vec4to3( dirRight4 ); + + LLVector3 up; + LLVector3 dir; + LLVector3 left; + + up = mNormalLeft; + up.normVec(); + if (mFlipFeet) + { + up *= -1.0f; + } + dir = dirLeft; + dir.normVec(); + left = up % dir; + left.normVec(); + dir = left % up; + mRotationLeft = LLQuaternion( dir, left, up ); + + up = mNormalRight; + up.normVec(); + if (mFlipFeet) + { + up *= -1.0f; + } + dir = dirRight; + dir.normVec(); + left = up % dir; + left.normVec(); + dir = left % up; + mRotationRight = LLQuaternion( dir, left, up ); + } + mAnkleLeftJoint.setWorldRotation( mRotationLeft ); + mAnkleRightJoint.setWorldRotation( mRotationRight ); + + //------------------------------------------------------------------------- + // propagate joint rotations to joint states + //------------------------------------------------------------------------- + mHipLeftState->setRotation( mHipLeftJoint.getRotation() ); + mKneeLeftState->setRotation( mKneeLeftJoint.getRotation() ); + mAnkleLeftState->setRotation( mAnkleLeftJoint.getRotation() ); + + mHipRightState->setRotation( mHipRightJoint.getRotation() ); + mKneeRightState->setRotation( mKneeRightJoint.getRotation() ); + mAnkleRightState->setRotation( mAnkleRightJoint.getRotation() ); + + //llinfos << "Stand drift amount " << (mCharacter->getCharacterPosition() - mLastGoodPosition).magVec() << llendl; + +// llinfos << "DEBUG: " << speed << " : " << mTrackAnkles << llendl; + return TRUE; +} + +// End |