5 files changed, 77 insertions, 16 deletions
diff --git a/indra/llcharacter/CMakeLists.txt b/indra/llcharacter/CMakeLists.txt
index 14841b5d3d..6eb154458d 100644
--- a/indra/llcharacter/CMakeLists.txt
+++ b/indra/llcharacter/CMakeLists.txt
@@ -77,12 +77,13 @@ list(APPEND llcharacter_SOURCE_FILES ${llcharacter_HEADER_FILES})
 add_library (llcharacter ${llcharacter_SOURCE_FILES})
 
 
-if(LL_TESTS)
-  # Add tests
-  include(LLAddBuildTest)
-  # UNIT TESTS
-  SET(llcharacter_TEST_SOURCE_FILES
-      lljoint.cpp
-      )
-  LL_ADD_PROJECT_UNIT_TESTS(llcharacter "${llcharacter_TEST_SOURCE_FILES}")
-endif(LL_TESTS)
+# Add tests
+if (LL_TESTS)
+	include(LLAddBuildTest)
+	# UNIT TESTS
+	SET(llcharacter_TEST_SOURCE_FILES
+	  lljoint.cpp
+	  )
+	LL_ADD_PROJECT_UNIT_TESTS(llcharacter "${llcharacter_TEST_SOURCE_FILES}")
+endif (LL_TESTS)
+
diff --git a/indra/llcharacter/llbvhloader.cpp b/indra/llcharacter/llbvhloader.cpp
index 532a2c1b0d..f3cf950afa 100644
--- a/indra/llcharacter/llbvhloader.cpp
+++ b/indra/llcharacter/llbvhloader.cpp
@@ -42,10 +42,10 @@ using namespace std;
 
 #define INCHES_TO_METERS 0.02540005f
 
-const F32 POSITION_KEYFRAME_THRESHOLD = 0.03f;
+const F32 POSITION_KEYFRAME_THRESHOLD_SQUARED = 0.03f * 0.03f;
 const F32 ROTATION_KEYFRAME_THRESHOLD = 0.01f;
 
-const F32 POSITION_MOTION_THRESHOLD = 0.001f;
+const F32 POSITION_MOTION_THRESHOLD_SQUARED = 0.001f * 0.001f;
 const F32 ROTATION_MOTION_THRESHOLD = 0.001f;
 
 char gInFile[1024];		/* Flawfinder: ignore */
@@ -1196,7 +1196,7 @@ void LLBVHLoader::optimize()
 				if (ki_prev == ki_last_good_pos)
 				{
 					joint->mNumPosKeys++;
-					if (dist_vec(LLVector3(ki_prev->mPos), first_frame_pos) > POSITION_MOTION_THRESHOLD)
+					if (dist_vec_squared(LLVector3(ki_prev->mPos), first_frame_pos) > POSITION_MOTION_THRESHOLD_SQUARED)
 					{
 						pos_changed = TRUE;
 					}
@@ -1209,12 +1209,12 @@ void LLBVHLoader::optimize()
 					LLVector3 current_pos(ki->mPos);
 					LLVector3 interp_pos = lerp(current_pos, last_good_pos, 1.f / (F32)numPosFramesConsidered);
 
-					if (dist_vec(current_pos, first_frame_pos) > POSITION_MOTION_THRESHOLD)
+					if (dist_vec_squared(current_pos, first_frame_pos) > POSITION_MOTION_THRESHOLD_SQUARED)
 					{
 						pos_changed = TRUE;
 					}
 
-					if (dist_vec(interp_pos, test_pos) < POSITION_KEYFRAME_THRESHOLD)
+					if (dist_vec_squared(interp_pos, test_pos) < POSITION_KEYFRAME_THRESHOLD_SQUARED)
 					{
 						ki_prev->mIgnorePos = TRUE;
 						numPosFramesConsidered++;
diff --git a/indra/llcharacter/lljoint.cpp b/indra/llcharacter/lljoint.cpp
index 5d750c6c96..19907933cb 100644
--- a/indra/llcharacter/lljoint.cpp
+++ b/indra/llcharacter/lljoint.cpp
@@ -50,6 +50,7 @@ LLJoint::LLJoint()
 	mUpdateXform = TRUE;
 	mJointNum = -1;
 	touch();
+	mResetAfterRestoreOldXform = false;
 }
 
 
@@ -235,6 +236,42 @@ void LLJoint::setPosition( const LLVector3& pos )
 
 
 //--------------------------------------------------------------------
+// setPosition()
+//--------------------------------------------------------------------
+void LLJoint::setDefaultFromCurrentXform( void )
+{
+	mDefaultXform = mXform;
+	touch(MATRIX_DIRTY | POSITION_DIRTY);
+	
+}
+
+//--------------------------------------------------------------------
+// storeCurrentXform()
+//--------------------------------------------------------------------
+void LLJoint::storeCurrentXform( const LLVector3& pos )
+{
+	mOldXform = mXform;
+	mResetAfterRestoreOldXform = true;
+	setPosition( pos );
+}
+//--------------------------------------------------------------------
+// restoreOldXform()
+//--------------------------------------------------------------------
+void LLJoint::restoreOldXform( void )
+{
+	mResetAfterRestoreOldXform = false;
+	mXform = mOldXform;
+}
+//--------------------------------------------------------------------
+// restoreOldXform()
+//--------------------------------------------------------------------
+void LLJoint::restoreToDefaultXform( void )
+{	
+	mXform = mDefaultXform;
+	setPosition( mXform.getPosition() );	
+}
+
+//--------------------------------------------------------------------
 // getWorldPosition()
 //--------------------------------------------------------------------
 LLVector3 LLJoint::getWorldPosition()
@@ -522,3 +559,4 @@ void LLJoint::clampRotation(LLQuaternion old_rot, LLQuaternion new_rot)
 }
 
 // End
