BVHData.cpp

#include <cmath>
#include <algorithm>
#include "BVHData.h"
#include <cmath>


BVHData::BVHData()
{ // constructor
} // constructor

// read data from bvh file
bool BVHData::ReadFileBVH(const char* fileName)
{ // ReadFileBVH()
// open the file & test success
	this->bvh_path = fileName;
	std::ifstream inFile(fileName);
	if (inFile.bad())
		return false;

	// load vertex, normal, face, weight
	std::vector<std::string> tokens;
	std::string line;
	while (std::getline(inFile, line))
	{  // per line
	// not an empty line
		if (line.size() != 0)
		{ // non-empty line
			StringSplit(line, tokens);
			if (tokens[0] == "HIERARCHY")
			{ // HIERARCHY line
				NewLine(inFile, tokens);
				Joint joint;
				ReadHierarchy(inFile, tokens, joint, -1);
				this->root = joint;
			} // HIERARCHY line
			else if (tokens[0] == "MOTION")
			{ // MOTION line
				ReadMotion(inFile);
				break;
			} // MOTION line
		} // non-empty line
	} // per line

// get all joints in a sequence by searching the tree structure and store it into this class
	GetAllJoints(this->root, this->all_joints);
	// load all rotation and translation data into this class
	loadAllData(this->boneRotations, this->boneTranslations, this->frames);
	return true;
} // ReadFileBVH()

// move the file stream pointer to next line
void BVHData::NewLine(std::ifstream& inFile, std::vector<std::string>& tokens)
{ // NewLine()
	std::string line;
	tokens.clear();
	std::getline(inFile, line);
	StringSplit(line, tokens);
} // NewLine()

// split string with the given key character
void BVHData::StringSplit(std::string input, std::vector<std::string>& tokens)
{ // StringSplit()
	tokens.clear();
	std::istringstream iss(input);
	while (!iss.eof())
	{ // not EOF
		std::string token;
		iss >> token;
		tokens.push_back(token);
	} // not EOF
} // StringSplit()

// check whether the given string is a number
bool BVHData::isNumeric(const std::string& str)
{ // isNumeric()
	for (char c : str)
	{ // per character
		if (!std::isdigit(c))
			return false;
	} // per character
	return true;
} // isNumeric()

// read bvh hierarchy in a recusive way
void BVHData::ReadHierarchy(std::ifstream& inFile, std::vector<std::string>& line, Joint& joint, int parent)
{ // ReadHierarchy()
	joint.id = this->Bones.size();
	joint.joint_name = line[1];
	this->Bones.push_back(joint.joint_name);
	this->parentBones.push_back(parent);
	int id = this->parentBones.size() - 1;
	NewLine(inFile, line);
	if (line[0] == "{")
	{ // not { line
		NewLine(inFile, line);
		while (line[0] != "}")
		{ // not } line
		// read offset by key word
			if (line[0] == "OFFSET")
			{
				joint.joint_offset[0] = std::stof(line[1]);
				joint.joint_offset[1] = std::stof(line[2]);
				joint.joint_offset[2] = std::stof(line[3]);
			}
			// read channels by key word
			else if (line[0] == "CHANNELS")
			{
				for (int i = 0; i < std::stoi(line[1]); i++)
					joint.joint_channel.push_back(line[i + 2]);
			}
			// read joint by key word
			else if (line[0] == "JOINT")
			{
				Joint child;
				ReadHierarchy(inFile, line, child, id);
				joint.Children.push_back(child);
			}
			// read end by key word
			else if (line[0] == "End")
			{
				for (int i = 0; i < 3; i++)
					NewLine(inFile, line);
			}
			NewLine(inFile, line);
		} // not } line
	} // not { line
} // ReadHierarchy()

// read motion(frames) from file
void BVHData::ReadMotion(std::ifstream& inFile)
{ // ReadMotion()
	std::string line;
	std::vector<std::string> tokens;
	// Frames
	NewLine(inFile, tokens);
	this->frame_count = std::stoi(tokens[1]);
	// Frame Time:
	NewLine(inFile, tokens);
	this->frame_time = std::stof(tokens[2]);
	while (std::getline(inFile, line))
	{ // per line
		if (line.size() != 0)
		{ // non-empty line
			StringSplit(line, tokens);
			std::vector<float> frame;
			for (size_t i = 0; i < tokens.size(); i++)
			{ // per token
				if (!isNumeric(tokens[i]))
					frame.push_back(std::stof(tokens[i]));
			} // per token
		// store frame into this class
			this->frames.push_back(frame);
		} // non-empty line
	} // per line
} // ReadMotion()

// get all joints in a sequence by searching the tree structure and store it into this class
void BVHData::GetAllJoints(Joint& joint, std::vector<Joint*>& joint_list)
{ // GetAllJoints()
	joint_list.push_back(&joint);
	if (joint.Children.size() == 0)
		return;
	else
		for (size_t i = 0; i < joint.Children.size(); i++)
			GetAllJoints(joint.Children[i], joint_list);
} // GetAllJoints()

