Print done when done.

1 files changed, 544 insertions, 0 deletions

diff --git a/indra/llcharacter/llpose.cpp b/indra/llcharacter/llpose.cpp
new file mode 100644
index 0000000000..ff56dba585
--- /dev/null
+++ b/indra/llcharacter/llpose.cpp
@@ -0,0 +1,544 @@
+/** 
+ * @file llpose.cpp
+ * @brief Implementation of LLPose class.
+ *
+ * Copyright (c) 2001-$CurrentYear$, Linden Research, Inc.
+ * $License$
+ */
+
+//-----------------------------------------------------------------------------
+// Header Files
+//-----------------------------------------------------------------------------
+#include "linden_common.h"
+
+#include "llpose.h"
+
+#include "llmotion.h"
+#include "llmath.h"
+
+//-----------------------------------------------------------------------------
+// Static
+//-----------------------------------------------------------------------------
+
+//-----------------------------------------------------------------------------
+// LLPose
+//-----------------------------------------------------------------------------
+LLPose::~LLPose()
+{
+}
+
+//-----------------------------------------------------------------------------
+// getFirstJointState()
+//-----------------------------------------------------------------------------
+LLJointState *LLPose::getFirstJointState()
+{
+	mListIter = mJointMap.begin();
+	if (mListIter == mJointMap.end())
+	{
+		return NULL;
+	}
+	else
+	{
+		return mListIter->second;
+	}
+}
+
+//-----------------------------------------------------------------------------
+// getNextJointState()
+//-----------------------------------------------------------------------------
+LLJointState *LLPose::getNextJointState()
+{
+	mListIter++;
+	if (mListIter == mJointMap.end())
+	{
+		return NULL;
+	}
+	else
+	{
+		return mListIter->second;
+	}
+}
+
+//-----------------------------------------------------------------------------
+// addJointState()
+//-----------------------------------------------------------------------------
+BOOL LLPose::addJointState(LLJointState *jointState)
+{
+	if (mJointMap.find(jointState->getJoint()->getName()) == mJointMap.end())
+	{
+		mJointMap[jointState->getJoint()->getName()] = jointState;
+	}
+	return TRUE;
+}
+
+//-----------------------------------------------------------------------------
+// removeJointState()
+//-----------------------------------------------------------------------------
+BOOL LLPose::removeJointState(LLJointState *jointState)
+{
+	mJointMap.erase(jointState->getJoint()->getName());
+	return TRUE;
+}
+
+//-----------------------------------------------------------------------------
+// removeAllJointStates()
+//-----------------------------------------------------------------------------
+BOOL LLPose::removeAllJointStates()
+{
+	mJointMap.clear();
+	return TRUE;
+}
+
+//-----------------------------------------------------------------------------
+// findJointState()
+//-----------------------------------------------------------------------------
+LLJointState* LLPose::findJointState(LLJoint *joint)
+{
+	joint_map_iterator iter = mJointMap.find(joint->getName());
+
+	if (iter == mJointMap.end())
+	{
+		return NULL;
+	}
+	else
+	{
+		return iter->second;
+	}
+}
+
+//-----------------------------------------------------------------------------
+// findJointState()
+//-----------------------------------------------------------------------------
+LLJointState* LLPose::findJointState(const std::string &name)
+{
+	joint_map_iterator iter = mJointMap.find(name);
+
+	if (iter == mJointMap.end())
+	{
+		return NULL;
+	}
+	else
+	{
+		return iter->second;
+	}
+}
+
+//-----------------------------------------------------------------------------
+// setWeight()
+//-----------------------------------------------------------------------------
+void LLPose::setWeight(F32 weight)
+{
+	joint_map_iterator iter;
+	for(iter = mJointMap.begin(); 
+		iter != mJointMap.end();
+		++iter)
+	{
+		iter->second->setWeight(weight);
+	}
+	mWeight = weight;
+}
+
+//-----------------------------------------------------------------------------
+// getWeight()
+//-----------------------------------------------------------------------------
+F32 LLPose::getWeight() const
+{
+	return mWeight;
+}
+
+//-----------------------------------------------------------------------------
+// getNumJointStates()
+//-----------------------------------------------------------------------------
+S32 LLPose::getNumJointStates() const
+{
+	return (S32)mJointMap.size();
+}
+
+//-----------------------------------------------------------------------------
+// LLJointStateBlender
+//-----------------------------------------------------------------------------
+
+LLJointStateBlender::LLJointStateBlender()
+{
+	for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
+	{
+		mJointStates[i] = NULL;
+		mPriorities[i] = S32_MIN;
+	}
+}
+
+LLJointStateBlender::~LLJointStateBlender()
+{
+	
+}
+
+//-----------------------------------------------------------------------------
+// addJointState()
+//-----------------------------------------------------------------------------
+BOOL LLJointStateBlender::addJointState(LLJointState *joint_state, S32 priority, BOOL additive_blend)
+{
+	llassert(joint_state);
+
+	if (!joint_state->getJoint())
+		// this joint state doesn't point to an actual joint, so we don't care about applying it
+		return FALSE;
+
+	for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
+	{
+		if (NULL == mJointStates[i])
+		{
+			mJointStates[i] = joint_state;
+			mPriorities[i] = priority;
+			mAdditiveBlends[i] = additive_blend;
+			return TRUE;
+		} 
+		else if (priority > mPriorities[i])
+		{
+			// we're at a higher priority than the current joint state in this slot
+			// so shift everyone over
+			// previous joint states (newer motions) with same priority should stay in place
+			for (S32 j = JSB_NUM_JOINT_STATES - 1; j > i; j--)
+			{
+				mJointStates[j] = mJointStates[j - 1];
+				mPriorities[j] = mPriorities[j - 1];
+				mAdditiveBlends[j] = mAdditiveBlends[j - 1];
+			}
+			// now store ourselves in this slot
+			mJointStates[i] = joint_state;
+			mPriorities[i] = priority;
+			mAdditiveBlends[i] = additive_blend;
+			return TRUE;
+		}
+	}
+
+	return FALSE;
+}
+
+//-----------------------------------------------------------------------------
+// blendJointStates()
+//-----------------------------------------------------------------------------
+void LLJointStateBlender::blendJointStates(BOOL apply_now)
+{
+	// we need at least one joint to blend
+	// if there is one, it will be in slot zero according to insertion logic
+	// instead of resetting joint state to default, just leave it unchanged from last frame
+	if (NULL == mJointStates[0])
+	{
+		return;
+	}
+
+	LLJoint* target_joint = apply_now ? mJointStates[0]->getJoint() : &mJointCache;
+
+	const S32 POS_WEIGHT = 0;
+	const S32 ROT_WEIGHT = 1;
+	const S32 SCALE_WEIGHT = 2;
+
+	F32				sum_weights[3];
+	U32				sum_usage = 0;
+
+	LLVector3		blended_pos = target_joint->getPosition();
+	LLQuaternion	blended_rot = target_joint->getRotation();
+	LLVector3		blended_scale = target_joint->getScale();
+
+	LLVector3		added_pos;
+	LLQuaternion	added_rot;
+	LLVector3		added_scale;
+
+	//S32				joint_state_index;
+
+	sum_weights[POS_WEIGHT] = 0.f;
+	sum_weights[ROT_WEIGHT] = 0.f;
+	sum_weights[SCALE_WEIGHT] = 0.f;
+
+	for(S32 joint_state_index = 0; 
+		joint_state_index < JSB_NUM_JOINT_STATES && mJointStates[joint_state_index] != NULL;
+		joint_state_index++)
+	{
+		U32 current_usage = mJointStates[joint_state_index]->getUsage();
+		F32 current_weight = mJointStates[joint_state_index]->getWeight();
+		LLJointState* jsp = mJointStates[joint_state_index];
+
+		if (current_weight == 0.f)
+		{
+			continue;
+		}
+
+		if (mAdditiveBlends[joint_state_index])
+		{
+			if(current_usage & LLJointState::POS)
+			{
+				F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[POS_WEIGHT]);
+
+				// add in pos for this jointstate modulated by weight
+				added_pos += jsp->getPosition() * (new_weight_sum - sum_weights[POS_WEIGHT]);
+				//sum_weights[POS_WEIGHT] = new_weight_sum;
+			}
+
+			// now do scale
+			if(current_usage & LLJointState::SCALE)
+			{
+				F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[SCALE_WEIGHT]);
+
+				// add in scale for this jointstate modulated by weight
+				added_scale += jsp->getScale() * (new_weight_sum - sum_weights[SCALE_WEIGHT]);
+				//sum_weights[SCALE_WEIGHT] = new_weight_sum;
+			}
+
+			if (current_usage & LLJointState::ROT)
+			{
+				F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[ROT_WEIGHT]);
+
+				// add in rotation for this jointstate modulated by weight
+				added_rot = nlerp((new_weight_sum - sum_weights[ROT_WEIGHT]), added_rot, jsp->getRotation()) * added_rot;
+				//sum_weights[ROT_WEIGHT] = new_weight_sum;
+			}
+		}
+		else
+		{
+			// blend two jointstates together
+		
+			// blend position
+			if(current_usage & LLJointState::POS)
+			{
+				if(sum_usage & LLJointState::POS)
+				{
+					F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[POS_WEIGHT]);
+
+					// blend positions from both
+					blended_pos = lerp(mJointStates[joint_state_index]->getPosition(), blended_pos, sum_weights[POS_WEIGHT] / new_weight_sum);
+					sum_weights[POS_WEIGHT] = new_weight_sum;
+				} 
+				else
+				{
+					// copy position from current
+					blended_pos = mJointStates[joint_state_index]->getPosition();
+					sum_weights[POS_WEIGHT] = current_weight;
+				}
+			}
+			
+			// now do scale
+			if(current_usage & LLJointState::SCALE)
+			{
+				if(sum_usage & LLJointState::SCALE)
+				{
+					F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[SCALE_WEIGHT]);
+
+					// blend scales from both
+					blended_scale = lerp(mJointStates[joint_state_index]->getScale(), blended_scale, sum_weights[SCALE_WEIGHT] / new_weight_sum);
+					sum_weights[SCALE_WEIGHT] = new_weight_sum;
+				} 
+				else
+				{
+					// copy scale from current
+					blended_scale = mJointStates[joint_state_index]->getScale();
+					sum_weights[SCALE_WEIGHT] = current_weight;
+				}
+			}
+
+			// rotation
+			if (current_usage & LLJointState::ROT)
+			{
+				if(sum_usage & LLJointState::ROT)
+				{
+					F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[ROT_WEIGHT]);
+
+					// blend rotations from both
+					blended_rot = nlerp(sum_weights[ROT_WEIGHT] / new_weight_sum, mJointStates[joint_state_index]->getRotation(), blended_rot);
+					sum_weights[ROT_WEIGHT] = new_weight_sum;
+				} 
+				else
+				{
+					// copy rotation from current
+					blended_rot = mJointStates[joint_state_index]->getRotation();
+					sum_weights[ROT_WEIGHT] = current_weight;
+				}
+			}
+
+			// update resulting usage mask
+			sum_usage = sum_usage | current_usage;
+		}
+	}
+
+	// apply blended transforms
+	target_joint->setPosition(blended_pos);
+	target_joint->setScale(blended_scale);
+	target_joint->setRotation(blended_rot);
+
+	// apply additive transforms
+	target_joint->setPosition(target_joint->getPosition() + added_pos);
+	target_joint->setScale(target_joint->getScale() + added_scale);
+	target_joint->setRotation(added_rot * target_joint->getRotation());
+
+	if (apply_now)
+	{
+		// now clear joint states
+		for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
+		{
+			mJointStates[i] = NULL;
+		}
+	}
+}
+
+//-----------------------------------------------------------------------------
+// interpolate()
+//-----------------------------------------------------------------------------
+void LLJointStateBlender::interpolate(F32 u)
+{
+	// only interpolate if we have a joint state
+	if (!mJointStates[0])
+	{
+		return;
+	}
+	LLJoint* target_joint = mJointStates[0]->getJoint();
+
+	if (!target_joint)
+	{
+		return;
+	}
+
+	target_joint->setPosition(lerp(target_joint->getPosition(), mJointCache.getPosition(), u));
+	target_joint->setScale(lerp(target_joint->getScale(), mJointCache.getScale(), u));
+	target_joint->setRotation(nlerp(u, target_joint->getRotation(), mJointCache.getRotation()));
+}
+
+//-----------------------------------------------------------------------------
+// clear()
+//-----------------------------------------------------------------------------
+void LLJointStateBlender::clear()
+{
+	// now clear joint states
+	for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
+	{
+		mJointStates[i] = NULL;
+	}
+}
+
+//-----------------------------------------------------------------------------
+// resetCachedJoint()
+//-----------------------------------------------------------------------------
+void LLJointStateBlender::resetCachedJoint()
+{
+	if (!mJointStates[0])
+	{
+		return;
+	}
+	LLJoint* source_joint = mJointStates[0]->getJoint();
+	mJointCache.setPosition(source_joint->getPosition());
+	mJointCache.setScale(source_joint->getScale());
+	mJointCache.setRotation(source_joint->getRotation());
+}
+
+//-----------------------------------------------------------------------------
+// LLPoseBlender
+//-----------------------------------------------------------------------------
+
+LLPoseBlender::LLPoseBlender()
+{
+}
+
+LLPoseBlender::~LLPoseBlender()
+{
+	mJointStateBlenderPool.deleteAllData();
+}
+
+//-----------------------------------------------------------------------------
+// addMotion()
+//-----------------------------------------------------------------------------
+BOOL LLPoseBlender::addMotion(LLMotion* motion)
+{
+	LLPose* pose = motion->getPose();
+
+	for(LLJointState *jsp = pose->getFirstJointState(); jsp; jsp = pose->getNextJointState())
+	{
+		LLJoint *jointp = jsp->getJoint();
+		LLJointStateBlender* joint_blender;
+		if (!mJointStateBlenderPool.checkData(jointp))
+		{
+			// this is the first time we are animating this joint
+			// so create new jointblender and add it to our pool
+			joint_blender = new LLJointStateBlender();
+			mJointStateBlenderPool.addData(jointp, joint_blender);
+		} else
+		{
+			joint_blender = mJointStateBlenderPool.getData(jointp);
+		}
+
+		if (jsp->getPriority() == LLJoint::USE_MOTION_PRIORITY)
+		{
+			joint_blender->addJointState(jsp, motion->getPriority(), motion->getBlendType() == LLMotion::ADDITIVE_BLEND);
+		}
+		else
+		{
+			joint_blender->addJointState(jsp, jsp->getPriority(), motion->getBlendType() == LLMotion::ADDITIVE_BLEND);
+		}
+
+		// add it to our list of active blenders
+		if(!mActiveBlenders.checkData(joint_blender))
+		{
+			mActiveBlenders.addData(joint_blender);
+		}
+	}
+	return TRUE;
+}
+
+//-----------------------------------------------------------------------------
+// blendAndApply()
+//-----------------------------------------------------------------------------
+void LLPoseBlender::blendAndApply()
+{
+	for (LLJointStateBlender* jsbp = mActiveBlenders.getFirstData(); 
+		jsbp;
+		jsbp = mActiveBlenders.getNextData())
+	{
+		jsbp->blendJointStates();
+	}
+
+	// we're done now so there are no more active blenders for this frame
+	mActiveBlenders.removeAllNodes();
+}
+
+//-----------------------------------------------------------------------------
+// blendAndCache()
+//-----------------------------------------------------------------------------
+void LLPoseBlender::blendAndCache(BOOL reset_cached_joints)
+{
+	for (LLJointStateBlender* jsbp = mActiveBlenders.getFirstData();
+		jsbp;
+		jsbp = mActiveBlenders.getNextData())
+	{
+		if (reset_cached_joints)
+		{
+			jsbp->resetCachedJoint();
+		}
+		jsbp->blendJointStates(FALSE);
+	}
+}
+
+//-----------------------------------------------------------------------------
+// interpolate()
+//-----------------------------------------------------------------------------
+void LLPoseBlender::interpolate(F32 u)
+{
+	for (LLJointStateBlender* jsbp = mActiveBlenders.getFirstData();
+		jsbp;
+		jsbp = mActiveBlenders.getNextData())
+	{
+		jsbp->interpolate(u);
+	}
+}
+
+//-----------------------------------------------------------------------------
+// clearBlenders()
+//-----------------------------------------------------------------------------
+void LLPoseBlender::clearBlenders()
+{
+	for (LLJointStateBlender* jsbp = mActiveBlenders.getFirstData();
+		jsbp;
+		jsbp = mActiveBlenders.getNextData())
+	{
+		jsbp->clear();
+	}
+
+	mActiveBlenders.removeAllNodes();
+}
+