+
diff --git a/indra/llcharacter/lljoint.h b/indra/llcharacter/lljoint.h
index 8c8e5930fb..dc3c58cf64 100644
--- a/indra/llcharacter/lljoint.h
+++ b/indra/llcharacter/lljoint.h
@@ -80,11 +80,16 @@ protected:
 
 	// explicit transformation members
 	LLXformMatrix		mXform;
+	LLXformMatrix		mOldXform;
+	LLXformMatrix		mDefaultXform;
 
+	LLUUID				mId;
 public:
 	U32				mDirtyFlags;
 	BOOL			mUpdateXform;
 
+	BOOL			mResetAfterRestoreOldXform;
+
 	// describes the skin binding pose
 	LLVector3		mSkinOffset;
 
@@ -130,7 +135,9 @@ public:
 	// get/set local position
 	const LLVector3& getPosition();
 	void setPosition( const LLVector3& pos );
-
+	
+	void setDefaultPosition( const LLVector3& pos );
+	
 	// get/set world position
 	LLVector3 getWorldPosition();
 	LLVector3 getLastWorldPosition();
@@ -172,6 +179,21 @@ public:
 
 	S32 getJointNum() const { return mJointNum; }
 	void setJointNum(S32 joint_num) { mJointNum = joint_num; }
+	
+	void restoreOldXform( void );
+	void restoreToDefaultXform( void );
+	void setDefaultFromCurrentXform( void );
+	void storeCurrentXform( const LLVector3& pos );
+
+	//Accessor for the joint id
+	LLUUID getId( void ) { return mId; }
+	//Setter for the joints id
+	void setId( const LLUUID& id ) { mId = id;}
+
+	//If the old transform flag has been set, then the reset logic in avatar needs to be aware(test) of it
+	const BOOL doesJointNeedToBeReset( void ) const { return mResetAfterRestoreOldXform; }
+	//Setter for joint reset flag
+	void setJointToBeReset( BOOL val ) { mResetAfterRestoreOldXform = val; }
 };
 #endif // LL_LLJOINT_H
 
diff --git a/indra/llcharacter/llkeyframemotion.cpp b/indra/llcharacter/llkeyframemotion.cpp
index 5b0867524e..9df033a4ca 100644
--- a/indra/llcharacter/llkeyframemotion.cpp
+++ b/indra/llcharacter/llkeyframemotion.cpp
@@ -1145,7 +1145,7 @@ void LLKeyframeMotion::applyConstraint(JointConstraint* constraint, F32 time, U8
 			constraint->mPositions[joint_num] = new_pos;
 		}
 		constraint->mFixupDistanceRMS *= 1.f / (constraint->mTotalLength * (F32)(shared_data->mChainLength - 1));
-		constraint->mFixupDistanceRMS = fsqrtf(constraint->mFixupDistanceRMS);
+		constraint->mFixupDistanceRMS = (F32) sqrt(constraint->mFixupDistanceRMS);
 
 		//reset old joint rots
 		for (joint_num = 0; joint_num <= shared_data->mChainLength; joint_num++)