/indra/llcharacter/llpose.cpp

https://bitbucket.org/lindenlab/viewer-beta/ · C++ · 567 lines · 365 code · 71 blank · 131 comment · 60 complexity · e4336015c98eca10c63f1a70e4653200 MD5 · raw file

  1. /**
  2. * @file llpose.cpp
  3. * @brief Implementation of LLPose 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 "llpose.h"
  31. #include "llmotion.h"
  32. #include "llmath.h"
  33. #include "llstl.h"
  34. //-----------------------------------------------------------------------------
  35. // Static
  36. //-----------------------------------------------------------------------------
  37. //-----------------------------------------------------------------------------
  38. // LLPose
  39. //-----------------------------------------------------------------------------
  40. LLPose::~LLPose()
  41. {
  42. }
  43. //-----------------------------------------------------------------------------
  44. // getFirstJointState()
  45. //-----------------------------------------------------------------------------
  46. LLJointState* LLPose::getFirstJointState()
  47. {
  48. mListIter = mJointMap.begin();
  49. if (mListIter == mJointMap.end())
  50. {
  51. return NULL;
  52. }
  53. else
  54. {
  55. return mListIter->second;
  56. }
  57. }
  58. //-----------------------------------------------------------------------------
  59. // getNextJointState()
  60. //-----------------------------------------------------------------------------
  61. LLJointState *LLPose::getNextJointState()
  62. {
  63. mListIter++;
  64. if (mListIter == mJointMap.end())
  65. {
  66. return NULL;
  67. }
  68. else
  69. {
  70. return mListIter->second;
  71. }
  72. }
  73. //-----------------------------------------------------------------------------
  74. // addJointState()
  75. //-----------------------------------------------------------------------------
  76. BOOL LLPose::addJointState(const LLPointer<LLJointState>& jointState)
  77. {
  78. if (mJointMap.find(jointState->getJoint()->getName()) == mJointMap.end())
  79. {
  80. mJointMap[jointState->getJoint()->getName()] = jointState;
  81. }
  82. return TRUE;
  83. }
  84. //-----------------------------------------------------------------------------
  85. // removeJointState()
  86. //-----------------------------------------------------------------------------
  87. BOOL LLPose::removeJointState(const LLPointer<LLJointState>& jointState)
  88. {
  89. mJointMap.erase(jointState->getJoint()->getName());
  90. return TRUE;
  91. }
  92. //-----------------------------------------------------------------------------
  93. // removeAllJointStates()
  94. //-----------------------------------------------------------------------------
  95. BOOL LLPose::removeAllJointStates()
  96. {
  97. mJointMap.clear();
  98. return TRUE;
  99. }
  100. //-----------------------------------------------------------------------------
  101. // findJointState()
  102. //-----------------------------------------------------------------------------
  103. LLJointState* LLPose::findJointState(LLJoint *joint)
  104. {
  105. joint_map_iterator iter = mJointMap.find(joint->getName());
  106. if (iter == mJointMap.end())
  107. {
  108. return NULL;
  109. }
  110. else
  111. {
  112. return iter->second;
  113. }
  114. }
  115. //-----------------------------------------------------------------------------
  116. // findJointState()
  117. //-----------------------------------------------------------------------------
  118. LLJointState* LLPose::findJointState(const std::string &name)
  119. {
  120. joint_map_iterator iter = mJointMap.find(name);
  121. if (iter == mJointMap.end())
  122. {
  123. return NULL;
  124. }
  125. else
  126. {
  127. return iter->second;
  128. }
  129. }
  130. //-----------------------------------------------------------------------------
  131. // setWeight()
  132. //-----------------------------------------------------------------------------
  133. void LLPose::setWeight(F32 weight)
  134. {
  135. joint_map_iterator iter;
  136. for(iter = mJointMap.begin();
  137. iter != mJointMap.end();
  138. ++iter)
  139. {
  140. iter->second->setWeight(weight);
  141. }
  142. mWeight = weight;
  143. }
  144. //-----------------------------------------------------------------------------
  145. // getWeight()
  146. //-----------------------------------------------------------------------------
  147. F32 LLPose::getWeight() const
  148. {
  149. return mWeight;
  150. }
  151. //-----------------------------------------------------------------------------
  152. // getNumJointStates()
  153. //-----------------------------------------------------------------------------
  154. S32 LLPose::getNumJointStates() const
  155. {
  156. return (S32)mJointMap.size();
  157. }
  158. //-----------------------------------------------------------------------------
  159. // LLJointStateBlender
  160. //-----------------------------------------------------------------------------
  161. LLJointStateBlender::LLJointStateBlender()
  162. {
  163. for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
  164. {
  165. mJointStates[i] = NULL;
  166. mPriorities[i] = S32_MIN;
  167. mAdditiveBlends[i] = FALSE;
  168. }
  169. }
  170. LLJointStateBlender::~LLJointStateBlender()
  171. {
  172. }
  173. //-----------------------------------------------------------------------------
  174. // addJointState()
  175. //-----------------------------------------------------------------------------
  176. BOOL LLJointStateBlender::addJointState(const LLPointer<LLJointState>& joint_state, S32 priority, BOOL additive_blend)
  177. {
  178. llassert(joint_state);
  179. if (!joint_state->getJoint())
  180. // this joint state doesn't point to an actual joint, so we don't care about applying it
  181. return FALSE;
  182. for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
  183. {
  184. if (mJointStates[i].isNull())
  185. {
  186. mJointStates[i] = joint_state;
  187. mPriorities[i] = priority;
  188. mAdditiveBlends[i] = additive_blend;
  189. return TRUE;
  190. }
  191. else if (priority > mPriorities[i])
  192. {
  193. // we're at a higher priority than the current joint state in this slot
  194. // so shift everyone over
  195. // previous joint states (newer motions) with same priority should stay in place
  196. for (S32 j = JSB_NUM_JOINT_STATES - 1; j > i; j--)
  197. {
  198. mJointStates[j] = mJointStates[j - 1];
  199. mPriorities[j] = mPriorities[j - 1];
  200. mAdditiveBlends[j] = mAdditiveBlends[j - 1];
  201. }
  202. // now store ourselves in this slot
  203. mJointStates[i] = joint_state;
  204. mPriorities[i] = priority;
  205. mAdditiveBlends[i] = additive_blend;
  206. return TRUE;
  207. }
  208. }
  209. return FALSE;
  210. }
  211. //-----------------------------------------------------------------------------
  212. // blendJointStates()
  213. //-----------------------------------------------------------------------------
  214. void LLJointStateBlender::blendJointStates(BOOL apply_now)
  215. {
  216. // we need at least one joint to blend
  217. // if there is one, it will be in slot zero according to insertion logic
  218. // instead of resetting joint state to default, just leave it unchanged from last frame
  219. if (mJointStates[0].isNull())
  220. {
  221. return;
  222. }
  223. LLJoint* target_joint = apply_now ? mJointStates[0]->getJoint() : &mJointCache;
  224. const S32 POS_WEIGHT = 0;
  225. const S32 ROT_WEIGHT = 1;
  226. const S32 SCALE_WEIGHT = 2;
  227. F32 sum_weights[3];
  228. U32 sum_usage = 0;
  229. LLVector3 blended_pos = target_joint->getPosition();
  230. LLQuaternion blended_rot = target_joint->getRotation();
  231. LLVector3 blended_scale = target_joint->getScale();
  232. LLVector3 added_pos;
  233. LLQuaternion added_rot;
  234. LLVector3 added_scale;
  235. //S32 joint_state_index;
  236. sum_weights[POS_WEIGHT] = 0.f;
  237. sum_weights[ROT_WEIGHT] = 0.f;
  238. sum_weights[SCALE_WEIGHT] = 0.f;
  239. for(S32 joint_state_index = 0;
  240. joint_state_index < JSB_NUM_JOINT_STATES && mJointStates[joint_state_index].notNull();
  241. joint_state_index++)
  242. {
  243. LLJointState* jsp = mJointStates[joint_state_index];
  244. U32 current_usage = jsp->getUsage();
  245. F32 current_weight = jsp->getWeight();
  246. if (current_weight == 0.f)
  247. {
  248. continue;
  249. }
  250. if (mAdditiveBlends[joint_state_index])
  251. {
  252. if(current_usage & LLJointState::POS)
  253. {
  254. F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[POS_WEIGHT]);
  255. // add in pos for this jointstate modulated by weight
  256. added_pos += jsp->getPosition() * (new_weight_sum - sum_weights[POS_WEIGHT]);
  257. }
  258. if(current_usage & LLJointState::SCALE)
  259. {
  260. F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[SCALE_WEIGHT]);
  261. // add in scale for this jointstate modulated by weight
  262. added_scale += jsp->getScale() * (new_weight_sum - sum_weights[SCALE_WEIGHT]);
  263. }
  264. if (current_usage & LLJointState::ROT)
  265. {
  266. F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[ROT_WEIGHT]);
  267. // add in rotation for this jointstate modulated by weight
  268. added_rot = nlerp((new_weight_sum - sum_weights[ROT_WEIGHT]), added_rot, jsp->getRotation()) * added_rot;
  269. }
  270. }
  271. else
  272. {
  273. // blend two jointstates together
  274. // blend position
  275. if(current_usage & LLJointState::POS)
  276. {
  277. if(sum_usage & LLJointState::POS)
  278. {
  279. F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[POS_WEIGHT]);
  280. // blend positions from both
  281. blended_pos = lerp(jsp->getPosition(), blended_pos, sum_weights[POS_WEIGHT] / new_weight_sum);
  282. sum_weights[POS_WEIGHT] = new_weight_sum;
  283. }
  284. else
  285. {
  286. // copy position from current
  287. blended_pos = jsp->getPosition();
  288. sum_weights[POS_WEIGHT] = current_weight;
  289. }
  290. }
  291. // now do scale
  292. if(current_usage & LLJointState::SCALE)
  293. {
  294. if(sum_usage & LLJointState::SCALE)
  295. {
  296. F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[SCALE_WEIGHT]);
  297. // blend scales from both
  298. blended_scale = lerp(jsp->getScale(), blended_scale, sum_weights[SCALE_WEIGHT] / new_weight_sum);
  299. sum_weights[SCALE_WEIGHT] = new_weight_sum;
  300. }
  301. else
  302. {
  303. // copy scale from current
  304. blended_scale = jsp->getScale();
  305. sum_weights[SCALE_WEIGHT] = current_weight;
  306. }
  307. }
  308. // rotation
  309. if (current_usage & LLJointState::ROT)
  310. {
  311. if(sum_usage & LLJointState::ROT)
  312. {
  313. F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[ROT_WEIGHT]);
  314. // blend rotations from both
  315. blended_rot = nlerp(sum_weights[ROT_WEIGHT] / new_weight_sum, jsp->getRotation(), blended_rot);
  316. sum_weights[ROT_WEIGHT] = new_weight_sum;
  317. }
  318. else
  319. {
  320. // copy rotation from current
  321. blended_rot = jsp->getRotation();
  322. sum_weights[ROT_WEIGHT] = current_weight;
  323. }
  324. }
  325. // update resulting usage mask
  326. sum_usage = sum_usage | current_usage;
  327. }
  328. }
  329. if (!added_scale.isFinite())
  330. {
  331. added_scale.clearVec();
  332. }
  333. if (!blended_scale.isFinite())
  334. {
  335. blended_scale.setVec(1,1,1);
  336. }
  337. // apply transforms
  338. target_joint->setPosition(blended_pos + added_pos);
  339. target_joint->setScale(blended_scale + added_scale);
  340. target_joint->setRotation(added_rot * blended_rot);
  341. if (apply_now)
  342. {
  343. // now clear joint states
  344. for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
  345. {
  346. mJointStates[i] = NULL;
  347. }
  348. }
  349. }
  350. //-----------------------------------------------------------------------------
  351. // interpolate()
  352. //-----------------------------------------------------------------------------
  353. void LLJointStateBlender::interpolate(F32 u)
  354. {
  355. // only interpolate if we have a joint state
  356. if (!mJointStates[0])
  357. {
  358. return;
  359. }
  360. LLJoint* target_joint = mJointStates[0]->getJoint();
  361. if (!target_joint)
  362. {
  363. return;
  364. }
  365. target_joint->setPosition(lerp(target_joint->getPosition(), mJointCache.getPosition(), u));
  366. target_joint->setScale(lerp(target_joint->getScale(), mJointCache.getScale(), u));
  367. target_joint->setRotation(nlerp(u, target_joint->getRotation(), mJointCache.getRotation()));
  368. }
  369. //-----------------------------------------------------------------------------
  370. // clear()
  371. //-----------------------------------------------------------------------------
  372. void LLJointStateBlender::clear()
  373. {
  374. // now clear joint states
  375. for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
  376. {
  377. mJointStates[i] = NULL;
  378. }
  379. }
  380. //-----------------------------------------------------------------------------
  381. // resetCachedJoint()
  382. //-----------------------------------------------------------------------------
  383. void LLJointStateBlender::resetCachedJoint()
  384. {
  385. if (!mJointStates[0])
  386. {
  387. return;
  388. }
  389. LLJoint* source_joint = mJointStates[0]->getJoint();
  390. mJointCache.setPosition(source_joint->getPosition());
  391. mJointCache.setScale(source_joint->getScale());
  392. mJointCache.setRotation(source_joint->getRotation());
  393. }
  394. //-----------------------------------------------------------------------------
  395. // LLPoseBlender
  396. //-----------------------------------------------------------------------------
  397. LLPoseBlender::LLPoseBlender()
  398. : mNextPoseSlot(0)
  399. {
  400. }
  401. LLPoseBlender::~LLPoseBlender()
  402. {
  403. for_each(mJointStateBlenderPool.begin(), mJointStateBlenderPool.end(), DeletePairedPointer());
  404. }
  405. //-----------------------------------------------------------------------------
  406. // addMotion()
  407. //-----------------------------------------------------------------------------
  408. BOOL LLPoseBlender::addMotion(LLMotion* motion)
  409. {
  410. LLPose* pose = motion->getPose();
  411. for(LLJointState* jsp = pose->getFirstJointState(); jsp; jsp = pose->getNextJointState())
  412. {
  413. LLJoint *jointp = jsp->getJoint();
  414. LLJointStateBlender* joint_blender;
  415. if (mJointStateBlenderPool.find(jointp) == mJointStateBlenderPool.end())
  416. {
  417. // this is the first time we are animating this joint
  418. // so create new jointblender and add it to our pool
  419. joint_blender = new LLJointStateBlender();
  420. mJointStateBlenderPool[jointp] = joint_blender;
  421. }
  422. else
  423. {
  424. joint_blender = mJointStateBlenderPool[jointp];
  425. }
  426. if (jsp->getPriority() == LLJoint::USE_MOTION_PRIORITY)
  427. {
  428. joint_blender->addJointState(jsp, motion->getPriority(), motion->getBlendType() == LLMotion::ADDITIVE_BLEND);
  429. }
  430. else
  431. {
  432. joint_blender->addJointState(jsp, jsp->getPriority(), motion->getBlendType() == LLMotion::ADDITIVE_BLEND);
  433. }
  434. // add it to our list of active blenders
  435. if (std::find(mActiveBlenders.begin(), mActiveBlenders.end(), joint_blender) == mActiveBlenders.end())
  436. {
  437. mActiveBlenders.push_front(joint_blender);
  438. }
  439. }
  440. return TRUE;
  441. }
  442. //-----------------------------------------------------------------------------
  443. // blendAndApply()
  444. //-----------------------------------------------------------------------------
  445. void LLPoseBlender::blendAndApply()
  446. {
  447. for (blender_list_t::iterator iter = mActiveBlenders.begin();
  448. iter != mActiveBlenders.end(); )
  449. {
  450. LLJointStateBlender* jsbp = *iter++;
  451. jsbp->blendJointStates();
  452. }
  453. // we're done now so there are no more active blenders for this frame
  454. mActiveBlenders.clear();
  455. }
  456. //-----------------------------------------------------------------------------
  457. // blendAndCache()
  458. //-----------------------------------------------------------------------------
  459. void LLPoseBlender::blendAndCache(BOOL reset_cached_joints)
  460. {
  461. for (blender_list_t::iterator iter = mActiveBlenders.begin();
  462. iter != mActiveBlenders.end(); ++iter)
  463. {
  464. LLJointStateBlender* jsbp = *iter;
  465. if (reset_cached_joints)
  466. {
  467. jsbp->resetCachedJoint();
  468. }
  469. jsbp->blendJointStates(FALSE);
  470. }
  471. }
  472. //-----------------------------------------------------------------------------
  473. // interpolate()
  474. //-----------------------------------------------------------------------------
  475. void LLPoseBlender::interpolate(F32 u)
  476. {
  477. for (blender_list_t::iterator iter = mActiveBlenders.begin();
  478. iter != mActiveBlenders.end(); ++iter)
  479. {
  480. LLJointStateBlender* jsbp = *iter;
  481. jsbp->interpolate(u);
  482. }
  483. }
  484. //-----------------------------------------------------------------------------
  485. // clearBlenders()
  486. //-----------------------------------------------------------------------------
  487. void LLPoseBlender::clearBlenders()
  488. {
  489. for (blender_list_t::iterator iter = mActiveBlenders.begin();
  490. iter != mActiveBlenders.end(); ++iter)
  491. {
  492. LLJointStateBlender* jsbp = *iter;
  493. jsbp->clear();
  494. }
  495. mActiveBlenders.clear();
  496. }