// load all rotation and translation data into this class
void BVHData::loadAllData(std::vector<std::vector<Cartesian3>>& rotations, std::vector<Cartesian3>& translations, std::vector<std::vector<float>>& frames)
{ // loadAllData()
//store all rotations
	for (size_t i = 0; i < frames.size(); i++) // for each frame
	{ // per frame
		std::vector<Cartesian3> frame_rotations;
		loadRotationData(frame_rotations, frames[i]);
		rotations.push_back(frame_rotations);
	} // per frame
//store all offsets/translations
	for (size_t i = 0; i < this->all_joints.size(); i++)// for each joints
	{ // per joint
		float x = this->all_joints[i]->joint_offset[0];
		float y = this->all_joints[i]->joint_offset[1];
		float z = this->all_joints[i]->joint_offset[2];
		translations.push_back(Cartesian3(x, y, z));
	} // per joint
} // loadAllData()

// load all rotation data into this class
void BVHData::loadRotationData(std::vector<Cartesian3>& rotations, std::vector<float>& frames)
{ // loadRotationData()
	for (size_t j = 0, j_c = 0; j < frames.size(); j_c++) // for each bone
	{ // per frame
		float rotation[3] = { 0, 0, 0 };
		for (size_t k = 0; k < this->all_joints[j_c]->joint_channel.size(); k++) // for each channel
		{ // per joint
		// load data to data structure
			if (this->all_joints[j_c]->joint_channel[k].substr(1) == "rotation")
			{
				int rotation_id = BVH_CHANNEL[this->all_joints[j_c]->joint_channel[k]];
				rotation[rotation_id - 3] = frames[j + k];
			}
		} // per joint
		Cartesian3 rot_3(rotation[0], rotation[1], rotation[2]);
		rotations.push_back(rot_3);
		j += this->all_joints[j_c]->joint_channel.size();
	} // per frame
} // loadRotationData()

// routine to render
void BVHData::Render(unsigned long frame)
{
	glPushMatrix();
	glScalef(0.03, 0.03, 0.03);
	glRotatef(90, 1, 0, 0);
	glRotatef(90, 0, 1, 0);
	if (!forward) 
	{
		glRotatef(180, boneTranslations[0].x, boneTranslations[0].y, boneTranslations[0].z);
	}


	RenderBone(0, frame);

	glPopMatrix();

}
// routine to render blending animation
void BVHData::blendingRender(unsigned long interpFrames, unsigned long frame, std::vector<Quaternion> quatSet1, std::vector<Quaternion> quatSet2)
{
	glPushMatrix();
	glScalef(0.03, 0.03, 0.03);
	glRotatef(90, 1, 0, 0);
	glRotatef(90, 0, 1, 0);
	if (!forward)
	{
		glRotatef(180, boneTranslations[0].x, boneTranslations[0].y, boneTranslations[0].z);
	}
	RenderBone1(0, slerp(interpFrames, frame, quatSet1, quatSet2));

	glPopMatrix();
}

void BVHData::RenderBone(int index, unsigned long frame)
{
	glPushMatrix();
	glTranslatef(boneTranslations[index].x, boneTranslations[index].y, boneTranslations[index].z);

	Quaternion quat = euler2quat(boneRotations[frame][index].x, boneRotations[frame][index].y, boneRotations[frame][index].z);
	applyquatrot(quat);

	//glRotatef(boneRotations[frame][index].x, 1, 0, 0);
	//glRotatef(boneRotations[frame][index].y, 0, 1, 0);
	//glRotatef(boneRotations[frame][index].z, 0, 0, 1);

	for (Joint joint : all_joints[index]->Children)
	{
		drawCylinder(
			1, 
			Cartesian3(0.f, 0.f, 0.f), 
			Cartesian3(
				boneTranslations[joint.id].x, 
				boneTranslations[joint.id].y, 
				boneTranslations[joint.id].z), 
			32);
		RenderBone(joint.id, frame);
	}

	glPopMatrix();
}

void BVHData::RenderBone1(int index, std::vector<Quaternion> quatSet)
{
	glPushMatrix();
	glTranslatef(boneTranslations[index].x, boneTranslations[index].y, boneTranslations[index].z);

	applyquatrot(quatSet[index]);

	for (Joint joint : all_joints[index]->Children)
	{
		drawCylinder(
			1,
			Cartesian3(0.f, 0.f, 0.f),
			Cartesian3(
				boneTranslations[joint.id].x,
				boneTranslations[joint.id].y,
				boneTranslations[joint.id].z),
			32);
		RenderBone1(joint.id, quatSet);
	}

	glPopMatrix();
}

