WIP bvh diagnostics

3 files changed, 90 insertions, 45 deletions

diff --git a/indra/llcharacter/llbvhloader.cpp b/indra/llcharacter/llbvhloader.cpp
index 0d558aeaa2..754b9362cd 100755
--- a/indra/llcharacter/llbvhloader.cpp
+++ b/indra/llcharacter/llbvhloader.cpp
@@ -42,10 +42,12 @@ using namespace std;
 
 #define INCHES_TO_METERS 0.02540005f
 
-const F32 POSITION_KEYFRAME_THRESHOLD_SQUARED = 0.03f * 0.03f;
+//const F32 POSITION_KEYFRAME_THRESHOLD_SQUARED = 0.03f * 0.03f;
+const F32 POSITION_KEYFRAME_THRESHOLD_SQUARED = 0.0f;
 const F32 ROTATION_KEYFRAME_THRESHOLD = 0.01f;
 
-const F32 POSITION_MOTION_THRESHOLD_SQUARED = 0.001f * 0.001f;
+//const F32 POSITION_MOTION_THRESHOLD_SQUARED = 0.001f * 0.001f;
+const F32 POSITION_MOTION_THRESHOLD_SQUARED = 0.0f;
 const F32 ROTATION_MOTION_THRESHOLD = 0.001f;
 
 char gInFile[1024];		/* Flawfinder: ignore */
