Skip to content
This repository was archived by the owner on Sep 29, 2020. It is now read-only.

Commit c8406cc

Browse files
InsaneGroxInsaneGrox
authored
Changed emulation to emulate 8 trackers instead of 3
1 parent 059312f commit c8406cc

4 files changed

Lines changed: 510 additions & 497 deletions

File tree

‎driver_kinectV2/CKinectHandler.cpp‎

Lines changed: 154 additions & 149 deletions
Original file line numberDiff line numberDiff line change
@@ -1,149 +1,154 @@
1-
#include "stdafx.h"
2-
3-
#include "CKinectHandler.h"
4-
#include "CJointFilter.h"
5-
6-
size_t g_AssignedTrackerJoint[3U]
7-
{
8-
JointType_SpineBase,
9-
JointType_AnkleLeft,
10-
JointType_AnkleRight,
11-
};
12-
13-
CKinectHandler::CKinectHandler()
14-
{
15-
m_kinectSensor = nullptr;
16-
m_bodyFrameReader = nullptr;
17-
m_sensorData = { { { 0.f } }, 0, 0U };
18-
for(size_t i = 0U; i < JI_Count; i++) m_jointFilters.push_back(new CJointFilter());
19-
m_paused = false;
20-
}
21-
CKinectHandler::~CKinectHandler()
22-
{
23-
Cleanup();
24-
}
25-
26-
bool CKinectHandler::Initialize()
27-
{
28-
bool l_result = false;
29-
if(!m_kinectSensor)
30-
{
31-
if(GetDefaultKinectSensor(&m_kinectSensor) >= S_OK)
32-
{
33-
IBodyFrameSource *l_bodyFrameSource = nullptr;
34-
if(m_kinectSensor->Open() >= S_OK)
35-
{
36-
if(m_kinectSensor->get_BodyFrameSource(&l_bodyFrameSource) >= S_OK)
37-
{
38-
if(l_bodyFrameSource->OpenReader(&m_bodyFrameReader) >= S_OK)
39-
{
40-
m_paused = false;
41-
l_result = true;
42-
}
43-
}
44-
}
45-
46-
if(l_bodyFrameSource) l_bodyFrameSource->Release();
47-
}
48-
}
49-
if(!l_result) Cleanup();
50-
return l_result;
51-
}
52-
void CKinectHandler::Terminate()
53-
{
54-
Cleanup();
55-
}
56-
57-
void CKinectHandler::Cleanup()
58-
{
59-
for(auto l_filter : m_jointFilters) delete l_filter;
60-
m_jointFilters.clear();
61-
62-
if(m_bodyFrameReader)
63-
{
64-
m_bodyFrameReader->Release();
65-
m_bodyFrameReader = nullptr;
66-
}
67-
68-
if(m_kinectSensor)
69-
{
70-
m_kinectSensor->Close();
71-
m_kinectSensor->Release();
72-
m_kinectSensor = nullptr;
73-
}
74-
75-
m_paused = false;
76-
}
77-
78-
void CKinectHandler::SetPaused(bool f_state)
79-
{
80-
if(m_kinectSensor && m_bodyFrameReader) m_paused = f_state;
81-
}
82-
83-
void CKinectHandler::Update()
84-
{
85-
if(m_kinectSensor && m_bodyFrameReader && !m_paused)
86-
{
87-
IBodyFrame* l_bodyFrame = nullptr;
88-
if(m_bodyFrameReader->AcquireLatestFrame(&l_bodyFrame) >= S_OK)
89-
{
90-
TIMESPAN l_timeSpan = 0;
91-
l_bodyFrame->get_RelativeTime(&l_timeSpan);
92-
if(l_timeSpan != m_sensorData.m_frameTime)
93-
{
94-
m_sensorData.m_frameTime = l_timeSpan;
95-
m_sensorData.m_tick = GetTickCount64();
96-
}
97-
98-
IBody *l_bodies[BODY_COUNT] = { nullptr };
99-
if(l_bodyFrame->GetAndRefreshBodyData(BODY_COUNT, l_bodies) >= S_OK) // Only first visible body
100-
{
101-
for(size_t i = 0U, j = static_cast<size_t>(BODY_COUNT); i < j; i++)
102-
{
103-
if(l_bodies[i])
104-
{
105-
unsigned char l_isTracked = 0U;
106-
if(l_bodies[i]->get_IsTracked(&l_isTracked) >= S_OK)
107-
{
108-
if(l_isTracked > 0U)
109-
{
110-
Joint l_joints[JointType_Count];
111-
JointOrientation l_jointOrientations[JointType_Count];
112-
if((l_bodies[i]->GetJoints(JointType_Count, l_joints) >= S_OK) && (l_bodies[i]->GetJointOrientations(JointType_Count, l_jointOrientations) >= S_OK))
113-
{
114-
for(size_t k = 0U; k < JI_Count; k++)
115-
{
116-
m_jointFilters[k]->Update(l_joints[g_AssignedTrackerJoint[k]]);
117-
const glm::vec3 &l_position = m_jointFilters[k]->GetFiltered();
118-
JointData &l_jointData = m_sensorData.m_joints[k];
119-
l_jointData.x = l_position.x;
120-
l_jointData.y = l_position.y;
121-
l_jointData.z = l_position.z;
122-
123-
const Vector4 &l_kinectRotation = l_jointOrientations[g_AssignedTrackerJoint[k]].Orientation;
124-
const glm::quat l_newRotation(l_kinectRotation.w, l_kinectRotation.x, l_kinectRotation.y, l_kinectRotation.z);
125-
const glm::quat l_oldRotation(l_jointData.rw, l_jointData.rx, l_jointData.ry, l_jointData.rz);
126-
const glm::quat l_smoothedRotation = glm::slerp(l_oldRotation, l_newRotation, 0.7f);
127-
l_jointData.rx = l_smoothedRotation.x;
128-
l_jointData.ry = l_smoothedRotation.y;
129-
l_jointData.rz = l_smoothedRotation.z;
130-
l_jointData.rw = l_smoothedRotation.w;
131-
}
132-
}
133-
134-
break;
135-
}
136-
}
137-
}
138-
}
139-
}
140-
141-
for(size_t i = 0U, j = static_cast<size_t>(BODY_COUNT); i < j; i++)
142-
{
143-
if(l_bodies[i]) l_bodies[i]->Release();
144-
}
145-
}
146-
147-
if(l_bodyFrame) l_bodyFrame->Release();
148-
}
149-
}
1+
#include "stdafx.h"
2+
3+
#include "CKinectHandler.h"
4+
#include "CJointFilter.h"
5+
6+
size_t g_AssignedTrackerJoint[8U]
7+
{
8+
JointType_SpineBase,
9+
JointType_AnkleLeft,
10+
JointType_AnkleRight,
11+
JointType_KneeLeft,
12+
JointType_KneeRight,
13+
JointType_ElbowLeft,
14+
JointType_ElbowRight,
15+
JointType_SpineShoulder,
16+
};
17+
18+
CKinectHandler::CKinectHandler()
19+
{
20+
m_kinectSensor = nullptr;
21+
m_bodyFrameReader = nullptr;
22+
m_sensorData = { { { 0.f } }, 0, 0U };
23+
for(size_t i = 0U; i < JI_Count; i++) m_jointFilters.push_back(new CJointFilter());
24+
m_paused = false;
25+
}
26+
CKinectHandler::~CKinectHandler()
27+
{
28+
Cleanup();
29+
}
30+
31+
bool CKinectHandler::Initialize()
32+
{
33+
bool l_result = false;
34+
if(!m_kinectSensor)
35+
{
36+
if(GetDefaultKinectSensor(&m_kinectSensor) >= S_OK)
37+
{
38+
IBodyFrameSource *l_bodyFrameSource = nullptr;
39+
if(m_kinectSensor->Open() >= S_OK)
40+
{
41+
if(m_kinectSensor->get_BodyFrameSource(&l_bodyFrameSource) >= S_OK)
42+
{
43+
if(l_bodyFrameSource->OpenReader(&m_bodyFrameReader) >= S_OK)
44+
{
45+
m_paused = false;
46+
l_result = true;
47+
}
48+
}
49+
}
50+
51+
if(l_bodyFrameSource) l_bodyFrameSource->Release();
52+
}
53+
}
54+
if(!l_result) Cleanup();
55+
return l_result;
56+
}
57+
void CKinectHandler::Terminate()
58+
{
59+
Cleanup();
60+
}
61+
62+
void CKinectHandler::Cleanup()
63+
{
64+
for(auto l_filter : m_jointFilters) delete l_filter;
65+
m_jointFilters.clear();
66+
67+
if(m_bodyFrameReader)
68+
{
69+
m_bodyFrameReader->Release();
70+
m_bodyFrameReader = nullptr;
71+
}
72+
73+
if(m_kinectSensor)
74+
{
75+
m_kinectSensor->Close();
76+
m_kinectSensor->Release();
77+
m_kinectSensor = nullptr;
78+
}
79+
80+
m_paused = false;
81+
}
82+
83+
void CKinectHandler::SetPaused(bool f_state)
84+
{
85+
if(m_kinectSensor && m_bodyFrameReader) m_paused = f_state;
86+
}
87+
88+
void CKinectHandler::Update()
89+
{
90+
if(m_kinectSensor && m_bodyFrameReader && !m_paused)
91+
{
92+
IBodyFrame* l_bodyFrame = nullptr;
93+
if(m_bodyFrameReader->AcquireLatestFrame(&l_bodyFrame) >= S_OK)
94+
{
95+
TIMESPAN l_timeSpan = 0;
96+
l_bodyFrame->get_RelativeTime(&l_timeSpan);
97+
if(l_timeSpan != m_sensorData.m_frameTime)
98+
{
99+
m_sensorData.m_frameTime = l_timeSpan;
100+
m_sensorData.m_tick = GetTickCount64();
101+
}
102+
103+
IBody *l_bodies[BODY_COUNT] = { nullptr };
104+
if(l_bodyFrame->GetAndRefreshBodyData(BODY_COUNT, l_bodies) >= S_OK) // Only first visible body
105+
{
106+
for(size_t i = 0U, j = static_cast<size_t>(BODY_COUNT); i < j; i++)
107+
{
108+
if(l_bodies[i])
109+
{
110+
unsigned char l_isTracked = 0U;
111+
if(l_bodies[i]->get_IsTracked(&l_isTracked) >= S_OK)
112+
{
113+
if(l_isTracked > 0U)
114+
{
115+
Joint l_joints[JointType_Count];
116+
JointOrientation l_jointOrientations[JointType_Count];
117+
if((l_bodies[i]->GetJoints(JointType_Count, l_joints) >= S_OK) && (l_bodies[i]->GetJointOrientations(JointType_Count, l_jointOrientations) >= S_OK))
118+
{
119+
for(size_t k = 0U; k < JI_Count; k++)
120+
{
121+
m_jointFilters[k]->Update(l_joints[g_AssignedTrackerJoint[k]]);
122+
const glm::vec3 &l_position = m_jointFilters[k]->GetFiltered();
123+
JointData &l_jointData = m_sensorData.m_joints[k];
124+
l_jointData.x = l_position.x;
125+
l_jointData.y = l_position.y;
126+
l_jointData.z = l_position.z;
127+
128+
const Vector4 &l_kinectRotation = l_jointOrientations[g_AssignedTrackerJoint[k]].Orientation;
129+
const glm::quat l_newRotation(l_kinectRotation.w, l_kinectRotation.x, l_kinectRotation.y, l_kinectRotation.z);
130+
const glm::quat l_oldRotation(l_jointData.rw, l_jointData.rx, l_jointData.ry, l_jointData.rz);
131+
const glm::quat l_smoothedRotation = glm::slerp(l_oldRotation, l_newRotation, 0.7f);
132+
l_jointData.rx = l_smoothedRotation.x;
133+
l_jointData.ry = l_smoothedRotation.y;
134+
l_jointData.rz = l_smoothedRotation.z;
135+
l_jointData.rw = l_smoothedRotation.w;
136+
}
137+
}
138+
139+
break;
140+
}
141+
}
142+
}
143+
}
144+
}
145+
146+
for(size_t i = 0U, j = static_cast<size_t>(BODY_COUNT); i < j; i++)
147+
{
148+
if(l_bodies[i]) l_bodies[i]->Release();
149+
}
150+
}
151+
152+
if(l_bodyFrame) l_bodyFrame->Release();
153+
}
154+
}

0 commit comments

Comments
 (0)