// draw a cylinder as a bone
void BVHData::drawCylinder(float radius, Cartesian3 v1, Cartesian3 v2, int segments) {

	float height = sqrtf((v1.x - v2.x) * (v1.x - v2.x) + (v1.y - v2.y) * (v1.y - v2.y) + (v1.z - v2.z) * (v1.z - v2.z));
	// direction vector
	Cartesian3 dir = v2 - v1;
	// rotated axis
	Cartesian3 rot = Cartesian3(0, 0, 1).cross(dir);
	float dotProduct = std::clamp(dir.z / dir.length(), -1.0f, 1.0f);
	float angle = std::acos(dotProduct) * 180.0 / M_PI;

	glPushMatrix();
	glTranslatef(v1.x, v1.y, v1.z);
	if (rot.length() > 1e-6)
		// if not parallel
		glRotated(angle, rot.x, rot.y, rot.z);

	// draw top faces
	glBegin(GL_TRIANGLE_FAN);
	glNormal3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0, 0, height);
	for (int i = 0; i <= segments; ++i) {
		float theta = (2.0f * M_PI * i) / segments;
		float x = radius * std::cos(theta);
		float y = radius * std::sin(theta);
		glVertex3f(x, y, height);
	}
	glEnd();

	// draw bottom faces
	glBegin(GL_TRIANGLE_FAN);
	glNormal3f(0.0f, 0.0f, -1.0f);
	glVertex3f(0.f, 0.f, 0.f);
	for (int i = 0; i <= segments; ++i) {
		float theta = (2.0f * M_PI * i) / segments;
		float x = radius * std::cos(theta);
		float y = radius * std::sin(theta);
		glVertex3f(x, y, 0);
	}
	glEnd();

	// draw side faces
	glBegin(GL_QUAD_STRIP);
	for (int i = 0; i <= segments; ++i) {
		float theta = (2.0f * M_PI * i) / segments;
		float x = radius * std::cos(theta);
		float y = radius * std::sin(theta);

		Cartesian3 normal = Cartesian3(x, y, 0).unit();
		glNormal3f(normal.x, normal.y, normal.z);
		glVertex3f(x, y, 0.f);
		glVertex3f(x, y, height);
	}
	glEnd();

	glPopMatrix();

}

// convert from Euler angle to quaternion
Quaternion BVHData::euler2quat(float x, float y, float z)
{
	float alpha = x * M_PI / 180.f;
	float beta = y * M_PI / 180.f;
	float gamma = z * M_PI / 180.f;

	float c1 = cosf(alpha / 2);
	float c2 = cosf(beta / 2);
	float c3 = cosf(gamma / 2);

	float s1 = sinf(alpha / 2);
	float s2 = sinf(beta / 2);
	float s3 = sinf(gamma / 2);


	float w = c1 * c2 * c3 - s1 * s2 * s3;
	float q1 = s1 * c2 * c3 + c1 * s2 * s3;
	float q2 = c1 * s2 * c3 - s1 * c2 * s3;
	float q3 = c1 * c2 * s3 + s1 * s2 * c3;

	return Quaternion(q1, q2, q3, w);

}

// rotate based on quaternion
void BVHData::applyquatrot(Quaternion quat)
{
	float angle = 2.0f * acosf(quat.coords.w) * (180.0f / M_PI);


	float norm = sqrt(quat.coords[0] * quat.coords[0] + quat.coords[1] * quat.coords[1] + quat.coords[2] * quat.coords[2]);
	if (norm > 0.0f) 
	{
		quat.coords[0] /= norm;
		quat.coords[1] /= norm;
		quat.coords[2] /= norm;
	}


	glRotatef(angle, quat.coords[0], quat.coords[1], quat.coords[2]);

}

// slerp between q1 and q2
std::vector<Quaternion> BVHData::slerp(unsigned long interpFrames, unsigned long frames, std::vector<Quaternion> q1, std::vector<Quaternion> q2)
{
	float t = static_cast<float>(frames) / interpFrames;
	std::vector<Quaternion> result;

	for (int i = 0; i < q1.size(); i++)
	{
		q1[i] = q1[i].Unit();
		q2[i] = q2[i].Unit();

		float dot = q1[i].coords[0] * q2[i].coords[0] + q1[i].coords[1] * q2[i].coords[1] + q1[i].coords[2] * q2[i].coords[2] + q1[i].coords[3] * q2[i].coords[3];

		dot = std::clamp(dot, -1.f, 1.f);
		float angle = acosf(dot);
		if (dot < 0.0f) {
			q2[i] = -1 * q2[i];
			dot = -dot;
		}

		Quaternion q3 = sinf((1 - t) * angle) * q1[i] / sinf(angle) + sinf(t * angle) * q2[i] / sinf(angle);
		result.push_back(q3);
	}

	return result;

}

void BVHData::test()
{
	//std::cout << boneRotations[0][0] << std::endl;
	//Homogeneous4 quat = euler2quat(boneRotations[0][0][0], boneRotations[0][0][1], boneRotations[0][0][2]);
	//std::cout << quat << std::endl;
}