@@ -166,7 +168,7 @@ LLBVHLoader::LLBVHLoader(const char* buffer, ELoadStatus &loadStatus, S32 &error
 	LL_INFOS()<<"Load Status 00 : "<< loadStatus << LL_ENDL;
 	if (mStatus == E_ST_NO_XLT_FILE)
 	{
-		//LL_WARNS() << "NOTE: No translation table found." << LL_ENDL;
+		LL_WARNS() << "NOTE: No translation table found." << LL_ENDL;
 		loadStatus = mStatus;
 		return;
 	}
@@ -174,7 +176,7 @@ LLBVHLoader::LLBVHLoader(const char* buffer, ELoadStatus &loadStatus, S32 &error
 	{
 		if (mStatus != E_ST_OK)
 		{
-			//LL_WARNS() << "ERROR: [line: " << getLineNumber() << "] " << mStatus << LL_ENDL;
+			LL_WARNS() << "ERROR: [line: " << getLineNumber() << "] " << mStatus << LL_ENDL;
 			errorLine = getLineNumber();
 			loadStatus = mStatus;
 			return;
@@ -184,10 +186,13 @@ LLBVHLoader::LLBVHLoader(const char* buffer, ELoadStatus &loadStatus, S32 &error
 	char error_text[128];		/* Flawfinder: ignore */
 	S32 error_line;
 	mStatus = loadBVHFile(buffer, error_text, error_line);
+
+	LL_INFOS("BVH") << "Raw data from file" << LL_ENDL;
+	dumpBVHInfo();
 	
 	if (mStatus != E_ST_OK)
 	{
-		//LL_WARNS() << "ERROR: [line: " << getLineNumber() << "] " << mStatus << LL_ENDL;
+		LL_WARNS() << "ERROR: [line: " << getLineNumber() << "] " << mStatus << LL_ENDL;
 		loadStatus = mStatus;
 		errorLine = getLineNumber();
 		return;
@@ -196,6 +201,9 @@ LLBVHLoader::LLBVHLoader(const char* buffer, ELoadStatus &loadStatus, S32 &error
 	applyTranslations();
 	optimize();
 	
+	LL_INFOS("BVH") << "Ater optimize" << LL_ENDL;
+	dumpBVHInfo();
+
 	mInitialized = TRUE;
 }
 
@@ -666,6 +674,33 @@ ELoadStatus LLBVHLoader::loadTranslationTable(const char *fileName)
 	return E_ST_OK;
 }
 
+void LLBVHLoader::dumpBVHInfo()
+{
+	for (U32 j=0; j<mJoints.size(); j++)
+	{
+		Joint *joint = mJoints[j];
+		LL_INFOS() << joint->mName << LL_ENDL;
+		for (S32 i=0; i<mNumFrames; i++)
+		{
+			Key &prevkey = joint->mKeys[llmax(i-1,0)];
+			Key &key = joint->mKeys[i];
+			if ((i==0) ||
+				(key.mPos[0] != prevkey.mPos[0]) ||
+				(key.mPos[1] != prevkey.mPos[1]) ||
+				(key.mPos[2] != prevkey.mPos[2]) ||
+				(key.mRot[0] != prevkey.mRot[0]) ||
+				(key.mRot[1] != prevkey.mRot[1]) ||
+				(key.mRot[2] != prevkey.mRot[2])
+				)
+			{
+				LL_INFOS() << "FRAME " << i 
+						   << " POS " << key.mPos[0] << "," << key.mPos[1] << "," << key.mPos[2]
+						   << " ROT " << key.mRot[0] << "," << key.mRot[1] << "," << key.mRot[2] << LL_ENDL;
+			}
+		}
+	}
+
+}
 
 //------------------------------------------------------------------------
 // LLBVHLoader::loadBVHFile()
@@ -746,6 +781,7 @@ ELoadStatus LLBVHLoader::loadBVHFile(const char *buffer, char* error_text, S32 &
 		{
 			iter++; // {
 			iter++; //     OFFSET
+			iter++; // }
 			S32 depth = 0;
 			for (S32 j = (S32)parent_joints.size() - 1; j >= 0; j--)
 			{
@@ -790,11 +826,14 @@ ELoadStatus LLBVHLoader::loadBVHFile(const char *buffer, char* error_text, S32 &
 		//----------------------------------------------------------------
 		mJoints.push_back( new Joint( jointName ) );
 		Joint *joint = mJoints.back();
+		LL_INFOS() << "Created joint " << jointName << LL_ENDL;
+		LL_INFOS() << "- index " << mJoints.size()-1 << LL_ENDL;
 
 		S32 depth = 1;
 		for (S32 j = (S32)parent_joints.size() - 1; j >= 0; j--)
 		{
 			Joint *pjoint = mJoints[parent_joints[j]];
+			LL_INFOS() << "- ancestor " << pjoint->mName << LL_ENDL;
 			if (depth > pjoint->mChildTreeMaxDepth)
 			{
 				pjoint->mChildTreeMaxDepth = depth;
@@ -862,6 +901,19 @@ ELoadStatus LLBVHLoader::loadBVHFile(const char *buffer, char* error_text, S32 &
 			strncpy(error_text, line.c_str(), 127);		/*Flawfinder: ignore*/
 			return E_ST_NO_CHANNELS;
 		}
+		int res = sscanf(line.c_str(), " CHANNELS %d", &joint->mNumChannels);
+		if ( res != 1 )
+		{
+			// Assume default if not otherwise specified.
+			if (mJoints.size()==1)
+			{
+				joint->mNumChannels = 6;
+			}
+			else
+			{
+				joint->mNumChannels = 3;
+			}
+		}
 
 		//----------------------------------------------------------------
 		// get rotation order
@@ -961,57 +1013,39 @@ ELoadStatus LLBVHLoader::loadBVHFile(const char *buffer, char* error_text, S32 &
 		line = (*(iter++));
 		err_line++;
 
-		// read and store values
-		const char *p = line.c_str();
+		// Split line into a collection of floats.
+		std::deque<F32> floats;
+		boost::char_separator<char> whitespace_sep("\t ");
+		tokenizer float_tokens(line, whitespace_sep);
+		tokenizer::iterator float_token_iter = float_tokens.begin();
+		while (float_token_iter != float_tokens.end())
+		{
+			floats.push_back(std::stof(*(float_token_iter++)));
+		}
+		LL_INFOS() << "Got " << floats.size() << " floats " << LL_ENDL;
 		for (U32 j=0; j<mJoints.size(); j++)
 		{
 			Joint *joint = mJoints[j];
 			joint->mKeys.push_back( Key() );
 			Key &key = joint->mKeys.back();
 
-			// get 3 pos values for root joint only
-			if (j==0)
-			{
-				if ( sscanf(p, "%f %f %f", key.mPos, key.mPos+1, key.mPos+2) != 3 )
-				{
-					strncpy(error_text, line.c_str(), 127);	/*Flawfinder: ignore*/
-					return E_ST_NO_POS;
-				}
-			}
-
-			// skip to next 3 values in the line
-			p = find_next_whitespace(p);
-			if (!p) 
-			{
-				strncpy(error_text, line.c_str(), 127);		/*Flawfinder: ignore*/
-				return E_ST_NO_ROT;
-			}
-			p = find_next_whitespace(++p);
-			if (!p) 
-			{
-				strncpy(error_text, line.c_str(), 127);		/*Flawfinder: ignore*/
-				return E_ST_NO_ROT;
-			}
-			p = find_next_whitespace(++p);
-			if (!p)
+			if (floats.size() < joint->mNumChannels)
 			{
-				strncpy(error_text, line.c_str(), 127);		/*Flawfinder: ignore*/
-				return E_ST_NO_ROT;
+				strncpy(error_text, line.c_str(), 127);	/*Flawfinder: ignore*/
+				return E_ST_NO_POS;
 			}
 
-			// get 3 rot values for joint
-			F32 rot[3];
-			if ( sscanf(p, " %f %f %f", rot, rot+1, rot+2) != 3 )
+			// assume either numChannels == 6, in which case we have pos + rot,
+			// or numChannels == 3, in which case we have only rot.
+			if (joint->mNumChannels == 6)
 			{
-				strncpy(error_text, line.c_str(), 127);		/*Flawfinder: ignore*/
-				return E_ST_NO_ROT;
+				key.mPos[0] = floats.front(); floats.pop_front();
+				key.mPos[1] = floats.front(); floats.pop_front();
+				key.mPos[2] = floats.front(); floats.pop_front();
 			}
-
-			p++;
-
-			key.mRot[ joint->mOrder[0]-'X' ] = rot[0];
-			key.mRot[ joint->mOrder[1]-'X' ] = rot[1];
-			key.mRot[ joint->mOrder[2]-'X' ] = rot[2];
+			key.mRot[ joint->mOrder[0]-'X' ] = floats.front(); floats.pop_front();
+			key.mRot[ joint->mOrder[1]-'X' ] = floats.front(); floats.pop_front();
+			key.mRot[ joint->mOrder[2]-'X' ] = floats.front(); floats.pop_front();
 		}
 	}
 
diff --git a/indra/llcharacter/llbvhloader.h b/indra/llcharacter/llbvhloader.h
index f816b76277..033d8714a9 100755
--- a/indra/llcharacter/llbvhloader.h
+++ b/indra/llcharacter/llbvhloader.h
@@ -102,6 +102,7 @@ struct Joint
 		mNumRotKeys = 0;
 		mChildTreeMaxDepth = 0;
 		mPriority = 0;
+		mNumChannels = 3;
 	}
 
 	// Include aligned members first
@@ -123,6 +124,7 @@ struct Joint
 	S32				mNumRotKeys;
 	S32				mChildTreeMaxDepth;
 	S32				mPriority;
+	S32				mNumChannels;
 };
 
 
@@ -272,6 +274,8 @@ public:
 	// Returns status code.
 	ELoadStatus loadBVHFile(const char *buffer, char *error_text, S32 &error_line);
 
+	void dumpBVHInfo();
+
 	// Applies translations to BVH data loaded.
 	void applyTranslations();
 
diff --git a/indra/llcharacter/llkeyframemotion.cpp b/indra/llcharacter/llkeyframemotion.cpp
index cd201a65b4..8b463a2b8c 100755
--- a/indra/llcharacter/llkeyframemotion.cpp
+++ b/indra/llcharacter/llkeyframemotion.cpp
@@ -1881,6 +1881,8 @@ BOOL LLKeyframeMotion::serialize(LLDataPacker& dp) const
 	success &= dp.packU32(mJointMotionList->mHandPose, "hand_pose");
 	success &= dp.packU32(mJointMotionList->getNumJointMotions(), "num_joints");
 
+	LL_DEBUGS("BVH") << "bvh" << LL_ENDL;
+
 	for (U32 i = 0; i < mJointMotionList->getNumJointMotions(); i++)
 	{
 		JointMotion* joint_motionp = mJointMotionList->getJointMotion(i);
@@ -1888,6 +1890,7 @@ BOOL LLKeyframeMotion::serialize(LLDataPacker& dp) const
 		success &= dp.packS32(joint_motionp->mPriority, "joint_priority");
 		success &= dp.packS32(joint_motionp->mRotationCurve.mNumKeys, "num_rot_keys");
 
+		LL_DEBUGS("BVH") << "Joint " << joint_motionp->mJointName << LL_ENDL;
 		for (RotationCurve::key_map_t::iterator iter = joint_motionp->mRotationCurve.mKeys.begin();
 			 iter != joint_motionp->mRotationCurve.mKeys.end(); ++iter)
 		{
@@ -1905,6 +1908,8 @@ BOOL LLKeyframeMotion::serialize(LLDataPacker& dp) const
 			success &= dp.packU16(x, "rot_angle_x");
 			success &= dp.packU16(y, "rot_angle_y");
 			success &= dp.packU16(z, "rot_angle_z");
+
+			LL_DEBUGS("BVH") << "  rot " << rot_key.mTime << " - " <<rot_angles.mV[VX] <<","<< rot_angles.mV[VY] <<","<< rot_angles.mV[VZ] << LL_ENDL;
 		}
 
 		success &= dp.packS32(joint_motionp->mPositionCurve.mNumKeys, "num_pos_keys");
@@ -1923,6 +1928,8 @@ BOOL LLKeyframeMotion::serialize(LLDataPacker& dp) const
 			success &= dp.packU16(x, "pos_x");
 			success &= dp.packU16(y, "pos_y");
 			success &= dp.packU16(z, "pos_z");
+
+			LL_DEBUGS("BVH") << "  pos " << pos_key.mTime << pos_key.mPosition.mV[VX] <<","<< pos_key.mPosition.mV[VY] <<","<< pos_key.mPosition.mV[VZ] << LL_ENDL;
 		}
 	}