CKinectV1.cpp

//-----------------------------------------------
//
//	This file is part of the Siv3D Engine.
//
//	Copyright (C) 2008-2016 Ryo Suzuki
//
//	Licensed under the MIT License.
//
//-----------------------------------------------

# include "CKinectV1.hpp"
# include "../EngineUtility.hpp"
# include "../../../Inc/Siv3D/KinectV1.hpp"
# include "../../../Inc/Siv3D/DynamicTexture.hpp"
# include "../Logger/Logger.hpp"

namespace s3d
{
	NUI_IMAGE_RESOLUTION ToNUIResolution(const Point& size)
	{
		if (size == Point(80, 60))
		{
			return NUI_IMAGE_RESOLUTION_80x60;
		}
		else if (size == Point(320, 240))
		{
			return NUI_IMAGE_RESOLUTION_320x240;
		}
		else
		{
			return NUI_IMAGE_RESOLUTION_640x480;
		}
	}

	static Vec2 ToDepthSpace(const Vector4& pos, const Point& size)
	{
		float x = 0.0f, y = 0.0f;
		::NuiTransformSkeletonToDepthImage(pos, &x, &y, ToNUIResolution(size));
		return{ x, y };
	}

	CKinectV1::CKinectV1()
	{

	}

	CKinectV1::~CKinectV1()
	{
		stop();
	}

	bool CKinectV1::init()
	{
		m_kinect10 = ::LoadLibraryW(L"kinect10.dll");

		if (!m_kinect10)
		{
			LOG_INFO(L"Kinect v1 は使用できません。");

			return false;
		}

		p_NuiGetSensorCount = FunctionPointer(m_kinect10, "NuiGetSensorCount");

		p_NuiCreateSensorByIndex = FunctionPointer(m_kinect10, "NuiCreateSensorByIndex");

		if (!p_NuiGetSensorCount || !p_NuiCreateSensorByIndex)
		{
			LOG_FAIL(L"Kinect v1: NuiGetSensorCount と NuiCreateSensorByIndex のロードに失敗しました。");

			LOG_INFO(L"Kinect v1 は使用できません。");

			return false;
		}

		LOG_INFO(L"Kinect v1 を使用できます。");

		return true;
	}

	bool CKinectV1::connect()
	{
		if (m_sensor)
		{
			return true;
		}

		if (!p_NuiGetSensorCount || !p_NuiCreateSensorByIndex)
		{
			return false;
		}

		if (FAILED(p_NuiCreateSensorByIndex(0, &m_sensor))
			|| !m_sensor)
		{
			return false;
		}

		LOG_INFO(L"Kinect v1 に接続しました。");

		return true;
	}

	bool CKinectV1::isAvailable()
	{
		if (!p_NuiGetSensorCount)
		{
			return false;
		}

		int sensorCount = 0;
		p_NuiGetSensorCount(&sensorCount);

		return sensorCount != 0;
	}

