API tools faq
paste
Fieol

KinectV2Device.cpp

Fieol
Mar 31st, 2020
94
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 38.52 KB | None | 0 0
raw download clone embed print report
  1. //
  2. // Copyright 2018 Adam Horvath - MIRROR.UPLUGINS.COM - info@uplugins.com - All Rights Reserved.
  3. //
  4.  
  5. #include "VirtualMirrorPluginPrivatePCH.h"
  6. #include "KinectV2Device.h"
  7.  
  8.  
  9. #include <string>
  10. #include <iostream>
  11. #include <sstream>
  12.  
  13. //General Log
  14. DEFINE_LOG_CATEGORY(KinectV2);
  15.  
  16. #include "Core.h"
  17.  
  18. FKinectV2Device::FKinectV2Device()
  19. {
  20.     initiated = false;
  21.     DepthCoordinates = NULL;
  22.     QuadRGB = NULL;
  23.     QuadBACKGROUND = NULL;
  24.     QuadDEPTH = NULL;
  25.     QuadUSER = NULL;
  26.     KinectSensor = NULL;
  27.     MultiSourceFrameReader = NULL;
  28.     ColorFrameReader = NULL;
  29.     CoordinateMapper = NULL;
  30.     PlayerTrackingID = 0;
  31.     SensorOffset = FVector(0, 0, 0);
  32.     HandOcclusionMultiplier = 1.0;
  33.     bIsTracking = false;
  34.     KinectThread = NULL;
  35.     MultiSourceEventHandle = NULL;
  36.        
  37.     for (int i = 0; i < BODY_COUNT; i++)
  38.     {
  39.         FaceFrameSources[i] = NULL;
  40.         FaceFrameReaders[i] = NULL;
  41.     }
  42.  
  43.    
  44.     BoneOrientationFilter = 0;
  45.     DeviceName = "Kinect v2.0";
  46.  
  47.     UE_LOG(KinectV2, Log, TEXT("Device start"));
  48. }
  49.  
  50. FKinectV2Device::~FKinectV2Device()
  51. {
  52.     UE_LOG(KinectV2, Log, TEXT("Device shutdown"));
  53.     Cleanup();
  54. }
  55.  
  56. bool FKinectV2Device::StartupDevice()
  57. {
  58.     return true;
  59. }
  60.  
  61. uint32 FKinectV2Device::Run() {
  62.  
  63.     while (!bStop) {
  64.  
  65.  
  66.         if (!MultiSourceFrameReader)
  67.         {
  68.             return 0;
  69.         }
  70.  
  71.         HANDLE handles[] = { reinterpret_cast<HANDLE>(MultiSourceEventHandle) };
  72.         int idx;
  73.         // Wait for any of the events to be signalled
  74.         idx = WaitForMultipleObjects(1, handles, false, 0);
  75.         switch (idx) {
  76.         case WAIT_TIMEOUT:
  77.             continue;
  78.         case 0:
  79.             IMultiSourceFrameArrivedEventArgs *pFrameArgs = nullptr;
  80.             HRESULT hr = MultiSourceFrameReader->GetMultiSourceFrameArrivedEventData(MultiSourceEventHandle, &pFrameArgs);
  81.             MultiSourceFrameArrived(pFrameArgs);
  82.  
  83.             break;
  84.         }
  85.  
  86.     }
  87.  
  88.     return 0;
  89. }
  90.  
  91. void FKinectV2Device::Stop() {
  92.  
  93.     bStop = true;
  94.  
  95.  
  96. }
  97.  
  98. UTexture2D* FKinectV2Device::GetTextureRGB(){
  99.     if (this->initiated) {
  100.         return this->TextureRGB;
  101.     } else {
  102.         if (GEngine) {
  103.             GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Red, FString::Printf(TEXT("KinectV2 is not initialized properly. (RGB)")));
  104.         }
  105.         return this->DummyTexture;
  106.     }
  107. }
  108.  
  109.  
  110.  
  111. UTexture2D* FKinectV2Device::GetTextureDEPTH() {
  112.     if (this->initiated) {
  113.         return this->TextureDEPTH;
  114.     } else {
  115.         if (GEngine) {
  116.             GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Red, FString::Printf(TEXT("KinectV2 is not initialized properly. (DEPTH)")));
  117.         }
  118.         return this->DummyTexture;
  119.     }
  120. }
  121.  
  122. UTexture2D* FKinectV2Device::GetTextureUSER() {
  123.     if (this->initiated) {
  124.         return this->TextureUSER;
  125.     }
  126.     else {
  127.         if (GEngine) {
  128.             GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Red, FString::Printf(TEXT("KinectV2 is not initialized properly. (USER)")));
  129.         }
  130.         return this->DummyTexture;
  131.     }
  132. }
  133.  
  134.  
  135. void FKinectV2Device::ShutdownDevice()
  136. {
  137.     Cleanup();
  138. }
  139.  
  140. void FKinectV2Device::UpdateDevice(float DeltaTime) {
  141.    
  142.  
  143.         //Read color frame only
  144.         if (UpdateColorFrame()) {
  145.             UpdateTextureRGB();
  146.         }
  147.  
  148.         //If frames are ready update textures
  149.         if (MultiFrameReady) {
  150.             UpdateTextureDEPTH();
  151.             UpdateTextureUSER();
  152.             MultiFrameReady = false;
  153.         }
  154.    
  155. }
  156.  
  157. void FKinectV2Device::StartThread() {
  158.     if (KinectThread == nullptr) {
  159.         KinectThread = FRunnableThread::Create(this, TEXT("FKinectThread"), 0, EThreadPriority::TPri_AboveNormal);
  160.         UE_LOG(KinectV2, Log, TEXT("Kinect thread started"));
  161.     }
  162.  
  163. }
  164.  
  165. void FKinectV2Device::StopThread() {
  166.     Stop();
  167.     if (KinectThread) {
  168.  
  169.         KinectThread->Kill(true);
  170.         delete KinectThread;
  171.     }
  172.     KinectThread = nullptr;
  173.     UE_LOG(KinectV2, Log, TEXT("Kinect thread stopped"));
  174.  
  175. }
  176.  
  177.  
  178.  
  179.  
  180. void FKinectV2Device::UpdateTextureRGB(){
  181.    
  182.     if(QuadRGB){
  183.         const size_t SizeQuadRGB = 1920 * 1080 * sizeof(RGBQUAD);
  184.         uint8* Dest = (uint8*)TextureRGB->PlatformData->Mips[0].BulkData.Lock(LOCK_READ_WRITE);
  185.         FMemory::Memcpy(Dest, (uint8*)QuadRGB, SizeQuadRGB);
  186.  
  187.         TextureRGB->PlatformData->Mips[0].BulkData.Unlock();
  188.         TextureRGB->UpdateResource();
  189.        
  190.     }
  191.  
  192. }
  193.  
  194. void FKinectV2Device::UpdateTextureDEPTH() {
  195.     if (QuadDEPTH) {
  196.         const size_t SizeQuadDEPTH = 1920 * 1080 * sizeof(RGBQUAD);
  197.         uint8* Dest = (uint8*)TextureDEPTH->PlatformData->Mips[0].BulkData.Lock(LOCK_READ_WRITE);
  198.         FMemory::Memcpy(Dest, (uint8*)QuadDEPTH, SizeQuadDEPTH);
  199.  
  200.         TextureDEPTH->PlatformData->Mips[0].BulkData.Unlock();
  201.         TextureDEPTH->UpdateResource();
  202.  
  203.     }
  204.  
  205. }
  206.  
  207. void FKinectV2Device::UpdateTextureUSER()
  208. {
  209.     if (QuadUSER) {
  210.         const size_t SizeQuadUSER = 512 * 424 * sizeof(RGBQUAD);
  211.         uint8* Dest = (uint8*)TextureUSER->PlatformData->Mips[0].BulkData.Lock(LOCK_READ_WRITE);
  212.         FMemory::Memcpy(Dest, (uint8*)QuadUSER, SizeQuadUSER);
  213.         TextureUSER->PlatformData->Mips[0].BulkData.Unlock();
  214.         TextureUSER->UpdateResource();
  215.  
  216.     }
  217.  
  218.    
  219. }
  220.  
  221.  
  222. bool FKinectV2Device::Init(bool playOni){
  223.     RefreshTimer = 0;
  224.  
  225.     // create heap storage for the coorinate mapping from color to depth
  226.     DepthCoordinates = new DepthSpacePoint[1920 * 1080];
  227.  
  228.     //Defaults 
  229.    
  230.     FloorFound = false;
  231.     Calibrating = false;
  232.     SensorAngle = 0;
  233.     SensorHeight = 0;
  234.    
  235.     //Color texture
  236.     TextureRGB = UTexture2D::CreateTransient(1920,1080);
  237.     TextureRGB->SRGB = 1;
  238.     TextureRGB->UpdateResource();
  239.  
  240.     //Depth texture
  241.     TextureDEPTH = UTexture2D::CreateTransient(1920, 1080);
  242.     TextureDEPTH->SRGB = 0;
  243.     TextureDEPTH->UpdateResource();
  244.  
  245.     //Texture user
  246.     TextureUSER = UTexture2D::CreateTransient(512, 424);
  247.     TextureUSER->SRGB = 1;
  248.     TextureUSER->UpdateResource();
  249.  
  250.    
  251.     //Return this if WebcamTexture cannot be created by any reason.
  252.     DummyTexture = UTexture2D::CreateTransient(1920,1080);
  253.     DummyTexture->SRGB = 1;
  254.     DummyTexture->UpdateResource();
  255.  
  256.     //Create rgbquads for holding texture data
  257.     QuadRGB = new RGBQUAD[1920*1080];
  258.     QuadBACKGROUND = new RGBQUAD[1920 * 1080];
  259.     QuadDEPTH = new RGBQUAD[1920*1080];
  260.     QuadUSER = new RGBQUAD[512*424];
  261.  
  262.     //Filter
  263.     BoneOrientationFilter = new BoneOrientationDoubleExponentialFilter();
  264.    
  265.     //Init sensor
  266.     HRESULT hr;
  267.  
  268.     hr = GetDefaultKinectSensor(&KinectSensor);
  269.     if (FAILED(hr))
  270.     {
  271.         return false;
  272.     }
  273.    
  274.     if(KinectSensor)
  275.     {
  276.         // Initialize the Kinect and get coordinate mapper and the frame reader
  277.  
  278.         if (SUCCEEDED(hr))
  279.         {
  280.             hr = KinectSensor->get_CoordinateMapper(&CoordinateMapper);
  281.         }
  282.         else {
  283.             return false;
  284.         }
  285.  
  286.         hr = KinectSensor->Open();
  287.  
  288.         //Multi Source Frame Reader
  289.         if (SUCCEEDED(hr))
  290.         {
  291.             hr = KinectSensor->OpenMultiSourceFrameReader(
  292.                 FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_BodyIndex | FrameSourceTypes::FrameSourceTypes_Body | FrameSourceTypes::FrameSourceTypes_Infrared,
  293.                 &MultiSourceFrameReader);
  294.  
  295.            
  296.         }
  297.         else {
  298.             return false;
  299.         }
  300.  
  301.         if (SUCCEEDED(hr)) {
  302.             MultiSourceFrameReader->SubscribeMultiSourceFrameArrived(&MultiSourceEventHandle);
  303.         }
  304.  
  305.         //Color frame reader (cannot use multi frame becasue of the 15FPS color framedrop)
  306.         IColorFrameSource* ColorFrameSource = NULL;
  307.         if (SUCCEEDED(hr))
  308.         {
  309.             hr = KinectSensor->get_ColorFrameSource(&ColorFrameSource);
  310.         }
  311.         else {
  312.             return false;
  313.         }
  314.  
  315.         if (SUCCEEDED(hr))
  316.         {
  317.             hr = ColorFrameSource->OpenReader(&ColorFrameReader);
  318.         } else {
  319.             return false;
  320.         }
  321.  
  322.         SafeRelease(ColorFrameSource);
  323.  
  324.  
  325.         //Start face
  326.         if (SUCCEEDED(hr))
  327.         {
  328. #include "AllowWindowsPlatformTypes.h"
  329.             static const DWORD c_FaceFrameFeatures =
  330.                 FaceFrameFeatures::FaceFrameFeatures_BoundingBoxInColorSpace
  331.                 | FaceFrameFeatures::FaceFrameFeatures_PointsInColorSpace
  332.                 | FaceFrameFeatures::FaceFrameFeatures_RotationOrientation
  333.                 | FaceFrameFeatures::FaceFrameFeatures_Happy
  334.                 | FaceFrameFeatures::FaceFrameFeatures_RightEyeClosed
  335.                 | FaceFrameFeatures::FaceFrameFeatures_LeftEyeClosed
  336.                 | FaceFrameFeatures::FaceFrameFeatures_MouthOpen
  337.                 | FaceFrameFeatures::FaceFrameFeatures_MouthMoved
  338.                 | FaceFrameFeatures::FaceFrameFeatures_LookingAway
  339.                 | FaceFrameFeatures::FaceFrameFeatures_Glasses
  340.                 | FaceFrameFeatures::FaceFrameFeatures_FaceEngagement;
  341. #include "HideWindowsPlatformTypes.h"
  342.  
  343.             // create a face frame source + reader to track each body in the fov
  344.             for (int i = 0; i < BODY_COUNT; i++)
  345.             {
  346.                 if (SUCCEEDED(hr))
  347.                 {
  348.                     // create the face frame source by specifying the required face frame features
  349.                     // define the face frame features required to be computed by this application
  350.  
  351.                     hr = CreateFaceFrameSource(KinectSensor, 0, c_FaceFrameFeatures, &FaceFrameSources[i]);
  352.                 }
  353.                 if (SUCCEEDED(hr))
  354.                 {
  355.                     // open the corresponding reader
  356.                     hr = FaceFrameSources[i]->OpenReader(&FaceFrameReaders[i]);
  357.                     if (SUCCEEDED(hr))
  358.                     {
  359.                         UE_LOG(KinectV2, Log, TEXT("Face frame reader opened for: %d"), i);
  360.                         //Subscribe for arrived frame
  361.                     }
  362.                     else {
  363.                         UE_LOG(KinectV2, Error, TEXT("Face frame reader NOT opened for: %d"), i);
  364.                     }
  365.                 }
  366.                 else {
  367.                     UE_LOG(KinectV2, Error, TEXT("Face source could not created for: %d"), i);
  368.                 }
  369.             }
  370.         }
  371.         //End face reader
  372.     }
  373.  
  374.     if (!KinectSensor || FAILED(hr))
  375.     {
  376.         return false;
  377.     }
  378.     /*
  379.     BOOLEAN isAvailable = false;
  380.     KinectSensor->get_IsAvailable(&isAvailable);
  381.  
  382.     if (!isAvailable) {
  383.         UE_LOG(KinectV2, Error, TEXT("Sensor not connected."));
  384.         return false;
  385.     }
  386.     */
  387.     initiated = 1;
  388.     bStop = false; //Start thread
  389.    
  390.     StartThread();
  391.  
  392.     UE_LOG(KinectV2, Log, TEXT("Init complete !!\n"));
  393.  
  394.     return 1; //Succesfull init
  395. }
  396.  
  397. bool FKinectV2Device::SensorInit(bool playOni) {
  398.     return Init(playOni);
  399. }
  400.  
  401. void FKinectV2Device::AbortTracking() {
  402.  
  403. }
  404.  
  405. bool  FKinectV2Device::IsTracking()
  406. {
  407.    
  408.     if (PlayerTrackingID != 0) return true;
  409.     return false;
  410. }
  411.  
  412. FRotator FKinectV2Device::GetBoneRotation(EJointType skelJoint, bool flip)
  413. {
  414.     FQuat newQ = FRotator::ZeroRotator.Quaternion();
  415.     if (Joints[ConvertJoint(skelJoint)].TrackingState == TrackingState_Tracked) {
  416.         Vector4 jointOri = JointOrientations[ConvertJoint(skelJoint)].Orientation;
  417.        
  418.  
  419.         //TO DO: Check confidence value!
  420.         newQ.X = jointOri.x;
  421.         newQ.Y = -jointOri.z;
  422.         newQ.Z = jointOri.y;
  423.         newQ.W = jointOri.w;
  424.  
  425.  
  426.         newQ = newQ*FRotator(90, 180, 0).Quaternion();
  427.     }
  428.     //Try to return valid rotation only if not found check previous data
  429.     if (newQ != FRotator::ZeroRotator.Quaternion()) {
  430.         JointRotationsValid[skelJoint] = newQ.Rotator();
  431.     }
  432.     else {
  433.         if (JointRotationsValid.find(skelJoint) == JointRotationsValid.end()) {
  434.             // not found
  435.             return FRotator(newQ); //No valid data
  436.         }
  437.         else {
  438.             // found
  439.             return JointRotationsValid.find(skelJoint)->second;
  440.         }
  441.     }
  442.  
  443.     return newQ.Rotator();;
  444. }
  445.  
  446. FVector2D FKinectV2Device::GetBonePosition2D(EJointType skelJoint)
  447. {
  448.     ColorSpacePoint ColorPoint;
  449.     CameraSpacePoint SkeletonPoint;
  450.  
  451.     Joint joint = Joints[ConvertJoint(skelJoint)];
  452.     SkeletonPoint = joint.Position;
  453.  
  454.     // Skeleton-to-Color mapping
  455.     CoordinateMapper->MapCameraPointToColorSpace(SkeletonPoint, &ColorPoint);
  456.  
  457.     return FVector2D(ColorPoint.X, ColorPoint.Y);
  458. }
  459.  
  460. FVector FKinectV2Device::GetBonePosition(EJointType skelJoint, bool flip)
  461. {
  462.     FVector newPos = FVector::ZeroVector;
  463.  
  464.     CameraSpacePoint JointPosition = Joints[ConvertJoint(skelJoint)].Position;
  465.    
  466.     if(Joints[ConvertJoint(skelJoint)].TrackingState!=TrackingState_NotTracked){
  467.         newPos = FVector(JointPosition.X, JointPosition.Y, JointPosition.Z)*1000;
  468.        
  469.        
  470.         return newPos + (SensorOffset*10);
  471.  
  472.     }
  473.  
  474.     //Try to return valid rotation only if not found check previous data
  475.     if (newPos != FVector::ZeroVector) {
  476.         JointPositionsValid[skelJoint] = newPos;
  477.     }
  478.     else {
  479.         if (JointPositionsValid.find(skelJoint) == JointPositionsValid.end()) {
  480.             // not found
  481.             return newPos; //No valid data
  482.         }
  483.         else {
  484.             // found
  485.             return JointPositionsValid.find(skelJoint)->second+(SensorOffset * 10);
  486.         }
  487.     }
  488.  
  489.  
  490.  
  491.    
  492.  
  493.     return newPos;
  494. }
  495.  
  496.  
  497. bool FKinectV2Device::SensorShutdown() {
  498.     StopThread();
  499.    
  500.     if (KinectThread) {
  501.  
  502.         KinectThread->Kill(true);
  503.         delete KinectThread;
  504.     }
  505.  
  506.  
  507.  
  508.     if (DepthCoordinates)
  509.     {
  510.         delete[] DepthCoordinates;
  511.         DepthCoordinates = NULL;
  512.     }
  513.  
  514.    
  515.     if(KinectSensor) KinectSensor->Close();
  516.    
  517.     if(MultiSourceFrameReader){
  518.         MultiSourceFrameReader->UnsubscribeMultiSourceFrameArrived(MultiSourceEventHandle);
  519.         MultiSourceEventHandle = NULL;
  520.     }
  521.  
  522.     SafeRelease(MultiSourceFrameReader);
  523.     SafeRelease(ColorFrameReader);
  524.  
  525.     // done with face sources and readers
  526.     for (int i = 0; i < BODY_COUNT; i++)
  527.     {
  528.         if(FaceFrameSources[i]) SafeRelease(FaceFrameSources[i]);
  529.         if(FaceFrameReaders[i]) SafeRelease(FaceFrameReaders[i]);
  530.     }
  531.  
  532.  
  533.  
  534.     SafeRelease(CoordinateMapper);
  535.     SafeRelease(KinectSensor);
  536.    
  537.     KinectSensor = nullptr;
  538.     MultiSourceFrameReader = nullptr;
  539.     ColorFrameReader = nullptr;
  540.    
  541.     return true;
  542. }
  543.  
  544.  
  545.  
  546. void FKinectV2Device::Cleanup(void){
  547.     //Shutdown sensor
  548.     bIsTracking = false;
  549.     SensorShutdown();
  550.    
  551.  
  552.    
  553.     //Delete RGB quad
  554.     if (QuadRGB)
  555.     {
  556.         delete[] QuadRGB;
  557.         QuadRGB = NULL;
  558.     }
  559.  
  560.     //Delete BACKGROUND quad
  561.     if (QuadBACKGROUND)
  562.     {
  563.         delete[] QuadBACKGROUND;
  564.         QuadBACKGROUND = NULL;
  565.     }
  566.  
  567.     //Delete DEPTH quad
  568.     if (QuadDEPTH)
  569.     {
  570.         delete[] QuadRGB;
  571.         QuadRGB = NULL;
  572.     }
  573.  
  574.     //Delete USER quad
  575.     if (QuadUSER)
  576.     {
  577.         delete[] QuadUSER;
  578.         QuadUSER = NULL;
  579.     }
  580.  
  581.     //Filter
  582.     if (BoneOrientationFilter) {
  583.         delete BoneOrientationFilter;
  584.         BoneOrientationFilter = 0;
  585.     }
  586.     initiated = false;
  587.    
  588.  
  589.     UE_LOG(KinectV2, Log, TEXT("Cleanup ready !!\n"));
  590. }
  591.  
  592. bool FKinectV2Device::UpdateColorFrame() {
  593.     IColorFrame* ColorFrame = NULL;
  594.     IFrameDescription* pColorFrameDescription = NULL;
  595.     int nColorWidth = 0;
  596.     int nColorHeight = 0;
  597.     ColorImageFormat imageFormat = ColorImageFormat_None;
  598.     unsigned int nColorBufferSize = 0;
  599.     RGBQUAD *pColorBuffer = NULL;
  600.  
  601.     HRESULT hr = ColorFrameReader->AcquireLatestFrame(&ColorFrame);
  602.  
  603.     // get color frame data
  604.    
  605.     if (SUCCEEDED(hr))
  606.     {
  607.         hr = ColorFrame->get_FrameDescription(&pColorFrameDescription);
  608.     }
  609.     else {
  610.         return false;
  611.     }
  612.  
  613.     if (SUCCEEDED(hr))
  614.     {
  615.         hr = pColorFrameDescription->get_Width(&nColorWidth);
  616.     }
  617.  
  618.     if (SUCCEEDED(hr))
  619.     {
  620.         hr = pColorFrameDescription->get_Height(&nColorHeight);
  621.     }
  622.  
  623.     if (SUCCEEDED(hr))
  624.     {
  625.         hr = ColorFrame->get_RawColorImageFormat(&imageFormat);
  626.     }
  627.  
  628.     if (SUCCEEDED(hr))
  629.     {
  630.  
  631.         if (imageFormat == ColorImageFormat_Bgra){
  632.             hr = ColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
  633.         } else if (QuadRGB){
  634.             pColorBuffer =QuadRGB;
  635.             nColorBufferSize = 1920 * 1080 * sizeof(RGBQUAD);
  636.             hr = ColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
  637.         } else {
  638.             hr = E_FAIL;
  639.         }
  640.     }
  641.    
  642.     SafeRelease(pColorFrameDescription);
  643.     SafeRelease(ColorFrame);
  644.  
  645.     return true;
  646. }
  647.  
  648. void FKinectV2Device::MultiSourceFrameArrived(IMultiSourceFrameArrivedEventArgs* pArgs){
  649.  
  650.     IMultiSourceFrame* MultiSourceFrame = NULL;
  651.     IDepthFrame* DepthFrame = NULL;
  652.  
  653.     IInfraredFrame* InfraredFrame = NULL;
  654.     IBodyIndexFrame* BodyIndexFrame = NULL;
  655.     IBodyFrame* BodyFrame = NULL;
  656.  
  657.     HRESULT hr = MultiSourceFrameReader->AcquireLatestFrame(&MultiSourceFrame);
  658.  
  659.     if (SUCCEEDED(hr))
  660.     {
  661.         IDepthFrameReference* DepthFrameReference = NULL;
  662.  
  663.         hr = MultiSourceFrame->get_DepthFrameReference(&DepthFrameReference);
  664.         if (SUCCEEDED(hr))
  665.         {
  666.             hr = DepthFrameReference->AcquireFrame(&DepthFrame);
  667.         }
  668.  
  669.         SafeRelease(DepthFrameReference);
  670.     }
  671.  
  672.     if (SUCCEEDED(hr))
  673.     {
  674.         IInfraredFrameReference* InfraredFrameReference = NULL;
  675.  
  676.         hr = MultiSourceFrame->get_InfraredFrameReference(&InfraredFrameReference);
  677.         if (SUCCEEDED(hr))
  678.         {
  679.             hr = InfraredFrameReference->AcquireFrame(&InfraredFrame);
  680.         }
  681.  
  682.         SafeRelease(InfraredFrameReference);
  683.     }
  684.    
  685.  
  686.     if (SUCCEEDED(hr))
  687.     {
  688.         IBodyIndexFrameReference* BodyIndexFrameReference = NULL;
  689.  
  690.         hr = MultiSourceFrame->get_BodyIndexFrameReference(&BodyIndexFrameReference);
  691.         if (SUCCEEDED(hr))
  692.         {
  693.             hr = BodyIndexFrameReference->AcquireFrame(&BodyIndexFrame);
  694.         }
  695.  
  696.         SafeRelease(BodyIndexFrameReference);
  697.     }
  698.    
  699.         if (SUCCEEDED(hr))
  700.         {
  701.             IBodyFrameReference* BodyFrameReference = NULL;
  702.  
  703.             hr = MultiSourceFrame->get_BodyFrameReference(&BodyFrameReference);
  704.             if (SUCCEEDED(hr))
  705.             {
  706.                 hr = BodyFrameReference->AcquireFrame(&BodyFrame);
  707.             }
  708.  
  709.             SafeRelease(BodyFrameReference);
  710.         }
  711.  
  712.  
  713.         if (SUCCEEDED(hr))
  714.         {
  715.             IBody* ppBodies[BODY_COUNT] = { 0 };
  716.  
  717.             if (SUCCEEDED(hr))
  718.             {
  719.                 hr = BodyFrame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies);
  720.             }
  721.  
  722.             if (SUCCEEDED(hr))
  723.             {
  724.                 ProcessFaces(ppBodies);
  725.                 ProcessBody(BODY_COUNT, ppBodies); //only process body 1
  726.  
  727.  
  728.                 //Floor clipping plane
  729.                 hr = BodyFrame->get_FloorClipPlane(&FloorClippingPlane);
  730.  
  731.                 //Sensor height / tilt
  732.                 FVector floorNormal = FVector(FloorClippingPlane.x, FloorClippingPlane.y, FloorClippingPlane.z);
  733.  
  734.                 float lenProduct = floorNormal.Size()* FVector::UpVector.Size();
  735.  
  736.                 float f = FVector::DotProduct(floorNormal, FVector::UpVector) / lenProduct;
  737.                 f = FMath::Clamp(f, (float)-1.0, (float)1.0);
  738.                 float tilt = 90 - FMath::RadiansToDegrees(FMath::Acos(f));
  739.                 this->SensorAngle = tilt;
  740.  
  741.                 float sensorHeightD = (-floorNormal.Z)*FMath::Sin(tilt);
  742.                 float sensorHeight = abs(this->FloorClippingPlane.w) + sensorHeightD;
  743.                 this->SensorHeight = sensorHeight;
  744.  
  745.             }
  746.  
  747.             for (int i = 0; i < _countof(ppBodies); ++i)
  748.             {
  749.                 SafeRelease(ppBodies[i]);
  750.             }
  751.         }
  752.         SafeRelease(BodyFrame);
  753.    
  754.  
  755.  
  756.  
  757.  
  758.  
  759.     if (SUCCEEDED(hr))
  760.     {
  761.         INT64 nTime = 0;
  762.         IFrameDescription* InfraredFrameDescription = NULL;
  763.         int nInfraredWidth = 0;
  764.         int nInfraredHeight = 0;
  765.         unsigned int nInfraredBufferSize = 0;
  766.         UINT16 *InfraredBuffer = NULL;
  767.  
  768.         hr = InfraredFrame->get_RelativeTime(&nTime);
  769.  
  770.         if (SUCCEEDED(hr))
  771.         {
  772.             hr = InfraredFrame->get_FrameDescription(&InfraredFrameDescription);
  773.         }
  774.  
  775.         if (SUCCEEDED(hr))
  776.         {
  777.             hr = InfraredFrameDescription->get_Width(&nInfraredWidth);
  778.         }
  779.  
  780.         if (SUCCEEDED(hr))
  781.         {
  782.             hr = InfraredFrameDescription->get_Height(&nInfraredHeight);
  783.         }
  784.  
  785.         if (SUCCEEDED(hr))
  786.         {
  787.             hr = InfraredFrame->AccessUnderlyingBuffer(&nInfraredBufferSize, &InfraredBuffer);
  788.         }
  789.  
  790.         if (SUCCEEDED(hr))
  791.         {
  792.             //ProcessInfrared(nTime, InfraredBuffer, nInfraredHeight, nInfraredWidth);
  793.         }
  794.  
  795.         SafeRelease(InfraredFrameDescription);
  796.     }
  797.  
  798.  
  799.     if (SUCCEEDED(hr))
  800.     {
  801.         INT64 nDepthTime = 0;
  802.         IFrameDescription* pDepthFrameDescription = NULL;
  803.         int nDepthWidth = 0;
  804.         int nDepthHeight = 0;
  805.         USHORT nDepthMinReliableDistance = 0;
  806.         USHORT nDepthMaxDistance = 0;
  807.         unsigned int nDepthBufferSize = 0;
  808.         UINT16 *pDepthBuffer = NULL;
  809.  
  810.        
  811.  
  812.         IFrameDescription* pBodyIndexFrameDescription = NULL;
  813.         int nBodyIndexWidth = 0;
  814.         int nBodyIndexHeight = 0;
  815.         unsigned int nBodyIndexBufferSize = 0;
  816.         BYTE *pBodyIndexBuffer = NULL;
  817.  
  818.         // get depth frame data
  819.  
  820.         hr = DepthFrame->get_RelativeTime(&nDepthTime);
  821.  
  822.  
  823.  
  824.         if (SUCCEEDED(hr))
  825.         {
  826.             hr = DepthFrame->get_FrameDescription(&pDepthFrameDescription);
  827.         }
  828.  
  829.         if (SUCCEEDED(hr))
  830.         {
  831.             hr = pDepthFrameDescription->get_Width(&nDepthWidth);
  832.            
  833.         }
  834.  
  835.         if (SUCCEEDED(hr))
  836.         {
  837.             hr = pDepthFrameDescription->get_Height(&nDepthHeight);
  838.         }
  839.         if (SUCCEEDED(hr))
  840.         {
  841.             hr = DepthFrame->get_DepthMinReliableDistance(&nDepthMinReliableDistance);
  842.  
  843.         }
  844.  
  845.         if (SUCCEEDED(hr))
  846.         {
  847.             // In order to see the full range of depth (including the less reliable far field depth)
  848.             // we are setting nDepthMaxDistance to the extreme potential depth threshold
  849.             //nDepthMaxDistance = USHRT_MAX;
  850.  
  851.             // Note:  If you wish to filter by reliable depth distance, uncomment the following line.
  852.             hr = DepthFrame->get_DepthMaxReliableDistance(&nDepthMaxDistance);
  853.  
  854.         }
  855.  
  856.         if (SUCCEEDED(hr))
  857.         {
  858.             hr = DepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
  859.         }
  860.  
  861.         if (SUCCEEDED(hr))
  862.         {
  863.  
  864.             //ProcessDepth(nDepthTime, pDepthBuffer, nDepthWidth, nDepthHeight, nDepthMinReliableDistance, nDepthMaxDistance);
  865.  
  866.         }
  867.  
  868.  
  869.  
  870.  
  871.         //get color frame data
  872.  
  873.         //----
  874.  
  875.         // get body index frame data
  876.  
  877.  
  878.  
  879.         if (SUCCEEDED(hr))
  880.         {
  881.             hr = BodyIndexFrame->get_FrameDescription(&pBodyIndexFrameDescription);
  882.         }
  883.  
  884.         if (SUCCEEDED(hr))
  885.         {
  886.             hr = pBodyIndexFrameDescription->get_Width(&nBodyIndexWidth);
  887.         }
  888.  
  889.         if (SUCCEEDED(hr))
  890.         {
  891.             hr = pBodyIndexFrameDescription->get_Height(&nBodyIndexHeight);
  892.         }
  893.  
  894.         if (SUCCEEDED(hr))
  895.         {
  896.             hr = BodyIndexFrame->AccessUnderlyingBuffer(&nBodyIndexBufferSize, &pBodyIndexBuffer);
  897.         }
  898.         //Process body index buffer TODO:Write it's own function ProcessUser
  899.  
  900.         for (int i = 0; i < 512*424; i++) {
  901.             USHORT player = pBodyIndexBuffer[i];
  902.  
  903.             //User texture
  904.             if (player != 0xff) {
  905.                 QuadUSER[i].rgbRed = 255;
  906.                 QuadUSER[i].rgbGreen = 0;
  907.                 QuadUSER[i].rgbBlue = 0;
  908.                 QuadUSER[i].rgbReserved = 255;
  909.             }
  910.             else {
  911.                 QuadUSER[i].rgbRed = 0;
  912.                 QuadUSER[i].rgbGreen = 0;
  913.                 QuadUSER[i].rgbBlue = 0;
  914.                 QuadUSER[i].rgbReserved = 0;
  915.             }
  916.         }
  917.  
  918.         if (SUCCEEDED(hr))
  919.         {
  920.            
  921.             ProcessFrames(pDepthBuffer, nDepthWidth, nDepthHeight, pBodyIndexBuffer, nBodyIndexWidth, nBodyIndexHeight);
  922.             this->MultiFrameReady = true;
  923.         }
  924.  
  925.  
  926.  
  927.         SafeRelease(pDepthFrameDescription);
  928.        
  929.         SafeRelease(pBodyIndexFrameDescription);
  930.  
  931.  
  932.     }
  933.  
  934.     SafeRelease(DepthFrame);
  935.     SafeRelease(InfraredFrame);
  936.     SafeRelease(BodyIndexFrame);
  937.     SafeRelease(BodyFrame);
  938.     SafeRelease(MultiSourceFrame);
  939.  
  940.    
  941.  
  942.  
  943.  
  944. }
  945.  
  946. void FKinectV2Device::ProcessInfrared(INT64 nTime, const UINT16* pBuffer, int nHeight, int nWidth) {
  947.    
  948.  
  949.  
  950. }
  951.  
  952. void FKinectV2Device::ProcessDepth(INT64 nTime, const UINT16* pBuffer, int nWidth, int nHeight, USHORT nMinDepth, USHORT nMaxDepth)
  953. {
  954.     UE_LOG(KinectV2, Log, TEXT("%d %d"), nWidth, nHeight);
  955.     // Make sure we've received valid data
  956.     if (QuadDEPTH && pBuffer && (nWidth == 1920) && (nHeight == 1080))
  957.     {
  958.        
  959.  
  960.         // end pixel is start + width*height - 1
  961.         const UINT16* pBufferEnd = pBuffer + (nWidth * nHeight);
  962.  
  963.         while (pBuffer < pBufferEnd)
  964.         {
  965.             USHORT depth = *pBuffer;
  966.  
  967.             int32 r, g, b;
  968.  
  969.             UKinectV1FunctionLibrary::IntToRGB(depth, r, g, b);
  970.  
  971.             QuadDEPTH->rgbRed = 255;
  972.             QuadDEPTH->rgbGreen = 0;
  973.             QuadDEPTH->rgbBlue = 0;
  974.             QuadDEPTH->rgbReserved = 255;
  975.  
  976.             ++QuadDEPTH;
  977.             ++pBuffer;
  978.         }
  979.     }
  980. }
  981.  
  982. void FKinectV2Device::ProcessFrames(
  983.     const UINT16* pDepthBuffer, int nDepthWidth, int nDepthHeight,
  984.     const BYTE* pBodyIndexBuffer, int nBodyIndexWidth, int nBodyIndexHeight
  985.  
  986.     ) {
  987.  
  988.    
  989.         // Make sure we've received valid data
  990.         if (CoordinateMapper && DepthCoordinates && QuadRGB && QuadDEPTH &&
  991.             pDepthBuffer && (nDepthWidth == 512) && (nDepthHeight == 424) &&
  992.             pBodyIndexBuffer && (nBodyIndexWidth == 512) && (nBodyIndexHeight == 424)
  993.             )
  994.         {
  995.            
  996.             HRESULT hr = CoordinateMapper->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, 1920 * 1080, DepthCoordinates);
  997.             if (SUCCEEDED(hr))
  998.             {
  999.                 //Hands on depth map
  1000.                 FVector2D HandLeftPosition2D(0, 0);
  1001.                 FVector2D HandRightPosition2D(0, 0);
  1002.                 EJointType ClosestBodyJointLeftHand = EJointType::SpineBase;
  1003.                 EJointType ClosestBodyJointRightHand = EJointType::SpineBase;
  1004.                
  1005.                 float ClosestBodyJointLeftHandDepth = 450;
  1006.                 float ClosestBodyJointRightHandDepth = 450;
  1007.                 float HandLeftDepth = 450;
  1008.                 float HandRightDepth = 450;
  1009.                 float HandLeftRadius = 1024;
  1010.                 float HandRightRadius = 1024;
  1011.                
  1012.                 if (IsTracking()) {
  1013.                     int MinimumHandRadius = 40;
  1014.  
  1015.                    
  1016.                     //Hand left
  1017.                     HandLeftDepth = FMath::Abs(GetBonePosition(EJointType::HandLeft).Z);
  1018.                     HandLeftPosition2D = GetBonePosition2D(EJointType::HandLeft);
  1019.                     HandLeftRadius=(GetBonePosition2D(EJointType::WristLeft)- GetBonePosition2D(EJointType::HandTipLeft)).Size()*HandOcclusionMultiplier;
  1020.                     if (HandLeftRadius < MinimumHandRadius) HandLeftRadius = MinimumHandRadius;
  1021.                    
  1022.                     ClosestBodyJointLeftHand = GetClosestBodyJoint(EJointType::HandLeft);
  1023.                     ClosestBodyJointLeftHandDepth = FMath::Abs(GetBonePosition(ClosestBodyJointLeftHand).Z);
  1024.  
  1025.                    
  1026.                     //Hand right
  1027.                     HandRightDepth = FMath::Abs(GetBonePosition(EJointType::HandRight).Z);
  1028.                     HandRightPosition2D = GetBonePosition2D(EJointType::HandRight);
  1029.                     HandRightRadius = (GetBonePosition2D(EJointType::WristRight) - GetBonePosition2D(EJointType::HandTipRight)).Size()*HandOcclusionMultiplier;
  1030.                     if (HandRightRadius < MinimumHandRadius) HandRightRadius = MinimumHandRadius;
  1031.  
  1032.                     ClosestBodyJointRightHand = GetClosestBodyJoint(EJointType::HandRight);
  1033.                     ClosestBodyJointRightHandDepth = FMath::Abs(GetBonePosition(ClosestBodyJointRightHand).Z);
  1034.                 }
  1035.  
  1036.                
  1037.                 // loop over output pixels
  1038.                 for (int colorIndex = 0; colorIndex < (1920*1080); ++colorIndex)
  1039.                 {
  1040.                     // default setting source to copy from the background pixel
  1041.                     RGBQUAD* pSrc = QuadBACKGROUND + colorIndex;
  1042.                     RGBQUAD* pSrcDepth = QuadBACKGROUND + colorIndex;
  1043.                    
  1044.  
  1045.                     pSrc->rgbBlue = 0;
  1046.                     pSrc->rgbGreen = 0;
  1047.                     pSrc->rgbRed = 0;
  1048.                     pSrc->rgbReserved = 0;
  1049.  
  1050.                    
  1051.  
  1052.                     pSrcDepth->rgbBlue = 255;
  1053.                     pSrcDepth->rgbGreen = 255;
  1054.                     pSrcDepth->rgbRed = 255;
  1055.                     pSrcDepth->rgbReserved = 0;
  1056.  
  1057.  
  1058.                     int depthLast = 0;
  1059.                     bool playerPixel = false;
  1060.  
  1061.                     DepthSpacePoint p = DepthCoordinates[colorIndex];
  1062.  
  1063.                     // Values that are negative infinity means it is an invalid color to depth mapping so we
  1064.                     // skip processing for this pixel
  1065.                     if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
  1066.                     {
  1067.                         int depthX = static_cast<int>(p.X + 0.5f);
  1068.                         int depthY = static_cast<int>(p.Y + 0.5f);
  1069.  
  1070.                         if ((depthX >= 0 && depthX < nDepthWidth) && (depthY >= 0 && depthY < nDepthHeight))
  1071.                         {
  1072.                             USHORT player = pBodyIndexBuffer[depthX + (depthY * 512)];
  1073.                             USHORT depth = pDepthBuffer[depthX + (depthY * 512)];
  1074.  
  1075.  
  1076.                             // if we're tracking a player for the current pixel, draw from the color camera
  1077.                             int x = colorIndex % 1920;
  1078.                             int y = colorIndex / 1920;
  1079.                            
  1080.                            
  1081.                            
  1082.                             if ((player != 0xff) &&
  1083.                                     (
  1084.                                         ((FVector2D(x, y) - HandLeftPosition2D).Size() < HandLeftRadius) && (depth < ClosestBodyJointLeftHandDepth-175)
  1085.                                         || ((FVector2D(x, y) - HandRightPosition2D).Size() < HandRightRadius) && (depth < ClosestBodyJointRightHandDepth - 175)
  1086.  
  1087.                                     )
  1088.                                 )
  1089.                             {
  1090.                                 playerPixel = true;
  1091.                                 // set source for copy to the color pixel
  1092.  
  1093.                                 pSrc = QuadRGB + colorIndex;
  1094.  
  1095.                                 pSrcDepth->rgbRed = QuadRGB[depthX + (depthY * 512)].rgbRed;
  1096.                                 pSrcDepth->rgbGreen = QuadRGB[depthX + (depthY * 512)].rgbGreen;
  1097.                                 pSrcDepth->rgbBlue = QuadRGB[depthX + (depthY * 512)].rgbBlue;
  1098.                                 pSrcDepth->rgbReserved = QuadRGB[depthX + (depthY * 512)].rgbReserved;
  1099.  
  1100.  
  1101.                                
  1102.  
  1103.                                 int32 r, g, b;
  1104.                                 UKinectV1FunctionLibrary::IntToRGB(depth, r, g, b);
  1105.  
  1106.                                 QuadDEPTH[colorIndex].rgbRed = r;
  1107.                                 QuadDEPTH[colorIndex].rgbGreen = g;
  1108.                                 QuadDEPTH[colorIndex].rgbBlue = b;
  1109.                                 QuadDEPTH[colorIndex].rgbReserved = 255;
  1110.  
  1111.                                
  1112.  
  1113.                             } else {
  1114.                                 //Make the background very far
  1115.                                 QuadDEPTH[colorIndex].rgbRed = 255;
  1116.                                 QuadDEPTH[colorIndex].rgbGreen = 255;
  1117.                                 QuadDEPTH[colorIndex].rgbBlue = 255;
  1118.                                 QuadDEPTH[colorIndex].rgbReserved = 255;
  1119.  
  1120.                             }
  1121.                            
  1122.                            
  1123.  
  1124.                             //m_pOutputDepthRGBX[colorIndex] = *current; //Full depth to color space
  1125.                             //End
  1126.                         }
  1127.                     } else {
  1128.                         QuadDEPTH[colorIndex].rgbRed = 255;
  1129.                         QuadDEPTH[colorIndex].rgbGreen = 255;
  1130.                         QuadDEPTH[colorIndex].rgbBlue = 255;
  1131.                         QuadDEPTH[colorIndex].rgbReserved = 255;
  1132.                     }
  1133.  
  1134.                     // write output
  1135.                     QuadBACKGROUND[colorIndex] = *pSrc;
  1136.                     //m_pOutputDepthRGBX[colorIndex] = *pSrcDepth; //Player to color space
  1137.                 }
  1138.             }
  1139.         }
  1140. }
  1141.  
  1142. void FKinectV2Device::ProcessBody(int nBodyCount, IBody** ppBodies)
  1143. {
  1144.     bIsTracking = false; //Ezt lehet a for -ba kellene beletenni
  1145.  
  1146.     //Check if lost
  1147.     if (PlayerTrackingID != 0) {
  1148.         bool bFoundTrackedPlayer = false;
  1149.         for (int i = 0; i < nBodyCount; ++i)
  1150.         {
  1151.             IBody* pBody = ppBodies[i];
  1152.             if (pBody)
  1153.             {
  1154.                 BOOLEAN bTracked = false;
  1155.                 HRESULT hr = pBody->get_IsTracked(&bTracked);
  1156.                 if (SUCCEEDED(hr) && bTracked)
  1157.                 {
  1158.                     UINT64 trackingId;
  1159.                     pBody->get_TrackingId(&trackingId);
  1160.                     if (trackingId == PlayerTrackingID) {
  1161.                         bFoundTrackedPlayer = true;
  1162.                     }
  1163.                 }
  1164.             }
  1165.         }
  1166.  
  1167.         if (bFoundTrackedPlayer == false) {
  1168.             PlayerTrackingID = 0;
  1169.             bIsTracking = false;
  1170.         }
  1171.     }
  1172.  
  1173.     //End check if lost
  1174.     for (int i = 0; i < nBodyCount; ++i)
  1175.     {
  1176.         IBody* pBody = ppBodies[i];
  1177.         if (pBody)
  1178.         {
  1179.             BOOLEAN bTracked = false;
  1180.             HRESULT hr = pBody->get_IsTracked(&bTracked);
  1181.  
  1182.             if (SUCCEEDED(hr) && bTracked)
  1183.             {
  1184.                 UINT64 trackingId;
  1185.                 pBody->get_TrackingId(&trackingId);
  1186.  
  1187.                 hr = pBody->GetJoints(JointType_Count, Joints);
  1188.                
  1189.  
  1190.                
  1191.  
  1192.                 if (SUCCEEDED(hr))
  1193.                 {
  1194.                     //Only track one user
  1195.                     if (this->PlayerTrackingID == 0 && bIsTracking == false) {
  1196.  
  1197.                         //Check for pose
  1198.                        
  1199.                         FVector leftHand = this->GetBonePosition(EJointType::HandLeft);
  1200.                         FVector rightHand = this->GetBonePosition(EJointType::HandRight);
  1201.                         FVector neck = this->GetBonePosition(EJointType::Neck);
  1202.  
  1203.                        
  1204.  
  1205.                         if (leftHand.Y > neck.Y && rightHand.Y > neck.Y) {
  1206.                             PlayerTrackingID = trackingId;
  1207.                             bIsTracking = true;
  1208.  
  1209.                             this->BoneOrientationFilter->Reset();
  1210.                             UE_LOG(KinectV2, Log, TEXT("Tracking started!"));
  1211.                         }
  1212.                        
  1213.  
  1214.                     }
  1215.                     else if (PlayerTrackingID == trackingId) {
  1216.                         bIsTracking = true;
  1217.  
  1218.                         pBody->GetJoints(JointType_Count, Joints);
  1219.                         pBody->GetJointOrientations(JointType_Count, JointOrientations);
  1220.  
  1221.                         ////////////////////////
  1222.                         /// Filters ////////////
  1223.                         this->BoneOrientationFilter->UpdateFilter(pBody, this->JointOrientations, this->FaceOrientations);
  1224.                         ////////////////////////
  1225.                        
  1226.  
  1227.  
  1228.                         HandState leftHandState = HandState_Unknown;
  1229.                         HandState rightHandState = HandState_Unknown;
  1230.  
  1231.                         pBody->get_HandLeftState(&leftHandState);
  1232.                         pBody->get_HandRightState(&rightHandState);
  1233.  
  1234.                         /* Don't need that for now
  1235.                         PlayerHandStates[trackingId].LeftHandState = leftHandState;
  1236.                         PlayerHandStates[trackingId].RightHandState = rightHandState;
  1237.                         */
  1238.  
  1239.  
  1240.                         for (int u = 0; u < _countof(Joints); u++) {
  1241.                             this->CoordinateMapper->MapCameraPointToColorSpace(Joints[u].Position, &JointsColorSpace[u]);
  1242.                         }
  1243.                     }
  1244.                 }
  1245.  
  1246. #include "AllowWindowsPlatformTypes.h"
  1247.                 DWORD clippedEdges;
  1248. #include "HideWindowsPlatformTypes.h"
  1249.                 IsOutOfFrame = true;
  1250.                 hr = pBody->get_ClippedEdges(&clippedEdges);
  1251.                 if (SUCCEEDED(hr))
  1252.                 {
  1253.                     if (clippedEdges & FrameEdge_Left) {
  1254.                         //printf("Out of frame - LEFT\n");
  1255.                         IsOutOfFrame = true;
  1256.                     }
  1257.                     else if (clippedEdges & FrameEdge_Right) {
  1258.                         //printf("Out of frame - RIGHT\n");
  1259.                         IsOutOfFrame = true;
  1260.                     }
  1261.                     else if (clippedEdges & FrameEdge_Top) {
  1262.                         //printf("Out of frame - TOP\n");
  1263.                         IsOutOfFrame = true;
  1264.                     }
  1265.                     else if (clippedEdges & FrameEdge_Bottom) {
  1266.                         //printf("Out of frame - BOTTOM\n");
  1267.                         IsOutOfFrame = true;
  1268.                     }
  1269.                     else {
  1270.                         //printf("In the frame \n");
  1271.                         IsOutOfFrame = false;
  1272.                     }
  1273.                 }
  1274.             }
  1275.         }
  1276.         if (bIsTracking == true && PlayerTrackingID != 0) break;
  1277.     }
  1278. }
  1279.  
  1280. JointType FKinectV2Device::ConvertJoint(EJointType Joint) {
  1281.         switch (Joint) {
  1282.  
  1283.             //case EJointType::SpineBase: return JointType_SpineBase; break;
  1284.             case EJointType::SpineBase: return JointType_SpineMid; break;
  1285.            
  1286.             case EJointType::SpineMid:return JointType_SpineMid; break;
  1287.            
  1288.                 //case EJointType::Neck:return JointType_Neck; break;
  1289.             case EJointType::Neck:return JointType_SpineShoulder; break;
  1290.  
  1291.             case EJointType::Head:return JointType_Head; break;
  1292.             case EJointType::ShoulderLeft:return JointType_ShoulderLeft; break;
  1293.             case EJointType::ElbowLeft: return JointType_ElbowLeft; break;
  1294.             case EJointType::WristLeft: return JointType_WristLeft; break;
  1295.             case EJointType::HandLeft: return JointType_HandLeft; break;
  1296.             case EJointType::ShoulderRight:return JointType_ShoulderRight; break;
  1297.             case EJointType::ElbowRight: return JointType_ElbowRight; break;
  1298.             case EJointType::WristRight: return JointType_WristRight; break;
  1299.             case EJointType::HandRight: return JointType_HandRight; break;
  1300.             case EJointType::HipLeft: return JointType_HipLeft; break;
  1301.             case EJointType::KneeLeft:return JointType_KneeLeft; break;
  1302.             case EJointType::AnkleLeft: return JointType_AnkleLeft; break;
  1303.             case EJointType::FootLeft: return JointType_FootLeft; break;
  1304.             case EJointType::HipRight:return JointType_HipRight; break;
  1305.             case EJointType::KneeRight: return JointType_KneeRight; break;
  1306.             case EJointType::AnkleRight:return JointType_AnkleRight; break;
  1307.             case EJointType::FootRight:return JointType_FootRight; break;
  1308.             case EJointType::SpineShoulder:return JointType_SpineShoulder; break;
  1309.             case EJointType::HandTipLeft:return JointType_HandTipLeft; break;
  1310.             case EJointType::ThumbLeft:return JointType_ThumbLeft; break;
  1311.             case EJointType::HandTipRight:return JointType_HandTipRight; break;
  1312.             case EJointType::ThumbRight:return JointType_ThumbRight; break;
  1313.         };
  1314.         return JointType_SpineBase;
  1315.    
  1316. }
  1317.  
  1318. void FKinectV2Device::ProcessFaces(IBody** ppBodies)
  1319. {
  1320.     HRESULT hr;
  1321.     //IBody* ppBodies[BODY_COUNT] = { 0 };
  1322.     //bool bHaveBodyData = SUCCEEDED(UpdateBodyData(ppBodies));
  1323.     bool bHaveBodyData = true;
  1324.  
  1325.     // iterate through each face reader
  1326.     for (int iFace = 0; iFace < BODY_COUNT; ++iFace)
  1327.     {
  1328.         // retrieve the latest face frame from this reader
  1329.         IFaceFrame* pFaceFrame = nullptr;
  1330.         hr = FaceFrameReaders[iFace]->AcquireLatestFrame(&pFaceFrame);
  1331.  
  1332.  
  1333.  
  1334.         BOOLEAN bFaceTracked = false;
  1335.         if (SUCCEEDED(hr) && nullptr != pFaceFrame)
  1336.         {
  1337.             // check if a valid face is tracked in this face frame
  1338.             hr = pFaceFrame->get_IsTrackingIdValid(&bFaceTracked);
  1339.         }
  1340.  
  1341.         if (SUCCEEDED(hr))
  1342.         {
  1343.             if (bFaceTracked)
  1344.             {
  1345.                 IFaceFrameResult* pFaceFrameResult = nullptr;
  1346.                 RectI faceBox = { 0 };
  1347.                 PointF facePoints[FacePointType::FacePointType_Count];
  1348.                 Vector4 faceRotation=_Vector4();
  1349.                 DetectionResult faceProperties[FaceProperty::FaceProperty_Count];
  1350.  
  1351.  
  1352.  
  1353.                 hr = pFaceFrame->get_FaceFrameResult(&pFaceFrameResult);
  1354.  
  1355.  
  1356.  
  1357.                 // need to verify if pFaceFrameResult contains data before trying to access it
  1358.                 if (SUCCEEDED(hr) && pFaceFrameResult != nullptr)
  1359.                 {
  1360.                     uint64 faceTrackingId;
  1361.                     pFaceFrameResult->get_TrackingId(&faceTrackingId);
  1362.  
  1363.  
  1364.                     hr = pFaceFrameResult->get_FaceBoundingBoxInColorSpace(&faceBox);
  1365.  
  1366.                     if (SUCCEEDED(hr))
  1367.                     {
  1368.                         hr = pFaceFrameResult->GetFacePointsInColorSpace(FacePointType::FacePointType_Count, facePoints);
  1369.                     }
  1370.  
  1371.                     if (SUCCEEDED(hr))
  1372.                     {
  1373.                         hr = pFaceFrameResult->get_FaceRotationQuaternion(&faceRotation);
  1374.                     }
  1375.  
  1376.                     if (SUCCEEDED(hr))
  1377.                     {
  1378.                         FaceOrientations[faceTrackingId] = faceRotation;
  1379.                         hr = pFaceFrameResult->GetFaceProperties(FaceProperty::FaceProperty_Count, faceProperties);
  1380.                     }
  1381.  
  1382.                     if (SUCCEEDED(hr))
  1383.                     {
  1384.                         FString faceText = "";
  1385.  
  1386.                         // extract each face property information and store it is faceText
  1387.                         for (int iProperty = 0; iProperty < FaceProperty::FaceProperty_Count; iProperty++)
  1388.                         {
  1389.                             switch (iProperty)
  1390.                             {
  1391.                             case FaceProperty::FaceProperty_Happy:
  1392.                                 faceText += "Happy :";
  1393.  
  1394.  
  1395.                                 break;
  1396.                             case FaceProperty::FaceProperty_Engaged:
  1397.                                 faceText += "Engaged :";
  1398.                                 break;
  1399.                             case FaceProperty::FaceProperty_LeftEyeClosed:
  1400.                                 faceText += "LeftEyeClosed :";
  1401.                                 break;
  1402.                             case FaceProperty::FaceProperty_RightEyeClosed:
  1403.                                 faceText += "RightEyeClosed :";
  1404.                                 break;
  1405.                             case FaceProperty::FaceProperty_LookingAway:
  1406.                                 faceText += "LookingAway :";
  1407.                                 break;
  1408.                             case FaceProperty::FaceProperty_MouthMoved:
  1409.                                 faceText += "MouthMoved :";
  1410.                                 break;
  1411.                             case FaceProperty::FaceProperty_MouthOpen:
  1412.                                 faceText += "MouthOpen :";
  1413.                                 break;
  1414.                             case FaceProperty::FaceProperty_WearingGlasses:
  1415.                                 faceText += "WearingGlasses :";
  1416.                                 break;
  1417.                             default:
  1418.                                 break;
  1419.                             }
  1420.  
  1421.  
  1422.                             switch (faceProperties[iProperty])
  1423.                             {
  1424.                             case DetectionResult::DetectionResult_Unknown:
  1425.                                 faceText += " UnKnown";
  1426.                                 break;
  1427.                             case DetectionResult::DetectionResult_Yes:
  1428.                                 faceText += " Yes";
  1429.                                 break;
  1430.  
  1431.                                 // consider a "maybe" as a "no" to restrict
  1432.                                 // the detection result refresh rate
  1433.                             case DetectionResult::DetectionResult_No:
  1434.                             case DetectionResult::DetectionResult_Maybe:
  1435.                                 faceText += " No";
  1436.                                 break;
  1437.                             default:
  1438.                                 faceText += " Default";
  1439.                                 break;
  1440.                             }
  1441.  
  1442.                             faceText += "\n";
  1443.  
  1444.                         }
  1445.                         FaceText = faceText;
  1446.  
  1447.                     }
  1448.  
  1449.                     if (SUCCEEDED(hr))
  1450.                     {
  1451.                         // draw face frame results
  1452.                         //m_pDrawDataStreams->DrawFaceFrameResults(iFace, &faceBox, facePoints, &faceRotation, faceProperties, &faceTextLayout);
  1453.  
  1454.  
  1455.                     }
  1456.  
  1457.  
  1458.  
  1459.  
  1460.                 }
  1461.  
  1462.  
  1463.  
  1464.                 SafeRelease(pFaceFrameResult);
  1465.             }
  1466.             else
  1467.             {
  1468.                 // face tracking is not valid - attempt to fix the issue
  1469.                 // a valid body is required to perform this step
  1470.                 if (bHaveBodyData)
  1471.                 {
  1472.                     // check if the corresponding body is tracked
  1473.                     // if this is true then update the face frame source to track this body
  1474.                     IBody* pBody = ppBodies[iFace];
  1475.                     if (pBody != nullptr)
  1476.                     {
  1477.                         BOOLEAN bTracked = false;
  1478.                         hr = pBody->get_IsTracked(&bTracked);
  1479.  
  1480.                         UINT64 bodyTId;
  1481.                         if (SUCCEEDED(hr) && bTracked)
  1482.                         {
  1483.                             // get the tracking ID of this body
  1484.                             hr = pBody->get_TrackingId(&bodyTId);
  1485.                             if (SUCCEEDED(hr))
  1486.                             {
  1487.                                 // update the face frame source with the tracking ID
  1488.                                 FaceFrameSources[iFace]->put_TrackingId(bodyTId);
  1489.                             }
  1490.                         }
  1491.                     }
  1492.                 }
  1493.             }
  1494.         }
  1495.  
  1496.         SafeRelease(pFaceFrame);
  1497.     }
  1498.  
  1499.  
  1500. }
  1501. FString FKinectV2Device::GetDeviceName() {
  1502.     return this->DeviceName;
  1503. }
  1504.  
  1505.  
  1506. EJointType FKinectV2Device::GetClosestBodyJoint(EJointType HandJoint) {
  1507.     FVector HandPos = GetBonePosition(HandJoint);
  1508.     float distanceMinimum = 10000;
  1509.     float distance = 0;
  1510.     EJointType ResultJoint=EJointType::SpineBase;
  1511.  
  1512.     //Joints to check
  1513.     FVector SpineBase = GetBonePosition(EJointType::SpineBase);
  1514.     FVector Neck = GetBonePosition(EJointType::Neck);
  1515.     FVector LeftThigh = GetBonePosition(EJointType::HipLeft);
  1516.     FVector RightThigh = GetBonePosition(EJointType::HipRight);
  1517.  
  1518.     //Check distance of each from the hand joint
  1519.     if ((HandPos - SpineBase).Size() < distanceMinimum) { distanceMinimum = (HandPos - SpineBase).Size();  ResultJoint = EJointType::SpineBase; }
  1520.     if ((HandPos - Neck).Size() < distanceMinimum) { distanceMinimum = (HandPos - Neck).Size();  ResultJoint = EJointType::Neck; }
  1521.     if ((HandPos - LeftThigh).Size() < distanceMinimum) { distanceMinimum = (HandPos - LeftThigh).Size();  ResultJoint = EJointType::HipLeft; }
  1522.     if ((HandPos - RightThigh).Size() < distanceMinimum) { distanceMinimum = (HandPos - RightThigh).Size();  ResultJoint = EJointType::HipRight; }
  1523.  
  1524.     return ResultJoint;
  1525. }
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!