/indra/llcharacter/llkeyframestandmotion.cpp

https://bitbucket.org/lindenlab/viewer-beta/ · 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. // Header Files
  28. //-----------------------------------------------------------------------------
  29. #include "linden_common.h"
  30. #include "llkeyframestandmotion.h"
  31. #include "llcharacter.h"
  32. //-----------------------------------------------------------------------------
  33. // Macros and consts
  34. //-----------------------------------------------------------------------------
  35. #define GO_TO_KEY_POSE 1
  36. #define MIN_TRACK_SPEED 0.01f
  37. const F32 ROTATION_THRESHOLD = 0.6f;
  38. const F32 POSITION_THRESHOLD = 0.1f;
  39. //-----------------------------------------------------------------------------
  40. // LLKeyframeStandMotion()
  41. // Class Constructor
  42. //-----------------------------------------------------------------------------
  43. LLKeyframeStandMotion::LLKeyframeStandMotion(const LLUUID &id) : LLKeyframeMotion(id)
  44. {
  45. mFlipFeet = FALSE;
  46. mCharacter = NULL;
  47. // create kinematic hierarchy
  48. mPelvisJoint.addChild( &mHipLeftJoint );
  49. mHipLeftJoint.addChild( &mKneeLeftJoint );
  50. mKneeLeftJoint.addChild( &mAnkleLeftJoint );
  51. mPelvisJoint.addChild( &mHipRightJoint );
  52. mHipRightJoint.addChild( &mKneeRightJoint );
  53. mKneeRightJoint.addChild( &mAnkleRightJoint );
  54. mPelvisState = NULL;
  55. mHipLeftState = NULL;
  56. mKneeLeftState = NULL;
  57. mAnkleLeftState = NULL;
  58. mHipRightState = NULL;
  59. mKneeRightState = NULL;
  60. mAnkleRightState = NULL;
  61. mTrackAnkles = TRUE;
  62. mFrameNum = 0;
  63. }
  64. //-----------------------------------------------------------------------------
  65. // ~LLKeyframeStandMotion()
  66. // Class Destructor
  67. //-----------------------------------------------------------------------------
  68. LLKeyframeStandMotion::~LLKeyframeStandMotion()
  69. {
  70. }
  71. //-----------------------------------------------------------------------------
  72. // LLKeyframeStandMotion::onInitialize()
  73. //-----------------------------------------------------------------------------
  74. LLMotion::LLMotionInitStatus LLKeyframeStandMotion::onInitialize(LLCharacter *character)
  75. {
  76. // save character pointer for later use
  77. mCharacter = character;
  78. mFlipFeet = FALSE;
  79. // load keyframe data, setup pose and joint states
  80. LLMotion::LLMotionInitStatus status = LLKeyframeMotion::onInitialize(character);
  81. if ( status == STATUS_FAILURE )
  82. {
  83. return status;
  84. }
  85. // find the necessary joint states
  86. LLPose *pose = getPose();
  87. mPelvisState = pose->findJointState("mPelvis");
  88. mHipLeftState = pose->findJointState("mHipLeft");
  89. mKneeLeftState = pose->findJointState("mKneeLeft");
  90. mAnkleLeftState = pose->findJointState("mAnkleLeft");
  91. mHipRightState = pose->findJointState("mHipRight");
  92. mKneeRightState = pose->findJointState("mKneeRight");
  93. mAnkleRightState = pose->findJointState("mAnkleRight");
  94. if ( !mPelvisState ||
  95. !mHipLeftState ||
  96. !mKneeLeftState ||
  97. !mAnkleLeftState ||
  98. !mHipRightState ||
  99. !mKneeRightState ||
  100. !mAnkleRightState )
  101. {
  102. llinfos << getName() << ": Can't find necessary joint states" << llendl;
  103. return STATUS_FAILURE;
  104. }
  105. return STATUS_SUCCESS;
  106. }
  107. //-----------------------------------------------------------------------------
  108. // LLKeyframeStandMotion::onActivate()
  109. //-----------------------------------------------------------------------------
  110. BOOL LLKeyframeStandMotion::onActivate()
  111. {
  112. //-------------------------------------------------------------------------
  113. // setup the IK solvers
  114. //-------------------------------------------------------------------------
  115. mIKLeft.setPoleVector( LLVector3(1.0f, 0.0f, 0.0f));
  116. mIKRight.setPoleVector( LLVector3(1.0f, 0.0f, 0.0f));
  117. mIKLeft.setBAxis( LLVector3(0.05f, 1.0f, 0.0f));
  118. mIKRight.setBAxis( LLVector3(-0.05f, 1.0f, 0.0f));
  119. mLastGoodPelvisRotation.loadIdentity();
  120. mLastGoodPosition.clearVec();
  121. mFrameNum = 0;
  122. return LLKeyframeMotion::onActivate();
  123. }
  124. //-----------------------------------------------------------------------------
  125. // LLKeyframeStandMotion::onDeactivate()
  126. //-----------------------------------------------------------------------------
  127. void LLKeyframeStandMotion::onDeactivate()
  128. {
  129. LLKeyframeMotion::onDeactivate();
  130. }
  131. //-----------------------------------------------------------------------------
  132. // LLKeyframeStandMotion::onUpdate()
  133. //-----------------------------------------------------------------------------
  134. BOOL LLKeyframeStandMotion::onUpdate(F32 time, U8* joint_mask)
  135. {
  136. //-------------------------------------------------------------------------
  137. // let the base class update the cycle
  138. //-------------------------------------------------------------------------
  139. BOOL status = LLKeyframeMotion::onUpdate(time, joint_mask);
  140. if (!status)
  141. {
  142. return FALSE;
  143. }
  144. LLVector3 root_world_pos = mPelvisState->getJoint()->getParent()->getWorldPosition();
  145. // have we received a valid world position for this avatar?
  146. if (root_world_pos.isExactlyZero())
  147. {
  148. return TRUE;
  149. }
  150. //-------------------------------------------------------------------------
  151. // Stop tracking (start locking) ankles once ease in is done.
  152. // Setting this here ensures we track until we get valid foot position.
  153. //-------------------------------------------------------------------------
  154. if (dot(mPelvisState->getJoint()->getWorldRotation(), mLastGoodPelvisRotation) < ROTATION_THRESHOLD)
  155. {
  156. mLastGoodPelvisRotation = mPelvisState->getJoint()->getWorldRotation();
  157. mLastGoodPelvisRotation.normalize();
  158. mTrackAnkles = TRUE;
  159. }
  160. else if ((mCharacter->getCharacterPosition() - mLastGoodPosition).magVecSquared() > POSITION_THRESHOLD)
  161. {
  162. mLastGoodPosition = mCharacter->getCharacterPosition();
  163. mTrackAnkles = TRUE;
  164. }
  165. else if (mPose.getWeight() < 1.f)
  166. {
  167. mTrackAnkles = TRUE;
  168. }
  169. //-------------------------------------------------------------------------
  170. // propagate joint positions to internal versions
  171. //-------------------------------------------------------------------------
  172. mPelvisJoint.setPosition(
  173. root_world_pos +
  174. mPelvisState->getPosition() );
  175. mHipLeftJoint.setPosition( mHipLeftState->getJoint()->getPosition() );
  176. mKneeLeftJoint.setPosition( mKneeLeftState->getJoint()->getPosition() );
  177. mAnkleLeftJoint.setPosition( mAnkleLeftState->getJoint()->getPosition() );
  178. mHipLeftJoint.setScale( mHipLeftState->getJoint()->getScale() );
  179. mKneeLeftJoint.setScale( mKneeLeftState->getJoint()->getScale() );
  180. mAnkleLeftJoint.setScale( mAnkleLeftState->getJoint()->getScale() );
  181. mHipRightJoint.setPosition( mHipRightState->getJoint()->getPosition() );
  182. mKneeRightJoint.setPosition( mKneeRightState->getJoint()->getPosition() );
  183. mAnkleRightJoint.setPosition( mAnkleRightState->getJoint()->getPosition() );
  184. mHipRightJoint.setScale( mHipRightState->getJoint()->getScale() );
  185. mKneeRightJoint.setScale( mKneeRightState->getJoint()->getScale() );
  186. mAnkleRightJoint.setScale( mAnkleRightState->getJoint()->getScale() );
  187. //-------------------------------------------------------------------------
  188. // propagate joint rotations to internal versions
  189. //-------------------------------------------------------------------------
  190. mPelvisJoint.setRotation( mPelvisState->getJoint()->getWorldRotation() );
  191. #if GO_TO_KEY_POSE
  192. mHipLeftJoint.setRotation( mHipLeftState->getRotation() );
  193. mKneeLeftJoint.setRotation( mKneeLeftState->getRotation() );
  194. mAnkleLeftJoint.setRotation( mAnkleLeftState->getRotation() );
  195. mHipRightJoint.setRotation( mHipRightState->getRotation() );
  196. mKneeRightJoint.setRotation( mKneeRightState->getRotation() );
  197. mAnkleRightJoint.setRotation( mAnkleRightState->getRotation() );
  198. #else
  199. mHipLeftJoint.setRotation( mHipLeftState->getJoint()->getRotation() );
  200. mKneeLeftJoint.setRotation( mKneeLeftState->getJoint()->getRotation() );
  201. mAnkleLeftJoint.setRotation( mAnkleLeftState->getJoint()->getRotation() );
  202. mHipRightJoint.setRotation( mHipRightState->getJoint()->getRotation() );
  203. mKneeRightJoint.setRotation( mKneeRightState->getJoint()->getRotation() );
  204. mAnkleRightJoint.setRotation( mAnkleRightState->getJoint()->getRotation() );
  205. #endif
  206. // need to wait for underlying keyframe motion to affect the skeleton
  207. if (mFrameNum == 2)
  208. {
  209. mIKLeft.setupJoints( &mHipLeftJoint, &mKneeLeftJoint, &mAnkleLeftJoint, &mTargetLeft );
  210. mIKRight.setupJoints( &mHipRightJoint, &mKneeRightJoint, &mAnkleRightJoint, &mTargetRight );
  211. }
  212. else if (mFrameNum < 2)
  213. {
  214. mFrameNum++;
  215. return TRUE;
  216. }
  217. mFrameNum++;
  218. //-------------------------------------------------------------------------
  219. // compute target position by projecting ankles to the ground
  220. //-------------------------------------------------------------------------
  221. if ( mTrackAnkles )
  222. {
  223. mCharacter->getGround( mAnkleLeftJoint.getWorldPosition(), mPositionLeft, mNormalLeft);
  224. mCharacter->getGround( mAnkleRightJoint.getWorldPosition(), mPositionRight, mNormalRight);
  225. mTargetLeft.setPosition( mPositionLeft );
  226. mTargetRight.setPosition( mPositionRight );
  227. }
  228. //-------------------------------------------------------------------------
  229. // update solvers
  230. //-------------------------------------------------------------------------
  231. mIKLeft.solve();
  232. mIKRight.solve();
  233. //-------------------------------------------------------------------------
  234. // make ankle rotation conform to the ground
  235. //-------------------------------------------------------------------------
  236. if ( mTrackAnkles )
  237. {
  238. LLVector4 dirLeft4 = mAnkleLeftJoint.getWorldMatrix().getFwdRow4();
  239. LLVector4 dirRight4 = mAnkleRightJoint.getWorldMatrix().getFwdRow4();
  240. LLVector3 dirLeft = vec4to3( dirLeft4 );
  241. LLVector3 dirRight = vec4to3( dirRight4 );
  242. LLVector3 up;
  243. LLVector3 dir;
  244. LLVector3 left;
  245. up = mNormalLeft;
  246. up.normVec();
  247. if (mFlipFeet)
  248. {
  249. up *= -1.0f;
  250. }
  251. dir = dirLeft;
  252. dir.normVec();
  253. left = up % dir;
  254. left.normVec();
  255. dir = left % up;
  256. mRotationLeft = LLQuaternion( dir, left, up );
  257. up = mNormalRight;
  258. up.normVec();
  259. if (mFlipFeet)
  260. {
  261. up *= -1.0f;
  262. }
  263. dir = dirRight;
  264. dir.normVec();
  265. left = up % dir;
  266. left.normVec();
  267. dir = left % up;
  268. mRotationRight = LLQuaternion( dir, left, up );
  269. }
  270. mAnkleLeftJoint.setWorldRotation( mRotationLeft );
  271. mAnkleRightJoint.setWorldRotation( mRotationRight );
  272. //-------------------------------------------------------------------------
  273. // propagate joint rotations to joint states
  274. //-------------------------------------------------------------------------
  275. mHipLeftState->setRotation( mHipLeftJoint.getRotation() );
  276. mKneeLeftState->setRotation( mKneeLeftJoint.getRotation() );
  277. mAnkleLeftState->setRotation( mAnkleLeftJoint.getRotation() );
  278. mHipRightState->setRotation( mHipRightJoint.getRotation() );
  279. mKneeRightState->setRotation( mKneeRightJoint.getRotation() );
  280. mAnkleRightState->setRotation( mAnkleRightJoint.getRotation() );
  281. //llinfos << "Stand drift amount " << (mCharacter->getCharacterPosition() - mLastGoodPosition).magVec() << llendl;
  282. // llinfos << "DEBUG: " << speed << " : " << mTrackAnkles << llendl;
  283. return TRUE;
  284. }
  285. // End