/indra/llcharacter/llkeyframestandmotion.cpp
C++ | 338 lines | 197 code | 53 blank | 88 comment | 18 complexity | 078795424b52a9f38aa40f0377eb1304 MD5 | raw file
1/** 2 * @file llkeyframestandmotion.cpp 3 * @brief Implementation of LLKeyframeStandMotion class. 4 * 5 * $LicenseInfo:firstyear=2001&license=viewerlgpl$ 6 * Second Life Viewer Source Code 7 * Copyright (C) 2010, Linden Research, Inc. 8 * 9 * This library is free software; you can redistribute it and/or 10 * modify it under the terms of the GNU Lesser General Public 11 * License as published by the Free Software Foundation; 12 * version 2.1 of the License only. 13 * 14 * This library is distributed in the hope that it will be useful, 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 * Lesser General Public License for more details. 18 * 19 * You should have received a copy of the GNU Lesser General Public 20 * License along with this library; if not, write to the Free Software 21 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 22 * 23 * Linden Research, Inc., 945 Battery Street, San Francisco, CA 94111 USA 24 * $/LicenseInfo$ 25 */ 26 27//----------------------------------------------------------------------------- 28// Header Files 29//----------------------------------------------------------------------------- 30#include "linden_common.h" 31 32#include "llkeyframestandmotion.h" 33#include "llcharacter.h" 34 35//----------------------------------------------------------------------------- 36// Macros and consts 37//----------------------------------------------------------------------------- 38#define GO_TO_KEY_POSE 1 39#define MIN_TRACK_SPEED 0.01f 40const F32 ROTATION_THRESHOLD = 0.6f; 41const F32 POSITION_THRESHOLD = 0.1f; 42 43//----------------------------------------------------------------------------- 44// LLKeyframeStandMotion() 45// Class Constructor 46//----------------------------------------------------------------------------- 47LLKeyframeStandMotion::LLKeyframeStandMotion(const LLUUID &id) : LLKeyframeMotion(id) 48{ 49 mFlipFeet = FALSE; 50 mCharacter = NULL; 51 52 // create kinematic hierarchy 53 mPelvisJoint.addChild( &mHipLeftJoint ); 54 mHipLeftJoint.addChild( &mKneeLeftJoint ); 55 mKneeLeftJoint.addChild( &mAnkleLeftJoint ); 56 mPelvisJoint.addChild( &mHipRightJoint ); 57 mHipRightJoint.addChild( &mKneeRightJoint ); 58 mKneeRightJoint.addChild( &mAnkleRightJoint ); 59 60 mPelvisState = NULL; 61 62 mHipLeftState = NULL; 63 mKneeLeftState = NULL; 64 mAnkleLeftState = NULL; 65 66 mHipRightState = NULL; 67 mKneeRightState = NULL; 68 mAnkleRightState = NULL; 69 70 mTrackAnkles = TRUE; 71 72 mFrameNum = 0; 73} 74 75 76//----------------------------------------------------------------------------- 77// ~LLKeyframeStandMotion() 78// Class Destructor 79//----------------------------------------------------------------------------- 80LLKeyframeStandMotion::~LLKeyframeStandMotion() 81{ 82} 83 84 85//----------------------------------------------------------------------------- 86// LLKeyframeStandMotion::onInitialize() 87//----------------------------------------------------------------------------- 88LLMotion::LLMotionInitStatus LLKeyframeStandMotion::onInitialize(LLCharacter *character) 89{ 90 // save character pointer for later use 91 mCharacter = character; 92 93 mFlipFeet = FALSE; 94 95 // load keyframe data, setup pose and joint states 96 LLMotion::LLMotionInitStatus status = LLKeyframeMotion::onInitialize(character); 97 if ( status == STATUS_FAILURE ) 98 { 99 return status; 100 } 101 102 // find the necessary joint states 103 LLPose *pose = getPose(); 104 mPelvisState = pose->findJointState("mPelvis"); 105 106 mHipLeftState = pose->findJointState("mHipLeft"); 107 mKneeLeftState = pose->findJointState("mKneeLeft"); 108 mAnkleLeftState = pose->findJointState("mAnkleLeft"); 109 110 mHipRightState = pose->findJointState("mHipRight"); 111 mKneeRightState = pose->findJointState("mKneeRight"); 112 mAnkleRightState = pose->findJointState("mAnkleRight"); 113 114 if ( !mPelvisState || 115 !mHipLeftState || 116 !mKneeLeftState || 117 !mAnkleLeftState || 118 !mHipRightState || 119 !mKneeRightState || 120 !mAnkleRightState ) 121 { 122 llinfos << getName() << ": Can't find necessary joint states" << llendl; 123 return STATUS_FAILURE; 124 } 125 126 return STATUS_SUCCESS; 127} 128 129//----------------------------------------------------------------------------- 130// LLKeyframeStandMotion::onActivate() 131//----------------------------------------------------------------------------- 132BOOL LLKeyframeStandMotion::onActivate() 133{ 134 //------------------------------------------------------------------------- 135 // setup the IK solvers 136 //------------------------------------------------------------------------- 137 mIKLeft.setPoleVector( LLVector3(1.0f, 0.0f, 0.0f)); 138 mIKRight.setPoleVector( LLVector3(1.0f, 0.0f, 0.0f)); 139 mIKLeft.setBAxis( LLVector3(0.05f, 1.0f, 0.0f)); 140 mIKRight.setBAxis( LLVector3(-0.05f, 1.0f, 0.0f)); 141 142 mLastGoodPelvisRotation.loadIdentity(); 143 mLastGoodPosition.clearVec(); 144 145 mFrameNum = 0; 146 147 return LLKeyframeMotion::onActivate(); 148} 149 150//----------------------------------------------------------------------------- 151// LLKeyframeStandMotion::onDeactivate() 152//----------------------------------------------------------------------------- 153void LLKeyframeStandMotion::onDeactivate() 154{ 155 LLKeyframeMotion::onDeactivate(); 156} 157 158//----------------------------------------------------------------------------- 159// LLKeyframeStandMotion::onUpdate() 160//----------------------------------------------------------------------------- 161BOOL LLKeyframeStandMotion::onUpdate(F32 time, U8* joint_mask) 162{ 163 //------------------------------------------------------------------------- 164 // let the base class update the cycle 165 //------------------------------------------------------------------------- 166 BOOL status = LLKeyframeMotion::onUpdate(time, joint_mask); 167 if (!status) 168 { 169 return FALSE; 170 } 171 172 LLVector3 root_world_pos = mPelvisState->getJoint()->getParent()->getWorldPosition(); 173 174 // have we received a valid world position for this avatar? 175 if (root_world_pos.isExactlyZero()) 176 { 177 return TRUE; 178 } 179 180 //------------------------------------------------------------------------- 181 // Stop tracking (start locking) ankles once ease in is done. 182 // Setting this here ensures we track until we get valid foot position. 183 //------------------------------------------------------------------------- 184 if (dot(mPelvisState->getJoint()->getWorldRotation(), mLastGoodPelvisRotation) < ROTATION_THRESHOLD) 185 { 186 mLastGoodPelvisRotation = mPelvisState->getJoint()->getWorldRotation(); 187 mLastGoodPelvisRotation.normalize(); 188 mTrackAnkles = TRUE; 189 } 190 else if ((mCharacter->getCharacterPosition() - mLastGoodPosition).magVecSquared() > POSITION_THRESHOLD) 191 { 192 mLastGoodPosition = mCharacter->getCharacterPosition(); 193 mTrackAnkles = TRUE; 194 } 195 else if (mPose.getWeight() < 1.f) 196 { 197 mTrackAnkles = TRUE; 198 } 199 200 201 //------------------------------------------------------------------------- 202 // propagate joint positions to internal versions 203 //------------------------------------------------------------------------- 204 mPelvisJoint.setPosition( 205 root_world_pos + 206 mPelvisState->getPosition() ); 207 208 mHipLeftJoint.setPosition( mHipLeftState->getJoint()->getPosition() ); 209 mKneeLeftJoint.setPosition( mKneeLeftState->getJoint()->getPosition() ); 210 mAnkleLeftJoint.setPosition( mAnkleLeftState->getJoint()->getPosition() ); 211 212 mHipLeftJoint.setScale( mHipLeftState->getJoint()->getScale() ); 213 mKneeLeftJoint.setScale( mKneeLeftState->getJoint()->getScale() ); 214 mAnkleLeftJoint.setScale( mAnkleLeftState->getJoint()->getScale() ); 215 216 mHipRightJoint.setPosition( mHipRightState->getJoint()->getPosition() ); 217 mKneeRightJoint.setPosition( mKneeRightState->getJoint()->getPosition() ); 218 mAnkleRightJoint.setPosition( mAnkleRightState->getJoint()->getPosition() ); 219 220 mHipRightJoint.setScale( mHipRightState->getJoint()->getScale() ); 221 mKneeRightJoint.setScale( mKneeRightState->getJoint()->getScale() ); 222 mAnkleRightJoint.setScale( mAnkleRightState->getJoint()->getScale() ); 223 //------------------------------------------------------------------------- 224 // propagate joint rotations to internal versions 225 //------------------------------------------------------------------------- 226 mPelvisJoint.setRotation( mPelvisState->getJoint()->getWorldRotation() ); 227 228#if GO_TO_KEY_POSE 229 mHipLeftJoint.setRotation( mHipLeftState->getRotation() ); 230 mKneeLeftJoint.setRotation( mKneeLeftState->getRotation() ); 231 mAnkleLeftJoint.setRotation( mAnkleLeftState->getRotation() ); 232 233 mHipRightJoint.setRotation( mHipRightState->getRotation() ); 234 mKneeRightJoint.setRotation( mKneeRightState->getRotation() ); 235 mAnkleRightJoint.setRotation( mAnkleRightState->getRotation() ); 236#else 237 mHipLeftJoint.setRotation( mHipLeftState->getJoint()->getRotation() ); 238 mKneeLeftJoint.setRotation( mKneeLeftState->getJoint()->getRotation() ); 239 mAnkleLeftJoint.setRotation( mAnkleLeftState->getJoint()->getRotation() ); 240 241 mHipRightJoint.setRotation( mHipRightState->getJoint()->getRotation() ); 242 mKneeRightJoint.setRotation( mKneeRightState->getJoint()->getRotation() ); 243 mAnkleRightJoint.setRotation( mAnkleRightState->getJoint()->getRotation() ); 244#endif 245 246 // need to wait for underlying keyframe motion to affect the skeleton 247 if (mFrameNum == 2) 248 { 249 mIKLeft.setupJoints( &mHipLeftJoint, &mKneeLeftJoint, &mAnkleLeftJoint, &mTargetLeft ); 250 mIKRight.setupJoints( &mHipRightJoint, &mKneeRightJoint, &mAnkleRightJoint, &mTargetRight ); 251 } 252 else if (mFrameNum < 2) 253 { 254 mFrameNum++; 255 return TRUE; 256 } 257 258 mFrameNum++; 259 260 //------------------------------------------------------------------------- 261 // compute target position by projecting ankles to the ground 262 //------------------------------------------------------------------------- 263 if ( mTrackAnkles ) 264 { 265 mCharacter->getGround( mAnkleLeftJoint.getWorldPosition(), mPositionLeft, mNormalLeft); 266 mCharacter->getGround( mAnkleRightJoint.getWorldPosition(), mPositionRight, mNormalRight); 267 268 mTargetLeft.setPosition( mPositionLeft ); 269 mTargetRight.setPosition( mPositionRight ); 270 } 271 272 //------------------------------------------------------------------------- 273 // update solvers 274 //------------------------------------------------------------------------- 275 mIKLeft.solve(); 276 mIKRight.solve(); 277 278 //------------------------------------------------------------------------- 279 // make ankle rotation conform to the ground 280 //------------------------------------------------------------------------- 281 if ( mTrackAnkles ) 282 { 283 LLVector4 dirLeft4 = mAnkleLeftJoint.getWorldMatrix().getFwdRow4(); 284 LLVector4 dirRight4 = mAnkleRightJoint.getWorldMatrix().getFwdRow4(); 285 LLVector3 dirLeft = vec4to3( dirLeft4 ); 286 LLVector3 dirRight = vec4to3( dirRight4 ); 287 288 LLVector3 up; 289 LLVector3 dir; 290 LLVector3 left; 291 292 up = mNormalLeft; 293 up.normVec(); 294 if (mFlipFeet) 295 { 296 up *= -1.0f; 297 } 298 dir = dirLeft; 299 dir.normVec(); 300 left = up % dir; 301 left.normVec(); 302 dir = left % up; 303 mRotationLeft = LLQuaternion( dir, left, up ); 304 305 up = mNormalRight; 306 up.normVec(); 307 if (mFlipFeet) 308 { 309 up *= -1.0f; 310 } 311 dir = dirRight; 312 dir.normVec(); 313 left = up % dir; 314 left.normVec(); 315 dir = left % up; 316 mRotationRight = LLQuaternion( dir, left, up ); 317 } 318 mAnkleLeftJoint.setWorldRotation( mRotationLeft ); 319 mAnkleRightJoint.setWorldRotation( mRotationRight ); 320 321 //------------------------------------------------------------------------- 322 // propagate joint rotations to joint states 323 //------------------------------------------------------------------------- 324 mHipLeftState->setRotation( mHipLeftJoint.getRotation() ); 325 mKneeLeftState->setRotation( mKneeLeftJoint.getRotation() ); 326 mAnkleLeftState->setRotation( mAnkleLeftJoint.getRotation() ); 327 328 mHipRightState->setRotation( mHipRightJoint.getRotation() ); 329 mKneeRightState->setRotation( mKneeRightJoint.getRotation() ); 330 mAnkleRightState->setRotation( mAnkleRightJoint.getRotation() ); 331 332 //llinfos << "Stand drift amount " << (mCharacter->getCharacterPosition() - mLastGoodPosition).magVec() << llendl; 333 334// llinfos << "DEBUG: " << speed << " : " << mTrackAnkles << llendl; 335 return TRUE; 336} 337 338// End