	bool CKinectV1::start(int dataType)
	{
		if (!connect())
		{
			return false;
		}

		stop();

		const bool useColor = !!(dataType & (KinectV1DataType::Color_80x60 | KinectV1DataType::Color_320x240 | KinectV1DataType::Color_640x480));
		const bool useDepth = !!(dataType & (KinectV1DataType::Depth_80x60 | KinectV1DataType::Depth_320x240 | KinectV1DataType::Depth_640x480));
		const bool useBody = !!(dataType & KinectV1DataType::Body);
		const bool useBodyIndex = !!(dataType & KinectV1DataType::BodyIndex);
		const bool nearMode = !!(dataType & KinectV1DataType::NearMode);
		const bool seatedMode = !!(dataType & KinectV1DataType::SeatedMode);

		if (useBody && !useDepth)
		{
			LOG_FAIL(L"Body を取得するには Depth を有効にしてください。");

			return false;
		}

		if (useBodyIndex && !useDepth)
		{
			LOG_FAIL(L"BodyIndex を取得するには Depth を有効にしてください。");

			return false;
		}

		DWORD nuiFlags = 0;

		if (useColor)
		{
			nuiFlags |= NUI_INITIALIZE_FLAG_USES_COLOR;
		}

		if (useBody)
		{
			nuiFlags |= NUI_INITIALIZE_FLAG_USES_SKELETON;
		}

		if (useDepth)
		{
			if (useBodyIndex)
			{
				nuiFlags |= NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX;
			}
			else
			{
				nuiFlags |= NUI_INITIALIZE_FLAG_USES_DEPTH;
			}
		}

		if (FAILED(m_sensor->NuiInitialize(nuiFlags)))
		{
			LOG_FAIL(L"Kinect v1 の初期化に失敗しました。");

			return false;
		}

		m_colorEvent = ::CreateEventW(0, true, false, nullptr);

		m_depthEvent = ::CreateEventW(0, true, false, nullptr);

		m_bodyEvent = ::CreateEventW(0, true, false, nullptr);

		if (useColor)
		{
			const auto resolution =
				(dataType & KinectV1DataType::Color_640x480) ? NUI_IMAGE_RESOLUTION_640x480
				: (dataType & KinectV1DataType::Color_320x240) ? NUI_IMAGE_RESOLUTION_320x240
				: NUI_IMAGE_RESOLUTION_80x60;

			if (FAILED(m_sensor->NuiImageStreamOpen(
				NUI_IMAGE_TYPE_COLOR,
				resolution,
				0,
				2,
				m_colorEvent,
				&m_colorStream)))
			{
				LOG_FAIL(L"Kinect v1 Color の初期化に失敗");
				return false;
			}

			const auto imageResolution =
				(dataType & KinectV1DataType::Color_640x480) ? Point(640, 480)
				: (dataType & KinectV1DataType::Color_320x240) ? Point(320, 240)
				: Point(80, 60);

			m_colorImage.resize(imageResolution, Palette::Black);

			LOG_DEBUG(L"Kinect v1 Color ストリームを開始しました。");
		}

		if (useDepth)
		{
			const auto resolution =
				(dataType & KinectV1DataType::Depth_640x480) ? NUI_IMAGE_RESOLUTION_640x480
				: (dataType & KinectV1DataType::Depth_320x240) ? NUI_IMAGE_RESOLUTION_320x240
				: NUI_IMAGE_RESOLUTION_80x60;

			if (FAILED(m_sensor->NuiImageStreamOpen(
				useBody ? NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX : NUI_IMAGE_TYPE_DEPTH,
				resolution,
				0,
				2,
				m_depthEvent,
				&m_depthStream)))
			{
				LOG_FAIL(L"Kinect v1 Depth の初期化に失敗");
				return false;
			}

			if (nearMode)
			{
				m_sensor->NuiImageStreamSetImageFrameFlags(m_depthStream, NUI_IMAGE_STREAM_FLAG_ENABLE_NEAR_MODE);
			}

			LOG_DEBUG(L"Kinect v1 Depth ストリームを開始しました。");

			const auto imageResolution =
				(dataType & KinectV1DataType::Depth_640x480) ? Point(640, 480)
				: (dataType & KinectV1DataType::Depth_320x240) ? Point(320, 240)
				: Point(80, 60);

			m_depth.resize(imageResolution.x, imageResolution.y, 0);

			if (useBodyIndex)
			{
				m_bodyIndex.resize(imageResolution.x, imageResolution.y, 0);
			}
		}

		if (useBody)
		{
			DWORD bodyTrackingFlags = 0;

			if (nearMode)
			{
				bodyTrackingFlags |= NUI_SKELETON_TRACKING_FLAG_ENABLE_IN_NEAR_RANGE;
			}

			if (seatedMode)
			{
				bodyTrackingFlags |= NUI_SKELETON_TRACKING_FLAG_ENABLE_SEATED_SUPPORT;
			}

			if (FAILED(m_sensor->NuiSkeletonTrackingEnable(m_bodyEvent, bodyTrackingFlags)))
			{
				LOG_FAIL(L"fail: NuiSkeletonTrackingEnable");

				return false;
			}

			LOG_DEBUG(L"Kinect v1 Body トラッキングを開始しました。");
		}

		LOG_INFO(L"Kinect を初期化しました。");

		m_isActive = true;

		m_currentData = dataType;

		return true;
	}

