void FubiRecorder::recordNextSkeletonFrame(const FubiUser& user, const Fubi::FingerCountData& leftFingerCountData, const Fubi::FingerCountData& rightFingerCountData)
{
	const Fubi::TrackingData* data = m_useFilteredData
		? user.currentFilteredTrackingData()
		: user.currentTrackingData();

	int leftFingerCount = -1, rightFingerCount = -1;
	if (leftFingerCountData.trackingEnabled && leftFingerCountData.fingerCounts.size() > 0)
		leftFingerCount = leftFingerCountData.fingerCounts.back();
	if (rightFingerCountData.trackingEnabled && rightFingerCountData.fingerCounts.size() > 0)
		rightFingerCount = rightFingerCountData.fingerCounts.back();

	recordNextSkeletonFrame(data, leftFingerCount, rightFingerCount);
}
void FubiOpenNISensor::getSkeletonJointData(unsigned int id, Fubi::SkeletonJoint::Joint joint, Fubi::SkeletonJointPosition& position, Fubi::SkeletonJointOrientation& orientation)
{
	FubiUser* user = Fubi::getUser(id);
	if (user)
	{
		if (user->isTracked())
		{
			xn::SkeletonCapability cap = m_UserGenerator.GetSkeletonCap();
			// Standard case user is tracked
			// Get the current tracking data
			XnSkeletonJoint xjoint = JointToXNSkeletonJoint(joint);
			if (xjoint == XN_SKEL_WAIST) // special case currently not supported by OpenNI but approximated as center between hips
			{
				// Position is middle between the hips
				XnSkeletonJointPosition pos1, pos2;
				cap.GetSkeletonJointPosition(user->id(), XN_SKEL_LEFT_HIP, pos1);
				cap.GetSkeletonJointPosition(user->id(), XN_SKEL_RIGHT_HIP, pos2);
				position.m_confidence = minf(pos1.fConfidence, pos2.fConfidence);
				position.m_position = xnJointToVec3f(pos2) + (xnJointToVec3f(pos1) - xnJointToVec3f(pos2))*0.5f;

				// Rotation is the same as torso rotation
				XnSkeletonJointOrientation rot;
				cap.GetSkeletonJointOrientation(user->id(), XN_SKEL_TORSO, rot);
				orientation = SkeletonJointOrientation(rot.orientation.elements, rot.fConfidence);
			}
			else
			{
				// Standard case, just get the data
				XnSkeletonJointPosition pos;
				XnSkeletonJointOrientation rot;
				cap.GetSkeletonJointPosition(user->id(), xjoint, pos);
				cap.GetSkeletonJointOrientation(user->id(), xjoint, rot);
				position.m_position = xnJointToVec3f(pos);
				position.m_confidence = pos.fConfidence;
				orientation.m_orientation = Matrix3f(rot.orientation.elements);
				orientation.m_confidence = pos.fConfidence;
			}
		}
		else
		{
			// Not tracked return the center of mass instead
			// that should be independent of tracking state
			XnPoint3D com;
			m_UserGenerator.GetCoM(user->id(), com);
			position.m_confidence = 0.5f; // leave confidence at 0.5 as this is not really the wanted position
			position.m_position.x = com.X;
			position.m_position.y = com.Y;
			position.m_position.z = com.Z;
		}
	}
}
void FubiRecorder::recordNextSkeletonFrame(const FubiUser& user, const FubiUser::FingerCountData& leftFingerCountData, const FubiUser::FingerCountData& rightFingerCountData)
{
	if (m_isRecording && (user.currentTrackingData()->timeStamp - m_lastTimeStamp) > Math::Epsilon)
	{
		const FubiUser::TrackingData* data = m_useFilteredData
			? user.currentFilteredTrackingData()
			: user.currentTrackingData();

		m_lastTimeStamp = data->timeStamp;

		xml_document<> doc;
		// Create base node
		xml_node<>* frameNode = doc.allocate_node(node_element, "Frame");
		doc.append_node(frameNode);
		// With time attribute		
		xml_attribute<>* attr = doc.allocate_attribute("time", doc.allocate_string(numToString(m_lastTimeStamp-m_recordingStart, 6).c_str()));
		frameNode->append_attribute(attr);
		// And frame id		
		attr = doc.allocate_attribute("frameID", doc.allocate_string(numToString(m_currentFrameID++, 0).c_str()));
		frameNode->append_attribute(attr);

		// Create all sub nodes for the frame
		xml_node<>* jointPositions = doc.allocate_node(node_element, "JointPositions");
		frameNode->append_node(jointPositions);
		xml_node<>* jointOrientations = doc.allocate_node(node_element, "JointOrientations");
		frameNode->append_node(jointOrientations);

		// Now fill them with the joint data
		for (int i = 0; i < SkeletonJoint::NUM_JOINTS; ++i)
		{
			xml_node<>* joint = doc.allocate_node(node_element, "Joint");
			joint->append_attribute(doc.allocate_attribute("name", getJointName((SkeletonJoint::Joint)i)));
			const Vec3f* pos = &(data->jointPositions[i].m_position);
			joint->append_attribute(doc.allocate_attribute("x", doc.allocate_string(numToString(pos->x, 0).c_str())));
			joint->append_attribute(doc.allocate_attribute("y", doc.allocate_string(numToString(pos->y, 0).c_str())));
			joint->append_attribute(doc.allocate_attribute("z", doc.allocate_string(numToString(pos->z, 0).c_str())));
			joint->append_attribute(doc.allocate_attribute("confidence", doc.allocate_string(numToString(data->jointPositions[i].m_confidence, 0).c_str())));
			jointPositions->append_node(joint);

			joint = doc.allocate_node(node_element, "Joint");
			joint->append_attribute(doc.allocate_attribute("name", getJointName((SkeletonJoint::Joint)i)));
			const Vec3f* rot = &(data->jointOrientations[i].m_orientation.getRot());
			joint->append_attribute(doc.allocate_attribute("x", doc.allocate_string(numToString(rot->x, 2).c_str())));
			joint->append_attribute(doc.allocate_attribute("y", doc.allocate_string(numToString(rot->y, 2).c_str())));
			joint->append_attribute(doc.allocate_attribute("z", doc.allocate_string(numToString(rot->z, 2).c_str())));
			joint->append_attribute(doc.allocate_attribute("confidence", doc.allocate_string(numToString(data->jointOrientations[i].m_confidence, 0).c_str())));
			jointOrientations->append_node(joint);
		}

		int leftFingerCount = -1, rightFingerCount = -1;
		if (leftFingerCountData.trackingEnabled && leftFingerCountData.fingerCounts.size() > 0)
			leftFingerCount = leftFingerCountData.fingerCounts.back();
		if (rightFingerCountData.trackingEnabled && rightFingerCountData.fingerCounts.size() > 0)
			rightFingerCount = rightFingerCountData.fingerCounts.back();
		if (leftFingerCount > -1 || rightFingerCount > -1)
		{
			xml_node<>* fingercounts = doc.allocate_node(node_element, "FingerCounts");
			frameNode->append_node(fingercounts);
			fingercounts->append_attribute(doc.allocate_attribute("left", doc.allocate_string(numToString(leftFingerCount, 0).c_str())));
			fingercounts->append_attribute(doc.allocate_attribute("right", doc.allocate_string(numToString(rightFingerCount, 0).c_str())));
		}

		// Write the frame node to the file
		// Call the internal print_node directly to start at a different indent level
		internal::print_node(std::ostream_iterator<char>(m_file), frameNode, 0, 1);
		if (m_file.fail())
		{
			Fubi_logErr("Error writing skeleton data to file: %s \n", m_fileName.c_str());
			stopRecording();
		}
	}
}