00001
00032
00033
00034
00035 #include "linden_common.h"
00036
00037 #include "llpose.h"
00038
00039 #include "llmotion.h"
00040 #include "llmath.h"
00041 #include "llstl.h"
00042
00043
00044
00045
00046
00047
00048
00049
00050 LLPose::~LLPose()
00051 {
00052 }
00053
00054
00055
00056
00057 LLJointState* LLPose::getFirstJointState()
00058 {
00059 mListIter = mJointMap.begin();
00060 if (mListIter == mJointMap.end())
00061 {
00062 return NULL;
00063 }
00064 else
00065 {
00066 return mListIter->second;
00067 }
00068 }
00069
00070
00071
00072
00073 LLJointState *LLPose::getNextJointState()
00074 {
00075 mListIter++;
00076 if (mListIter == mJointMap.end())
00077 {
00078 return NULL;
00079 }
00080 else
00081 {
00082 return mListIter->second;
00083 }
00084 }
00085
00086
00087
00088
00089 BOOL LLPose::addJointState(const LLPointer<LLJointState>& jointState)
00090 {
00091 if (mJointMap.find(jointState->getJoint()->getName()) == mJointMap.end())
00092 {
00093 mJointMap[jointState->getJoint()->getName()] = jointState;
00094 }
00095 return TRUE;
00096 }
00097
00098
00099
00100
00101 BOOL LLPose::removeJointState(const LLPointer<LLJointState>& jointState)
00102 {
00103 mJointMap.erase(jointState->getJoint()->getName());
00104 return TRUE;
00105 }
00106
00107
00108
00109
00110 BOOL LLPose::removeAllJointStates()
00111 {
00112 mJointMap.clear();
00113 return TRUE;
00114 }
00115
00116
00117
00118
00119 LLJointState* LLPose::findJointState(LLJoint *joint)
00120 {
00121 joint_map_iterator iter = mJointMap.find(joint->getName());
00122
00123 if (iter == mJointMap.end())
00124 {
00125 return NULL;
00126 }
00127 else
00128 {
00129 return iter->second;
00130 }
00131 }
00132
00133
00134
00135
00136 LLJointState* LLPose::findJointState(const std::string &name)
00137 {
00138 joint_map_iterator iter = mJointMap.find(name);
00139
00140 if (iter == mJointMap.end())
00141 {
00142 return NULL;
00143 }
00144 else
00145 {
00146 return iter->second;
00147 }
00148 }
00149
00150
00151
00152
00153 void LLPose::setWeight(F32 weight)
00154 {
00155 joint_map_iterator iter;
00156 for(iter = mJointMap.begin();
00157 iter != mJointMap.end();
00158 ++iter)
00159 {
00160 iter->second->setWeight(weight);
00161 }
00162 mWeight = weight;
00163 }
00164
00165
00166
00167
00168 F32 LLPose::getWeight() const
00169 {
00170 return mWeight;
00171 }
00172
00173
00174
00175
00176 S32 LLPose::getNumJointStates() const
00177 {
00178 return (S32)mJointMap.size();
00179 }
00180
00181
00182
00183
00184
00185 LLJointStateBlender::LLJointStateBlender()
00186 {
00187 for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
00188 {
00189 mJointStates[i] = NULL;
00190 mPriorities[i] = S32_MIN;
00191 }
00192 }
00193
00194 LLJointStateBlender::~LLJointStateBlender()
00195 {
00196
00197 }
00198
00199
00200
00201
00202 BOOL LLJointStateBlender::addJointState(const LLPointer<LLJointState>& joint_state, S32 priority, BOOL additive_blend)
00203 {
00204 llassert(joint_state);
00205
00206 if (!joint_state->getJoint())
00207
00208 return FALSE;
00209
00210 for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
00211 {
00212 if (mJointStates[i].isNull())
00213 {
00214 mJointStates[i] = joint_state;
00215 mPriorities[i] = priority;
00216 mAdditiveBlends[i] = additive_blend;
00217 return TRUE;
00218 }
00219 else if (priority > mPriorities[i])
00220 {
00221
00222
00223
00224 for (S32 j = JSB_NUM_JOINT_STATES - 1; j > i; j--)
00225 {
00226 mJointStates[j] = mJointStates[j - 1];
00227 mPriorities[j] = mPriorities[j - 1];
00228 mAdditiveBlends[j] = mAdditiveBlends[j - 1];
00229 }
00230
00231 mJointStates[i] = joint_state;
00232 mPriorities[i] = priority;
00233 mAdditiveBlends[i] = additive_blend;
00234 return TRUE;
00235 }
00236 }
00237
00238 return FALSE;
00239 }
00240
00241
00242
00243
00244 void LLJointStateBlender::blendJointStates(BOOL apply_now)
00245 {
00246
00247
00248
00249 if (mJointStates[0].isNull())
00250 {
00251 return;
00252 }
00253
00254 LLJoint* target_joint = apply_now ? mJointStates[0]->getJoint() : &mJointCache;
00255
00256 const S32 POS_WEIGHT = 0;
00257 const S32 ROT_WEIGHT = 1;
00258 const S32 SCALE_WEIGHT = 2;
00259
00260 F32 sum_weights[3];
00261 U32 sum_usage = 0;
00262
00263 LLVector3 blended_pos = target_joint->getPosition();
00264 LLQuaternion blended_rot = target_joint->getRotation();
00265 LLVector3 blended_scale = target_joint->getScale();
00266
00267 LLVector3 added_pos;
00268 LLQuaternion added_rot;
00269 LLVector3 added_scale;
00270
00271
00272
00273 sum_weights[POS_WEIGHT] = 0.f;
00274 sum_weights[ROT_WEIGHT] = 0.f;
00275 sum_weights[SCALE_WEIGHT] = 0.f;
00276
00277 for(S32 joint_state_index = 0;
00278 joint_state_index < JSB_NUM_JOINT_STATES && mJointStates[joint_state_index].notNull();
00279 joint_state_index++)
00280 {
00281 LLJointState* jsp = mJointStates[joint_state_index];
00282 U32 current_usage = jsp->getUsage();
00283 F32 current_weight = jsp->getWeight();
00284
00285 if (current_weight == 0.f)
00286 {
00287 continue;
00288 }
00289
00290 if (mAdditiveBlends[joint_state_index])
00291 {
00292 if(current_usage & LLJointState::POS)
00293 {
00294 F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[POS_WEIGHT]);
00295
00296
00297 added_pos += jsp->getPosition() * (new_weight_sum - sum_weights[POS_WEIGHT]);
00298 }
00299
00300 if(current_usage & LLJointState::SCALE)
00301 {
00302 F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[SCALE_WEIGHT]);
00303
00304
00305 added_scale += jsp->getScale() * (new_weight_sum - sum_weights[SCALE_WEIGHT]);
00306 }
00307
00308 if (current_usage & LLJointState::ROT)
00309 {
00310 F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[ROT_WEIGHT]);
00311
00312
00313 added_rot = nlerp((new_weight_sum - sum_weights[ROT_WEIGHT]), added_rot, jsp->getRotation()) * added_rot;
00314 }
00315 }
00316 else
00317 {
00318
00319
00320
00321 if(current_usage & LLJointState::POS)
00322 {
00323 if(sum_usage & LLJointState::POS)
00324 {
00325 F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[POS_WEIGHT]);
00326
00327
00328 blended_pos = lerp(jsp->getPosition(), blended_pos, sum_weights[POS_WEIGHT] / new_weight_sum);
00329 sum_weights[POS_WEIGHT] = new_weight_sum;
00330 }
00331 else
00332 {
00333
00334 blended_pos = jsp->getPosition();
00335 sum_weights[POS_WEIGHT] = current_weight;
00336 }
00337 }
00338
00339
00340 if(current_usage & LLJointState::SCALE)
00341 {
00342 if(sum_usage & LLJointState::SCALE)
00343 {
00344 F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[SCALE_WEIGHT]);
00345
00346
00347 blended_scale = lerp(jsp->getScale(), blended_scale, sum_weights[SCALE_WEIGHT] / new_weight_sum);
00348 sum_weights[SCALE_WEIGHT] = new_weight_sum;
00349 }
00350 else
00351 {
00352
00353 blended_scale = jsp->getScale();
00354 sum_weights[SCALE_WEIGHT] = current_weight;
00355 }
00356 }
00357
00358
00359 if (current_usage & LLJointState::ROT)
00360 {
00361 if(sum_usage & LLJointState::ROT)
00362 {
00363 F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[ROT_WEIGHT]);
00364
00365
00366 blended_rot = nlerp(sum_weights[ROT_WEIGHT] / new_weight_sum, jsp->getRotation(), blended_rot);
00367 sum_weights[ROT_WEIGHT] = new_weight_sum;
00368 }
00369 else
00370 {
00371
00372 blended_rot = jsp->getRotation();
00373 sum_weights[ROT_WEIGHT] = current_weight;
00374 }
00375 }
00376
00377
00378 sum_usage = sum_usage | current_usage;
00379 }
00380 }
00381
00382 if (!added_scale.isFinite())
00383 {
00384 added_scale.clearVec();
00385 }
00386
00387 if (!blended_scale.isFinite())
00388 {
00389 blended_scale.setVec(1,1,1);
00390 }
00391
00392
00393 target_joint->setPosition(blended_pos + added_pos);
00394 target_joint->setScale(blended_scale + added_scale);
00395 target_joint->setRotation(added_rot * blended_rot);
00396
00397 if (apply_now)
00398 {
00399
00400 for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
00401 {
00402 mJointStates[i] = NULL;
00403 }
00404 }
00405 }
00406
00407
00408
00409
00410 void LLJointStateBlender::interpolate(F32 u)
00411 {
00412
00413 if (!mJointStates[0])
00414 {
00415 return;
00416 }
00417 LLJoint* target_joint = mJointStates[0]->getJoint();
00418
00419 if (!target_joint)
00420 {
00421 return;
00422 }
00423
00424 target_joint->setPosition(lerp(target_joint->getPosition(), mJointCache.getPosition(), u));
00425 target_joint->setScale(lerp(target_joint->getScale(), mJointCache.getScale(), u));
00426 target_joint->setRotation(nlerp(u, target_joint->getRotation(), mJointCache.getRotation()));
00427 }
00428
00429
00430
00431
00432 void LLJointStateBlender::clear()
00433 {
00434
00435 for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
00436 {
00437 mJointStates[i] = NULL;
00438 }
00439 }
00440
00441
00442
00443
00444 void LLJointStateBlender::resetCachedJoint()
00445 {
00446 if (!mJointStates[0])
00447 {
00448 return;
00449 }
00450 LLJoint* source_joint = mJointStates[0]->getJoint();
00451 mJointCache.setPosition(source_joint->getPosition());
00452 mJointCache.setScale(source_joint->getScale());
00453 mJointCache.setRotation(source_joint->getRotation());
00454 }
00455
00456
00457
00458
00459
00460 LLPoseBlender::LLPoseBlender()
00461 {
00462 }
00463
00464 LLPoseBlender::~LLPoseBlender()
00465 {
00466 for_each(mJointStateBlenderPool.begin(), mJointStateBlenderPool.end(), DeletePairedPointer());
00467 }
00468
00469
00470
00471
00472 BOOL LLPoseBlender::addMotion(LLMotion* motion)
00473 {
00474 LLPose* pose = motion->getPose();
00475
00476 for(LLJointState* jsp = pose->getFirstJointState(); jsp; jsp = pose->getNextJointState())
00477 {
00478 LLJoint *jointp = jsp->getJoint();
00479 LLJointStateBlender* joint_blender;
00480 if (mJointStateBlenderPool.find(jointp) == mJointStateBlenderPool.end())
00481 {
00482
00483
00484 joint_blender = new LLJointStateBlender();
00485 mJointStateBlenderPool[jointp] = joint_blender;
00486 }
00487 else
00488 {
00489 joint_blender = mJointStateBlenderPool[jointp];
00490 }
00491
00492 if (jsp->getPriority() == LLJoint::USE_MOTION_PRIORITY)
00493 {
00494 joint_blender->addJointState(jsp, motion->getPriority(), motion->getBlendType() == LLMotion::ADDITIVE_BLEND);
00495 }
00496 else
00497 {
00498 joint_blender->addJointState(jsp, jsp->getPriority(), motion->getBlendType() == LLMotion::ADDITIVE_BLEND);
00499 }
00500
00501
00502 if (std::find(mActiveBlenders.begin(), mActiveBlenders.end(), joint_blender) == mActiveBlenders.end())
00503 {
00504 mActiveBlenders.push_front(joint_blender);
00505 }
00506 }
00507 return TRUE;
00508 }
00509
00510
00511
00512
00513 void LLPoseBlender::blendAndApply()
00514 {
00515 for (blender_list_t::iterator iter = mActiveBlenders.begin();
00516 iter != mActiveBlenders.end(); ++iter)
00517 {
00518 LLJointStateBlender* jsbp = *iter;
00519 jsbp->blendJointStates();
00520 }
00521
00522
00523 mActiveBlenders.clear();
00524 }
00525
00526
00527
00528
00529 void LLPoseBlender::blendAndCache(BOOL reset_cached_joints)
00530 {
00531 for (blender_list_t::iterator iter = mActiveBlenders.begin();
00532 iter != mActiveBlenders.end(); ++iter)
00533 {
00534 LLJointStateBlender* jsbp = *iter;
00535 if (reset_cached_joints)
00536 {
00537 jsbp->resetCachedJoint();
00538 }
00539 jsbp->blendJointStates(FALSE);
00540 }
00541 }
00542
00543
00544
00545
00546 void LLPoseBlender::interpolate(F32 u)
00547 {
00548 for (blender_list_t::iterator iter = mActiveBlenders.begin();
00549 iter != mActiveBlenders.end(); ++iter)
00550 {
00551 LLJointStateBlender* jsbp = *iter;
00552 jsbp->interpolate(u);
00553 }
00554 }
00555
00556
00557
00558
00559 void LLPoseBlender::clearBlenders()
00560 {
00561 for (blender_list_t::iterator iter = mActiveBlenders.begin();
00562 iter != mActiveBlenders.end(); ++iter)
00563 {
00564 LLJointStateBlender* jsbp = *iter;
00565 jsbp->clear();
00566 }
00567
00568 mActiveBlenders.clear();
00569 }
00570