	void CKinectV1::stop()
	{
		if (m_colorEvent)
		{
			::ResetEvent(m_colorEvent);
		}

		if (m_depthEvent)
		{
			::ResetEvent(m_depthEvent);
		}

		if (m_bodyEvent)
		{
			::ResetEvent(m_bodyEvent);
		}

		if (m_sensor)
		{
			m_sensor->NuiShutdown();
		}

		if (m_colorEvent && m_colorEvent != INVALID_HANDLE_VALUE)
		{
			::CloseHandle(m_colorEvent);
			m_colorEvent = nullptr;
		}

		if (m_depthEvent && m_depthEvent != INVALID_HANDLE_VALUE)
		{
			::CloseHandle(m_depthEvent);
			m_depthEvent = nullptr;
		}

		if (m_bodyEvent && m_bodyEvent != INVALID_HANDLE_VALUE)
		{
			::CloseHandle(m_bodyEvent);
			m_bodyEvent = nullptr;
		}

		m_colorImage.clear();

		m_depthImage.clear();

		m_depth.clear();

		m_bodyIndex.clear();

		m_bodyIndexImage.clear();

		m_bodyData.fill(none);

		m_currentData = 0;

		m_isActive = false;
	}

	void CKinectV1::update()
	{
		if (!m_isActive)
		{
			return;
		}

		updateColor();

		updateDepth();

		updateBody();
	}

	bool CKinectV1::hasNewColorFrame() const
	{
		return m_hasNewColor;
	}

	bool CKinectV1::getColorFrame(Image& image)
	{
		if (!m_colorImage)
		{
			return false;
		}

		image.resize(m_colorImage.size);

		::memcpy(image.data(), m_colorImage.data(), image.memorySize());

		m_hasNewColor = false;

		return true;
	}

	bool CKinectV1::getColorFrame(DynamicTexture& texture)
	{
		if (!m_colorImage)
		{
			return false;
		}

		if (texture && texture.size != m_colorImage.size)
		{
			return false;
		}

		m_hasNewColor = false;

		return texture.fill(m_colorImage);
	}


	bool CKinectV1::hasNewDepthFrame() const
	{
		return m_hasNewDepth;
	}

	const Grid<unsigned short>& CKinectV1::getDepthData() const
	{
		return m_depth;
	}

	bool CKinectV1::getDepthFrame(Image& image, std::function<Color(uint16)> coloring)
	{
		if (m_depth.isEmpty)
		{
			return false;
		}

		image.resize(m_depth.width, m_depth.height);

		const uint16* pSrc = m_depth.data();
		const uint16* pSrcEnd = pSrc + m_depth.num_elements();
		Color* pDst = image[0];

		while (pSrc != pSrcEnd)
		{
			*pDst = coloring(*pSrc >> 3);
			++pSrc;
			++pDst;
		}

		m_hasNewDepth = false;

		return true;
	}

	bool CKinectV1::getDepthFrame(DynamicTexture& texture, std::function<Color(uint16)> coloring)
	{
		if (m_depth.isEmpty)
		{
			return false;
		}

		m_depthImage.resize(m_depth.width, m_depth.height);

		if (texture && texture.size != m_depthImage.size)
		{
			return false;
		}

		const uint16* pSrc = m_depth.data();
		const uint16* pSrcEnd = pSrc + m_depth.num_elements();
		Color* pDst = m_depthImage[0];

		while (pSrc != pSrcEnd)
		{
			*pDst = coloring(*pSrc >> 3);
			++pSrc;
			++pDst;
		}

		m_hasNewDepth = false;

		return texture.fill(m_depthImage);
	}


	bool CKinectV1::hasNewBodyIndexFrame() const
	{
		return m_hasNewBodyIndex;
	}

	const Grid<unsigned char>& CKinectV1::getBodyIndexData() const
	{
		return m_bodyIndex;
	}

