Second Life Viewer: llkeyframestandmotion.cpp Source File

00001 
00032 //-----------------------------------------------------------------------------
00033 // Header Files
00034 //-----------------------------------------------------------------------------
00035 #include "linden_common.h"
00036 
00037 #include "llkeyframestandmotion.h"
00038 #include "llcharacter.h"
00039 
00040 //-----------------------------------------------------------------------------
00041 // Macros and consts
00042 //-----------------------------------------------------------------------------
00043 #define GO_TO_KEY_POSE  1
00044 #define MIN_TRACK_SPEED 0.01f
00045 const F32 ROTATION_THRESHOLD = 0.6f;
00046 const F32 POSITION_THRESHOLD = 0.1f;
00047 
00048 //-----------------------------------------------------------------------------
00049 // LLKeyframeStandMotion()
00050 // Class Constructor
00051 //-----------------------------------------------------------------------------
00052 LLKeyframeStandMotion::LLKeyframeStandMotion(const LLUUID &id) : LLKeyframeMotion(id)
00053 {
00054         mFlipFeet = FALSE;
00055         mCharacter = NULL;
00056 
00057         // create kinematic hierarchy
00058         mPelvisJoint.addChild( &mHipLeftJoint );
00059                 mHipLeftJoint.addChild( &mKneeLeftJoint );
00060                         mKneeLeftJoint.addChild( &mAnkleLeftJoint );
00061         mPelvisJoint.addChild( &mHipRightJoint );
00062                 mHipRightJoint.addChild( &mKneeRightJoint );
00063                         mKneeRightJoint.addChild( &mAnkleRightJoint );
00064 
00065         mPelvisState = NULL;
00066 
00067         mHipLeftState =  NULL;
00068         mKneeLeftState =  NULL;
00069         mAnkleLeftState =  NULL;
00070 
00071         mHipRightState =  NULL;
00072         mKneeRightState =  NULL;
00073         mAnkleRightState =  NULL;
00074 
00075         mTrackAnkles = TRUE;
00076 
00077         mFrameNum = 0;
00078 }
00079 
00080 
00081 //-----------------------------------------------------------------------------
00082 // ~LLKeyframeStandMotion()
00083 // Class Destructor
00084 //-----------------------------------------------------------------------------
00085 LLKeyframeStandMotion::~LLKeyframeStandMotion()
00086 {
00087 }
00088 
00089 
00090 //-----------------------------------------------------------------------------
00091 // LLKeyframeStandMotion::onInitialize()
00092 //-----------------------------------------------------------------------------
00093 LLMotion::LLMotionInitStatus LLKeyframeStandMotion::onInitialize(LLCharacter *character)
00094 {
00095         // save character pointer for later use
00096         mCharacter = character;
00097 
00098         mFlipFeet = FALSE;
00099 
00100         // load keyframe data, setup pose and joint states
00101         LLMotion::LLMotionInitStatus status = LLKeyframeMotion::onInitialize(character);
00102         if ( status == STATUS_FAILURE )
00103         {
00104                 return status;
00105         }
00106 
00107         // find the necessary joint states
00108         LLPose *pose = getPose();
00109         mPelvisState = pose->findJointState("mPelvis");
00110         
00111         mHipLeftState = pose->findJointState("mHipLeft");
00112         mKneeLeftState = pose->findJointState("mKneeLeft");
00113         mAnkleLeftState = pose->findJointState("mAnkleLeft");
00114 
00115         mHipRightState = pose->findJointState("mHipRight");
00116         mKneeRightState = pose->findJointState("mKneeRight");
00117         mAnkleRightState = pose->findJointState("mAnkleRight");
00118 
00119         if (    !mPelvisState ||
00120                         !mHipLeftState ||
00121                         !mKneeLeftState ||
00122                         !mAnkleLeftState ||
00123                         !mHipRightState ||
00124                         !mKneeRightState ||
00125                         !mAnkleRightState )
00126         {
00127                 llinfos << getName() << ": Can't find necessary joint states" << llendl;
00128                 return STATUS_FAILURE;
00129         }
00130 
00131         return STATUS_SUCCESS;
00132 }
00133 
00134 //-----------------------------------------------------------------------------
00135 // LLKeyframeStandMotion::onActivate()
00136 //-----------------------------------------------------------------------------
00137 BOOL LLKeyframeStandMotion::onActivate()
00138 {
00139         //-------------------------------------------------------------------------
00140         // setup the IK solvers
00141         //-------------------------------------------------------------------------
00142         mIKLeft.setPoleVector( LLVector3(1.0f, 0.0f, 0.0f));
00143         mIKRight.setPoleVector( LLVector3(1.0f, 0.0f, 0.0f));
00144         mIKLeft.setBAxis( LLVector3(0.05f, 1.0f, 0.0f));
00145         mIKRight.setBAxis( LLVector3(-0.05f, 1.0f, 0.0f));
00146 
00147         mLastGoodPelvisRotation.loadIdentity();
00148         mLastGoodPosition.clearVec();
00149 
00150         mFrameNum = 0;
00151 
00152         return LLKeyframeMotion::onActivate();
00153 }
00154 
00155 //-----------------------------------------------------------------------------
00156 // LLKeyframeStandMotion::onDeactivate()
00157 //-----------------------------------------------------------------------------
00158 void LLKeyframeStandMotion::onDeactivate()
00159 {
00160         LLKeyframeMotion::onDeactivate();
00161 }
00162 
00163 //-----------------------------------------------------------------------------
00164 // LLKeyframeStandMotion::onUpdate()
00165 //-----------------------------------------------------------------------------
00166 BOOL LLKeyframeStandMotion::onUpdate(F32 time, U8* joint_mask)
00167 {
00168         //-------------------------------------------------------------------------
00169         // let the base class update the cycle
00170         //-------------------------------------------------------------------------
00171         BOOL status = LLKeyframeMotion::onUpdate(time, joint_mask);
00172         if (!status)
00173         {
00174                 return FALSE;
00175         }
00176 
00177         LLVector3 root_world_pos = mPelvisState->getJoint()->getParent()->getWorldPosition();
00178 
00179         // have we received a valid world position for this avatar?
00180         if (root_world_pos.isExactlyZero())
00181         {
00182                 return TRUE;
00183         }
00184 
00185         //-------------------------------------------------------------------------
00186         // Stop tracking (start locking) ankles once ease in is done.
00187         // Setting this here ensures we track until we get valid foot position.
00188         //-------------------------------------------------------------------------
00189         if (dot(mPelvisState->getJoint()->getWorldRotation(), mLastGoodPelvisRotation) < ROTATION_THRESHOLD)
00190         {
00191                 mLastGoodPelvisRotation = mPelvisState->getJoint()->getWorldRotation();
00192                 mLastGoodPelvisRotation.normQuat();
00193                 mTrackAnkles = TRUE;
00194         }
00195         else if ((mCharacter->getCharacterPosition() - mLastGoodPosition).magVecSquared() > POSITION_THRESHOLD)
00196         {
00197                 mLastGoodPosition = mCharacter->getCharacterPosition();
00198                 mTrackAnkles = TRUE;
00199         }
00200         else if (mPose.getWeight() < 1.f)
00201         {
00202                 mTrackAnkles = TRUE;
00203         }
00204 
00205 
00206         //-------------------------------------------------------------------------
00207         // propagate joint positions to internal versions
00208         //-------------------------------------------------------------------------
00209         mPelvisJoint.setPosition(
00210                         root_world_pos +
00211                         mPelvisState->getPosition() );
00212 
00213         mHipLeftJoint.setPosition( mHipLeftState->getJoint()->getPosition() );
00214         mKneeLeftJoint.setPosition( mKneeLeftState->getJoint()->getPosition() );
00215         mAnkleLeftJoint.setPosition( mAnkleLeftState->getJoint()->getPosition() );
00216 
00217         mHipLeftJoint.setScale( mHipLeftState->getJoint()->getScale() );
00218         mKneeLeftJoint.setScale( mKneeLeftState->getJoint()->getScale() );
00219         mAnkleLeftJoint.setScale( mAnkleLeftState->getJoint()->getScale() );
00220 
00221         mHipRightJoint.setPosition( mHipRightState->getJoint()->getPosition() );
00222         mKneeRightJoint.setPosition( mKneeRightState->getJoint()->getPosition() );
00223         mAnkleRightJoint.setPosition( mAnkleRightState->getJoint()->getPosition() );
00224 
00225         mHipRightJoint.setScale( mHipRightState->getJoint()->getScale() );
00226         mKneeRightJoint.setScale( mKneeRightState->getJoint()->getScale() );
00227         mAnkleRightJoint.setScale( mAnkleRightState->getJoint()->getScale() );
00228         //-------------------------------------------------------------------------
00229         // propagate joint rotations to internal versions
00230         //-------------------------------------------------------------------------
00231         mPelvisJoint.setRotation( mPelvisState->getJoint()->getWorldRotation() );
00232 
00233 #if GO_TO_KEY_POSE
00234         mHipLeftJoint.setRotation( mHipLeftState->getRotation() );
00235         mKneeLeftJoint.setRotation( mKneeLeftState->getRotation() );
00236         mAnkleLeftJoint.setRotation( mAnkleLeftState->getRotation() );
00237 
00238         mHipRightJoint.setRotation( mHipRightState->getRotation() );
00239         mKneeRightJoint.setRotation( mKneeRightState->getRotation() );
00240         mAnkleRightJoint.setRotation( mAnkleRightState->getRotation() );
00241 #else
00242         mHipLeftJoint.setRotation( mHipLeftState->getJoint()->getRotation() );
00243         mKneeLeftJoint.setRotation( mKneeLeftState->getJoint()->getRotation() );
00244         mAnkleLeftJoint.setRotation( mAnkleLeftState->getJoint()->getRotation() );
00245 
00246         mHipRightJoint.setRotation( mHipRightState->getJoint()->getRotation() );
00247         mKneeRightJoint.setRotation( mKneeRightState->getJoint()->getRotation() );
00248         mAnkleRightJoint.setRotation( mAnkleRightState->getJoint()->getRotation() );
00249 #endif
00250 
00251         // need to wait for underlying keyframe motion to affect the skeleton
00252         if (mFrameNum == 2)
00253         {
00254                 mIKLeft.setupJoints( &mHipLeftJoint, &mKneeLeftJoint, &mAnkleLeftJoint, &mTargetLeft );
00255                 mIKRight.setupJoints( &mHipRightJoint, &mKneeRightJoint, &mAnkleRightJoint, &mTargetRight );
00256         }
00257         else if (mFrameNum < 2)
00258         {
00259                 mFrameNum++;
00260                 return TRUE;
00261         }
00262 
00263         mFrameNum++;
00264 
00265         //-------------------------------------------------------------------------
00266         // compute target position by projecting ankles to the ground
00267         //-------------------------------------------------------------------------
00268         if ( mTrackAnkles )
00269         {
00270                 mCharacter->getGround( mAnkleLeftJoint.getWorldPosition(), mPositionLeft, mNormalLeft);
00271                 mCharacter->getGround( mAnkleRightJoint.getWorldPosition(), mPositionRight, mNormalRight);
00272 
00273                 mTargetLeft.setPosition( mPositionLeft );
00274                 mTargetRight.setPosition( mPositionRight );
00275         }
00276 
00277         //-------------------------------------------------------------------------
00278         // update solvers
00279         //-------------------------------------------------------------------------
00280         mIKLeft.solve();
00281         mIKRight.solve();
00282 
00283         //-------------------------------------------------------------------------
00284         // make ankle rotation conform to the ground
00285         //-------------------------------------------------------------------------
00286         if ( mTrackAnkles )
00287         {
00288                 LLVector4 dirLeft4 = mAnkleLeftJoint.getWorldMatrix().getFwdRow4();
00289                 LLVector4 dirRight4 = mAnkleRightJoint.getWorldMatrix().getFwdRow4();
00290                 LLVector3 dirLeft = vec4to3( dirLeft4 );
00291                 LLVector3 dirRight = vec4to3( dirRight4 );
00292 
00293                 LLVector3 up;
00294                 LLVector3 dir;
00295                 LLVector3 left;
00296 
00297                 up = mNormalLeft;
00298                 up.normVec();
00299                 if (mFlipFeet)
00300                 {
00301                         up *= -1.0f;
00302                 }
00303                 dir = dirLeft;
00304                 dir.normVec();
00305                 left = up % dir;
00306                 left.normVec();
00307                 dir = left % up;
00308                 mRotationLeft = LLQuaternion( dir, left, up );
00309 
00310                 up = mNormalRight;
00311                 up.normVec();
00312                 if (mFlipFeet)
00313                 {
00314                         up *= -1.0f;
00315                 }
00316                 dir = dirRight;
00317                 dir.normVec();
00318                 left = up % dir;
00319                 left.normVec();
00320                 dir = left % up;
00321                 mRotationRight = LLQuaternion( dir, left, up );
00322         }
00323         mAnkleLeftJoint.setWorldRotation( mRotationLeft );
00324         mAnkleRightJoint.setWorldRotation( mRotationRight );
00325 
00326         //-------------------------------------------------------------------------
00327         // propagate joint rotations to joint states
00328         //-------------------------------------------------------------------------
00329         mHipLeftState->setRotation( mHipLeftJoint.getRotation() );
00330         mKneeLeftState->setRotation( mKneeLeftJoint.getRotation() );
00331         mAnkleLeftState->setRotation( mAnkleLeftJoint.getRotation() );
00332 
00333         mHipRightState->setRotation( mHipRightJoint.getRotation() );
00334         mKneeRightState->setRotation( mKneeRightJoint.getRotation() );
00335         mAnkleRightState->setRotation( mAnkleRightJoint.getRotation() );
00336 
00337         //llinfos << "Stand drift amount " << (mCharacter->getCharacterPosition() - mLastGoodPosition).magVec() << llendl;
00338 
00339 //      llinfos << "DEBUG: " << speed << " : " << mTrackAnkles << llendl;
00340         return TRUE;
00341 }
00342 
00343 // End