예제 #1
0
void writeFrameBvh(ostream &os, Character::Pose &pose, vector< int > order) {
	Quatd rot = rotation( -(double)M_PI * 0.5, make_vector(1.0, 0.0, 0.0) );
	Quatd xrot;
	Quatd yrot;
	Quatd zrot;
	xrot.x = 1/sqrt(2); xrot.y = 0; xrot.z = 0; xrot.w = -1/sqrt(2);
	yrot.y = 1/sqrt(2); yrot.x = 0; yrot.z = 0; yrot.w = -1/sqrt(2);
	zrot.z = 1/sqrt(2); zrot.x = 0; zrot.y = 0; zrot.w = 1/sqrt(2);

	Vector3d root_position = vecd(pose.root_position);
	//root_position = rotate(root_position, conjugate(rot));
	// GOOD, but root doesn't match

	// old
	root_position = rotate(root_position, frame_rot());
	os  << FACTOR * root_position.x << " " << FACTOR * root_position.y << " " << FACTOR * root_position.z;
	//os << "0 0 0";

	double d[3];
	//Quatd root_orientation = multiply(quatd(pose.root_orientation), xrot);
	// rotate 180 degrees about the z axis, and -90 about the x-axis
	Quatd root_orientation = quatd(pose.root_orientation);
	root_orientation = normalize(multiply(frame_rot(), multiply(root_orientation, -frame_rot())));
	root_orientation = normalize(multiply(xrot, root_orientation));

	put_dof_rot(DOF_ORDER, root_orientation, d, 0);
	os << " " << d[0] << " " << d[1] << " " << d[2];
	//os << " 0 0 0";

	/*Character::Angles angles;
	Library::Skeleton trans;
	to_euler_angles(pose, angles, trans);*/

	//cout << "Root xyz " << d[0] << " " << d[1] << " " << d[2] << endl;
	//os << " 0 0 0";
	//cout << "Frame start: " << endl;
	for (unsigned int i = 0; i < order.size(); i++) {
		int b = order[i];
		/*os << " " << angles.angles[angles.skeleton->bones[b].frame_offset + 0] << " ";
		os << -angles.angles[angles.skeleton->bones[b].frame_offset + 1] << " ";
		os << angles.angles[angles.skeleton->bones[b].frame_offset + 2];*/

		//local, parent-relative, orientation.
		Quatd orientation = quatd(pose.bone_orientations[b]);
		orientation = normalize(multiply(frame_rot(), multiply(orientation, -frame_rot())));
		put_dof_rot(DOF_ORDER, orientation, d, 0);

		Quatd check = get_dof_rot(DOF_ORDER, d, 0);
		if (length(orientation.xyzw - check.xyzw) > 0.001 && length(-orientation.xyzw - check.xyzw) > 0.001) {
			cout << "Back quaternion conversion: " << orientation << " -> " << check << endl;
			cout << " via " << d[0] << " " << d[1] << " " << d[2] << endl;
		}
		os << " " << d[0] << " " << d[1] << " " << d[2];
		
		// GOOD - don't delete! - xyz order, plain old writing 0 1 2
		//os << " " << d[0] << " " << d[1] << " " << d[2];
	}
	os << endl;
}
예제 #2
0
파일: WriteAsfAmc.cpp 프로젝트: Sosi/Recon
void writeVSKToASF(unsigned int motion) {
	Library::Motion &m = Library::motion_nonconst(motion);
	Library::Skeleton transformer;
	get_euler_skeleton(transformer, *(m.skeleton));

	string asffilename = m.filename.substr(0, m.filename.rfind('/', m.filename.size())) + "/participant.ASF";
	ofstream outASF(asffilename.c_str());

	outASF << ":version 1.10" << endl;
	outASF << ":name VICON" << endl;
	outASF << ":units" << endl;
	outASF << "  mass 1.0" << endl;
	outASF << "  length .045" << endl;
	outASF << "  angle deg" << endl;
	outASF << ":documentation" << endl;
	outASF << "  Carnegie Mellon University Motion Capture Lab" << endl;
	outASF << "  Written by WriteAsfAmc.cpp" << endl;
	outASF << ":root" << endl;
	outASF << "  order TX TY TZ RX RY RZ" << endl;
	outASF << "  axis XYZ" << endl;
	outASF << "  position 0 0 0" << endl;
	outASF << "  orientation 0 0 0" << endl;
	outASF << ":bonedata" << endl;

	// write out the BONE DATA
	for (unsigned int b = 0; b < transformer.bones.size(); b++) {
		outASF << " begin" << endl;
		outASF << "   id " << b << endl;
		outASF << "   name " << transformer.bones[b].name << endl;
		outASF << "   direction "
			   << transformer.bones[b].direction.x << " "
			   << transformer.bones[b].direction.y << " "
			   << transformer.bones[b].direction.z << endl;
		outASF << "   length " << transformer.bones[b].length << endl;
		double d[3];
		put_dof_rot(transformer.bones[b].offset_order, transformer.bones[b].global_to_local, d, 0);
		outASF << "   axis " << d[0] << " " << d[1] << " " << d[2] << "   ";
		for (unsigned int i = 0; i < transformer.bones[b].offset_order.size(); ++i)
			outASF << (char)toupper(transformer.bones[b].offset_order[i]);
		outASF << endl;
		outASF << "   dof";
		if (transformer.bones[b].euler_axes.size()==3)
			outASF << " rx ry rz";
		else if (transformer.bones[b].euler_axes.size()==2)
			outASF << " rx rz";
		else if (transformer.bones[b].euler_axes.size()==1)
			outASF << " rx";

		//for (unsigned int i = 0; i < transformer.bones[b].dof.size(); ++i) {
		//	outASF << " r" << transformer.bones[b].dof[i];
		//}
		outASF << endl;
		//outASF << "   limits " << endl;
		outASF << " end" << endl;
	}





	// write out the HIERARCHY
	outASF << ":hierarchy" << endl;
	outASF << "  begin" << endl;

	outASF << "    root ";
	for (unsigned int b = 0; b < m.skeleton->bones.size(); b++) {
			if (m.skeleton->bones[b].parent==-1)
				outASF << m.skeleton->bones[b].name << " ";
	}
	outASF << endl;

	// loop through all bones...
	for (unsigned int b = 0; b < m.skeleton->bones.size(); ++b) {
		int id = b;

		// ... if they have any children ...
		bool foundchildren = false;
		for (unsigned int bb = 0; bb < m.skeleton->bones.size(); ++bb)
			if (m.skeleton->bones[bb].parent==id)
				foundchildren = true;
		if (! foundchildren )
			continue;

		// ... then write the children out
		outASF << "    " << m.skeleton->bones[b].name;
		for (unsigned int bb = 0; bb < m.skeleton->bones.size(); ++bb)
			if (m.skeleton->bones[bb].parent==id)
				outASF << " " << m.skeleton->bones[bb].name;
		outASF << endl;
	}

	outASF << "  end" << endl;
	cout << "Wrote ASF: " << asffilename << endl;

}