	bool CKinectV1::getBodyIndexFrame(Image& image, std::function<Color(uint8)> coloring)
	{
		if (m_bodyIndex.isEmpty)
		{
			return false;
		}

		image.resize(m_bodyIndex.width, m_bodyIndex.height);

		const uint8* pSrc = m_bodyIndex.data();
		const uint8* pSrcEnd = pSrc + m_bodyIndex.num_elements();
		Color* pDst = image[0];

		while (pSrc != pSrcEnd)
		{
			*pDst = coloring(*pSrc);
			++pSrc;
			++pDst;
		}

		m_hasNewBodyIndex = false;

		return true;
	}

	bool CKinectV1::getBodyIndexFrame(DynamicTexture& texture, std::function<Color(uint8)> coloring)
	{
		if (m_bodyIndex.isEmpty)
		{
			return false;
		}

		m_bodyIndexImage.resize(m_bodyIndex.width, m_bodyIndex.height);

		if (texture && texture.size != m_bodyIndexImage.size)
		{
			return false;
		}

		const uint8* pSrc = m_bodyIndex.data();
		const uint8* pSrcEnd = pSrc + m_bodyIndex.num_elements();
		Color* pDst = m_bodyIndexImage[0];

		while (pSrc != pSrcEnd)
		{
			*pDst = coloring(*pSrc);
			++pSrc;
			++pDst;
		}

		m_hasNewBodyIndex = false;

		return texture.fill(m_bodyIndexImage);
	}


	bool CKinectV1::hasNewBodyFrame() const
	{
		return m_hasNewBody;
	}

	bool CKinectV1::getBodyFrame(std::array<Optional<KinectV1Body>, 2>& bodies)
	{
		bodies = m_bodyData;

		m_hasNewBody = false;

		return true;
	}


	int CKinectV1::getElevation()
	{
		if (!m_isActive)
		{
			return 0;
		}

		long angleDegree = 0;

		m_sensor->NuiCameraElevationGetAngle(&angleDegree);

		return angleDegree;
	}

	void CKinectV1::setElevation(int angleDegree)
	{
		if (!m_isActive)
		{
			return;
		}

		m_sensor->NuiCameraElevationSetAngle(Clamp(angleDegree, NUI_CAMERA_ELEVATION_MINIMUM, NUI_CAMERA_ELEVATION_MAXIMUM));
	}

	Vec3 CKinectV1::getAcceleration()
	{
		if (!m_isActive)
		{
			return Vec3::Zero;
		}

		Vector4 accelerometer = { 0 };

		if (!m_sensor || FAILED(m_sensor->NuiAccelerometerGetCurrentReading(&accelerometer)))
		{
			return Vec3::Zero;
		}

		return{ accelerometer.x, accelerometer.y, accelerometer.z };
	}


	void CKinectV1::updateColor()
	{
		const bool useColor = !!(m_currentData & (KinectV1DataType::Color_80x60 | KinectV1DataType::Color_320x240 | KinectV1DataType::Color_640x480));

		if (!useColor)
		{
			return;
		}

		if (::WaitForSingleObject(m_colorEvent, 0) != WAIT_OBJECT_0)
		{
			return;
		}

		NUI_IMAGE_FRAME imageFrame;

		if (FAILED(m_sensor->NuiImageStreamGetNextFrame(m_colorStream, 0, &imageFrame)))
		{
			LOG_FAIL(L"Fail GetNextFrame RGB");
			return;
		}

		NUI_LOCKED_RECT rect;
		imageFrame.pFrameTexture->LockRect(0, &rect, 0, 0);

		assert(m_colorImage.memorySize() == static_cast<size_t>(rect.size));

		::memcpy(m_colorImage.data(), rect.pBits, rect.size);

		if (FAILED(m_sensor->NuiImageStreamReleaseFrame(m_colorStream, &imageFrame)))
		{
			LOG_FAIL(L"Fail NuiImageStreamReleaseFrame");
			return;
		}

		for (auto& pixel : m_colorImage)
		{
			const unsigned char t = pixel.r;
			pixel.r = pixel.b;
			pixel.b = t;
			pixel.a = 255;
		}

		m_hasNewColor = true;
	}

	void CKinectV1::updateDepth()
	{
		const bool useDepth = !!(m_currentData & (KinectV1DataType::Depth_80x60 | KinectV1DataType::Depth_320x240 | KinectV1DataType::Depth_640x480));
		const bool useBodyIndex = !!(m_currentData & KinectV1DataType::BodyIndex);

		if (!useDepth)
		{
			return;
		}

		if (::WaitForSingleObject(m_depthEvent, 0) != WAIT_OBJECT_0)
		{
			return;
		}

		NUI_IMAGE_FRAME imageFrame;

		if (FAILED(m_sensor->NuiImageStreamGetNextFrame(m_depthStream, 0, &imageFrame)))
		{
			LOG_FAIL(L"Fail GetNextFrame Depth");
			return;
		}

		NUI_LOCKED_RECT rect;
		imageFrame.pFrameTexture->LockRect(0, &rect, 0, 0);

		assert(m_depth.memorySize() == static_cast<size_t>(rect.size));

		::memcpy(m_depth.data(), rect.pBits, rect.size);

		if (FAILED(m_sensor->NuiImageStreamReleaseFrame(m_depthStream, &imageFrame)))
		{
			LOG_FAIL(L"Fail NuiImageStreamReleaseFrame");
			return;
		}

		m_hasNewDepth = true;

		if (useBodyIndex)
		{
			m_hasNewBodyIndex = true;

			const uint16* pSrc = m_depth.data();
			const uint16* pSrcEnd = pSrc + m_depth.num_elements();
			uint8* pDst = m_bodyIndex[0];

			while (pSrc != pSrcEnd)
			{
				*pDst = *pSrc & 0x7;
				++pSrc;
				++pDst;
			}
		}

		const bool useBody = !!(m_currentData & KinectV1DataType::Body);

		if (useBody)
		{
			m_hasNewBody = true;
		}
	}

	void CKinectV1::updateBody()
	{
		const bool useBody = !!(m_currentData & KinectV1DataType::Body);

		m_bodyData.fill(none);

		if (!useBody)
		{
			return;
		}

		bool foundSkeleton = false;

		if (SUCCEEDED(m_sensor->NuiSkeletonGetNextFrame(0, &m_skeletonFrame)))
		{
			for (int i = 0; i < NUI_SKELETON_COUNT; ++i)
			{
				NUI_SKELETON_TRACKING_STATE trackingState = m_skeletonFrame.SkeletonData[i].eTrackingState;

				if (trackingState == NUI_SKELETON_TRACKED || trackingState == NUI_SKELETON_POSITION_ONLY)
				{
					foundSkeleton = true;
				}
			}
		}

		if (!foundSkeleton)
		{
			return;
		}

		if (FAILED(m_sensor->NuiTransformSmooth(&m_skeletonFrame, nullptr)))
		{
			return;
		}

		m_trackIDs[0] = m_trackIDs[1] = 0;

		if (ChooserModeClosest1 == m_chooserMode || ChooserModeClosest2 == m_chooserMode)
		{
			chooseClosestSkeletons(m_trackIDs);
		}
		else if (ChooserModeSticky1 == m_chooserMode || ChooserModeSticky2 == m_chooserMode)
		{
			chooseStickySkeletons(m_trackIDs);
		}

		if (ChooserModeClosest1 == m_chooserMode || ChooserModeSticky1 == m_chooserMode)
		{
			// Track only one player ID. The second ID is not used
			m_trackIDs[SecondTrackID] = 0;
		}

		m_sensor->NuiSkeletonSetTrackedSkeletons(m_trackIDs);

		for (int i = 0; i < NUI_SKELETON_COUNT; ++i)
		{
			const auto& skelton = m_skeletonFrame.SkeletonData[i];

			if (skelton.eTrackingState == NUI_SKELETON_NOT_TRACKED)
			{
				continue;
			}

			if (skelton.dwTrackingID == m_trackIDs[0]
				|| skelton.dwTrackingID == m_trackIDs[1])
			{
				auto& bodyData = m_bodyData[skelton.dwTrackingID == m_trackIDs[1]];
				bodyData = KinectV1Body();

				for (int s = 0; s < 20; ++s)
				{
					const Vector4 pos = skelton.SkeletonPositions[s];

					bodyData->joints[s].cameraSpacePos = Vec3(pos.x, pos.y, pos.z);

					bodyData->joints[s].depthSpacePos = ToDepthSpace(pos, m_depthImage.size);

					bodyData->joints[s].trackingState =
						skelton.eSkeletonPositionTrackingState[s] == NUI_SKELETON_POSITION_TRACKED ?
						TrackingState::Tracked
						: skelton.eSkeletonPositionTrackingState[s] == NUI_SKELETON_POSITION_INFERRED ?
						TrackingState::Inferred
						: TrackingState::NotTracked;
				}

				bodyData->trackingID = skelton.dwTrackingID;

				const Vector4 bodyPos = skelton.Position;

				bodyData->cameraSpaceBodyPos = Vec3(bodyPos.x, bodyPos.y, bodyPos.z);

				bodyData->depthSpaceBodyPos = ToDepthSpace(bodyPos, m_depthImage.size);
			}
		}
	}


	/// <summary>
	/// Find sticky skeletons to set tracked
	/// </summary>
	/// <param name="trackIDs">Array of skeleton tracking IDs</param>
	void CKinectV1::chooseStickySkeletons(DWORD trackIDs[TrackIDIndexCount])
	{
		ZeroMemory(trackIDs, TrackIDIndexCount * sizeof(DWORD));

		findStickyIDs(trackIDs);
		assignNewStickyIDs(trackIDs);

		// Update stored sticky IDs
		m_stickyIDs[FirstTrackID] = trackIDs[FirstTrackID];
		m_stickyIDs[SecondTrackID] = trackIDs[SecondTrackID];
	}

	void CKinectV1::chooseClosestSkeletons(DWORD trackIDs[TrackIDIndexCount])
	{
		ZeroMemory(trackIDs, TrackIDIndexCount * sizeof(DWORD));

		// Initial depth array with max posible value
		USHORT nearestDepth[TrackIDIndexCount] = { NUI_IMAGE_DEPTH_MAXIMUM, NUI_IMAGE_DEPTH_MAXIMUM };

		for (int i = 0; i < NUI_SKELETON_COUNT; i++)
		{
			if (NUI_SKELETON_NOT_TRACKED != m_skeletonFrame.SkeletonData[i].eTrackingState)
			{
				LONG   x, y;
				USHORT depth;

				// Transform skeleton coordinates to depth image
				NuiTransformSkeletonToDepthImage(m_skeletonFrame.SkeletonData[i].Position, &x, &y, &depth);

				// Compare depth to peviously found item
				if (depth < nearestDepth[FirstTrackID])
				{
					// Move depth and track ID in first place to second place and assign with the new closer one
					nearestDepth[SecondTrackID] = nearestDepth[FirstTrackID];
					nearestDepth[FirstTrackID] = depth;

					trackIDs[SecondTrackID] = trackIDs[FirstTrackID];
					trackIDs[FirstTrackID] = m_skeletonFrame.SkeletonData[i].dwTrackingID;
				}
				else if (depth < nearestDepth[SecondTrackID])
				{
					// Replace old depth and track ID in second place with the newly found closer one
					nearestDepth[SecondTrackID] = depth;
					trackIDs[SecondTrackID] = m_skeletonFrame.SkeletonData[i].dwTrackingID;
				}
			}
		}
	}

	/// <summary>
	/// Verify if stored tracked IDs are found in new skeleton frame
	/// </summary>
	/// <param name="trackIDs">Array of skeleton tracking IDs</param>
	void CKinectV1::findStickyIDs(DWORD trackIDs[TrackIDIndexCount])
	{
		for (int i = 0; i < TrackIDIndexCount; i++)
		{
			for (int j = 0; j < NUI_SKELETON_COUNT; j++)
			{
				if (NUI_SKELETON_NOT_TRACKED != m_skeletonFrame.SkeletonData[j].eTrackingState)
				{
					DWORD trackID = m_skeletonFrame.SkeletonData[j].dwTrackingID;
					if (trackID == m_stickyIDs[i])
					{
						trackIDs[i] = trackID;
						break;
					}
				}
			}
		}
	}

	/// <summary>
	/// Assign a new ID if old sticky is not found in new skeleton frame
	/// </summary>
	/// <param name="trackIDs">Array of skeleton tracking IDs</param>
	void CKinectV1::assignNewStickyIDs(DWORD trackIDs[TrackIDIndexCount])
	{
		for (int i = 0; i < NUI_SKELETON_COUNT; i++)
		{
			if (trackIDs[FirstTrackID] && trackIDs[SecondTrackID])
			{
				break;
			}

			if (NUI_SKELETON_NOT_TRACKED != m_skeletonFrame.SkeletonData[i].eTrackingState)
			{
				DWORD trackID = m_skeletonFrame.SkeletonData[i].dwTrackingID;

				if (!trackIDs[FirstTrackID] && trackID != trackIDs[SecondTrackID])
				{
					trackIDs[FirstTrackID] = trackID;
				}
				else if (!trackIDs[SecondTrackID] && trackID != trackIDs[FirstTrackID])
				{
					trackIDs[SecondTrackID] = trackID;
				}
			}
		}
	}
}

CKinectV1.hpp

//-----------------------------------------------
//
//	This file is part of the Siv3D Engine.
//
//	Copyright (C) 2008-2016 Ryo Suzuki
//
//	Licensed under the MIT License.
//
//-----------------------------------------------

# pragma once
# define  NOMINMAX
# define  STRICT
# define  _WIN32_WINNT _WIN32_WINNT_WIN7
# define  NTDDI_VERSION NTDDI_WIN7
# include <Windows.h>
# include <wrl.h>
using namespace Microsoft::WRL;
# include <NuiApi.h>
# include "IKinectV1.hpp"
# include "../../../Inc/Siv3D/Image.hpp"
# include "../../../Inc/Siv3D/Grid.hpp"

namespace s3d
{
	enum ChooserMode
	{
		ChooserModeClosest1,
		ChooserModeClosest2,
		ChooserModeSticky1,
		ChooserModeSticky2,
	};

	enum TrackIDIndex
	{
		FirstTrackID = 0,
		SecondTrackID,
		TrackIDIndexCount
	};

	class CKinectV1 : public ISiv3DKinectV1
	{
	private:

		HINSTANCE m_kinect10 = nullptr;

		decltype(NuiGetSensorCount)* p_NuiGetSensorCount = nullptr;

		decltype(NuiCreateSensorByIndex)* p_NuiCreateSensorByIndex = nullptr;

		ComPtr<INuiSensor> m_sensor;

		HANDLE m_colorEvent = nullptr;

		HANDLE m_depthEvent = nullptr;

		HANDLE m_bodyEvent = nullptr;

		HANDLE m_colorStream = nullptr;

		HANDLE m_depthStream = nullptr;

		int m_currentData = 0;

		Image m_colorImage;

		Image m_depthImage;

		Image m_bodyIndexImage;

		Grid<uint16> m_depth;

		Grid<uint8> m_bodyIndex;

		std::array<Optional<KinectV1Body>, 2> m_bodyData;

		NUI_SKELETON_FRAME m_skeletonFrame;

		DWORD m_trackIDs[TrackIDIndexCount];

		DWORD m_stickyIDs[TrackIDIndexCount];

		ChooserMode m_chooserMode = ChooserModeClosest2;

		bool m_isActive = false;

		bool m_hasNewColor = false;

		bool m_hasNewDepth = false;

		bool m_hasNewBodyIndex = false;

		bool m_hasNewBody = false;

		void updateColor();

		void updateDepth();

		void updateBody();


		void chooseStickySkeletons(DWORD trackIDs[TrackIDIndexCount]);

		void chooseClosestSkeletons(DWORD trackIDs[TrackIDIndexCount]);

		void findStickyIDs(DWORD trackIDs[TrackIDIndexCount]);

		void assignNewStickyIDs(DWORD trackIDs[TrackIDIndexCount]);

	public:

		CKinectV1();

		~CKinectV1() override;

		bool init() override;

		bool connect() override;

		bool isAvailable() override;

		bool start(int dataType) override;

		void stop() override;

		void update() override;


		bool hasNewColorFrame() const override;

		bool getColorFrame(Image& image) override;

		bool getColorFrame(DynamicTexture& texture) override;


		bool hasNewDepthFrame() const override;

		const Grid<uint16>& getDepthData() const override;

		bool getDepthFrame(Image& image, std::function<Color(uint16)> coloring) override;

		bool getDepthFrame(DynamicTexture& texture, std::function<Color(uint16)> coloring) override;


		bool hasNewBodyIndexFrame() const override;

		const Grid<uint8>& getBodyIndexData() const override;

		bool getBodyIndexFrame(Image& image, std::function<Color(uint8)> coloring) override;

		bool getBodyIndexFrame(DynamicTexture& texture, std::function<Color(uint8)> coloring) override;


		bool hasNewBodyFrame() const override;

		bool getBodyFrame(std::array<Optional<KinectV1Body>, 2>& bodies) override;


		int getElevation() override;

		void setElevation(int angleDegree) override;

		Vec3 getAcceleration() override;
	};
}

SivKinectV1.cpp

//-----------------------------------------------
//
//	This file is part of the Siv3D Engine.
//
//	Copyright (C) 2008-2016 Ryo Suzuki
//
//	Licensed under the MIT License.
//
//-----------------------------------------------

# include "../Siv3DEngine.hpp"
# include "IKinectV1.hpp"
# include "../../../Inc/Siv3D/KinectV1.hpp"

namespace s3d
{
	namespace KinectV1
	{
		bool IsAvailable()
		{
			return Siv3DEngine::GetKinectV1()->isAvailable();
		}

		bool Start(int32 dataType)
		{
			return Siv3DEngine::GetKinectV1()->start(dataType);
		}

		void Stop()
		{
			Siv3DEngine::GetKinectV1()->stop();
		}


		bool HasNewColorFrame()
		{
			return Siv3DEngine::GetKinectV1()->hasNewColorFrame();
		}

		bool GetColorFrame(Image& image)
		{
			return Siv3DEngine::GetKinectV1()->getColorFrame(image);
		}

		bool GetColorFrame(DynamicTexture& texture)
		{
			return Siv3DEngine::GetKinectV1()->getColorFrame(texture);
		}


		bool HasNewDepthFrame()
		{
			return Siv3DEngine::GetKinectV1()->hasNewDepthFrame();
		}

		const Grid<unsigned short>& GetRawDepthData()
		{
			return Siv3DEngine::GetKinectV1()->getDepthData();
		}

		bool GetDepthFrame(Image& image, std::function<Color(unsigned short)> coloring)
		{
			return Siv3DEngine::GetKinectV1()->getDepthFrame(image, coloring);
		}

		bool GetDepthFrame(DynamicTexture& texture, std::function<Color(unsigned short)> coloring)
		{
			return Siv3DEngine::GetKinectV1()->getDepthFrame(texture, coloring);
		}


		bool HasNewBodyIndexFrame()
		{
			return Siv3DEngine::GetKinectV1()->hasNewBodyIndexFrame();
		}

		const Grid<unsigned char>& GetRawBodyIndexData()
		{
			return Siv3DEngine::GetKinectV1()->getBodyIndexData();
		}

		bool GetBodyIndexFrame(Image& image, std::function<Color(unsigned char)> coloring)
		{
			return Siv3DEngine::GetKinectV1()->getBodyIndexFrame(image, coloring);
		}

		bool GetBodyIndexFrame(DynamicTexture& texture, std::function<Color(unsigned char)> coloring)
		{
			return Siv3DEngine::GetKinectV1()->getBodyIndexFrame(texture, coloring);
		}

		bool HasNewBodyFrame()
		{
			return Siv3DEngine::GetKinectV1()->hasNewBodyFrame();
		}

		bool GetBodyFrame(std::array<Optional<KinectV1Body>, 2>& bodies)
		{
			return Siv3DEngine::GetKinectV1()->getBodyFrame(bodies);
		}

		int GetElevation()
		{
			return Siv3DEngine::GetKinectV1()->getElevation();
		}

		void SetElevation(int angleDegree)
		{
			Siv3DEngine::GetKinectV1()->setElevation(angleDegree);
		}

		Vec3 GetAcceleration()
		{
			return Siv3DEngine::GetKinectV1()->getAcceleration();
		}
	}
}