t1 / TFDContents / Assets / KinectScripts / KinectManager.cs @ 3
이력 | 보기 | 이력해설 | 다운로드 (152 KB)
1 |
// comment out the following #define, if you want to use the depth sensor and the KinectManager on per-scene basis |
---|---|
2 |
#define USE_SINGLE_KM_IN_MULTIPLE_SCENES |
3 |
|
4 |
|
5 |
using UnityEngine; |
6 |
using UnityEngine.Networking; |
7 |
|
8 |
using System; |
9 |
using System.Collections; |
10 |
using System.Collections.Generic; |
11 |
//using System.Linq; |
12 |
|
13 |
/// <summary> |
14 |
/// KinectManager is the main and the most basic Kinect-related component. It is used to control the sensor and poll the data streams. |
15 |
/// </summary> |
16 |
public class KinectManager : MonoBehaviour |
17 |
{ |
18 |
[Tooltip("How high above the ground is the sensor, in meters.")] |
19 |
public float sensorHeight = 1.0f; |
20 |
|
21 |
[Tooltip("Kinect elevation angle (in degrees). May be positive or negative.")] |
22 |
public float sensorAngle = 0f; |
23 |
|
24 |
public enum AutoHeightAngle : int { DontUse, ShowInfoOnly, AutoUpdate, AutoUpdateAndShowInfo } |
25 |
[Tooltip("Whether to automatically set the sensor height and angle or not. The user must stay in front of the sensor, in order the automatic detection to work.")] |
26 |
public AutoHeightAngle autoHeightAngle = AutoHeightAngle.DontUse; |
27 |
|
28 |
[Tooltip("Whether to flip left and right, relative to the sensor.")] |
29 |
private bool flipLeftRight = false; |
30 |
|
31 |
public enum UserMapType : int { None, RawUserDepth, BodyTexture, UserTexture, CutOutTexture } |
32 |
[Tooltip("Whether and how to utilize the user and depth images.")] |
33 |
public UserMapType computeUserMap = UserMapType.RawUserDepth; |
34 |
|
35 |
[Tooltip("Whether to utilize the color camera image.")] |
36 |
public bool computeColorMap = false; |
37 |
|
38 |
[Tooltip("Whether to utilize the IR camera image.")] |
39 |
public bool computeInfraredMap = false; |
40 |
|
41 |
[Tooltip("Whether to display the user map on the screen.")] |
42 |
public bool displayUserMap = false; |
43 |
|
44 |
[Tooltip("Whether to display the color camera image on the screen.")] |
45 |
public bool displayColorMap = false; |
46 |
|
47 |
[Tooltip("Whether to display skeleton lines over the the user map.")] |
48 |
public bool displaySkeletonLines = false; |
49 |
|
50 |
// if percent is zero, it is calculated internally to match the selected width and height of the depth image |
51 |
[Tooltip("Depth and color image width on the screen, as percent of the screen width. The image height is calculated depending on the width.")] |
52 |
public float DisplayMapsWidthPercent = 20f; |
53 |
|
54 |
[Tooltip("Whether to use the multi-source reader, if one is available (K2-only feature).")] |
55 |
public bool useMultiSourceReader = false; |
56 |
|
57 |
// Public Bool to determine whether to use sensor's audio source, if available |
58 |
//public bool useAudioSource = false; |
59 |
|
60 |
[Tooltip("Minimum distance to a user, in order to be considered for skeleton data processing.")] |
61 |
public float minUserDistance = 0.5f; |
62 |
|
63 |
[Tooltip("Maximum distance to a user, in order to be considered for skeleton data processing. Value of 0 means no maximum distance limitation.")] |
64 |
public float maxUserDistance = 0f; |
65 |
|
66 |
[Tooltip("Maximum left or right distance to a user, in order to be considered for skeleton data processing. Value of 0 means no left/right distance limitation.")] |
67 |
public float maxLeftRightDistance = 0f; |
68 |
|
69 |
[Tooltip("Maximum number of users, which may be tracked simultaneously.")] |
70 |
public int maxTrackedUsers = 6; |
71 |
|
72 |
[Tooltip("Whether to display the tracked users within the allowed distance only, or all users (higher fps).")] |
73 |
public bool showTrackedUsersOnly = true; |
74 |
|
75 |
public enum UserDetectionOrder : int { Appearance = 0, Distance = 1, LeftToRight = 2 } |
76 |
[Tooltip("How to assign users to player indices - by order of appearance, distance or left-to-right.")] |
77 |
public UserDetectionOrder userDetectionOrder = UserDetectionOrder.Appearance; |
78 |
|
79 |
[Tooltip("Whether to utilize only the really tracked joints (and ignore the inferred ones) or not.")] |
80 |
public bool ignoreInferredJoints = false; |
81 |
|
82 |
[Tooltip("Whether to ignore the Z-coordinates of the joints (i.e. to use them in 2D-scenes) or not.")] |
83 |
public bool ignoreZCoordinates = false; |
84 |
|
85 |
[Tooltip("Whether to update the AvatarControllers in LateUpdate(), instead of in Update(). Needed for Mecanim animation blending.")] |
86 |
public bool lateUpdateAvatars = false; |
87 |
|
88 |
[Tooltip("Whether to skip the remote avatar controllers in multiplayer games.")] |
89 |
public bool skipRemoteAvatars = false; |
90 |
|
91 |
// [Tooltip("Uses own thread for processing kinect data.")] |
92 |
// public bool useOwnThread = false; |
93 |
|
94 |
public enum Smoothing : int { None, Default, Light, Medium, Aggressive } |
95 |
[Tooltip("Set of joint smoothing parameters.")] |
96 |
public Smoothing smoothing = Smoothing.Default; |
97 |
|
98 |
[Tooltip("Whether to apply the bone orientation constraints.")] |
99 |
public bool useBoneOrientationConstraints = false; |
100 |
//public bool useBoneOrientationsFilter = false; |
101 |
|
102 |
[Tooltip("Whether to estimate the body joints velocities.")] |
103 |
public bool estimateJointVelocities = false; |
104 |
|
105 |
[Tooltip("Set of joint velocities smoothing parameters.")] |
106 |
public Smoothing velocitySmoothing = Smoothing.Light; |
107 |
|
108 |
[Tooltip("Whether to allow detection of body turn arounds or not.")] |
109 |
public bool allowTurnArounds = false; |
110 |
|
111 |
public enum AllowedRotations : int { None = 0, Default = 1, All = 2 } |
112 |
[Tooltip("Allowed wrist and hand rotations: None - no hand rotations are allowed, Default - hand rotations are allowed except for the twists, All - all rotations are allowed.")] |
113 |
public AllowedRotations allowedHandRotations = AllowedRotations.Default; |
114 |
|
115 |
[Tooltip("Wait time in seconds, before a lost user gets removed. This is to prevent sporadical user switches.")] |
116 |
public float waitTimeBeforeRemove = 1f; |
117 |
|
118 |
[Tooltip("List of the avatar controllers in the scene. If the list is empty, the available avatar controllers are detected at the scene start up.")] |
119 |
public List<AvatarController> avatarControllers = new List<AvatarController>(); |
120 |
|
121 |
[Tooltip("Calibration pose required, to turn on the tracking of respective player.")] |
122 |
public KinectGestures.Gestures playerCalibrationPose; |
123 |
|
124 |
[Tooltip("List of common gestures, to be detected for each player.")] |
125 |
public List<KinectGestures.Gestures> playerCommonGestures = new List<KinectGestures.Gestures>(); |
126 |
|
127 |
[Tooltip("Minimum time between gesture detections (in seconds).")] |
128 |
public float minTimeBetweenGestures = 0.7f; |
129 |
|
130 |
[Tooltip("Gesture manager, used to detect programmatic Kinect gestures.")] |
131 |
public KinectGestures gestureManager; |
132 |
|
133 |
[Tooltip("List of the gesture listeners in the scene. If the list is empty, the available gesture listeners will be detected at the scene start up.")] |
134 |
public List<MonoBehaviour> gestureListeners = new List<MonoBehaviour>(); |
135 |
|
136 |
[Tooltip("GUI-Text to display user detection messages.")] |
137 |
public GUIText calibrationText; |
138 |
|
139 |
[Tooltip("GUI-Text to display debug messages for the currently tracked gestures.")] |
140 |
public GUIText gesturesDebugText; |
141 |
|
142 |
|
143 |
// Bool to keep track of whether Kinect has been initialized |
144 |
protected bool kinectInitialized = false; |
145 |
|
146 |
// The singleton instance of KinectManager |
147 |
protected static KinectManager instance = null; |
148 |
|
149 |
// available sensor interfaces |
150 |
protected List<DepthSensorInterface> sensorInterfaces = null; |
151 |
// primary SensorData structure |
152 |
protected KinectInterop.SensorData sensorData = null; |
153 |
|
154 |
// Depth and user maps |
155 |
//protected KinectInterop.DepthBuffer depthImage; |
156 |
//protected KinectInterop.BodyIndexBuffer bodyIndexImage; |
157 |
//protected KinectInterop.UserHistogramBuffer userHistogramImage; |
158 |
protected Color32[] usersHistogramImage; |
159 |
protected ushort[] usersPrevState; |
160 |
protected float[] usersHistogramMap; |
161 |
|
162 |
protected Texture2D usersLblTex; |
163 |
protected Rect usersMapRect; |
164 |
protected int usersMapSize; |
165 |
//protected int minDepth; |
166 |
//protected int maxDepth; |
167 |
|
168 |
// Color map |
169 |
//protected KinectInterop.ColorBuffer colorImage; |
170 |
//protected Texture2D usersClrTex; |
171 |
protected Rect usersClrRect; |
172 |
protected int usersClrSize; |
173 |
|
174 |
// Kinect body frame data |
175 |
protected KinectInterop.BodyFrameData bodyFrame; |
176 |
//private Int64 lastBodyFrameTime = 0; |
177 |
|
178 |
// List of all users |
179 |
protected List<Int64> alUserIds = new List<Int64>(); |
180 |
protected Dictionary<Int64, int> dictUserIdToIndex = new Dictionary<Int64, int>(); |
181 |
protected Int64[] aUserIndexIds = new Int64[KinectInterop.Constants.MaxBodyCount]; |
182 |
protected Dictionary<Int64, float> dictUserIdToTime = new Dictionary<Int64, float>(); |
183 |
|
184 |
// Whether the users are limited by number or distance |
185 |
protected bool bLimitedUsers = false; |
186 |
|
187 |
// Primary (first or closest) user ID |
188 |
protected Int64 liPrimaryUserId = 0; |
189 |
|
190 |
// Kinect to world matrix |
191 |
protected Matrix4x4 kinectToWorld = Matrix4x4.zero; |
192 |
//private Matrix4x4 mOrient = Matrix4x4.zero; |
193 |
|
194 |
// Calibration gesture data for each player |
195 |
protected Dictionary<Int64, KinectGestures.GestureData> playerCalibrationData = new Dictionary<Int64, KinectGestures.GestureData>(); |
196 |
|
197 |
// gestures data and parameters |
198 |
protected Dictionary<Int64, List<KinectGestures.GestureData>> playerGesturesData = new Dictionary<Int64, List<KinectGestures.GestureData>>(); |
199 |
protected Dictionary<Int64, float> gesturesTrackingAtTime = new Dictionary<Int64, float>(); |
200 |
|
201 |
//// List of Gesture Listeners. They must implement KinectGestures.GestureListenerInterface |
202 |
//public List<KinectGestures.GestureListenerInterface> gestureListenerInts; |
203 |
|
204 |
// Body filter instances |
205 |
protected JointPositionsFilter jointPositionFilter = null; |
206 |
protected BoneOrientationsConstraint boneConstraintsFilter = null; |
207 |
//protected BoneOrientationsFilter boneOrientationFilter = null; |
208 |
|
209 |
protected JointVelocitiesFilter jointVelocityFilter = null; |
210 |
|
211 |
|
212 |
// background kinect thread |
213 |
//protected System.Threading.Thread kinectReaderThread = null; |
214 |
protected bool kinectReaderRunning = false; |
215 |
|
216 |
// if the background removal was used before pause |
217 |
protected bool backgroundRemovalInited = false; |
218 |
protected bool backgroundRemovalHiRes = false; |
219 |
|
220 |
|
221 |
/// <summary> |
222 |
/// Gets the single KinectManager instance. |
223 |
/// </summary> |
224 |
/// <value>The KinectManager instance.</value> |
225 |
public static KinectManager Instance |
226 |
{ |
227 |
get |
228 |
{ |
229 |
return instance; |
230 |
} |
231 |
} |
232 |
|
233 |
/// <summary> |
234 |
/// Determines if the sensor and KinectManager-component are initialized and ready to use. |
235 |
/// </summary> |
236 |
/// <returns><c>true</c> if Kinect is initialized; otherwise, <c>false</c>.</returns> |
237 |
public static bool IsKinectInitialized() |
238 |
{ |
239 |
return instance != null ? instance.kinectInitialized : false; |
240 |
} |
241 |
|
242 |
/// <summary> |
243 |
/// Determines if the sensor and KinectManager-component are initialized and ready to use. |
244 |
/// </summary> |
245 |
/// <returns><c>true</c> if Kinect is initialized; otherwise, <c>false</c>.</returns> |
246 |
public bool IsInitialized() |
247 |
{ |
248 |
return kinectInitialized; |
249 |
} |
250 |
|
251 |
/// <summary> |
252 |
/// Gets the sensor data structure (this structure should not be modified and must be used only internally). |
253 |
/// </summary> |
254 |
/// <returns>The sensor data.</returns> |
255 |
internal KinectInterop.SensorData GetSensorData() |
256 |
{ |
257 |
return sensorData; |
258 |
} |
259 |
|
260 |
/// <summary> |
261 |
/// Gets the selected depth-sensor platform. |
262 |
/// </summary> |
263 |
/// <returns>The selected depth-sensor platform.</returns> |
264 |
public KinectInterop.DepthSensorPlatform GetSensorPlatform() |
265 |
{ |
266 |
if(sensorData != null && sensorData.sensorInterface != null) |
267 |
{ |
268 |
return sensorData.sensorInterface.GetSensorPlatform(); |
269 |
} |
270 |
|
271 |
return KinectInterop.DepthSensorPlatform.None; |
272 |
} |
273 |
|
274 |
/// <summary> |
275 |
/// Gets the number of bodies, tracked by the sensor. |
276 |
/// </summary> |
277 |
/// <returns>The body count.</returns> |
278 |
public int GetBodyCount() |
279 |
{ |
280 |
return sensorData != null ? sensorData.bodyCount : 0; |
281 |
} |
282 |
|
283 |
/// <summary> |
284 |
/// Gets the the number of body joints, tracked by the sensor. |
285 |
/// </summary> |
286 |
/// <returns>The count of joints.</returns> |
287 |
public int GetJointCount() |
288 |
{ |
289 |
return sensorData != null ? sensorData.jointCount : 0; |
290 |
} |
291 |
|
292 |
/// <summary> |
293 |
/// Gets the index of the joint in the joint's array |
294 |
/// </summary> |
295 |
/// <returns>The joint's index in the array.</returns> |
296 |
/// <param name="joint">Joint.</param> |
297 |
public int GetJointIndex(KinectInterop.JointType joint) |
298 |
{ |
299 |
if(sensorData != null && sensorData.sensorInterface != null) |
300 |
{ |
301 |
return sensorData.sensorInterface.GetJointIndex(joint); |
302 |
} |
303 |
|
304 |
// fallback - index matches the joint |
305 |
return (int)joint; |
306 |
} |
307 |
|
308 |
// // returns the joint at given index |
309 |
// public KinectInterop.JointType GetJointAtIndex(int index) |
310 |
// { |
311 |
// if(sensorData != null && sensorData.sensorInterface != null) |
312 |
// { |
313 |
// return sensorData.sensorInterface.GetJointAtIndex(index); |
314 |
// } |
315 |
// |
316 |
// // fallback - index matches the joint |
317 |
// return (KinectInterop.JointType)index; |
318 |
// } |
319 |
|
320 |
/// <summary> |
321 |
/// Gets the parent joint of the given joint. |
322 |
/// </summary> |
323 |
/// <returns>The parent joint.</returns> |
324 |
/// <param name="joint">Joint.</param> |
325 |
public KinectInterop.JointType GetParentJoint(KinectInterop.JointType joint) |
326 |
{ |
327 |
if(sensorData != null && sensorData.sensorInterface != null) |
328 |
{ |
329 |
return sensorData.sensorInterface.GetParentJoint(joint); |
330 |
} |
331 |
|
332 |
// fall back - return the same joint (i.e. end-joint) |
333 |
return joint; |
334 |
} |
335 |
|
336 |
/// <summary> |
337 |
/// Gets the next joint of the given joint. |
338 |
/// </summary> |
339 |
/// <returns>The next joint.</returns> |
340 |
/// <param name="joint">Joint.</param> |
341 |
public KinectInterop.JointType GetNextJoint(KinectInterop.JointType joint) |
342 |
{ |
343 |
if(sensorData != null && sensorData.sensorInterface != null) |
344 |
{ |
345 |
return sensorData.sensorInterface.GetNextJoint(joint); |
346 |
} |
347 |
|
348 |
// fall back - return the same joint (i.e. end-joint) |
349 |
return joint; |
350 |
} |
351 |
|
352 |
/// <summary> |
353 |
/// Gets the width of the color image, returned by the sensor. |
354 |
/// </summary> |
355 |
/// <returns>The color image width.</returns> |
356 |
public int GetColorImageWidth() |
357 |
{ |
358 |
return sensorData != null ? sensorData.colorImageWidth : 0; |
359 |
} |
360 |
|
361 |
/// <summary> |
362 |
/// Gets the height of the color image, returned by the sensor. |
363 |
/// </summary> |
364 |
/// <returns>The color image height.</returns> |
365 |
public int GetColorImageHeight() |
366 |
{ |
367 |
return sensorData != null ? sensorData.colorImageHeight : 0; |
368 |
} |
369 |
|
370 |
/// <summary> |
371 |
/// Gets the width of the depth image, returned by the sensor. |
372 |
/// </summary> |
373 |
/// <returns>The depth image width.</returns> |
374 |
public int GetDepthImageWidth() |
375 |
{ |
376 |
return sensorData != null ? sensorData.depthImageWidth : 0; |
377 |
} |
378 |
|
379 |
/// <summary> |
380 |
/// Gets the height of the depth image, returned by the sensor. |
381 |
/// </summary> |
382 |
/// <returns>The depth image height.</returns> |
383 |
public int GetDepthImageHeight() |
384 |
{ |
385 |
return sensorData != null ? sensorData.depthImageHeight : 0; |
386 |
} |
387 |
|
388 |
/// <summary> |
389 |
/// Gets the raw body index data, if ComputeUserMap is true. |
390 |
/// </summary> |
391 |
/// <returns>The raw body index data.</returns> |
392 |
public byte[] GetRawBodyIndexMap() |
393 |
{ |
394 |
return sensorData != null ? sensorData.bodyIndexImage : null; |
395 |
} |
396 |
|
397 |
/// <summary> |
398 |
/// Gets the raw depth data, if ComputeUserMap is true. |
399 |
/// </summary> |
400 |
/// <returns>The raw depth map.</returns> |
401 |
public ushort[] GetRawDepthMap() |
402 |
{ |
403 |
return sensorData != null ? sensorData.depthImage : null; |
404 |
} |
405 |
|
406 |
/// <summary> |
407 |
/// Gets the raw infrared data, if ComputeInfraredMap is true. |
408 |
/// </summary> |
409 |
/// <returns>The raw infrared map.</returns> |
410 |
public ushort[] GetRawInfraredMap() |
411 |
{ |
412 |
return sensorData != null ? sensorData.infraredImage : null; |
413 |
} |
414 |
|
415 |
|
416 |
/// <summary> |
417 |
/// Gets the users' histogram texture, if ComputeUserMap is true |
418 |
/// </summary> |
419 |
/// <returns>The users histogram texture.</returns> |
420 |
public Texture2D GetUsersLblTex() |
421 |
{ |
422 |
return usersLblTex; |
423 |
} |
424 |
|
425 |
/// <summary> |
426 |
/// Gets the color image texture,if ComputeColorMap is true |
427 |
/// </summary> |
428 |
/// <returns>The color image texture.</returns> |
429 |
public Texture2D GetUsersClrTex() |
430 |
{ |
431 |
//return usersClrTex; |
432 |
return sensorData != null ? sensorData.colorImageTexture : null; |
433 |
} |
434 |
|
435 |
/// <summary> |
436 |
/// Determines whether at least one user is currently detected by the sensor |
437 |
/// </summary> |
438 |
/// <returns><c>true</c> if at least one user is detected; otherwise, <c>false</c>.</returns> |
439 |
public bool IsUserDetected() |
440 |
{ |
441 |
return kinectInitialized && (alUserIds.Count > 0); |
442 |
} |
443 |
|
444 |
/// <summary> |
445 |
/// Determines whether the user with the specified index is currently detected by the sensor |
446 |
/// </summary> |
447 |
/// <returns><c>true</c> if the user is detected; otherwise, <c>false</c>.</returns> |
448 |
/// <param name="i">The user index.</param> |
449 |
public bool IsUserDetected(int i) |
450 |
{ |
451 |
if(i >= 0 && i < KinectInterop.Constants.MaxBodyCount) |
452 |
{ |
453 |
return (aUserIndexIds[i] != 0); |
454 |
} |
455 |
|
456 |
return false; |
457 |
} |
458 |
|
459 |
/// <summary> |
460 |
/// Determines whether the user with the specified userId is in the list of tracked users or not. |
461 |
/// </summary> |
462 |
/// <returns><c>true</c> if the user with the specified userId is tracked; otherwise, <c>false</c>.</returns> |
463 |
/// <param name="userId">User identifier.</param> |
464 |
public bool IsUserTracked(Int64 userId) |
465 |
{ |
466 |
return dictUserIdToIndex.ContainsKey(userId); |
467 |
} |
468 |
|
469 |
/// <summary> |
470 |
/// Gets the number of currently detected users. |
471 |
/// </summary> |
472 |
/// <returns>The users count.</returns> |
473 |
public int GetUsersCount() |
474 |
{ |
475 |
return alUserIds.Count; |
476 |
} |
477 |
|
478 |
/// <summary> |
479 |
/// Gets IDs of all currently tracked users. |
480 |
/// </summary> |
481 |
/// <returns>The list of all currently tracked users.</returns> |
482 |
public List<long> GetAllUserIds() |
483 |
{ |
484 |
return new List<long>(alUserIds); |
485 |
} |
486 |
|
487 |
/// <summary> |
488 |
/// Gets the user ID by the specified user index. |
489 |
/// </summary> |
490 |
/// <returns>The user ID by index.</returns> |
491 |
/// <param name="i">The user index.</param> |
492 |
public Int64 GetUserIdByIndex(int i) |
493 |
{ |
494 |
// if(i >= 0 && i < alUserIds.Count) |
495 |
// { |
496 |
// return alUserIds[i]; |
497 |
// } |
498 |
|
499 |
if(i >= 0 && i < KinectInterop.Constants.MaxBodyCount) |
500 |
{ |
501 |
return aUserIndexIds[i]; |
502 |
} |
503 |
|
504 |
return 0; |
505 |
} |
506 |
|
507 |
/// <summary> |
508 |
/// Gets the user index by the specified user ID. |
509 |
/// </summary> |
510 |
/// <returns>The user index by user ID.</returns> |
511 |
/// <param name="userId">User ID</param> |
512 |
public int GetUserIndexById(Int64 userId) |
513 |
{ |
514 |
// for(int i = 0; i < alUserIds.Count; i++) |
515 |
// { |
516 |
// if(alUserIds[i] == userId) |
517 |
// { |
518 |
// return i; |
519 |
// } |
520 |
// } |
521 |
|
522 |
if(userId == 0) |
523 |
return -1; |
524 |
|
525 |
for(int i = 0; i < aUserIndexIds.Length; i++) |
526 |
{ |
527 |
if(aUserIndexIds[i] == userId) |
528 |
{ |
529 |
return i; |
530 |
} |
531 |
} |
532 |
|
533 |
return -1; |
534 |
} |
535 |
|
536 |
/// <summary> |
537 |
/// Gets the body index by the specified user ID, or -1 if the user ID does not exist. |
538 |
/// </summary> |
539 |
/// <returns>The body index by user ID.</returns> |
540 |
/// <param name="userId">User ID</param> |
541 |
public int GetBodyIndexByUserId(Int64 userId) |
542 |
{ |
543 |
if(dictUserIdToIndex.ContainsKey(userId)) |
544 |
{ |
545 |
int index = dictUserIdToIndex[userId]; |
546 |
return index; |
547 |
} |
548 |
|
549 |
return -1; |
550 |
} |
551 |
|
552 |
/// <summary> |
553 |
/// Gets the list of tracked body indices. |
554 |
/// </summary> |
555 |
/// <returns>The list of body indices.</returns> |
556 |
public List<int> GetTrackedBodyIndices() |
557 |
{ |
558 |
List<int> alBodyIndices = new List<int>(dictUserIdToIndex.Values); |
559 |
return alBodyIndices; |
560 |
} |
561 |
|
562 |
/// <summary> |
563 |
/// Determines whether the tracked users are limited by their number or distance or not. |
564 |
/// </summary> |
565 |
/// <returns><c>true</c> if the users are limited by number or distance; otherwise, <c>false</c>.</returns> |
566 |
public bool IsTrackedUsersLimited() |
567 |
{ |
568 |
return bLimitedUsers; |
569 |
} |
570 |
|
571 |
/// <summary> |
572 |
/// Gets the UserID of the primary user (the first or the closest one), or 0 if no user is detected. |
573 |
/// </summary> |
574 |
/// <returns>The primary user ID.</returns> |
575 |
public Int64 GetPrimaryUserID() |
576 |
{ |
577 |
return liPrimaryUserId; |
578 |
} |
579 |
|
580 |
/// <summary> |
581 |
/// Sets the primary user ID, in order to change the active user. |
582 |
/// </summary> |
583 |
/// <returns><c>true</c>, if primary user ID was set, <c>false</c> otherwise.</returns> |
584 |
/// <param name="userId">User ID</param> |
585 |
public bool SetPrimaryUserID(Int64 userId) |
586 |
{ |
587 |
bool bResult = false; |
588 |
|
589 |
if(alUserIds.Contains(userId) || (userId == 0)) |
590 |
{ |
591 |
liPrimaryUserId = userId; |
592 |
bResult = true; |
593 |
} |
594 |
|
595 |
return bResult; |
596 |
} |
597 |
|
598 |
/// <summary> |
599 |
/// Gets the body index [0-5], if there is single body selected to be displayed on the user map, or -1 if all bodies are displayed. |
600 |
/// </summary> |
601 |
/// <returns>The displayed body index [0-5], or -1 if all bodies are displayed.</returns> |
602 |
public int GetDisplayedBodyIndex() |
603 |
{ |
604 |
if(sensorData != null) |
605 |
{ |
606 |
return sensorData.selectedBodyIndex != 255 ? sensorData.selectedBodyIndex : -1; |
607 |
} |
608 |
|
609 |
return -1; |
610 |
} |
611 |
|
612 |
/// <summary> |
613 |
/// Sets the body index [0-5], if a single body must be displayed on the user map, or -1 if all bodies must be displayed. |
614 |
/// </summary> |
615 |
/// <returns><c>true</c>, if the change was successful, <c>false</c> otherwise.</returns> |
616 |
/// <param name="iBodyIndex">The single body index, or -1 if all bodies must be displayed.</param> |
617 |
public bool SetDisplayedBodyIndex(int iBodyIndex) |
618 |
{ |
619 |
if(sensorData != null) |
620 |
{ |
621 |
sensorData.selectedBodyIndex = (byte)(iBodyIndex >= 0 ? iBodyIndex : 255); |
622 |
} |
623 |
|
624 |
return false; |
625 |
} |
626 |
|
627 |
/// <summary> |
628 |
/// Gets the last body frame timestamp. |
629 |
/// </summary> |
630 |
/// <returns>The last body frame timestamp.</returns> |
631 |
public long GetBodyFrameTimestamp() |
632 |
{ |
633 |
return bodyFrame.liRelativeTime; |
634 |
} |
635 |
|
636 |
// do not change the data in the structure directly |
637 |
/// <summary> |
638 |
/// Gets the user body data (for internal purposes only). |
639 |
/// </summary> |
640 |
/// <returns>The user body data.</returns> |
641 |
/// <param name="userId">User ID</param> |
642 |
internal KinectInterop.BodyData GetUserBodyData(Int64 userId) |
643 |
{ |
644 |
if(dictUserIdToIndex.ContainsKey(userId)) |
645 |
{ |
646 |
int index = dictUserIdToIndex[userId]; |
647 |
|
648 |
if(index >= 0 && index < sensorData.bodyCount) |
649 |
{ |
650 |
return bodyFrame.bodyData[index]; |
651 |
} |
652 |
} |
653 |
|
654 |
return new KinectInterop.BodyData(); |
655 |
} |
656 |
|
657 |
/// <summary> |
658 |
/// Gets the kinect to world matrix. |
659 |
/// </summary> |
660 |
/// <returns>The kinect to world matrix.</returns> |
661 |
public Matrix4x4 GetKinectToWorldMatrix() |
662 |
{ |
663 |
return kinectToWorld; |
664 |
} |
665 |
|
666 |
/// <summary> |
667 |
/// Updates the kinect to world transform matrix, according to the current values of SensorHeight, SensorAngle and FlipLeftRight. |
668 |
/// </summary> |
669 |
public void UpdateKinectToWorldMatrix() |
670 |
{ |
671 |
//create the transform matrix - kinect to world |
672 |
Vector3 vSensorPos = new Vector3(0f, sensorHeight, 0f); |
673 |
Quaternion qSensorRot = Quaternion.Euler(-sensorAngle, 0f, 0f); |
674 |
Vector3 vSensorScale = !flipLeftRight ? Vector3.one : new Vector3(-1f, 1f, 1f); |
675 |
|
676 |
kinectToWorld.SetTRS(vSensorPos, qSensorRot, vSensorScale); |
677 |
} |
678 |
|
679 |
/// <summary> |
680 |
/// Sets the kinect to world matrix. |
681 |
/// </summary> |
682 |
/// <param name="sensorPos">Sensor position.</param> |
683 |
/// <param name="sensorRot">Sensor rotation.</param> |
684 |
/// <param name="sensorScale">Position scale (could be used to flip left-right).</param> |
685 |
public void SetKinectToWorldMatrix(Vector3 sensorPos, Quaternion sensorRot, Vector3 sensorScale) |
686 |
{ |
687 |
kinectToWorld.SetTRS(sensorPos, sensorRot, sensorScale); |
688 |
|
689 |
sensorHeight = sensorPos.y; |
690 |
sensorAngle = -sensorRot.eulerAngles.x; |
691 |
flipLeftRight = sensorScale.x < 0f; |
692 |
|
693 |
// enable or disable getting height and angle info |
694 |
autoHeightAngle = AutoHeightAngle.DontUse; |
695 |
sensorData.hintHeightAngle = (autoHeightAngle != AutoHeightAngle.DontUse); |
696 |
} |
697 |
|
698 |
/// <summary> |
699 |
/// Gets the user position, relative to the sensor, in meters. |
700 |
/// </summary> |
701 |
/// <returns>The user position.</returns> |
702 |
/// <param name="userId">User ID</param> |
703 |
public Vector3 GetUserPosition(Int64 userId) |
704 |
{ |
705 |
if(dictUserIdToIndex.ContainsKey(userId)) |
706 |
{ |
707 |
int index = dictUserIdToIndex[userId]; |
708 |
|
709 |
if(index >= 0 && index < sensorData.bodyCount && |
710 |
bodyFrame.bodyData[index].bIsTracked != 0) |
711 |
{ |
712 |
return bodyFrame.bodyData[index].position; |
713 |
} |
714 |
} |
715 |
|
716 |
return Vector3.zero; |
717 |
} |
718 |
|
719 |
/// <summary> |
720 |
/// Gets the user orientation. |
721 |
/// </summary> |
722 |
/// <returns>The user rotation.</returns> |
723 |
/// <param name="userId">User ID</param> |
724 |
/// <param name="flip">If set to <c>true</c>, this means non-mirrored rotation.</param> |
725 |
public Quaternion GetUserOrientation(Int64 userId, bool flip) |
726 |
{ |
727 |
if(dictUserIdToIndex.ContainsKey(userId)) |
728 |
{ |
729 |
int index = dictUserIdToIndex[userId]; |
730 |
|
731 |
if(index >= 0 && index < sensorData.bodyCount && |
732 |
bodyFrame.bodyData[index].bIsTracked != 0) |
733 |
{ |
734 |
if(flip) |
735 |
return bodyFrame.bodyData[index].normalRotation; |
736 |
else |
737 |
return bodyFrame.bodyData[index].mirroredRotation; |
738 |
} |
739 |
} |
740 |
|
741 |
return Quaternion.identity; |
742 |
} |
743 |
|
744 |
/// <summary> |
745 |
/// Gets the tracking state of the joint. |
746 |
/// </summary> |
747 |
/// <returns>The joint tracking state.</returns> |
748 |
/// <param name="userId">User ID</param> |
749 |
/// <param name="joint">Joint index</param> |
750 |
public KinectInterop.TrackingState GetJointTrackingState(Int64 userId, int joint) |
751 |
{ |
752 |
if(dictUserIdToIndex.ContainsKey(userId)) |
753 |
{ |
754 |
int index = dictUserIdToIndex[userId]; |
755 |
|
756 |
if(index >= 0 && index < sensorData.bodyCount && |
757 |
bodyFrame.bodyData[index].bIsTracked != 0) |
758 |
{ |
759 |
if(joint >= 0 && joint < sensorData.jointCount) |
760 |
{ |
761 |
return bodyFrame.bodyData[index].joint[joint].trackingState; |
762 |
} |
763 |
} |
764 |
} |
765 |
|
766 |
return KinectInterop.TrackingState.NotTracked; |
767 |
} |
768 |
|
769 |
/// <summary> |
770 |
/// Determines whether the given joint of the specified user is being tracked. |
771 |
/// </summary> |
772 |
/// <returns><c>true</c> if this instance is joint tracked the specified userId joint; otherwise, <c>false</c>.</returns> |
773 |
/// <param name="userId">User ID</param> |
774 |
/// <param name="joint">Joint index</param> |
775 |
public bool IsJointTracked(Int64 userId, int joint) |
776 |
{ |
777 |
if(dictUserIdToIndex.ContainsKey(userId)) |
778 |
{ |
779 |
int index = dictUserIdToIndex[userId]; |
780 |
|
781 |
if(index >= 0 && index < sensorData.bodyCount && |
782 |
bodyFrame.bodyData[index].bIsTracked != 0) |
783 |
{ |
784 |
if(joint >= 0 && joint < sensorData.jointCount) |
785 |
{ |
786 |
KinectInterop.JointData jointData = bodyFrame.bodyData[index].joint[joint]; |
787 |
|
788 |
return ignoreInferredJoints ? (jointData.trackingState == KinectInterop.TrackingState.Tracked) : |
789 |
(jointData.trackingState != KinectInterop.TrackingState.NotTracked); |
790 |
} |
791 |
} |
792 |
} |
793 |
|
794 |
return false; |
795 |
} |
796 |
|
797 |
/// <summary> |
798 |
/// Gets the joint position of the specified user, in Kinect coordinate system, in meters. |
799 |
/// </summary> |
800 |
/// <returns>The joint kinect position.</returns> |
801 |
/// <param name="userId">User ID</param> |
802 |
/// <param name="joint">Joint index</param> |
803 |
public Vector3 GetJointKinectPosition(Int64 userId, int joint) |
804 |
{ |
805 |
if(dictUserIdToIndex.ContainsKey(userId)) |
806 |
{ |
807 |
int index = dictUserIdToIndex[userId]; |
808 |
|
809 |
if(index >= 0 && index < sensorData.bodyCount && |
810 |
bodyFrame.bodyData[index].bIsTracked != 0) |
811 |
{ |
812 |
if(joint >= 0 && joint < sensorData.jointCount) |
813 |
{ |
814 |
KinectInterop.JointData jointData = bodyFrame.bodyData[index].joint[joint]; |
815 |
return jointData.kinectPos; |
816 |
} |
817 |
} |
818 |
} |
819 |
|
820 |
return Vector3.zero; |
821 |
} |
822 |
|
823 |
/// <summary> |
824 |
/// Gets the joint position of the specified user, in meters. |
825 |
/// </summary> |
826 |
/// <returns>The joint position.</returns> |
827 |
/// <param name="userId">User ID</param> |
828 |
/// <param name="joint">Joint index</param> |
829 |
public Vector3 GetJointPosition(Int64 userId, int joint) |
830 |
{ |
831 |
if(dictUserIdToIndex.ContainsKey(userId)) |
832 |
{ |
833 |
int index = dictUserIdToIndex[userId]; |
834 |
|
835 |
if(index >= 0 && index < sensorData.bodyCount && |
836 |
bodyFrame.bodyData[index].bIsTracked != 0) |
837 |
{ |
838 |
if(joint >= 0 && joint < sensorData.jointCount) |
839 |
{ |
840 |
KinectInterop.JointData jointData = bodyFrame.bodyData[index].joint[joint]; |
841 |
return jointData.position; |
842 |
} |
843 |
} |
844 |
} |
845 |
|
846 |
return Vector3.zero; |
847 |
} |
848 |
|
849 |
/// <summary> |
850 |
/// Gets the joint position of the specified user with flipped x-coordinate, in meters. |
851 |
/// </summary> |
852 |
/// <returns>The joint position.</returns> |
853 |
/// <param name="userId">User ID</param> |
854 |
/// <param name="joint">Joint index</param> |
855 |
public Vector3 GetJointPositionFlipX(Int64 userId, int joint) |
856 |
{ |
857 |
if(dictUserIdToIndex.ContainsKey(userId)) |
858 |
{ |
859 |
int index = dictUserIdToIndex[userId]; |
860 |
|
861 |
if(index >= 0 && index < sensorData.bodyCount && |
862 |
bodyFrame.bodyData[index].bIsTracked != 0) |
863 |
{ |
864 |
if(joint >= 0 && joint < sensorData.jointCount) |
865 |
{ |
866 |
KinectInterop.JointData jointData = bodyFrame.bodyData[index].joint[joint]; |
867 |
Vector3 jointPos = jointData.position; |
868 |
jointPos.x = -jointPos.x; |
869 |
|
870 |
return jointPos; |
871 |
} |
872 |
} |
873 |
} |
874 |
|
875 |
return Vector3.zero; |
876 |
} |
877 |
|
878 |
/// <summary> |
879 |
/// Gets the joint velocity for the specified user and joint, in meters/s. |
880 |
/// </summary> |
881 |
/// <returns>The joint velocity.</returns> |
882 |
/// <param name="userId">User ID.</param> |
883 |
/// <param name="joint">Joint index.</param> |
884 |
public Vector3 GetJointVelocity(Int64 userId, int joint) |
885 |
{ |
886 |
if(dictUserIdToIndex.ContainsKey(userId)) |
887 |
{ |
888 |
int index = dictUserIdToIndex[userId]; |
889 |
|
890 |
if(index >= 0 && index < sensorData.bodyCount && |
891 |
bodyFrame.bodyData[index].bIsTracked != 0) |
892 |
{ |
893 |
if(joint >= 0 && joint < sensorData.jointCount) |
894 |
{ |
895 |
return bodyFrame.bodyData[index].joint[joint].posVel; |
896 |
} |
897 |
} |
898 |
} |
899 |
|
900 |
return Vector3.zero; |
901 |
} |
902 |
|
903 |
/// <summary> |
904 |
/// Gets the joint direction of the specified user, relative to its parent joint. |
905 |
/// </summary> |
906 |
/// <returns>The joint direction.</returns> |
907 |
/// <param name="userId">User ID</param> |
908 |
/// <param name="joint">Joint index</param> |
909 |
/// <param name="flipX">If set to <c>true</c> flips the X-coordinate</param> |
910 |
/// <param name="flipZ">If set to <c>true</c> flips the Z-coordinate</param> |
911 |
public Vector3 GetJointDirection(Int64 userId, int joint, bool flipX, bool flipZ) |
912 |
{ |
913 |
if(dictUserIdToIndex.ContainsKey(userId)) |
914 |
{ |
915 |
int index = dictUserIdToIndex[userId]; |
916 |
|
917 |
if(index >= 0 && index < sensorData.bodyCount && |
918 |
bodyFrame.bodyData[index].bIsTracked != 0) |
919 |
{ |
920 |
if(joint >= 0 && joint < sensorData.jointCount) |
921 |
{ |
922 |
KinectInterop.JointData jointData = bodyFrame.bodyData[index].joint[joint]; |
923 |
Vector3 jointDir = jointData.direction; |
924 |
|
925 |
if(flipX) |
926 |
jointDir.x = -jointDir.x; |
927 |
|
928 |
if(flipZ) |
929 |
jointDir.z = -jointDir.z; |
930 |
|
931 |
return jointDir; |
932 |
} |
933 |
} |
934 |
} |
935 |
|
936 |
return Vector3.zero; |
937 |
} |
938 |
|
939 |
/// <summary> |
940 |
/// Gets the direction between the given joints of the specified user. |
941 |
/// </summary> |
942 |
/// <returns>The direction between joints.</returns> |
943 |
/// <param name="userId">User ID</param> |
944 |
/// <param name="firstJoint">First joint index</param> |
945 |
/// <param name="secondJoint">Second joint index</param> |
946 |
/// <param name="flipX">If set to <c>true</c> flips the X-coordinate</param> |
947 |
/// <param name="flipZ">If set to <c>true</c> flips the Z-coordinate</param> |
948 |
public Vector3 GetDirectionBetweenJoints(Int64 userId, int firstJoint, int secondJoint, bool flipX, bool flipZ) |
949 |
{ |
950 |
if(dictUserIdToIndex.ContainsKey(userId)) |
951 |
{ |
952 |
int index = dictUserIdToIndex[userId]; |
953 |
|
954 |
if(index >= 0 && index < sensorData.bodyCount && |
955 |
bodyFrame.bodyData[index].bIsTracked != 0) |
956 |
{ |
957 |
KinectInterop.BodyData bodyData = bodyFrame.bodyData[index]; |
958 |
|
959 |
if(firstJoint >= 0 && firstJoint < sensorData.jointCount && |
960 |
secondJoint >= 0 && secondJoint < sensorData.jointCount) |
961 |
{ |
962 |
Vector3 firstJointPos = bodyData.joint[firstJoint].position; |
963 |
Vector3 secondJointPos = bodyData.joint[secondJoint].position; |
964 |
Vector3 jointDir = secondJointPos - firstJointPos; |
965 |
|
966 |
if(flipX) |
967 |
jointDir.x = -jointDir.x; |
968 |
|
969 |
if(flipZ) |
970 |
jointDir.z = -jointDir.z; |
971 |
|
972 |
return jointDir; |
973 |
} |
974 |
} |
975 |
} |
976 |
|
977 |
return Vector3.zero; |
978 |
} |
979 |
|
980 |
/// <summary> |
981 |
/// Gets the joint orientation of the specified user. |
982 |
/// </summary> |
983 |
/// <returns>The joint rotation.</returns> |
984 |
/// <param name="userId">User ID</param> |
985 |
/// <param name="joint">Joint index</param> |
986 |
/// <param name="flip">If set to <c>true</c>, this means non-mirrored rotation</param> |
987 |
public Quaternion GetJointOrientation(Int64 userId, int joint, bool flip) |
988 |
{ |
989 |
if(dictUserIdToIndex.ContainsKey(userId)) |
990 |
{ |
991 |
int index = dictUserIdToIndex[userId]; |
992 |
|
993 |
if(index >= 0 && index < sensorData.bodyCount && |
994 |
bodyFrame.bodyData[index].bIsTracked != 0) |
995 |
{ |
996 |
if(flip) |
997 |
return bodyFrame.bodyData[index].joint[joint].normalRotation; |
998 |
else |
999 |
return bodyFrame.bodyData[index].joint[joint].mirroredRotation; |
1000 |
} |
1001 |
} |
1002 |
|
1003 |
return Quaternion.identity; |
1004 |
} |
1005 |
|
1006 |
/// <summary> |
1007 |
/// Gets the angle between bones at the given joint. |
1008 |
/// </summary> |
1009 |
/// <returns>The angle at joint.</returns> |
1010 |
/// <param name="userId">User ID</param> |
1011 |
/// <param name="joint">Joint index</param> |
1012 |
public float GetAngleAtJoint(Int64 userId, int joint) |
1013 |
{ |
1014 |
int pjoint = (int)GetParentJoint((KinectInterop.JointType)joint); |
1015 |
int njoint = (int)GetNextJoint((KinectInterop.JointType)joint); |
1016 |
|
1017 |
if (pjoint != joint && njoint != joint) |
1018 |
{ |
1019 |
Vector3 pos1 = GetJointPosition(userId, pjoint); |
1020 |
Vector3 pos2 = GetJointPosition(userId, joint); |
1021 |
Vector3 pos3 = GetJointPosition(userId, njoint); |
1022 |
|
1023 |
if (pos1 != Vector3.zero && pos2 != Vector3.zero && pos3 != Vector3.zero) |
1024 |
{ |
1025 |
Vector3 dirP = pos1 - pos2; |
1026 |
Vector3 dirN = pos3 - pos2; |
1027 |
float fAngle = Vector3.Angle(dirP, dirN); |
1028 |
|
1029 |
return fAngle; |
1030 |
} |
1031 |
} |
1032 |
|
1033 |
return 0f; |
1034 |
} |
1035 |
|
1036 |
/// <summary> |
1037 |
/// Gets the 3d overlay position of the given joint over the depth-image. |
1038 |
/// </summary> |
1039 |
/// <returns>The joint position for depth overlay.</returns> |
1040 |
/// <param name="userId">User ID</param> |
1041 |
/// <param name="joint">Joint index</param> |
1042 |
/// <param name="camera">Camera used to visualize the 3d overlay position</param> |
1043 |
/// <param name="imageRect">Depth image rectangle on the screen</param> |
1044 |
public Vector3 GetJointPosDepthOverlay(Int64 userId, int joint, Camera camera, Rect imageRect) |
1045 |
{ |
1046 |
if(dictUserIdToIndex.ContainsKey(userId) && camera != null) |
1047 |
{ |
1048 |
int index = dictUserIdToIndex[userId]; |
1049 |
|
1050 |
if(index >= 0 && index < sensorData.bodyCount && |
1051 |
bodyFrame.bodyData[index].bIsTracked != 0) |
1052 |
{ |
1053 |
if(joint >= 0 && joint < sensorData.jointCount) |
1054 |
{ |
1055 |
KinectInterop.JointData jointData = bodyFrame.bodyData[index].joint[joint]; |
1056 |
Vector3 posJointRaw = jointData.kinectPos; |
1057 |
|
1058 |
if(posJointRaw != Vector3.zero) |
1059 |
{ |
1060 |
// 3d position to depth |
1061 |
Vector2 posDepth = MapSpacePointToDepthCoords(posJointRaw); |
1062 |
|
1063 |
if(posDepth != Vector2.zero && sensorData != null) |
1064 |
{ |
1065 |
if(!float.IsInfinity(posDepth.x) && !float.IsInfinity(posDepth.y)) |
1066 |
{ |
1067 |
float xScaled = (float)posDepth.x * imageRect.width / sensorData.depthImageWidth; |
1068 |
float yScaled = (float)posDepth.y * imageRect.height / sensorData.depthImageHeight; |
1069 |
|
1070 |
float xScreen = imageRect.x + xScaled; |
1071 |
//float yScreen = camera.pixelHeight - (imageRect.y + yScaled); |
1072 |
float yScreen = imageRect.y + imageRect.height - yScaled; |
1073 |
|
1074 |
Plane cameraPlane = new Plane(camera.transform.forward, camera.transform.position); |
1075 |
float zDistance = cameraPlane.GetDistanceToPoint(posJointRaw); |
1076 |
|
1077 |
Vector3 vPosJoint = camera.ScreenToWorldPoint(new Vector3(xScreen, yScreen, zDistance)); |
1078 |
|
1079 |
return vPosJoint; |
1080 |
} |
1081 |
} |
1082 |
} |
1083 |
} |
1084 |
} |
1085 |
} |
1086 |
|
1087 |
return Vector3.zero; |
1088 |
} |
1089 |
|
1090 |
/// <summary> |
1091 |
/// Gets the 3d overlay position of the given joint over the color-image. |
1092 |
/// </summary> |
1093 |
/// <returns>The joint position for color overlay.</returns> |
1094 |
/// <param name="userId">User ID</param> |
1095 |
/// <param name="joint">Joint index</param> |
1096 |
/// <param name="camera">Camera used to visualize the 3d overlay position</param> |
1097 |
/// <param name="imageRect">Color image rectangle on the screen</param> |
1098 |
public Vector3 GetJointPosColorOverlay(Int64 userId, int joint, Camera camera, Rect imageRect) |
1099 |
{ |
1100 |
if(dictUserIdToIndex.ContainsKey(userId) && camera != null) |
1101 |
{ |
1102 |
int index = dictUserIdToIndex[userId]; |
1103 |
|
1104 |
if(index >= 0 && index < sensorData.bodyCount && |
1105 |
bodyFrame.bodyData[index].bIsTracked != 0) |
1106 |
{ |
1107 |
if(joint >= 0 && joint < sensorData.jointCount) |
1108 |
{ |
1109 |
KinectInterop.JointData jointData = bodyFrame.bodyData[index].joint[joint]; |
1110 |
Vector3 posJointRaw = jointData.kinectPos; |
1111 |
|
1112 |
if(posJointRaw != Vector3.zero) |
1113 |
{ |
1114 |
// 3d position to depth |
1115 |
Vector2 posDepth = MapSpacePointToDepthCoords(posJointRaw); |
1116 |
ushort depthValue = GetDepthForPixel((int)posDepth.x, (int)posDepth.y); |
1117 |
|
1118 |
if(posDepth != Vector2.zero && depthValue > 0 && sensorData != null) |
1119 |
{ |
1120 |
// depth pos to color pos |
1121 |
Vector2 posColor = MapDepthPointToColorCoords(posDepth, depthValue); |
1122 |
|
1123 |
if(!float.IsInfinity(posColor.x) && !float.IsInfinity(posColor.y)) |
1124 |
{ |
1125 |
float xScaled = (float)posColor.x * imageRect.width / sensorData.colorImageWidth; |
1126 |
float yScaled = (float)posColor.y * imageRect.height / sensorData.colorImageHeight; |
1127 |
|
1128 |
float xScreen = imageRect.x + xScaled; |
1129 |
//float yScreen = camera.pixelHeight - (imageRect.y + yScaled); |
1130 |
float yScreen = imageRect.y + imageRect.height - yScaled; |
1131 |
|
1132 |
Plane cameraPlane = new Plane(camera.transform.forward, camera.transform.position); |
1133 |
float zDistance = cameraPlane.GetDistanceToPoint(posJointRaw); |
1134 |
//float zDistance = (jointData.kinectPos - camera.transform.position).magnitude; |
1135 |
|
1136 |
//Vector3 vPosJoint = camera.ViewportToWorldPoint(new Vector3(xNorm, yNorm, zDistance)); |
1137 |
Vector3 vPosJoint = camera.ScreenToWorldPoint(new Vector3(xScreen, yScreen, zDistance)); |
1138 |
|
1139 |
return vPosJoint; |
1140 |
} |
1141 |
} |
1142 |
} |
1143 |
} |
1144 |
} |
1145 |
} |
1146 |
|
1147 |
return Vector3.zero; |
1148 |
} |
1149 |
|
1150 |
/// <summary> |
1151 |
/// Gets the joint position on the depth map texture. |
1152 |
/// </summary> |
1153 |
/// <returns>The joint position in texture coordinates.</returns> |
1154 |
/// <param name="userId">User ID</param> |
1155 |
/// <param name="joint">Joint index</param> |
1156 |
public Vector2 GetJointDepthMapPos(Int64 userId, int joint) |
1157 |
{ |
1158 |
Vector2 posDepth = Vector2.zero; |
1159 |
|
1160 |
Vector3 posJointRaw = GetJointKinectPosition(userId, joint); |
1161 |
if(posJointRaw != Vector3.zero) |
1162 |
{ |
1163 |
posDepth = MapSpacePointToDepthCoords(posJointRaw); |
1164 |
|
1165 |
if(posDepth != Vector2.zero) |
1166 |
{ |
1167 |
float xScaled = (float)posDepth.x / GetDepthImageWidth(); |
1168 |
float yScaled = (float)posDepth.y / GetDepthImageHeight(); |
1169 |
|
1170 |
posDepth = new Vector2(xScaled, 1f - yScaled); |
1171 |
} |
1172 |
} |
1173 |
|
1174 |
return posDepth; |
1175 |
} |
1176 |
|
1177 |
/// <summary> |
1178 |
/// Gets the joint position on the color map texture. |
1179 |
/// </summary> |
1180 |
/// <returns>The joint position in texture coordinates.</returns> |
1181 |
/// <param name="userId">User ID</param> |
1182 |
/// <param name="joint">Joint index</param> |
1183 |
public Vector2 GetJointColorMapPos(Int64 userId, int joint) |
1184 |
{ |
1185 |
Vector2 posColor = Vector2.zero; |
1186 |
|
1187 |
Vector3 posJointRaw = GetJointKinectPosition(userId, joint); |
1188 |
if(posJointRaw != Vector3.zero) |
1189 |
{ |
1190 |
// 3d position to depth |
1191 |
Vector2 posDepth = MapSpacePointToDepthCoords(posJointRaw); |
1192 |
ushort depthValue = GetDepthForPixel((int)posDepth.x, (int)posDepth.y); |
1193 |
|
1194 |
if(posDepth != Vector2.zero && depthValue > 0) |
1195 |
{ |
1196 |
// depth pos to color pos |
1197 |
posColor = MapDepthPointToColorCoords(posDepth, depthValue); |
1198 |
|
1199 |
if(!float.IsInfinity(posColor.x) && !float.IsInfinity(posColor.y)) |
1200 |
{ |
1201 |
float xScaled = (float)posColor.x / GetColorImageWidth(); |
1202 |
float yScaled = (float)posColor.y / GetColorImageHeight(); |
1203 |
|
1204 |
posColor = new Vector2(xScaled, 1f - yScaled); |
1205 |
} |
1206 |
else |
1207 |
{ |
1208 |
posColor = Vector2.zero; |
1209 |
} |
1210 |
} |
1211 |
} |
1212 |
|
1213 |
return posColor; |
1214 |
} |
1215 |
|
1216 |
/// <summary> |
1217 |
/// Determines whether the given user is turned around or not. |
1218 |
/// </summary> |
1219 |
/// <returns><c>true</c> if the user is turned around; otherwise, <c>false</c>.</returns> |
1220 |
/// <param name="userId">User ID</param> |
1221 |
public bool IsUserTurnedAround(Int64 userId) |
1222 |
{ |
1223 |
if(dictUserIdToIndex.ContainsKey(userId)) |
1224 |
{ |
1225 |
int index = dictUserIdToIndex[userId]; |
1226 |
|
1227 |
if(index >= 0 && index < sensorData.bodyCount && |
1228 |
bodyFrame.bodyData[index].bIsTracked != 0) |
1229 |
{ |
1230 |
return bodyFrame.bodyData[index].isTurnedAround; |
1231 |
} |
1232 |
} |
1233 |
|
1234 |
return false; |
1235 |
} |
1236 |
|
1237 |
/// <summary> |
1238 |
/// Determines whether the left hand confidence is high for the specified user. |
1239 |
/// </summary> |
1240 |
/// <returns><c>true</c> if the left hand confidence is high; otherwise, <c>false</c>.</returns> |
1241 |
/// <param name="userId">User ID</param> |
1242 |
public bool IsLeftHandConfidenceHigh(Int64 userId) |
1243 |
{ |
1244 |
if(dictUserIdToIndex.ContainsKey(userId)) |
1245 |
{ |
1246 |
int index = dictUserIdToIndex[userId]; |
1247 |
|
1248 |
if(index >= 0 && index < sensorData.bodyCount && |
1249 |
bodyFrame.bodyData[index].bIsTracked != 0) |
1250 |
{ |
1251 |
return (bodyFrame.bodyData[index].leftHandConfidence == KinectInterop.TrackingConfidence.High); |
1252 |
} |
1253 |
} |
1254 |
|
1255 |
return false; |
1256 |
} |
1257 |
|
1258 |
/// <summary> |
1259 |
/// Determines whether the right hand confidence is high for the specified user. |
1260 |
/// </summary> |
1261 |
/// <returns><c>true</c> if the right hand confidence is high; otherwise, <c>false</c>.</returns> |
1262 |
/// <param name="userId">User ID</param> |
1263 |
public bool IsRightHandConfidenceHigh(Int64 userId) |
1264 |
{ |
1265 |
if(dictUserIdToIndex.ContainsKey(userId)) |
1266 |
{ |
1267 |
int index = dictUserIdToIndex[userId]; |
1268 |
|
1269 |
if(index >= 0 && index < sensorData.bodyCount && |
1270 |
bodyFrame.bodyData[index].bIsTracked != 0) |
1271 |
{ |
1272 |
return (bodyFrame.bodyData[index].rightHandConfidence == KinectInterop.TrackingConfidence.High); |
1273 |
} |
1274 |
} |
1275 |
|
1276 |
return false; |
1277 |
} |
1278 |
|
1279 |
/// <summary> |
1280 |
/// Gets the left hand state for the specified user. |
1281 |
/// </summary> |
1282 |
/// <returns>The left hand state.</returns> |
1283 |
/// <param name="userId">User ID</param> |
1284 |
public KinectInterop.HandState GetLeftHandState(Int64 userId) |
1285 |
{ |
1286 |
if(dictUserIdToIndex.ContainsKey(userId)) |
1287 |
{ |
1288 |
int index = dictUserIdToIndex[userId]; |
1289 |
|
1290 |
if(index >= 0 && index < sensorData.bodyCount && |
1291 |
bodyFrame.bodyData[index].bIsTracked != 0) |
1292 |
{ |
1293 |
return bodyFrame.bodyData[index].leftHandState; |
1294 |
} |
1295 |
} |
1296 |
|
1297 |
return KinectInterop.HandState.NotTracked; |
1298 |
} |
1299 |
|
1300 |
/// <summary> |
1301 |
/// Gets the right hand state for the specified user. |
1302 |
/// </summary> |
1303 |
/// <returns>The right hand state.</returns> |
1304 |
/// <param name="userId">User ID</param> |
1305 |
public KinectInterop.HandState GetRightHandState(Int64 userId) |
1306 |
{ |
1307 |
if(dictUserIdToIndex.ContainsKey(userId)) |
1308 |
{ |
1309 |
int index = dictUserIdToIndex[userId]; |
1310 |
|
1311 |
if(index >= 0 && index < sensorData.bodyCount && |
1312 |
bodyFrame.bodyData[index].bIsTracked != 0) |
1313 |
{ |
1314 |
return bodyFrame.bodyData[index].rightHandState; |
1315 |
} |
1316 |
} |
1317 |
|
1318 |
return KinectInterop.HandState.NotTracked; |
1319 |
} |
1320 |
|
1321 |
/// <summary> |
1322 |
/// Gets the left hand interaction box for the specified user. |
1323 |
/// </summary> |
1324 |
/// <returns><c>true</c>, if left hand interaction box was gotten, <c>false</c> otherwise.</returns> |
1325 |
/// <param name="userId">User ID</param> |
1326 |
/// <param name="leftBotBack">Vector containing the left, bottom and back coordinates, in meters</param> |
1327 |
/// <param name="rightTopFront">Vector containing the right, top and front coordinates, in meters</param> |
1328 |
/// <param name="bValidBox">If set to <c>true</c>, the previously set coordinates are valid</param> |
1329 |
public bool GetLeftHandInteractionBox(Int64 userId, ref Vector3 leftBotBack, ref Vector3 rightTopFront, bool bValidBox) |
1330 |
{ |
1331 |
if(dictUserIdToIndex.ContainsKey(userId)) |
1332 |
{ |
1333 |
int index = dictUserIdToIndex[userId]; |
1334 |
|
1335 |
if(index >= 0 && index < sensorData.bodyCount && |
1336 |
bodyFrame.bodyData[index].bIsTracked != 0) |
1337 |
{ |
1338 |
KinectInterop.BodyData bodyData = bodyFrame.bodyData[index]; |
1339 |
bool bResult = true; |
1340 |
|
1341 |
if(bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].trackingState != KinectInterop.TrackingState.NotTracked && |
1342 |
bodyData.joint[(int)KinectInterop.JointType.HipLeft].trackingState != KinectInterop.TrackingState.NotTracked) |
1343 |
{ |
1344 |
rightTopFront.x = bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].position.x; |
1345 |
leftBotBack.x = rightTopFront.x - 2 * (rightTopFront.x - bodyData.joint[(int)KinectInterop.JointType.HipLeft].position.x); |
1346 |
} |
1347 |
else |
1348 |
{ |
1349 |
bResult = bValidBox; |
1350 |
} |
1351 |
|
1352 |
if(bodyData.joint[(int)KinectInterop.JointType.HipRight].trackingState != KinectInterop.TrackingState.NotTracked && |
1353 |
bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].trackingState != KinectInterop.TrackingState.NotTracked) |
1354 |
{ |
1355 |
leftBotBack.y = bodyData.joint[(int)KinectInterop.JointType.HipRight].position.y; |
1356 |
rightTopFront.y = bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].position.y; |
1357 |
|
1358 |
float fDelta = (rightTopFront.y - leftBotBack.y) * 0.35f; // * 2 / 3; |
1359 |
leftBotBack.y += fDelta; |
1360 |
rightTopFront.y += fDelta; |
1361 |
} |
1362 |
else |
1363 |
{ |
1364 |
bResult = bValidBox; |
1365 |
} |
1366 |
|
1367 |
if(bodyData.joint[(int)KinectInterop.JointType.SpineBase].trackingState != KinectInterop.TrackingState.NotTracked) |
1368 |
{ |
1369 |
//leftBotBack.z = bodyData.joint[(int)KinectInterop.JointType.SpineBase].position.z; |
1370 |
leftBotBack.z = !ignoreZCoordinates ? bodyData.joint[(int)KinectInterop.JointType.SpineBase].position.z : |
1371 |
(bodyData.joint[(int)KinectInterop.JointType.HandLeft].position.z + 0.1f); |
1372 |
rightTopFront.z = leftBotBack.z - 0.5f; |
1373 |
} |
1374 |
else |
1375 |
{ |
1376 |
bResult = bValidBox; |
1377 |
} |
1378 |
|
1379 |
return bResult; |
1380 |
} |
1381 |
} |
1382 |
|
1383 |
return false; |
1384 |
} |
1385 |
|
1386 |
// returns the interaction box for the right hand of the specified user, in meters |
1387 |
/// <summary> |
1388 |
/// Gets the right hand interaction box for the specified user. |
1389 |
/// </summary> |
1390 |
/// <returns><c>true</c>, if right hand interaction box was gotten, <c>false</c> otherwise.</returns> |
1391 |
/// <param name="userId">User ID</param> |
1392 |
/// <param name="leftBotBack">Vector containing the left, bottom and back coordinates, in meters</param> |
1393 |
/// <param name="rightTopFront">ector containing the right, top and front coordinates, in meters</param> |
1394 |
/// <param name="bValidBox">If set to <c>true</c>, the previously set coordinates are valid</param> |
1395 |
public bool GetRightHandInteractionBox(Int64 userId, ref Vector3 leftBotBack, ref Vector3 rightTopFront, bool bValidBox) |
1396 |
{ |
1397 |
if(dictUserIdToIndex.ContainsKey(userId)) |
1398 |
{ |
1399 |
int index = dictUserIdToIndex[userId]; |
1400 |
|
1401 |
if(index >= 0 && index < sensorData.bodyCount && |
1402 |
bodyFrame.bodyData[index].bIsTracked != 0) |
1403 |
{ |
1404 |
KinectInterop.BodyData bodyData = bodyFrame.bodyData[index]; |
1405 |
bool bResult = true; |
1406 |
|
1407 |
if(bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].trackingState != KinectInterop.TrackingState.NotTracked && |
1408 |
bodyData.joint[(int)KinectInterop.JointType.HipRight].trackingState != KinectInterop.TrackingState.NotTracked) |
1409 |
{ |
1410 |
leftBotBack.x = bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].position.x; |
1411 |
rightTopFront.x = leftBotBack.x + 2 * (bodyData.joint[(int)KinectInterop.JointType.HipRight].position.x - leftBotBack.x); |
1412 |
} |
1413 |
else |
1414 |
{ |
1415 |
bResult = bValidBox; |
1416 |
} |
1417 |
|
1418 |
if(bodyData.joint[(int)KinectInterop.JointType.HipLeft].trackingState != KinectInterop.TrackingState.NotTracked && |
1419 |
bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].trackingState != KinectInterop.TrackingState.NotTracked) |
1420 |
{ |
1421 |
leftBotBack.y = bodyData.joint[(int)KinectInterop.JointType.HipLeft].position.y; |
1422 |
rightTopFront.y = bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].position.y; |
1423 |
|
1424 |
float fDelta = (rightTopFront.y - leftBotBack.y) * 0.35f; // * 2 / 3; |
1425 |
leftBotBack.y += fDelta; |
1426 |
rightTopFront.y += fDelta; |
1427 |
} |
1428 |
else |
1429 |
{ |
1430 |
bResult = bValidBox; |
1431 |
} |
1432 |
|
1433 |
if(bodyData.joint[(int)KinectInterop.JointType.SpineBase].trackingState != KinectInterop.TrackingState.NotTracked) |
1434 |
{ |
1435 |
//leftBotBack.z = bodyData.joint[(int)KinectInterop.JointType.SpineBase].position.z; |
1436 |
leftBotBack.z = !ignoreZCoordinates ? bodyData.joint[(int)KinectInterop.JointType.SpineBase].position.z : |
1437 |
(bodyData.joint[(int)KinectInterop.JointType.HandRight].position.z + 0.1f); |
1438 |
rightTopFront.z = leftBotBack.z - 0.5f; |
1439 |
} |
1440 |
else |
1441 |
{ |
1442 |
bResult = bValidBox; |
1443 |
} |
1444 |
|
1445 |
return bResult; |
1446 |
} |
1447 |
} |
1448 |
|
1449 |
return false; |
1450 |
} |
1451 |
|
1452 |
/// <summary> |
1453 |
/// Gets the depth value for the specified pixel, if ComputeUserMap is true. |
1454 |
/// </summary> |
1455 |
/// <returns>The depth value.</returns> |
1456 |
/// <param name="x">The X coordinate of the pixel.</param> |
1457 |
/// <param name="y">The Y coordinate of the pixel.</param> |
1458 |
public ushort GetDepthForPixel(int x, int y) |
1459 |
{ |
1460 |
if(sensorData != null && sensorData.depthImage != null) |
1461 |
{ |
1462 |
int index = y * sensorData.depthImageWidth + x; |
1463 |
|
1464 |
if(index >= 0 && index < sensorData.depthImage.Length) |
1465 |
{ |
1466 |
return sensorData.depthImage[index]; |
1467 |
} |
1468 |
} |
1469 |
|
1470 |
return 0; |
1471 |
} |
1472 |
|
1473 |
/// <summary> |
1474 |
/// Gets the depth value for the specified pixel, if ComputeUserMap is true. |
1475 |
/// </summary> |
1476 |
/// <returns>The depth value.</returns> |
1477 |
/// <param name="index">Depth index.</param> |
1478 |
public ushort GetDepthForIndex(int index) |
1479 |
{ |
1480 |
if(sensorData != null && sensorData.depthImage != null) |
1481 |
{ |
1482 |
if(index >= 0 && index < sensorData.depthImage.Length) |
1483 |
{ |
1484 |
return sensorData.depthImage[index]; |
1485 |
} |
1486 |
} |
1487 |
|
1488 |
return 0; |
1489 |
} |
1490 |
|
1491 |
/// <summary> |
1492 |
/// Returns the space coordinates of a depth-map point, or Vector3.zero if the sensor is not initialized |
1493 |
/// </summary> |
1494 |
/// <returns>The space coordinates.</returns> |
1495 |
/// <param name="posPoint">Depth point coordinates</param> |
1496 |
/// <param name="depthValue">Depth value</param> |
1497 |
/// <param name="bWorldCoords">If set to <c>true</c>, applies the sensor height and angle to the space coordinates.</param> |
1498 |
public Vector3 MapDepthPointToSpaceCoords(Vector2 posPoint, ushort depthValue, bool bWorldCoords) |
1499 |
{ |
1500 |
Vector3 posKinect = Vector3.zero; |
1501 |
|
1502 |
if(kinectInitialized) |
1503 |
{ |
1504 |
posKinect = KinectInterop.MapDepthPointToSpaceCoords(sensorData, posPoint, depthValue); |
1505 |
|
1506 |
if(bWorldCoords) |
1507 |
{ |
1508 |
posKinect = kinectToWorld.MultiplyPoint3x4(posKinect); |
1509 |
} |
1510 |
} |
1511 |
|
1512 |
return posKinect; |
1513 |
} |
1514 |
|
1515 |
/// <summary> |
1516 |
/// Maps the depth frame to space coordinates. |
1517 |
/// </summary> |
1518 |
/// <returns><c>true</c> on success, <c>false</c> otherwise.</returns> |
1519 |
/// <param name="avSpaceCoords">Buffer for the depth-to-space coordinates.</param> |
1520 |
public bool MapDepthFrameToSpaceCoords(ref Vector3[] avSpaceCoords) |
1521 |
{ |
1522 |
bool bResult = false; |
1523 |
|
1524 |
if(kinectInitialized && sensorData.depthImage != null) |
1525 |
{ |
1526 |
if(avSpaceCoords == null || avSpaceCoords.Length == 0) |
1527 |
{ |
1528 |
avSpaceCoords = new Vector3[sensorData.depthImageWidth * sensorData.depthImageHeight]; |
1529 |
} |
1530 |
|
1531 |
bResult = KinectInterop.MapDepthFrameToSpaceCoords(sensorData, ref avSpaceCoords); |
1532 |
} |
1533 |
|
1534 |
return bResult; |
1535 |
} |
1536 |
|
1537 |
/// <summary> |
1538 |
/// Returns the depth-map coordinates of a space point, or Vector2.zero if Kinect is not initialized |
1539 |
/// </summary> |
1540 |
/// <returns>The depth-map coordinates.</returns> |
1541 |
/// <param name="posPoint">Space point coordinates</param> |
1542 |
public Vector2 MapSpacePointToDepthCoords(Vector3 posPoint) |
1543 |
{ |
1544 |
Vector2 posDepth = Vector2.zero; |
1545 |
|
1546 |
if(kinectInitialized) |
1547 |
{ |
1548 |
posDepth = KinectInterop.MapSpacePointToDepthCoords(sensorData, posPoint); |
1549 |
} |
1550 |
|
1551 |
return posDepth; |
1552 |
} |
1553 |
|
1554 |
/// <summary> |
1555 |
/// Returns the color-map coordinates of a depth point. |
1556 |
/// </summary> |
1557 |
/// <returns>The color-map coordinates.</returns> |
1558 |
/// <param name="posPoint">Depth point coordinates</param> |
1559 |
/// <param name="depthValue">Depth value</param> |
1560 |
public Vector2 MapDepthPointToColorCoords(Vector2 posPoint, ushort depthValue) |
1561 |
{ |
1562 |
Vector2 posColor = Vector2.zero; |
1563 |
|
1564 |
if(kinectInitialized) |
1565 |
{ |
1566 |
posColor = KinectInterop.MapDepthPointToColorCoords(sensorData, posPoint, depthValue); |
1567 |
} |
1568 |
|
1569 |
return posColor; |
1570 |
} |
1571 |
|
1572 |
/// <summary> |
1573 |
/// Maps the depth frame to color coordinates. |
1574 |
/// </summary> |
1575 |
/// <returns><c>true</c> on success, <c>false</c> otherwise.</returns> |
1576 |
/// <param name="avColorCoords">Buffer for depth-to-color coordinates.</param> |
1577 |
public bool MapDepthFrameToColorCoords(ref Vector2[] avColorCoords) |
1578 |
{ |
1579 |
bool bResult = false; |
1580 |
|
1581 |
if(kinectInitialized && sensorData.depthImage != null && sensorData.colorImage != null) |
1582 |
{ |
1583 |
if(avColorCoords == null || avColorCoords.Length == 0) |
1584 |
{ |
1585 |
avColorCoords = new Vector2[sensorData.depthImageWidth * sensorData.depthImageHeight]; |
1586 |
} |
1587 |
|
1588 |
bResult = KinectInterop.MapDepthFrameToColorCoords(sensorData, ref avColorCoords); |
1589 |
} |
1590 |
|
1591 |
return bResult; |
1592 |
} |
1593 |
|
1594 |
/// <summary> |
1595 |
/// Maps the color frame to depth coordinates. |
1596 |
/// </summary> |
1597 |
/// <returns><c>true</c> on success, <c>false</c> otherwise.</returns> |
1598 |
/// <param name="avDepthCoords">Buffer for color-to-depth coordinates.</param> |
1599 |
public bool MapColorFrameToDepthCoords(ref Vector2[] avDepthCoords) |
1600 |
{ |
1601 |
bool bResult = false; |
1602 |
|
1603 |
if(kinectInitialized && sensorData.colorImage != null && sensorData.depthImage != null) |
1604 |
{ |
1605 |
if(avDepthCoords == null || avDepthCoords.Length == 0) |
1606 |
{ |
1607 |
avDepthCoords = new Vector2[sensorData.colorImageWidth * sensorData.colorImageWidth]; |
1608 |
} |
1609 |
|
1610 |
bResult = KinectInterop.MapColorFrameToDepthCoords(sensorData, ref avDepthCoords); |
1611 |
} |
1612 |
|
1613 |
return bResult; |
1614 |
} |
1615 |
|
1616 |
/// <summary> |
1617 |
/// Returns the depth-map coordinates of a color point. |
1618 |
/// </summary> |
1619 |
/// <returns>The depth coords.</returns> |
1620 |
/// <param name="colorPos">Color position.</param> |
1621 |
/// <param name="bReadDepthCoordsIfNeeded">If set to <c>true</c> allows reading of depth coords, if needed.</param> |
1622 |
public Vector2 MapColorPointToDepthCoords(Vector2 colorPos, bool bReadDepthCoordsIfNeeded) |
1623 |
{ |
1624 |
Vector2 posDepth = Vector2.zero; |
1625 |
|
1626 |
if(kinectInitialized && sensorData.colorImage != null && sensorData.depthImage != null) |
1627 |
{ |
1628 |
posDepth = KinectInterop.MapColorPointToDepthCoords(sensorData, colorPos, bReadDepthCoordsIfNeeded); |
1629 |
} |
1630 |
|
1631 |
return posDepth; |
1632 |
} |
1633 |
|
1634 |
/// <summary> |
1635 |
/// Removes all currently detected users, allowing new user-detection process to start. |
1636 |
/// </summary> |
1637 |
public void ClearKinectUsers() |
1638 |
{ |
1639 |
if(!kinectInitialized) |
1640 |
return; |
1641 |
|
1642 |
// remove current users |
1643 |
for(int i = alUserIds.Count - 1; i >= 0; i--) |
1644 |
{ |
1645 |
Int64 userId = alUserIds[i]; |
1646 |
RemoveUser(userId); |
1647 |
} |
1648 |
|
1649 |
ResetFilters(); |
1650 |
} |
1651 |
|
1652 |
/// <summary> |
1653 |
/// Resets the Kinect data filters. |
1654 |
/// </summary> |
1655 |
public void ResetFilters() |
1656 |
{ |
1657 |
if(jointPositionFilter != null) |
1658 |
{ |
1659 |
jointPositionFilter.Reset(); |
1660 |
} |
1661 |
|
1662 |
if( jointVelocityFilter != null) |
1663 |
{ |
1664 |
jointVelocityFilter.Reset(); |
1665 |
} |
1666 |
} |
1667 |
|
1668 |
/// <summary> |
1669 |
/// Adds a gesture to the list of detected gestures for the specified user. |
1670 |
/// </summary> |
1671 |
/// <param name="UserId">User ID</param> |
1672 |
/// <param name="gesture">Gesture type</param> |
1673 |
public void DetectGesture(Int64 UserId, KinectGestures.Gestures gesture) |
1674 |
{ |
1675 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData.ContainsKey(UserId) ? playerGesturesData[UserId] : new List<KinectGestures.GestureData>(); |
1676 |
int index = GetGestureIndex(gesture, ref gesturesData); |
1677 |
|
1678 |
if(index >= 0) |
1679 |
{ |
1680 |
DeleteGesture(UserId, gesture); |
1681 |
} |
1682 |
|
1683 |
KinectGestures.GestureData gestureData = new KinectGestures.GestureData(); |
1684 |
|
1685 |
gestureData.userId = UserId; |
1686 |
gestureData.gesture = gesture; |
1687 |
gestureData.state = 0; |
1688 |
gestureData.joint = 0; |
1689 |
gestureData.progress = 0f; |
1690 |
gestureData.complete = false; |
1691 |
gestureData.cancelled = false; |
1692 |
|
1693 |
gestureData.checkForGestures = new List<KinectGestures.Gestures>(); |
1694 |
switch(gesture) |
1695 |
{ |
1696 |
case KinectGestures.Gestures.ZoomIn: |
1697 |
gestureData.checkForGestures.Add(KinectGestures.Gestures.ZoomOut); |
1698 |
gestureData.checkForGestures.Add(KinectGestures.Gestures.Wheel); |
1699 |
break; |
1700 |
|
1701 |
case KinectGestures.Gestures.ZoomOut: |
1702 |
gestureData.checkForGestures.Add(KinectGestures.Gestures.ZoomIn); |
1703 |
gestureData.checkForGestures.Add(KinectGestures.Gestures.Wheel); |
1704 |
break; |
1705 |
|
1706 |
case KinectGestures.Gestures.Wheel: |
1707 |
gestureData.checkForGestures.Add(KinectGestures.Gestures.ZoomIn); |
1708 |
gestureData.checkForGestures.Add(KinectGestures.Gestures.ZoomOut); |
1709 |
break; |
1710 |
} |
1711 |
|
1712 |
gesturesData.Add(gestureData); |
1713 |
playerGesturesData[UserId] = gesturesData; |
1714 |
|
1715 |
if(!gesturesTrackingAtTime.ContainsKey(UserId)) |
1716 |
{ |
1717 |
gesturesTrackingAtTime[UserId] = 0f; |
1718 |
} |
1719 |
} |
1720 |
|
1721 |
/// <summary> |
1722 |
/// Resets the gesture state for the given gesture of the specified user. |
1723 |
/// </summary> |
1724 |
/// <returns><c>true</c>, if gesture was reset, <c>false</c> otherwise.</returns> |
1725 |
/// <param name="UserId">User ID</param> |
1726 |
/// <param name="gesture">Gesture type</param> |
1727 |
public bool ResetGesture(Int64 UserId, KinectGestures.Gestures gesture) |
1728 |
{ |
1729 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData.ContainsKey(UserId) ? playerGesturesData[UserId] : null; |
1730 |
int index = gesturesData != null ? GetGestureIndex(gesture, ref gesturesData) : -1; |
1731 |
if(index < 0) |
1732 |
return false; |
1733 |
|
1734 |
KinectGestures.GestureData gestureData = gesturesData[index]; |
1735 |
|
1736 |
gestureData.state = 0; |
1737 |
gestureData.joint = 0; |
1738 |
gestureData.progress = 0f; |
1739 |
gestureData.complete = false; |
1740 |
gestureData.cancelled = false; |
1741 |
gestureData.startTrackingAtTime = Time.realtimeSinceStartup + KinectInterop.Constants.MinTimeBetweenSameGestures; |
1742 |
|
1743 |
gesturesData[index] = gestureData; |
1744 |
playerGesturesData[UserId] = gesturesData; |
1745 |
|
1746 |
return true; |
1747 |
} |
1748 |
|
1749 |
/// <summary> |
1750 |
/// Resets the gesture states for all gestures of the specified user. |
1751 |
/// </summary> |
1752 |
/// <param name="UserId">User ID</param> |
1753 |
public void ResetPlayerGestures(Int64 UserId) |
1754 |
{ |
1755 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData.ContainsKey(UserId) ? playerGesturesData[UserId] : null; |
1756 |
|
1757 |
if(gesturesData != null) |
1758 |
{ |
1759 |
int listSize = gesturesData.Count; |
1760 |
|
1761 |
for(int i = 0; i < listSize; i++) |
1762 |
{ |
1763 |
ResetGesture(UserId, gesturesData[i].gesture); |
1764 |
} |
1765 |
} |
1766 |
} |
1767 |
|
1768 |
/// <summary> |
1769 |
/// Deletes the gesture for the specified user. |
1770 |
/// </summary> |
1771 |
/// <returns><c>true</c>, if gesture was deleted, <c>false</c> otherwise.</returns> |
1772 |
/// <param name="UserId">User ID</param> |
1773 |
/// <param name="gesture">Gesture type</param> |
1774 |
public bool DeleteGesture(Int64 UserId, KinectGestures.Gestures gesture) |
1775 |
{ |
1776 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData.ContainsKey(UserId) ? playerGesturesData[UserId] : null; |
1777 |
int index = gesturesData != null ? GetGestureIndex(gesture, ref gesturesData) : -1; |
1778 |
if(index < 0) |
1779 |
return false; |
1780 |
|
1781 |
gesturesData.RemoveAt(index); |
1782 |
playerGesturesData[UserId] = gesturesData; |
1783 |
|
1784 |
return true; |
1785 |
} |
1786 |
|
1787 |
/// <summary> |
1788 |
/// Deletes all gestures for the specified user. |
1789 |
/// </summary> |
1790 |
/// <param name="UserId">User ID</param> |
1791 |
public void ClearGestures(Int64 UserId) |
1792 |
{ |
1793 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData.ContainsKey(UserId) ? playerGesturesData[UserId] : null; |
1794 |
|
1795 |
if(gesturesData != null) |
1796 |
{ |
1797 |
gesturesData.Clear(); |
1798 |
playerGesturesData[UserId] = gesturesData; |
1799 |
} |
1800 |
} |
1801 |
|
1802 |
/// <summary> |
1803 |
/// Gets the list of gestures for the specified user. |
1804 |
/// </summary> |
1805 |
/// <returns>The gestures list.</returns> |
1806 |
/// <param name="UserId">User ID</param> |
1807 |
public List<KinectGestures.Gestures> GetGesturesList(Int64 UserId) |
1808 |
{ |
1809 |
List<KinectGestures.Gestures> list = new List<KinectGestures.Gestures>(); |
1810 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData.ContainsKey(UserId) ? playerGesturesData[UserId] : null; |
1811 |
|
1812 |
if(gesturesData != null) |
1813 |
{ |
1814 |
foreach(KinectGestures.GestureData data in gesturesData) |
1815 |
list.Add(data.gesture); |
1816 |
} |
1817 |
|
1818 |
return list; |
1819 |
} |
1820 |
|
1821 |
/// <summary> |
1822 |
/// Gets the gestures count for the specified user. |
1823 |
/// </summary> |
1824 |
/// <returns>The gestures count.</returns> |
1825 |
/// <param name="UserId">User ID</param> |
1826 |
public int GetGesturesCount(Int64 UserId) |
1827 |
{ |
1828 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData.ContainsKey(UserId) ? playerGesturesData[UserId] : null; |
1829 |
|
1830 |
if(gesturesData != null) |
1831 |
{ |
1832 |
return gesturesData.Count; |
1833 |
} |
1834 |
|
1835 |
return 0; |
1836 |
} |
1837 |
|
1838 |
/// <summary> |
1839 |
/// Gets the gesture at the specified index for the given user. |
1840 |
/// </summary> |
1841 |
/// <returns>The gesture at specified index.</returns> |
1842 |
/// <param name="UserId">User ID</param> |
1843 |
/// <param name="i">Index</param> |
1844 |
public KinectGestures.Gestures GetGestureAtIndex(Int64 UserId, int i) |
1845 |
{ |
1846 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData.ContainsKey(UserId) ? playerGesturesData[UserId] : null; |
1847 |
|
1848 |
if(gesturesData != null) |
1849 |
{ |
1850 |
if(i >= 0 && i < gesturesData.Count) |
1851 |
{ |
1852 |
return gesturesData[i].gesture; |
1853 |
} |
1854 |
} |
1855 |
|
1856 |
return KinectGestures.Gestures.None; |
1857 |
} |
1858 |
|
1859 |
/// <summary> |
1860 |
/// Determines whether the given gesture is in the list of gestures for the specified user. |
1861 |
/// </summary> |
1862 |
/// <returns><c>true</c> if the gesture is in the list of gestures for the specified user; otherwise, <c>false</c>.</returns> |
1863 |
/// <param name="UserId">User ID</param> |
1864 |
/// <param name="gesture">Gesture type</param> |
1865 |
public bool IsTrackingGesture(Int64 UserId, KinectGestures.Gestures gesture) |
1866 |
{ |
1867 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData.ContainsKey(UserId) ? playerGesturesData[UserId] : null; |
1868 |
int index = gesturesData != null ? GetGestureIndex(gesture, ref gesturesData) : -1; |
1869 |
|
1870 |
return index >= 0; |
1871 |
} |
1872 |
|
1873 |
/// <summary> |
1874 |
/// Determines whether the given gesture for the specified user is complete. |
1875 |
/// </summary> |
1876 |
/// <returns><c>true</c> if the gesture is complete; otherwise, <c>false</c>.</returns> |
1877 |
/// <param name="UserId">User ID</param> |
1878 |
/// <param name="gesture">Gesture type</param> |
1879 |
/// <param name="bResetOnComplete">If set to <c>true</c>, resets the gesture state.</param> |
1880 |
public bool IsGestureComplete(Int64 UserId, KinectGestures.Gestures gesture, bool bResetOnComplete) |
1881 |
{ |
1882 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData.ContainsKey(UserId) ? playerGesturesData[UserId] : null; |
1883 |
int index = gesturesData != null ? GetGestureIndex(gesture, ref gesturesData) : -1; |
1884 |
|
1885 |
if(index >= 0) |
1886 |
{ |
1887 |
KinectGestures.GestureData gestureData = gesturesData[index]; |
1888 |
|
1889 |
if(bResetOnComplete && gestureData.complete) |
1890 |
{ |
1891 |
ResetPlayerGestures(UserId); |
1892 |
return true; |
1893 |
} |
1894 |
|
1895 |
return gestureData.complete; |
1896 |
} |
1897 |
|
1898 |
return false; |
1899 |
} |
1900 |
|
1901 |
/// <summary> |
1902 |
/// Determines whether the given gesture for the specified user is canceled. |
1903 |
/// </summary> |
1904 |
/// <returns><c>true</c> if the gesture is canceled; otherwise, <c>false</c>.</returns> |
1905 |
/// <param name="UserId">User ID</param> |
1906 |
/// <param name="gesture">Gesture type</param> |
1907 |
public bool IsGestureCancelled(Int64 UserId, KinectGestures.Gestures gesture) |
1908 |
{ |
1909 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData.ContainsKey(UserId) ? playerGesturesData[UserId] : null; |
1910 |
int index = gesturesData != null ? GetGestureIndex(gesture, ref gesturesData) : -1; |
1911 |
|
1912 |
if(index >= 0) |
1913 |
{ |
1914 |
KinectGestures.GestureData gestureData = gesturesData[index]; |
1915 |
return gestureData.cancelled; |
1916 |
} |
1917 |
|
1918 |
return false; |
1919 |
} |
1920 |
|
1921 |
/// <summary> |
1922 |
/// Gets the progress (in range [0, 1]) of the given gesture for the specified user. |
1923 |
/// </summary> |
1924 |
/// <returns>The gesture progress.</returns> |
1925 |
/// <param name="UserId">User ID</param> |
1926 |
/// <param name="gesture">Gesture type</param> |
1927 |
public float GetGestureProgress(Int64 UserId, KinectGestures.Gestures gesture) |
1928 |
{ |
1929 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData.ContainsKey(UserId) ? playerGesturesData[UserId] : null; |
1930 |
int index = gesturesData != null ? GetGestureIndex(gesture, ref gesturesData) : -1; |
1931 |
|
1932 |
if(index >= 0) |
1933 |
{ |
1934 |
KinectGestures.GestureData gestureData = gesturesData[index]; |
1935 |
return gestureData.progress; |
1936 |
} |
1937 |
|
1938 |
return 0f; |
1939 |
} |
1940 |
|
1941 |
/// <summary> |
1942 |
/// Gets the normalized screen position of the given gesture for the specified user. |
1943 |
/// </summary> |
1944 |
/// <returns>The normalized screen position.</returns> |
1945 |
/// <param name="UserId">User ID</param> |
1946 |
/// <param name="gesture">Gesture type</param> |
1947 |
public Vector3 GetGestureScreenPos(Int64 UserId, KinectGestures.Gestures gesture) |
1948 |
{ |
1949 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData.ContainsKey(UserId) ? playerGesturesData[UserId] : null; |
1950 |
int index = gesturesData != null ? GetGestureIndex(gesture, ref gesturesData) : -1; |
1951 |
|
1952 |
if(index >= 0) |
1953 |
{ |
1954 |
KinectGestures.GestureData gestureData = gesturesData[index]; |
1955 |
return gestureData.screenPos; |
1956 |
} |
1957 |
|
1958 |
return Vector3.zero; |
1959 |
} |
1960 |
|
1961 |
|
1962 |
/// <summary> |
1963 |
/// Gets the world matrix data as one csv line. |
1964 |
/// </summary> |
1965 |
/// <returns>The world matrix data as a csv line.</returns> |
1966 |
public string GetWorldMatrixData(char delimiter) |
1967 |
{ |
1968 |
return KinectInterop.GetMatrixAsCsv(ref kinectToWorld, delimiter); |
1969 |
} |
1970 |
|
1971 |
|
1972 |
/// <summary> |
1973 |
/// Gets the body hand as one csv line, or returns empty string if there is no new body frame. |
1974 |
/// </summary> |
1975 |
/// <returns>The body hand as a csv line.</returns> |
1976 |
/// <param name="liRelTime">Reference to variable, used to compare frame times.</param> |
1977 |
public string GetBodyHandData(ref long liRelTime, char delimiter) |
1978 |
{ |
1979 |
return KinectInterop.GetHandsDataAsCsv(sensorData, ref bodyFrame, ref liRelTime, delimiter); |
1980 |
} |
1981 |
|
1982 |
|
1983 |
/// <summary> |
1984 |
/// Gets the body frame as one csv line, or returns empty string if there is no new body frame. |
1985 |
/// </summary> |
1986 |
/// <returns>The body frame as a csv line.</returns> |
1987 |
/// <param name="liRelTime">Reference to variable, used to compare frame times.</param> |
1988 |
/// <param name="fUnityTime">Reference to variable, used to save the current Unity time.</param> |
1989 |
public string GetBodyFrameData(ref long liRelTime, ref float fUnityTime, char delimiter) |
1990 |
{ |
1991 |
return KinectInterop.GetBodyFrameAsCsv(sensorData, ref bodyFrame, ref liRelTime, ref fUnityTime, delimiter); |
1992 |
} |
1993 |
|
1994 |
|
1995 |
/// <summary> |
1996 |
/// Determines whether the play mode is enabled or not. |
1997 |
/// </summary> |
1998 |
/// <returns><c>true</c> if the play mode is enabled; otherwise, <c>false</c>.</returns> |
1999 |
public bool IsPlayModeEnabled() |
2000 |
{ |
2001 |
if(sensorData != null) |
2002 |
{ |
2003 |
return sensorData.isPlayModeEnabled; |
2004 |
} |
2005 |
|
2006 |
return false; |
2007 |
} |
2008 |
|
2009 |
|
2010 |
/// <summary> |
2011 |
/// Enables or displables the play mode. |
2012 |
/// </summary> |
2013 |
/// <param name="bEnabled">If set to <c>true</c> enables the play mode.</param> |
2014 |
public void EnablePlayMode(bool bEnabled) |
2015 |
{ |
2016 |
if(sensorData != null) |
2017 |
{ |
2018 |
sensorData.isPlayModeEnabled = bEnabled; |
2019 |
sensorData.playModeData = string.Empty; |
2020 |
} |
2021 |
} |
2022 |
|
2023 |
/// <summary> |
2024 |
/// Sets the world matrix data from the given csv line. |
2025 |
/// </summary> |
2026 |
/// <returns><c>true</c> on success, <c>false</c> otherwise.</returns> |
2027 |
/// <param name="sLine">The kinect2world data as csv line.</param> |
2028 |
public bool SetWorldMatrixData(string sLine) |
2029 |
{ |
2030 |
if(sensorData != null && sensorData.isPlayModeEnabled) |
2031 |
{ |
2032 |
return KinectInterop.SetMatrixFromCsv(sLine, ref kinectToWorld); |
2033 |
} |
2034 |
|
2035 |
return false; |
2036 |
} |
2037 |
|
2038 |
/// <summary> |
2039 |
/// Sets the body hand data from the given csv line. |
2040 |
/// </summary> |
2041 |
/// <returns><c>true</c> on success, <c>false</c> otherwise.</returns> |
2042 |
/// <param name="sLine">The hand data as csv line.</param> |
2043 |
public bool SetBodyHandData(string sLine) |
2044 |
{ |
2045 |
if(sensorData != null && sensorData.isPlayModeEnabled) |
2046 |
{ |
2047 |
sensorData.playModeHandData = sLine; |
2048 |
return true; |
2049 |
} |
2050 |
|
2051 |
return false; |
2052 |
} |
2053 |
|
2054 |
/// <summary> |
2055 |
/// Sets the body frame from the given csv line. |
2056 |
/// </summary> |
2057 |
/// <returns><c>true</c> on success, <c>false</c> otherwise.</returns> |
2058 |
/// <param name="sLine">The body frame as csv line.</param> |
2059 |
public bool SetBodyFrameData(string sLine) |
2060 |
{ |
2061 |
if(sensorData != null && sensorData.isPlayModeEnabled) |
2062 |
{ |
2063 |
sensorData.playModeData = sLine; |
2064 |
return true; |
2065 |
} |
2066 |
|
2067 |
return false; |
2068 |
} |
2069 |
|
2070 |
|
2071 |
// KinectManager's Internal Methods |
2072 |
|
2073 |
|
2074 |
void Awake() |
2075 |
{ |
2076 |
// set the singleton instance |
2077 |
instance = this; |
2078 |
|
2079 |
try |
2080 |
{ |
2081 |
bool bOnceRestarted = false; |
2082 |
if(System.IO.File.Exists("KMrestart.txt")) |
2083 |
{ |
2084 |
bOnceRestarted = true; |
2085 |
|
2086 |
try |
2087 |
{ |
2088 |
System.IO.File.Delete("KMrestart.txt"); |
2089 |
} |
2090 |
catch(Exception ex) |
2091 |
{ |
2092 |
Debug.LogError("Error deleting KMrestart.txt"); |
2093 |
Debug.LogError(ex.ToString()); |
2094 |
} |
2095 |
} |
2096 |
|
2097 |
// init the available sensor interfaces |
2098 |
bool bNeedRestart = false; |
2099 |
sensorInterfaces = KinectInterop.InitSensorInterfaces(bOnceRestarted, ref bNeedRestart); |
2100 |
|
2101 |
if(bNeedRestart) |
2102 |
{ |
2103 |
System.IO.File.WriteAllText("KMrestart.txt", "Restarting level..."); |
2104 |
KinectInterop.RestartLevel(gameObject, "KM"); |
2105 |
return; |
2106 |
} |
2107 |
else |
2108 |
{ |
2109 |
// set graphics shader level |
2110 |
KinectInterop.SetGraphicsShaderLevel(SystemInfo.graphicsShaderLevel); |
2111 |
|
2112 |
// start the sensor |
2113 |
StartKinect(); |
2114 |
} |
2115 |
} |
2116 |
catch (Exception ex) |
2117 |
{ |
2118 |
Debug.LogError(ex.ToString()); |
2119 |
|
2120 |
if(calibrationText != null) |
2121 |
{ |
2122 |
calibrationText.text = ex.Message; |
2123 |
} |
2124 |
} |
2125 |
|
2126 |
} |
2127 |
|
2128 |
private void StartKinect() |
2129 |
{ |
2130 |
try |
2131 |
{ |
2132 |
// try to initialize the default Kinect2 sensor |
2133 |
KinectInterop.FrameSource dwFlags = KinectInterop.FrameSource.TypeBody; |
2134 |
|
2135 |
if(computeUserMap != UserMapType.None) |
2136 |
dwFlags |= KinectInterop.FrameSource.TypeDepth | KinectInterop.FrameSource.TypeBodyIndex; |
2137 |
if(computeColorMap) |
2138 |
dwFlags |= KinectInterop.FrameSource.TypeColor; |
2139 |
if(computeInfraredMap) |
2140 |
dwFlags |= KinectInterop.FrameSource.TypeInfrared; |
2141 |
// if(useAudioSource) |
2142 |
// dwFlags |= KinectInterop.FrameSource.TypeAudio; |
2143 |
|
2144 |
// open the default sensor |
2145 |
BackgroundRemovalManager brManager = gameObject.GetComponentInChildren<BackgroundRemovalManager>(); |
2146 |
sensorData = KinectInterop.OpenDefaultSensor(sensorInterfaces, dwFlags, sensorAngle, useMultiSourceReader, computeUserMap, brManager); |
2147 |
|
2148 |
if (sensorData == null) |
2149 |
{ |
2150 |
if(sensorInterfaces == null || sensorInterfaces.Count == 0) |
2151 |
throw new Exception("No sensor found. Make sure you have installed the SDK and the sensor is connected."); |
2152 |
else |
2153 |
throw new Exception("OpenDefaultSensor failed."); |
2154 |
} |
2155 |
|
2156 |
// enable or disable getting height and angle info |
2157 |
sensorData.hintHeightAngle = (autoHeightAngle != AutoHeightAngle.DontUse); |
2158 |
|
2159 |
//create the transform matrix - kinect to world |
2160 |
// Quaternion quatTiltAngle = Quaternion.Euler(-sensorAngle, 0.0f, 0.0f); |
2161 |
// kinectToWorld.SetTRS(new Vector3(0.0f, sensorHeight, 0.0f), quatTiltAngle, Vector3.one); |
2162 |
UpdateKinectToWorldMatrix(); |
2163 |
} |
2164 |
catch(DllNotFoundException ex) |
2165 |
{ |
2166 |
string message = ex.Message + " cannot be loaded. Please check the Kinect SDK installation."; |
2167 |
|
2168 |
Debug.LogError(message); |
2169 |
Debug.LogException(ex); |
2170 |
|
2171 |
if(calibrationText != null) |
2172 |
{ |
2173 |
calibrationText.text = message; |
2174 |
} |
2175 |
|
2176 |
return; |
2177 |
} |
2178 |
catch(Exception ex) |
2179 |
{ |
2180 |
string message = ex.Message; |
2181 |
|
2182 |
Debug.LogError(message); |
2183 |
Debug.LogException(ex); |
2184 |
|
2185 |
if(calibrationText != null) |
2186 |
{ |
2187 |
calibrationText.text = message; |
2188 |
} |
2189 |
|
2190 |
return; |
2191 |
} |
2192 |
|
2193 |
// init skeleton structures |
2194 |
bodyFrame = new KinectInterop.BodyFrameData(sensorData.bodyCount, KinectInterop.Constants.MaxJointCount); // sensorData.jointCount |
2195 |
bodyFrame.bTurnAnalisys = ( allowTurnArounds || estimateJointVelocities ); // bTurnAnalisys is needed to turn on velocities computing ( setup jointData.posDrv in KinectInterop.cs ) -> done this way to not modify other files |
2196 |
|
2197 |
// init data filters |
2198 |
KinectInterop.SmoothParameters positionSmoothParams = InitSmoothParameters( smoothing ); |
2199 |
jointPositionFilter = new JointPositionsFilter(); |
2200 |
jointPositionFilter.Init(positionSmoothParams); |
2201 |
|
2202 |
KinectInterop.SmoothParameters velocitySmoothParams = InitSmoothParameters( velocitySmoothing ); |
2203 |
jointVelocityFilter = new JointVelocitiesFilter(); |
2204 |
jointVelocityFilter.Init(velocitySmoothParams); |
2205 |
|
2206 |
// init the bone orientation constraints |
2207 |
if(useBoneOrientationConstraints) |
2208 |
{ |
2209 |
boneConstraintsFilter = new BoneOrientationsConstraint(); |
2210 |
boneConstraintsFilter.AddDefaultConstraints(); |
2211 |
boneConstraintsFilter.SetDebugText(calibrationText); |
2212 |
} |
2213 |
|
2214 |
if(computeUserMap != UserMapType.None && computeUserMap != UserMapType.RawUserDepth) |
2215 |
{ |
2216 |
// Initialize depth & label map related stuff |
2217 |
usersLblTex = new Texture2D(sensorData.depthImageWidth, sensorData.depthImageHeight, TextureFormat.ARGB32, false); |
2218 |
|
2219 |
usersMapSize = sensorData.depthImageWidth * sensorData.depthImageHeight; |
2220 |
usersHistogramImage = new Color32[usersMapSize]; |
2221 |
usersPrevState = new ushort[usersMapSize]; |
2222 |
usersHistogramMap = new float[5001]; |
2223 |
} |
2224 |
|
2225 |
if(computeColorMap) |
2226 |
{ |
2227 |
// Initialize color map related stuff |
2228 |
//usersClrTex = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.RGBA32, false); |
2229 |
usersClrSize = sensorData.colorImageWidth * sensorData.colorImageHeight; |
2230 |
} |
2231 |
|
2232 |
// try to automatically use the available avatar controllers in the scene |
2233 |
if(avatarControllers.Count == 0) |
2234 |
{ |
2235 |
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[]; |
2236 |
|
2237 |
foreach(MonoBehaviour monoScript in monoScripts) |
2238 |
{ |
2239 |
// if(typeof(AvatarController).IsAssignableFrom(monoScript.GetType()) && monoScript.enabled) |
2240 |
if((monoScript is AvatarController) && monoScript.enabled) |
2241 |
{ |
2242 |
AvatarController avatar = (AvatarController)monoScript; |
2243 |
avatarControllers.Add(avatar); |
2244 |
} |
2245 |
} |
2246 |
} |
2247 |
|
2248 |
// set up the gesture manager, if not already set |
2249 |
if(gestureManager == null) |
2250 |
{ |
2251 |
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[]; |
2252 |
|
2253 |
foreach(MonoBehaviour monoScript in monoScripts) |
2254 |
{ |
2255 |
// if(typeof(KinectGestures).IsAssignableFrom(monoScript.GetType()) && monoScript.enabled) |
2256 |
if((monoScript is KinectGestures) && monoScript.enabled) |
2257 |
{ |
2258 |
gestureManager = (KinectGestures)monoScript; |
2259 |
break; |
2260 |
} |
2261 |
} |
2262 |
|
2263 |
} |
2264 |
|
2265 |
// try to automatically use the available gesture listeners in the scene |
2266 |
if(gestureListeners.Count == 0) |
2267 |
{ |
2268 |
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[]; |
2269 |
|
2270 |
foreach(MonoBehaviour monoScript in monoScripts) |
2271 |
{ |
2272 |
// if(typeof(KinectGestures.GestureListenerInterface).IsAssignableFrom(monoScript.GetType()) && |
2273 |
// monoScript.enabled) |
2274 |
if((monoScript is KinectGestures.GestureListenerInterface) && monoScript.enabled) |
2275 |
{ |
2276 |
//KinectGestures.GestureListenerInterface gl = (KinectGestures.GestureListenerInterface)monoScript; |
2277 |
gestureListeners.Add(monoScript); |
2278 |
} |
2279 |
} |
2280 |
} |
2281 |
|
2282 |
// Initialize user list to contain all users. |
2283 |
//alUserIds = new List<Int64>(); |
2284 |
//dictUserIdToIndex = new Dictionary<Int64, int>(); |
2285 |
|
2286 |
// // start the background reader |
2287 |
// if(useOwnThread) |
2288 |
// { |
2289 |
// kinectReaderThread = new System.Threading.Thread(UpdateKinectStreamsThread); |
2290 |
// kinectReaderThread.Name = "KinectReaderThread"; |
2291 |
// kinectReaderThread.IsBackground = true; |
2292 |
// kinectReaderThread.Start(); |
2293 |
// kinectReaderRunning = true; |
2294 |
// } |
2295 |
|
2296 |
kinectInitialized = true; |
2297 |
|
2298 |
#if USE_SINGLE_KM_IN_MULTIPLE_SCENES |
2299 |
DontDestroyOnLoad(gameObject); |
2300 |
#endif |
2301 |
|
2302 |
// GUI Text. |
2303 |
if(calibrationText != null) |
2304 |
{ |
2305 |
calibrationText.text = "WAITING FOR USERS"; |
2306 |
} |
2307 |
|
2308 |
Debug.Log("Waiting for users."); |
2309 |
} |
2310 |
|
2311 |
private KinectInterop.SmoothParameters InitSmoothParameters( Smoothing smoothing ) |
2312 |
{ |
2313 |
KinectInterop.SmoothParameters smoothParameters = new KinectInterop.SmoothParameters(); |
2314 |
|
2315 |
switch(smoothing) |
2316 |
{ |
2317 |
case Smoothing.Light: |
2318 |
smoothParameters.smoothing = 0.3f; |
2319 |
smoothParameters.correction = 0.35f; |
2320 |
smoothParameters.prediction = 0.35f; |
2321 |
smoothParameters.jitterRadius = 0.15f; |
2322 |
smoothParameters.maxDeviationRadius = 0.15f; |
2323 |
break; |
2324 |
case Smoothing.Default: |
2325 |
smoothParameters.smoothing = 0.5f; |
2326 |
smoothParameters.correction = 0.5f; |
2327 |
smoothParameters.prediction = 0.5f; |
2328 |
smoothParameters.jitterRadius = 0.05f; |
2329 |
smoothParameters.maxDeviationRadius = 0.04f; |
2330 |
break; |
2331 |
case Smoothing.Medium: |
2332 |
smoothParameters.smoothing = 0.5f; |
2333 |
smoothParameters.correction = 0.1f; |
2334 |
smoothParameters.prediction = 0.5f; |
2335 |
smoothParameters.jitterRadius = 0.1f; |
2336 |
smoothParameters.maxDeviationRadius = 0.1f; |
2337 |
break; |
2338 |
case Smoothing.Aggressive: |
2339 |
smoothParameters.smoothing = 0.7f; |
2340 |
smoothParameters.correction = 0.3f; |
2341 |
smoothParameters.prediction = 1.0f; |
2342 |
smoothParameters.jitterRadius = 1.0f; |
2343 |
smoothParameters.maxDeviationRadius = 1.0f; |
2344 |
break; |
2345 |
} |
2346 |
|
2347 |
return smoothParameters; |
2348 |
} |
2349 |
|
2350 |
#if UNITY_WSA |
2351 |
//void OnApplicationFocus(bool hasFocus) |
2352 |
void OnApplicationPause(bool isPaused) |
2353 |
{ |
2354 |
//bool isPaused = !hasFocus; |
2355 |
Debug.Log("Application-Pause: " + isPaused); |
2356 |
|
2357 |
if (isPaused) |
2358 |
{ |
2359 |
// pause |
2360 |
if (kinectInitialized) |
2361 |
{ |
2362 |
backgroundRemovalInited = sensorData != null ? sensorData.backgroundRemovalInited : false; |
2363 |
backgroundRemovalHiRes = sensorData != null ? sensorData.backgroundRemovalHiRes : false; |
2364 |
|
2365 |
OnDestroy(); |
2366 |
} |
2367 |
} |
2368 |
else |
2369 |
{ |
2370 |
// resume |
2371 |
if (!kinectInitialized) |
2372 |
{ |
2373 |
instance = this; |
2374 |
StartKinect(); |
2375 |
|
2376 |
if (backgroundRemovalInited) |
2377 |
{ |
2378 |
KinectInterop.InitBackgroundRemoval(sensorData, backgroundRemovalHiRes); |
2379 |
} |
2380 |
} |
2381 |
} |
2382 |
|
2383 |
//Debug.Log("OnApplicationPause() finished. isPaused was " + isPaused); |
2384 |
} |
2385 |
#endif |
2386 |
|
2387 |
void OnApplicationQuit() |
2388 |
{ |
2389 |
OnDestroy(); |
2390 |
} |
2391 |
|
2392 |
void OnDestroy() |
2393 |
{ |
2394 |
//Debug.Log("KM was destroyed"); |
2395 |
|
2396 |
// shut down the Kinect on quitting. |
2397 |
if(kinectInitialized) |
2398 |
{ |
2399 |
// stop the background thread |
2400 |
kinectReaderRunning = false; |
2401 |
|
2402 |
// if(useOwnThread) |
2403 |
// { |
2404 |
// // set below false to avoid external thread stack in while(x) { Sleep(1); } in many places in KinectInterop.cs |
2405 |
// // ( previously this coused that after one start unity/game crashes/not respondes becouse this thread was still runing and app had to be killed and restarteted almost every time after start and stop KinectManager with external thread ) |
2406 |
// sensorData.bodyFrameReady = false; |
2407 |
// sensorData.bodyIndexBufferReady = false; |
2408 |
// sensorData.colorImageBufferReady = false; |
2409 |
// sensorData.depthCoordsBufferReady = false; |
2410 |
// sensorData.depthImageBufferReady = false; |
2411 |
// sensorData.spaceCoordsBufferReady = false; |
2412 |
// |
2413 |
// // wait for thread stops |
2414 |
// kinectReaderThread.Join( 2000 ); |
2415 |
// |
2416 |
// if( kinectReaderThread.ThreadState != System.Threading.ThreadState.Stopped ) |
2417 |
// { |
2418 |
// Debug.LogError( "KinectThread stop timeout! Aborting..."); |
2419 |
// kinectReaderThread.Abort(); |
2420 |
// } |
2421 |
// |
2422 |
// kinectReaderThread = null; |
2423 |
// } |
2424 |
|
2425 |
// close the sensor |
2426 |
KinectInterop.CloseSensor(sensorData); |
2427 |
// KinectInterop.ShutdownKinectSensor(); |
2428 |
|
2429 |
kinectInitialized = false; |
2430 |
} |
2431 |
|
2432 |
instance = null; |
2433 |
} |
2434 |
|
2435 |
void OnGUI() |
2436 |
{ |
2437 |
if(kinectInitialized) |
2438 |
{ |
2439 |
if(displayUserMap && !sensorData.color2DepthTexture && |
2440 |
(computeUserMap != UserMapType.None && computeUserMap != UserMapType.RawUserDepth)) |
2441 |
{ |
2442 |
if(usersMapRect.width == 0 || usersMapRect.height == 0) |
2443 |
{ |
2444 |
// get the main camera rectangle |
2445 |
Rect cameraRect = Camera.main != null ? Camera.main.pixelRect : new Rect(0, 0, Screen.width, Screen.height); |
2446 |
|
2447 |
// calculate map width and height in percent, if needed |
2448 |
if(DisplayMapsWidthPercent == 0f) |
2449 |
{ |
2450 |
DisplayMapsWidthPercent = (sensorData.depthImageWidth / 2) * 100 / cameraRect.width; |
2451 |
} |
2452 |
|
2453 |
float displayMapsWidthPercent = DisplayMapsWidthPercent / 100f; |
2454 |
float displayMapsHeightPercent = displayMapsWidthPercent * sensorData.depthImageHeight / sensorData.depthImageWidth; |
2455 |
|
2456 |
float displayWidth = cameraRect.width * displayMapsWidthPercent; |
2457 |
float displayHeight = cameraRect.width * displayMapsHeightPercent; |
2458 |
|
2459 |
usersMapRect = new Rect(cameraRect.width - displayWidth, cameraRect.height, displayWidth, -displayHeight); |
2460 |
} |
2461 |
|
2462 |
GUI.DrawTexture(usersMapRect, usersLblTex); |
2463 |
} |
2464 |
else if(computeColorMap && displayColorMap) |
2465 |
{ |
2466 |
if(usersClrRect.width == 0 || usersClrRect.height == 0) |
2467 |
{ |
2468 |
// get the main camera rectangle |
2469 |
Rect cameraRect = Camera.main != null ? Camera.main.pixelRect : new Rect(0, 0, Screen.width, Screen.height); |
2470 |
|
2471 |
// calculate map width and height in percent, if needed |
2472 |
if(DisplayMapsWidthPercent == 0f) |
2473 |
{ |
2474 |
DisplayMapsWidthPercent = (sensorData.depthImageWidth / 2) * 100 / cameraRect.width; |
2475 |
} |
2476 |
|
2477 |
float displayMapsWidthPercent = DisplayMapsWidthPercent / 100f; |
2478 |
float displayMapsHeightPercent = displayMapsWidthPercent * sensorData.colorImageHeight / sensorData.colorImageWidth; |
2479 |
|
2480 |
float displayWidth = cameraRect.width * displayMapsWidthPercent; |
2481 |
float displayHeight = cameraRect.width * displayMapsHeightPercent; |
2482 |
|
2483 |
usersClrRect = new Rect(cameraRect.width - displayWidth, cameraRect.height, displayWidth, -displayHeight); |
2484 |
|
2485 |
// if(computeUserMap && displayColorMap) |
2486 |
// { |
2487 |
// usersMapRect.x -= cameraRect.width * displayMapsWidthPercent; |
2488 |
// } |
2489 |
} |
2490 |
|
2491 |
//GUI.DrawTexture(usersClrRect, usersClrTex); |
2492 |
GUI.DrawTexture(usersClrRect, sensorData.colorImageTexture); |
2493 |
} |
2494 |
} |
2495 |
} |
2496 |
|
2497 |
// updates Kinect streams and structures |
2498 |
private void UpdateKinectStreams() |
2499 |
{ |
2500 |
if(kinectInitialized) |
2501 |
{ |
2502 |
KinectInterop.UpdateSensorData(sensorData); |
2503 |
|
2504 |
// check user limits and update sensor data |
2505 |
bLimitedUsers = showTrackedUsersOnly && ((maxTrackedUsers < 6 /**&& (alUserIds.Count > maxTrackedUsers)*/) || |
2506 |
minUserDistance >= 0.51f || maxUserDistance >= 0.51f || maxLeftRightDistance >= 0.01f); |
2507 |
//Debug.Log ("Limited-users: " + bLimitedUsers); |
2508 |
|
2509 |
if(useMultiSourceReader) |
2510 |
{ |
2511 |
KinectInterop.GetMultiSourceFrame(sensorData); |
2512 |
} |
2513 |
|
2514 |
// poll color map |
2515 |
if(computeColorMap) |
2516 |
{ |
2517 |
if((sensorData.newColorImage = KinectInterop.PollColorFrame(sensorData))) |
2518 |
{ |
2519 |
//UpdateColorMap(); |
2520 |
} |
2521 |
} |
2522 |
|
2523 |
// poll user map |
2524 |
if(computeUserMap != UserMapType.None) |
2525 |
{ |
2526 |
sensorData.firstUserIndex = liPrimaryUserId != 0 && dictUserIdToIndex.ContainsKey(liPrimaryUserId) ? |
2527 |
dictUserIdToIndex[liPrimaryUserId] : -1; |
2528 |
|
2529 |
if((sensorData.newDepthImage = KinectInterop.PollDepthFrame(sensorData, computeUserMap, bLimitedUsers, dictUserIdToIndex.Values))) |
2530 |
{ |
2531 |
//UpdateUserMap(computeUserMap); |
2532 |
} |
2533 |
} |
2534 |
|
2535 |
// poll infrared map |
2536 |
if(computeInfraredMap) |
2537 |
{ |
2538 |
if((sensorData.newInfraredImage = KinectInterop.PollInfraredFrame(sensorData))) |
2539 |
{ |
2540 |
//UpdateInfraredMap(); |
2541 |
} |
2542 |
} |
2543 |
|
2544 |
// poll or play body frame |
2545 |
sensorData.newBodyFrame = false; |
2546 |
if(sensorData == null || !sensorData.isPlayModeEnabled) |
2547 |
{ |
2548 |
sensorData.newBodyFrame = KinectInterop.PollBodyFrame(sensorData, ref bodyFrame, ref kinectToWorld, ignoreZCoordinates); |
2549 |
} |
2550 |
else |
2551 |
{ |
2552 |
if(!string.IsNullOrEmpty(sensorData.playModeData)) |
2553 |
{ |
2554 |
sensorData.newBodyFrame = KinectInterop.SetBodyFrameFromCsv(sensorData.playModeData, sensorData, ref bodyFrame, ref kinectToWorld); |
2555 |
sensorData.playModeData = string.Empty; |
2556 |
} |
2557 |
|
2558 |
if(!string.IsNullOrEmpty(sensorData.playModeHandData)) |
2559 |
{ |
2560 |
KinectInterop.SetHandsDataFromCsv(sensorData.playModeHandData, sensorData, ref bodyFrame); |
2561 |
sensorData.playModeHandData = string.Empty; |
2562 |
} |
2563 |
} |
2564 |
|
2565 |
// process the body frame |
2566 |
if(sensorData.newBodyFrame) |
2567 |
{ |
2568 |
// filter the tracked joint positions |
2569 |
if(smoothing != Smoothing.None) |
2570 |
{ |
2571 |
jointPositionFilter.UpdateFilter(ref bodyFrame); |
2572 |
} |
2573 |
|
2574 |
//ProcessBodyFrameData(); |
2575 |
|
2576 |
if(estimateJointVelocities && velocitySmoothing != Smoothing.None) |
2577 |
{ |
2578 |
jointVelocityFilter.UpdateFilter(ref bodyFrame); |
2579 |
} |
2580 |
} |
2581 |
|
2582 |
if(useMultiSourceReader) |
2583 |
{ |
2584 |
KinectInterop.FreeMultiSourceFrame(sensorData); |
2585 |
} |
2586 |
} |
2587 |
} |
2588 |
|
2589 |
// background kinect thread procedure |
2590 |
private void UpdateKinectStreamsThread() |
2591 |
{ |
2592 |
while(kinectReaderRunning) |
2593 |
{ |
2594 |
UpdateKinectStreams(); |
2595 |
KinectInterop.Sleep(5); |
2596 |
} |
2597 |
} |
2598 |
|
2599 |
// process data from Kinect streams |
2600 |
private void ProcessKinectStreams() |
2601 |
{ |
2602 |
// render color texture |
2603 |
if(sensorData.colorImageBufferReady) |
2604 |
{ |
2605 |
KinectInterop.RenderColorTexture(sensorData); |
2606 |
UpdateColorMap(); |
2607 |
} |
2608 |
|
2609 |
// render body-index texture |
2610 |
bool newDepthImage = sensorData.bodyIndexBufferReady || sensorData.depthImageBufferReady; |
2611 |
|
2612 |
if(sensorData.bodyIndexBufferReady) |
2613 |
{ |
2614 |
KinectInterop.RenderBodyIndexTexture(sensorData, computeUserMap); |
2615 |
} |
2616 |
|
2617 |
// render depth-image texture |
2618 |
if(sensorData.depthImageBufferReady) |
2619 |
{ |
2620 |
KinectInterop.RenderDepthImageTexture(sensorData); |
2621 |
} |
2622 |
|
2623 |
// update user map |
2624 |
if(newDepthImage) |
2625 |
{ |
2626 |
UpdateUserMap(computeUserMap); |
2627 |
} |
2628 |
|
2629 |
// update infrared map |
2630 |
UpdateInfraredMap(); |
2631 |
|
2632 |
if(sensorData.bodyFrameReady) |
2633 |
{ |
2634 |
ProcessBodyFrameData(); |
2635 |
|
2636 |
// frame is released |
2637 |
lock(sensorData.bodyFrameLock) |
2638 |
{ |
2639 |
sensorData.bodyFrameReady = false; |
2640 |
} |
2641 |
} |
2642 |
} |
2643 |
|
2644 |
void Update() |
2645 |
{ |
2646 |
if(kinectInitialized) |
2647 |
{ |
2648 |
if(!kinectReaderRunning) |
2649 |
{ |
2650 |
// update Kinect streams and structures |
2651 |
UpdateKinectStreams(); |
2652 |
} |
2653 |
|
2654 |
// process the data from Kinect streams |
2655 |
ProcessKinectStreams(); |
2656 |
|
2657 |
// update the avatars |
2658 |
if(!lateUpdateAvatars) |
2659 |
{ |
2660 |
foreach (AvatarController controller in avatarControllers) |
2661 |
{ |
2662 |
//int userIndex = controller ? controller.playerIndex : -1; |
2663 |
Int64 userId = controller ? controller.playerId : 0; |
2664 |
|
2665 |
//if((userIndex >= 0) && (userIndex < alUserIds.Count)) |
2666 |
if(userId != 0 && dictUserIdToIndex.ContainsKey(userId)) |
2667 |
{ |
2668 |
//Int64 userId = alUserIds[userIndex]; |
2669 |
controller.UpdateAvatar(userId); |
2670 |
} |
2671 |
} |
2672 |
} |
2673 |
|
2674 |
// check for gestures |
2675 |
foreach(Int64 userId in alUserIds) |
2676 |
{ |
2677 |
if(!playerGesturesData.ContainsKey(userId)) |
2678 |
continue; |
2679 |
|
2680 |
// Check for player's gestures |
2681 |
CheckForGestures(userId); |
2682 |
|
2683 |
// Check for complete gestures |
2684 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData[userId]; |
2685 |
int userIndex = GetUserIndexById(userId); |
2686 |
|
2687 |
//foreach(KinectGestures.GestureData gestureData in gesturesData) |
2688 |
for(int g = 0; g < gesturesData.Count; g++) |
2689 |
{ |
2690 |
KinectGestures.GestureData gestureData = gesturesData[g]; |
2691 |
|
2692 |
if(gestureData.complete) |
2693 |
{ |
2694 |
// if(gestureData.gesture == KinectGestures.Gestures.Click) |
2695 |
// { |
2696 |
// if(controlMouseCursor) |
2697 |
// { |
2698 |
// MouseControl.MouseClick(); |
2699 |
// } |
2700 |
// } |
2701 |
|
2702 |
foreach(KinectGestures.GestureListenerInterface listener in gestureListeners) |
2703 |
{ |
2704 |
if(listener != null && listener.GestureCompleted(userId, userIndex, gestureData.gesture, (KinectInterop.JointType)gestureData.joint, gestureData.screenPos)) |
2705 |
{ |
2706 |
ResetPlayerGestures(userId); |
2707 |
} |
2708 |
} |
2709 |
} |
2710 |
else if(gestureData.cancelled) |
2711 |
{ |
2712 |
foreach(KinectGestures.GestureListenerInterface listener in gestureListeners) |
2713 |
{ |
2714 |
if(listener != null && listener.GestureCancelled(userId, userIndex, gestureData.gesture, (KinectInterop.JointType)gestureData.joint)) |
2715 |
{ |
2716 |
ResetGesture(userId, gestureData.gesture); |
2717 |
} |
2718 |
} |
2719 |
} |
2720 |
else if(gestureData.progress >= 0.1f) |
2721 |
{ |
2722 |
// if((gestureData.gesture == KinectGestures.Gestures.RightHandCursor || |
2723 |
// gestureData.gesture == KinectGestures.Gestures.LeftHandCursor) && |
2724 |
// gestureData.progress >= 0.5f) |
2725 |
// { |
2726 |
// if(handCursor != null) |
2727 |
// { |
2728 |
// handCursor.transform.position = Vector3.Lerp(handCursor.transform.position, gestureData.screenPos, 3 * Time.deltaTime); |
2729 |
// } |
2730 |
// |
2731 |
// if(controlMouseCursor) |
2732 |
// { |
2733 |
// MouseControl.MouseMove(gestureData.screenPos); |
2734 |
// } |
2735 |
// } |
2736 |
|
2737 |
foreach(KinectGestures.GestureListenerInterface listener in gestureListeners) |
2738 |
{ |
2739 |
if(listener != null) |
2740 |
{ |
2741 |
listener.GestureInProgress(userId, userIndex, gestureData.gesture, gestureData.progress, |
2742 |
(KinectInterop.JointType)gestureData.joint, gestureData.screenPos); |
2743 |
} |
2744 |
} |
2745 |
} |
2746 |
|
2747 |
//gesturesData[g] = gestureData; |
2748 |
} |
2749 |
} |
2750 |
|
2751 |
} |
2752 |
} |
2753 |
|
2754 |
void LateUpdate() |
2755 |
{ |
2756 |
// late update the avatars |
2757 |
if(lateUpdateAvatars) |
2758 |
{ |
2759 |
foreach (AvatarController controller in avatarControllers) |
2760 |
{ |
2761 |
//int userIndex = controller ? controller.playerIndex : -1; |
2762 |
Int64 userId = controller ? controller.playerId : 0; |
2763 |
|
2764 |
//if((userIndex >= 0) && (userIndex < alUserIds.Count)) |
2765 |
if(userId != 0 && dictUserIdToIndex.ContainsKey(userId)) |
2766 |
{ |
2767 |
//Int64 userId = alUserIds[userIndex]; |
2768 |
controller.UpdateAvatar(userId); |
2769 |
} |
2770 |
} |
2771 |
} |
2772 |
} |
2773 |
|
2774 |
// Update the color image |
2775 |
void UpdateColorMap() |
2776 |
{ |
2777 |
//usersClrTex.LoadRawTextureData(sensorData.colorImage); |
2778 |
|
2779 |
if(sensorData != null && sensorData.sensorInterface != null && sensorData.colorImageTexture != null) |
2780 |
{ |
2781 |
if(sensorData.sensorInterface.IsFaceTrackingActive() && |
2782 |
sensorData.sensorInterface.IsDrawFaceRect()) |
2783 |
{ |
2784 |
// visualize face tracker (face rectangles) |
2785 |
//sensorData.sensorInterface.VisualizeFaceTrackerOnColorTex(usersClrTex); |
2786 |
sensorData.sensorInterface.VisualizeFaceTrackerOnColorTex(sensorData.colorImageTexture); |
2787 |
sensorData.colorImageTexture.Apply(); |
2788 |
} |
2789 |
} |
2790 |
|
2791 |
//usersClrTex.Apply(); |
2792 |
} |
2793 |
|
2794 |
// Update the user histogram |
2795 |
void UpdateUserMap(UserMapType userMapType) |
2796 |
{ |
2797 |
if(sensorData != null && sensorData.sensorInterface != null && |
2798 |
!sensorData.sensorInterface.IsBackgroundRemovalActive()) |
2799 |
{ |
2800 |
if(!KinectInterop.IsDirectX11Available()) |
2801 |
{ |
2802 |
if(userMapType != UserMapType.RawUserDepth) |
2803 |
{ |
2804 |
UpdateUserHistogramImage(userMapType); |
2805 |
usersLblTex.SetPixels32(usersHistogramImage); |
2806 |
} |
2807 |
} |
2808 |
else |
2809 |
{ |
2810 |
if(userMapType == UserMapType.CutOutTexture) |
2811 |
{ |
2812 |
if(!sensorData.color2DepthTexture && sensorData.depth2ColorTexture && |
2813 |
KinectInterop.RenderDepth2ColorTex(sensorData)) |
2814 |
{ |
2815 |
KinectInterop.RenderTex2Tex2D(sensorData.depth2ColorTexture, ref usersLblTex); |
2816 |
} |
2817 |
else if(!sensorData.color2DepthTexture) |
2818 |
{ |
2819 |
KinectInterop.RenderTex2Tex2D(sensorData.bodyIndexTexture, ref usersLblTex); |
2820 |
} |
2821 |
} |
2822 |
else if(userMapType == UserMapType.BodyTexture && sensorData.bodyIndexTexture) |
2823 |
{ |
2824 |
KinectInterop.RenderTex2Tex2D(sensorData.bodyIndexTexture, ref usersLblTex); |
2825 |
} |
2826 |
else if(userMapType == UserMapType.UserTexture && sensorData.depthImageTexture) |
2827 |
{ |
2828 |
KinectInterop.RenderTex2Tex2D(sensorData.depthImageTexture, ref usersLblTex); |
2829 |
} |
2830 |
} |
2831 |
|
2832 |
if(userMapType != UserMapType.RawUserDepth) |
2833 |
{ |
2834 |
// draw skeleton lines |
2835 |
if(displaySkeletonLines) |
2836 |
{ |
2837 |
for(int i = 0; i < alUserIds.Count; i++) |
2838 |
{ |
2839 |
Int64 liUserId = alUserIds[i]; |
2840 |
int index = dictUserIdToIndex[liUserId]; |
2841 |
|
2842 |
if(index >= 0 && index < sensorData.bodyCount) |
2843 |
{ |
2844 |
DrawSkeleton(usersLblTex, ref bodyFrame.bodyData[index]); |
2845 |
} |
2846 |
} |
2847 |
} |
2848 |
|
2849 |
usersLblTex.Apply(); |
2850 |
} |
2851 |
} |
2852 |
} |
2853 |
|
2854 |
// Update the user infrared map |
2855 |
void UpdateInfraredMap() |
2856 |
{ |
2857 |
// does nothing at the moment |
2858 |
} |
2859 |
|
2860 |
// Update the user histogram map |
2861 |
void UpdateUserHistogramImage(UserMapType userMapType) |
2862 |
{ |
2863 |
int numOfPoints = 0; |
2864 |
Array.Clear(usersHistogramMap, 0, usersHistogramMap.Length); |
2865 |
|
2866 |
// Calculate cumulative histogram for depth |
2867 |
for (int i = 0; i < usersMapSize; i++) |
2868 |
{ |
2869 |
// Only calculate for depth that contains users |
2870 |
if (sensorData.bodyIndexImage[i] != 255) |
2871 |
{ |
2872 |
ushort depth = sensorData.depthImage[i]; |
2873 |
if(depth > 5000) |
2874 |
depth = 5000; |
2875 |
|
2876 |
usersHistogramMap[depth]++; |
2877 |
numOfPoints++; |
2878 |
} |
2879 |
} |
2880 |
|
2881 |
if (numOfPoints > 0) |
2882 |
{ |
2883 |
for (int i = 1; i < usersHistogramMap.Length; i++) |
2884 |
{ |
2885 |
usersHistogramMap[i] += usersHistogramMap[i - 1]; |
2886 |
} |
2887 |
|
2888 |
for (int i = 0; i < usersHistogramMap.Length; i++) |
2889 |
{ |
2890 |
usersHistogramMap[i] = 1.0f - (usersHistogramMap[i] / numOfPoints); |
2891 |
} |
2892 |
} |
2893 |
|
2894 |
//List<int> alTrackedIndexes = new List<int>(dictUserIdToIndex.Values); |
2895 |
byte btSelBI = sensorData.selectedBodyIndex; |
2896 |
Color32 clrClear = Color.clear; |
2897 |
|
2898 |
// convert the body indices to string |
2899 |
string sTrackedIndices = string.Empty; |
2900 |
|
2901 |
if (bLimitedUsers) |
2902 |
{ |
2903 |
foreach(int bodyIndex in dictUserIdToIndex.Values) |
2904 |
{ |
2905 |
sTrackedIndices += (char)(0x30 + bodyIndex); |
2906 |
} |
2907 |
} |
2908 |
|
2909 |
// Create the actual users texture based on label map and depth histogram |
2910 |
for (int i = 0; i < usersMapSize; i++) |
2911 |
{ |
2912 |
ushort userMap = sensorData.bodyIndexImage[i]; |
2913 |
ushort userDepth = sensorData.depthImage[i]; |
2914 |
|
2915 |
if(userDepth > 5000) |
2916 |
userDepth = 5000; |
2917 |
|
2918 |
ushort nowUserPixel = userMap != 255 ? (ushort)((userMap << 13) | userDepth) : userDepth; |
2919 |
ushort wasUserPixel = usersPrevState[i]; |
2920 |
|
2921 |
// draw only the changed pixels |
2922 |
if(nowUserPixel != wasUserPixel) |
2923 |
{ |
2924 |
usersPrevState[i] = nowUserPixel; |
2925 |
|
2926 |
bool bUserTracked = btSelBI != 255 ? btSelBI == (byte)userMap : |
2927 |
//(bLimitedUsers ? alTrackedIndexes.Contains(userMap): userMap != 255); |
2928 |
(bLimitedUsers ? sTrackedIndices.IndexOf((char)(0x30 + userMap)) >= 0 : userMap != 255); |
2929 |
|
2930 |
if(!bUserTracked) |
2931 |
{ |
2932 |
usersHistogramImage[i] = clrClear; |
2933 |
} |
2934 |
else |
2935 |
{ |
2936 |
if(userMapType == UserMapType.CutOutTexture && sensorData.colorImage != null) |
2937 |
{ |
2938 |
Vector2 vColorPos = Vector2.zero; |
2939 |
|
2940 |
if(sensorData.depth2ColorCoords != null) |
2941 |
{ |
2942 |
vColorPos = sensorData.depth2ColorCoords[i]; |
2943 |
} |
2944 |
else |
2945 |
{ |
2946 |
Vector2 vDepthPos = Vector2.zero; |
2947 |
vDepthPos.x = i % sensorData.depthImageWidth; |
2948 |
vDepthPos.y = i / sensorData.depthImageWidth; |
2949 |
|
2950 |
vColorPos = KinectInterop.MapDepthPointToColorCoords(sensorData, vDepthPos, userDepth); |
2951 |
} |
2952 |
|
2953 |
if(!float.IsInfinity(vColorPos.x) && !float.IsInfinity(vColorPos.y)) |
2954 |
{ |
2955 |
int cx = (int)vColorPos.x; |
2956 |
int cy = (int)vColorPos.y; |
2957 |
int colorIndex = cx + cy * sensorData.colorImageWidth; |
2958 |
|
2959 |
if(colorIndex >= 0 && colorIndex < usersClrSize) |
2960 |
{ |
2961 |
int ci = colorIndex << 2; |
2962 |
Color32 colorPixel = new Color32(sensorData.colorImage[ci], sensorData.colorImage[ci + 1], sensorData.colorImage[ci + 2], 255); |
2963 |
|
2964 |
usersHistogramImage[i] = colorPixel; |
2965 |
} |
2966 |
} |
2967 |
} |
2968 |
else |
2969 |
{ |
2970 |
// Create a blending color based on the depth histogram |
2971 |
float histDepth = usersHistogramMap[userDepth]; |
2972 |
Color c = new Color(histDepth, histDepth, histDepth, 0.9f); |
2973 |
|
2974 |
switch(userMap % 4) |
2975 |
{ |
2976 |
case 0: |
2977 |
usersHistogramImage[i] = Color.red * c; |
2978 |
break; |
2979 |
case 1: |
2980 |
usersHistogramImage[i] = Color.green * c; |
2981 |
break; |
2982 |
case 2: |
2983 |
usersHistogramImage[i] = Color.blue * c; |
2984 |
break; |
2985 |
case 3: |
2986 |
usersHistogramImage[i] = Color.magenta * c; |
2987 |
break; |
2988 |
} |
2989 |
} |
2990 |
} |
2991 |
|
2992 |
} |
2993 |
} |
2994 |
|
2995 |
} |
2996 |
|
2997 |
// Processes body frame data |
2998 |
private void ProcessBodyFrameData() |
2999 |
{ |
3000 |
List<Int64> addedUsers = new List<Int64>(); |
3001 |
List<int> addedIndexes = new List<int>(); |
3002 |
|
3003 |
List<Int64> lostUsers = new List<Int64>(); |
3004 |
lostUsers.AddRange(alUserIds); |
3005 |
|
3006 |
if((autoHeightAngle == AutoHeightAngle.ShowInfoOnly || autoHeightAngle == AutoHeightAngle.AutoUpdateAndShowInfo) && |
3007 |
(sensorData.sensorHgtDetected != 0f || sensorData.sensorRotDetected.eulerAngles.x != 0f) && |
3008 |
calibrationText != null) |
3009 |
{ |
3010 |
float angle = sensorData.sensorRotDetected.eulerAngles.x; |
3011 |
angle = angle > 180f ? (angle - 360f) : angle; |
3012 |
|
3013 |
calibrationText.text = string.Format("Sensor Height: {0:F2} m, Angle: {1:F0} deg", sensorData.sensorHgtDetected, -angle); |
3014 |
} |
3015 |
|
3016 |
if((autoHeightAngle == AutoHeightAngle.AutoUpdate || autoHeightAngle == AutoHeightAngle.AutoUpdateAndShowInfo) && |
3017 |
(sensorData.sensorHgtDetected != 0f || sensorData.sensorRotDetected.eulerAngles.x != 0f)) |
3018 |
{ |
3019 |
float angle = sensorData.sensorRotDetected.eulerAngles.x; |
3020 |
angle = angle > 180f ? (angle - 360f) : angle; |
3021 |
sensorAngle = -angle; |
3022 |
|
3023 |
float height = sensorData.sensorHgtDetected > 0f ? sensorData.sensorHgtDetected : sensorHeight; |
3024 |
sensorHeight = height; |
3025 |
|
3026 |
// update the kinect to world matrix |
3027 |
// Quaternion quatTiltAngle = Quaternion.Euler(-sensorAngle, 0.0f, 0.0f); |
3028 |
// kinectToWorld.SetTRS(new Vector3(0.0f, sensorHeight, 0.0f), quatTiltAngle, Vector3.one); |
3029 |
UpdateKinectToWorldMatrix(); |
3030 |
} |
3031 |
|
3032 |
//int trackedUsers = 0; |
3033 |
|
3034 |
for(int i = 0; i < sensorData.bodyCount; i++) |
3035 |
{ |
3036 |
KinectInterop.BodyData bodyData = bodyFrame.bodyData[i]; |
3037 |
Int64 userId = bodyData.liTrackingID; |
3038 |
|
3039 |
if(bodyData.bIsTracked != 0 && Mathf.Abs(bodyData.position.z) >= minUserDistance && |
3040 |
(maxUserDistance < 0.51f || Mathf.Abs(bodyData.position.z) <= maxUserDistance) && |
3041 |
(maxLeftRightDistance < 0.01f || Mathf.Abs(bodyData.position.x) <= maxLeftRightDistance)) |
3042 |
// && (maxTrackedUsers < 0 || trackedUsers < maxTrackedUsers)) |
3043 |
{ |
3044 |
// get the body position |
3045 |
Vector3 bodyPos = bodyData.position; |
3046 |
|
3047 |
// if(liPrimaryUserId == 0) |
3048 |
// { |
3049 |
// // check if this is the closest user |
3050 |
// bool bClosestUser = true; |
3051 |
// int iClosestUserIndex = i; |
3052 |
// |
3053 |
// if(detectClosestUser) |
3054 |
// { |
3055 |
// for(int j = 0; j < sensorData.bodyCount; j++) |
3056 |
// { |
3057 |
// if(j != i) |
3058 |
// { |
3059 |
// KinectInterop.BodyData bodyDataOther = bodyFrame.bodyData[j]; |
3060 |
// |
3061 |
// if((bodyDataOther.bIsTracked != 0) && |
3062 |
// (Mathf.Abs(bodyDataOther.position.z) < Mathf.Abs(bodyPos.z))) |
3063 |
// { |
3064 |
// bClosestUser = false; |
3065 |
// iClosestUserIndex = j; |
3066 |
// break; |
3067 |
// } |
3068 |
// } |
3069 |
// } |
3070 |
// } |
3071 |
// |
3072 |
// if(bClosestUser) |
3073 |
// { |
3074 |
// // add the first or closest userId to the list of new users |
3075 |
// if(!addedUsers.Contains(userId)) |
3076 |
// { |
3077 |
// addedUsers.Insert(0, userId); |
3078 |
// addedIndexes.Insert(0, iClosestUserIndex); |
3079 |
// trackedUsers++; |
3080 |
// } |
3081 |
// } |
3082 |
// } |
3083 |
|
3084 |
// add userId to the list of new users |
3085 |
if(!addedUsers.Contains(userId)) |
3086 |
{ |
3087 |
addedUsers.Add(userId); |
3088 |
addedIndexes.Add(i); |
3089 |
//trackedUsers++; |
3090 |
} |
3091 |
|
3092 |
// convert Kinect positions to world positions |
3093 |
bodyFrame.bodyData[i].position = bodyPos; |
3094 |
//string debugText = String.Empty; |
3095 |
|
3096 |
// process special cases |
3097 |
ProcessBodySpecialData(ref bodyData); |
3098 |
|
3099 |
////// turnaround mode start |
3100 |
// determine if the user is turned around |
3101 |
//float bodyTurnAngle = 0f; |
3102 |
//float neckTiltAngle = 0f; |
3103 |
|
3104 |
if(allowTurnArounds && // sensorData.sensorInterface.IsFaceTrackingActive() && |
3105 |
bodyData.joint[(int)KinectInterop.JointType.Neck].trackingState != KinectInterop.TrackingState.NotTracked) |
3106 |
{ |
3107 |
//bodyTurnAngle = bodyData.bodyTurnAngle > 180f ? bodyData.bodyTurnAngle - 360f : bodyData.bodyTurnAngle; |
3108 |
//neckTiltAngle = Vector3.Angle(Vector3.up, bodyData.joint[(int)KinectInterop.JointType.Neck].direction.normalized); |
3109 |
|
3110 |
//if(neckTiltAngle < 20f) |
3111 |
{ |
3112 |
bool bTurnedAround = sensorData.sensorInterface.IsBodyTurned(ref bodyData); |
3113 |
|
3114 |
if(bTurnedAround && bodyData.turnAroundFactor < 1f) |
3115 |
{ |
3116 |
bodyData.turnAroundFactor += 5f * Time.deltaTime; |
3117 |
if(bodyData.turnAroundFactor > 1f) |
3118 |
bodyData.turnAroundFactor = 1f; |
3119 |
} |
3120 |
else if(!bTurnedAround && bodyData.turnAroundFactor > 0f) |
3121 |
{ |
3122 |
bodyData.turnAroundFactor -= 5f * Time.deltaTime; |
3123 |
if(bodyData.turnAroundFactor < 0f) |
3124 |
bodyData.turnAroundFactor = 0f; |
3125 |
} |
3126 |
|
3127 |
bodyData.isTurnedAround = (bodyData.turnAroundFactor >= 1f) ? true : (bodyData.turnAroundFactor <= 0f ? false : bodyData.isTurnedAround); |
3128 |
//bodyData.isTurnedAround = bTurnedAround; // false; |
3129 |
|
3130 |
// RaiseHandListener handListener = RaiseHandListener.Instance; |
3131 |
// if(handListener != null) |
3132 |
// { |
3133 |
// if(handListener.IsRaiseRightHand()) |
3134 |
// { |
3135 |
// bodyData.isTurnedAround = true; |
3136 |
// } |
3137 |
// if(handListener.IsRaiseLeftHand()) |
3138 |
// { |
3139 |
// bodyData.isTurnedAround = false; |
3140 |
// } |
3141 |
// } |
3142 |
|
3143 |
if(bodyData.isTurnedAround) |
3144 |
{ |
3145 |
// switch left and right joints |
3146 |
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ShoulderLeft, (int)KinectInterop.JointType.ShoulderRight); |
3147 |
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ElbowLeft, (int)KinectInterop.JointType.ElbowRight); |
3148 |
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.WristLeft, (int)KinectInterop.JointType.WristRight); |
3149 |
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.HandLeft, (int)KinectInterop.JointType.HandRight); |
3150 |
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ThumbLeft, (int)KinectInterop.JointType.ThumbRight); |
3151 |
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.HandTipLeft, (int)KinectInterop.JointType.HandTipRight); |
3152 |
|
3153 |
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.HipLeft, (int)KinectInterop.JointType.HipRight); |
3154 |
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.KneeLeft, (int)KinectInterop.JointType.KneeRight); |
3155 |
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.AnkleLeft, (int)KinectInterop.JointType.AnkleRight); |
3156 |
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.FootLeft, (int)KinectInterop.JointType.FootRight); |
3157 |
|
3158 |
// recalculate the bone dirs and special data |
3159 |
KinectInterop.RecalcBoneDirs(sensorData, ref bodyData); |
3160 |
//ProcessBodySpecialData(ref bodyData); |
3161 |
} |
3162 |
} |
3163 |
} |
3164 |
|
3165 |
// if(allowTurnArounds && calibrationText) |
3166 |
// { |
3167 |
// calibrationText.text = string.Format("{0} - BodyAngle: {1:000}", |
3168 |
// (!bodyData.isTurnedAround ? "FACE" : "BACK"), bodyData.bodyTurnAngle); |
3169 |
// } |
3170 |
|
3171 |
////// turnaround mode end |
3172 |
|
3173 |
// calculate world orientations of the body joints |
3174 |
CalculateJointOrients(ref bodyData); |
3175 |
|
3176 |
if(sensorData != null && sensorData.sensorInterface != null) |
3177 |
{ |
3178 |
// do sensor-specific fixes of joint positions and orientations |
3179 |
sensorData.sensorInterface.FixJointOrientations(sensorData, ref bodyData); |
3180 |
} |
3181 |
|
3182 |
// filter orientation constraints |
3183 |
if(useBoneOrientationConstraints && boneConstraintsFilter != null) |
3184 |
{ |
3185 |
boneConstraintsFilter.Constrain(ref bodyData); |
3186 |
} |
3187 |
|
3188 |
lostUsers.Remove(userId); |
3189 |
bodyFrame.bodyData[i] = bodyData; |
3190 |
dictUserIdToTime[userId] = Time.time; |
3191 |
} |
3192 |
else |
3193 |
{ |
3194 |
// consider body as not tracked |
3195 |
bodyFrame.bodyData[i].bIsTracked = 0; |
3196 |
} |
3197 |
} |
3198 |
|
3199 |
// remove the lost users if any |
3200 |
if(lostUsers.Count > 0) |
3201 |
{ |
3202 |
foreach(Int64 userId in lostUsers) |
3203 |
{ |
3204 |
// prevent user removal upon sporadical tracking failures |
3205 |
if((Time.time - dictUserIdToTime[userId]) > waitTimeBeforeRemove) |
3206 |
{ |
3207 |
RemoveUser(userId); |
3208 |
} |
3209 |
} |
3210 |
|
3211 |
lostUsers.Clear(); |
3212 |
} |
3213 |
|
3214 |
// calibrate newly detected users |
3215 |
if(addedUsers.Count > 0) |
3216 |
{ |
3217 |
for(int i = 0; i < addedUsers.Count; i++) |
3218 |
{ |
3219 |
if (alUserIds.Count < maxTrackedUsers) |
3220 |
{ |
3221 |
Int64 userId = addedUsers[i]; |
3222 |
int userIndex = addedIndexes[i]; |
3223 |
|
3224 |
CalibrateUser(userId, userIndex); |
3225 |
} |
3226 |
} |
3227 |
|
3228 |
addedUsers.Clear(); |
3229 |
addedIndexes.Clear(); |
3230 |
} |
3231 |
} |
3232 |
|
3233 |
// calculates special directions and other useful data out of the body data |
3234 |
private void ProcessBodySpecialData(ref KinectInterop.BodyData bodyData) |
3235 |
{ |
3236 |
if(bodyData.joint[(int)KinectInterop.JointType.HipLeft].trackingState == KinectInterop.TrackingState.NotTracked && |
3237 |
bodyData.joint[(int)KinectInterop.JointType.SpineBase].trackingState != KinectInterop.TrackingState.NotTracked && |
3238 |
bodyData.joint[(int)KinectInterop.JointType.HipRight].trackingState != KinectInterop.TrackingState.NotTracked) |
3239 |
{ |
3240 |
bodyData.joint[(int)KinectInterop.JointType.HipLeft].trackingState = KinectInterop.TrackingState.Inferred; |
3241 |
|
3242 |
bodyData.joint[(int)KinectInterop.JointType.HipLeft].kinectPos = bodyData.joint[(int)KinectInterop.JointType.SpineBase].kinectPos + |
3243 |
(bodyData.joint[(int)KinectInterop.JointType.SpineBase].kinectPos - bodyData.joint[(int)KinectInterop.JointType.HipRight].kinectPos); |
3244 |
bodyData.joint[(int)KinectInterop.JointType.HipLeft].position = bodyData.joint[(int)KinectInterop.JointType.SpineBase].position + |
3245 |
(bodyData.joint[(int)KinectInterop.JointType.SpineBase].position - bodyData.joint[(int)KinectInterop.JointType.HipRight].position); |
3246 |
bodyData.joint[(int)KinectInterop.JointType.HipLeft].direction = bodyData.joint[(int)KinectInterop.JointType.HipLeft].position - |
3247 |
bodyData.joint[(int)KinectInterop.JointType.SpineBase].position; |
3248 |
} |
3249 |
|
3250 |
if(bodyData.joint[(int)KinectInterop.JointType.HipRight].trackingState == KinectInterop.TrackingState.NotTracked && |
3251 |
bodyData.joint[(int)KinectInterop.JointType.SpineBase].trackingState != KinectInterop.TrackingState.NotTracked && |
3252 |
bodyData.joint[(int)KinectInterop.JointType.HipLeft].trackingState != KinectInterop.TrackingState.NotTracked) |
3253 |
{ |
3254 |
bodyData.joint[(int)KinectInterop.JointType.HipRight].trackingState = KinectInterop.TrackingState.Inferred; |
3255 |
|
3256 |
bodyData.joint[(int)KinectInterop.JointType.HipRight].kinectPos = bodyData.joint[(int)KinectInterop.JointType.SpineBase].kinectPos + |
3257 |
(bodyData.joint[(int)KinectInterop.JointType.SpineBase].kinectPos - bodyData.joint[(int)KinectInterop.JointType.HipLeft].kinectPos); |
3258 |
bodyData.joint[(int)KinectInterop.JointType.HipRight].position = bodyData.joint[(int)KinectInterop.JointType.SpineBase].position + |
3259 |
(bodyData.joint[(int)KinectInterop.JointType.SpineBase].position - bodyData.joint[(int)KinectInterop.JointType.HipLeft].position); |
3260 |
bodyData.joint[(int)KinectInterop.JointType.HipRight].direction = bodyData.joint[(int)KinectInterop.JointType.HipRight].position - |
3261 |
bodyData.joint[(int)KinectInterop.JointType.SpineBase].position; |
3262 |
} |
3263 |
|
3264 |
if((bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].trackingState == KinectInterop.TrackingState.NotTracked && |
3265 |
bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].trackingState != KinectInterop.TrackingState.NotTracked && |
3266 |
bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].trackingState != KinectInterop.TrackingState.NotTracked)) |
3267 |
{ |
3268 |
bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].trackingState = KinectInterop.TrackingState.Inferred; |
3269 |
|
3270 |
bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].kinectPos = bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].kinectPos + |
3271 |
(bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].kinectPos - bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].kinectPos); |
3272 |
bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].position = bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].position + |
3273 |
(bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].position - bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].position); |
3274 |
bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].direction = bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].position - |
3275 |
bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].position; |
3276 |
} |
3277 |
|
3278 |
if((bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].trackingState == KinectInterop.TrackingState.NotTracked && |
3279 |
bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].trackingState != KinectInterop.TrackingState.NotTracked && |
3280 |
bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].trackingState != KinectInterop.TrackingState.NotTracked)) |
3281 |
{ |
3282 |
bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].trackingState = KinectInterop.TrackingState.Inferred; |
3283 |
|
3284 |
bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].kinectPos = bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].kinectPos + |
3285 |
(bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].kinectPos - bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].kinectPos); |
3286 |
bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].position = bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].position + |
3287 |
(bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].position - bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].position); |
3288 |
bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].direction = bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].position - |
3289 |
bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].position; |
3290 |
} |
3291 |
|
3292 |
// calculate special directions |
3293 |
if(bodyData.joint[(int)KinectInterop.JointType.HipLeft].trackingState != KinectInterop.TrackingState.NotTracked && |
3294 |
bodyData.joint[(int)KinectInterop.JointType.HipRight].trackingState != KinectInterop.TrackingState.NotTracked) |
3295 |
{ |
3296 |
Vector3 posRHip = bodyData.joint[(int)KinectInterop.JointType.HipRight].position; |
3297 |
Vector3 posLHip = bodyData.joint[(int)KinectInterop.JointType.HipLeft].position; |
3298 |
|
3299 |
bodyData.hipsDirection = posRHip - posLHip; |
3300 |
bodyData.hipsDirection -= Vector3.Project(bodyData.hipsDirection, Vector3.up); |
3301 |
} |
3302 |
|
3303 |
if(bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].trackingState != KinectInterop.TrackingState.NotTracked && |
3304 |
bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].trackingState != KinectInterop.TrackingState.NotTracked) |
3305 |
{ |
3306 |
Vector3 posRShoulder = bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].position; |
3307 |
Vector3 posLShoulder = bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].position; |
3308 |
|
3309 |
bodyData.shouldersDirection = posRShoulder - posLShoulder; |
3310 |
bodyData.shouldersDirection -= Vector3.Project(bodyData.shouldersDirection, Vector3.up); |
3311 |
|
3312 |
Vector3 shouldersDir = bodyData.shouldersDirection; |
3313 |
shouldersDir.z = -shouldersDir.z; |
3314 |
|
3315 |
Quaternion turnRot = Quaternion.FromToRotation(Vector3.right, shouldersDir); |
3316 |
bodyData.bodyTurnAngle = turnRot.eulerAngles.y; |
3317 |
} |
3318 |
|
3319 |
// if(bodyData.joint[(int)KinectInterop.JointType.ElbowLeft].trackingState != KinectInterop.TrackingState.NotTracked && |
3320 |
// bodyData.joint[(int)KinectInterop.JointType.WristLeft].trackingState != KinectInterop.TrackingState.NotTracked) |
3321 |
// { |
3322 |
// Vector3 pos1 = bodyData.joint[(int)KinectInterop.JointType.ElbowLeft].position; |
3323 |
// Vector3 pos2 = bodyData.joint[(int)KinectInterop.JointType.WristLeft].position; |
3324 |
// |
3325 |
// bodyData.leftArmDirection = pos2 - pos1; |
3326 |
// } |
3327 |
|
3328 |
// if(allowHandRotations && bodyData.leftArmDirection != Vector3.zero && |
3329 |
// bodyData.joint[(int)KinectInterop.JointType.WristLeft].trackingState != KinectInterop.TrackingState.NotTracked && |
3330 |
// bodyData.joint[(int)KinectInterop.JointType.ThumbLeft].trackingState != KinectInterop.TrackingState.NotTracked) |
3331 |
// { |
3332 |
// Vector3 pos1 = bodyData.joint[(int)KinectInterop.JointType.WristLeft].position; |
3333 |
// Vector3 pos2 = bodyData.joint[(int)KinectInterop.JointType.ThumbLeft].position; |
3334 |
// |
3335 |
// Vector3 armDir = bodyData.leftArmDirection; |
3336 |
// armDir.z = -armDir.z; |
3337 |
// |
3338 |
// bodyData.leftThumbDirection = pos2 - pos1; |
3339 |
// bodyData.leftThumbDirection.z = -bodyData.leftThumbDirection.z; |
3340 |
// bodyData.leftThumbDirection -= Vector3.Project(bodyData.leftThumbDirection, armDir); |
3341 |
// |
3342 |
// bodyData.leftThumbForward = Quaternion.AngleAxis(bodyData.bodyTurnAngle, Vector3.up) * Vector3.forward; |
3343 |
// bodyData.leftThumbForward -= Vector3.Project(bodyData.leftThumbForward, armDir); |
3344 |
// |
3345 |
// if(bodyData.leftThumbForward.sqrMagnitude < 0.01f) |
3346 |
// { |
3347 |
// bodyData.leftThumbForward = Vector3.zero; |
3348 |
// } |
3349 |
// } |
3350 |
// else |
3351 |
// { |
3352 |
// if(bodyData.leftThumbDirection != Vector3.zero) |
3353 |
// { |
3354 |
// bodyData.leftThumbDirection = Vector3.zero; |
3355 |
// bodyData.leftThumbForward = Vector3.zero; |
3356 |
// } |
3357 |
// } |
3358 |
|
3359 |
// if(bodyData.joint[(int)KinectInterop.JointType.ElbowRight].trackingState != KinectInterop.TrackingState.NotTracked && |
3360 |
// bodyData.joint[(int)KinectInterop.JointType.WristRight].trackingState != KinectInterop.TrackingState.NotTracked) |
3361 |
// { |
3362 |
// Vector3 pos1 = bodyData.joint[(int)KinectInterop.JointType.ElbowRight].position; |
3363 |
// Vector3 pos2 = bodyData.joint[(int)KinectInterop.JointType.WristRight].position; |
3364 |
// |
3365 |
// bodyData.rightArmDirection = pos2 - pos1; |
3366 |
// } |
3367 |
|
3368 |
// if(allowHandRotations && bodyData.rightArmDirection != Vector3.zero && |
3369 |
// bodyData.joint[(int)KinectInterop.JointType.WristRight].trackingState != KinectInterop.TrackingState.NotTracked && |
3370 |
// bodyData.joint[(int)KinectInterop.JointType.ThumbRight].trackingState != KinectInterop.TrackingState.NotTracked) |
3371 |
// { |
3372 |
// Vector3 pos1 = bodyData.joint[(int)KinectInterop.JointType.WristRight].position; |
3373 |
// Vector3 pos2 = bodyData.joint[(int)KinectInterop.JointType.ThumbRight].position; |
3374 |
// |
3375 |
// Vector3 armDir = bodyData.rightArmDirection; |
3376 |
// armDir.z = -armDir.z; |
3377 |
// |
3378 |
// bodyData.rightThumbDirection = pos2 - pos1; |
3379 |
// bodyData.rightThumbDirection.z = -bodyData.rightThumbDirection.z; |
3380 |
// bodyData.rightThumbDirection -= Vector3.Project(bodyData.rightThumbDirection, armDir); |
3381 |
// |
3382 |
// bodyData.rightThumbForward = Quaternion.AngleAxis(bodyData.bodyTurnAngle, Vector3.up) * Vector3.forward; |
3383 |
// bodyData.rightThumbForward -= Vector3.Project(bodyData.rightThumbForward, armDir); |
3384 |
// |
3385 |
// if(bodyData.rightThumbForward.sqrMagnitude < 0.01f) |
3386 |
// { |
3387 |
// bodyData.rightThumbForward = Vector3.zero; |
3388 |
// } |
3389 |
// } |
3390 |
// else |
3391 |
// { |
3392 |
// if(bodyData.rightThumbDirection != Vector3.zero) |
3393 |
// { |
3394 |
// bodyData.rightThumbDirection = Vector3.zero; |
3395 |
// bodyData.rightThumbForward = Vector3.zero; |
3396 |
// } |
3397 |
// } |
3398 |
|
3399 |
if(bodyData.joint[(int)KinectInterop.JointType.KneeLeft].trackingState != KinectInterop.TrackingState.NotTracked && |
3400 |
bodyData.joint[(int)KinectInterop.JointType.AnkleLeft].trackingState != KinectInterop.TrackingState.NotTracked && |
3401 |
bodyData.joint[(int)KinectInterop.JointType.FootLeft].trackingState != KinectInterop.TrackingState.NotTracked) |
3402 |
{ |
3403 |
Vector3 vFootProjected = Vector3.Project(bodyData.joint[(int)KinectInterop.JointType.FootLeft].direction, bodyData.joint[(int)KinectInterop.JointType.AnkleLeft].direction); |
3404 |
|
3405 |
bodyData.joint[(int)KinectInterop.JointType.AnkleLeft].kinectPos += vFootProjected; |
3406 |
bodyData.joint[(int)KinectInterop.JointType.AnkleLeft].position += vFootProjected; |
3407 |
bodyData.joint[(int)KinectInterop.JointType.FootLeft].direction -= vFootProjected; |
3408 |
} |
3409 |
|
3410 |
if(bodyData.joint[(int)KinectInterop.JointType.KneeRight].trackingState != KinectInterop.TrackingState.NotTracked && |
3411 |
bodyData.joint[(int)KinectInterop.JointType.AnkleRight].trackingState != KinectInterop.TrackingState.NotTracked && |
3412 |
bodyData.joint[(int)KinectInterop.JointType.FootRight].trackingState != KinectInterop.TrackingState.NotTracked) |
3413 |
{ |
3414 |
Vector3 vFootProjected = Vector3.Project(bodyData.joint[(int)KinectInterop.JointType.FootRight].direction, bodyData.joint[(int)KinectInterop.JointType.AnkleRight].direction); |
3415 |
|
3416 |
bodyData.joint[(int)KinectInterop.JointType.AnkleRight].kinectPos += vFootProjected; |
3417 |
bodyData.joint[(int)KinectInterop.JointType.AnkleRight].position += vFootProjected; |
3418 |
bodyData.joint[(int)KinectInterop.JointType.FootRight].direction -= vFootProjected; |
3419 |
} |
3420 |
} |
3421 |
|
3422 |
// switches the positional data of two joints |
3423 |
private void SwitchJointsData(ref KinectInterop.BodyData bodyData, int jointL, int jointR) |
3424 |
{ |
3425 |
KinectInterop.TrackingState trackingStateL = bodyData.joint[jointL].trackingState; |
3426 |
Vector3 kinectPosL = bodyData.joint[jointL].kinectPos; |
3427 |
Vector3 positionL = bodyData.joint[jointL].position; |
3428 |
|
3429 |
KinectInterop.TrackingState trackingStateR = bodyData.joint[jointR].trackingState; |
3430 |
Vector3 kinectPosR = bodyData.joint[jointR].kinectPos; |
3431 |
Vector3 positionR = bodyData.joint[jointR].position; |
3432 |
|
3433 |
bodyData.joint[jointL].trackingState = trackingStateR; |
3434 |
bodyData.joint[jointL].kinectPos = kinectPosR; // new Vector3(kinectPosR.x, kinectPosL.y, kinectPosL.z); |
3435 |
bodyData.joint[jointL].position = positionR; // new Vector3(positionR.x, positionL.y, positionL.z); |
3436 |
|
3437 |
bodyData.joint[jointR].trackingState = trackingStateL; |
3438 |
bodyData.joint[jointR].kinectPos = kinectPosL; // new Vector3(kinectPosL.x, kinectPosR.y, kinectPosR.z); |
3439 |
bodyData.joint[jointR].position = positionL; // new Vector3(positionL.x, positionR.y, positionR.z); |
3440 |
} |
3441 |
|
3442 |
/// <summary> |
3443 |
/// Rearranges the user indices, according to the current criteria |
3444 |
/// </summary> |
3445 |
public virtual void RearrangeUserIndices() |
3446 |
{ |
3447 |
|
3448 |
if (userDetectionOrder != UserDetectionOrder.Appearance) |
3449 |
{ |
3450 |
// get current user positions |
3451 |
Vector3[] userPos = new Vector3[aUserIndexIds.Length]; |
3452 |
for (int i = 0; i < aUserIndexIds.Length; i++) |
3453 |
{ |
3454 |
Int64 userId = aUserIndexIds[i]; |
3455 |
userPos[i] = userId != 0 ? GetUserPosition(userId) : Vector3.zero; |
3456 |
} |
3457 |
|
3458 |
// bubble sort |
3459 |
bool reorderDone = false; |
3460 |
for(int i = aUserIndexIds.Length - 1; i >= 1; i--) |
3461 |
{ |
3462 |
bool switchDone = false; |
3463 |
|
3464 |
for (int j = 0; j < i; j++) |
3465 |
{ |
3466 |
float userDist1 = 0f; |
3467 |
if(userDetectionOrder == UserDetectionOrder.Distance) |
3468 |
userDist1 = Mathf.Abs(userPos[j].x) + Mathf.Abs(userPos[j].z); |
3469 |
else if(userDetectionOrder == UserDetectionOrder.LeftToRight) |
3470 |
userDist1 = userPos[j].x; |
3471 |
|
3472 |
if (Mathf.Abs (userDist1) < 0.01f) |
3473 |
userDist1 = 1000f; // far away |
3474 |
|
3475 |
float userDist2 = 0f; |
3476 |
if(userDetectionOrder == UserDetectionOrder.Distance) |
3477 |
userDist2 = Mathf.Abs(userPos[j + 1].x) + Mathf.Abs(userPos[j + 1].z); |
3478 |
else if(userDetectionOrder == UserDetectionOrder.LeftToRight) |
3479 |
userDist2 = userPos[j + 1].x; |
3480 |
|
3481 |
if (Mathf.Abs (userDist2) < 0.01f) |
3482 |
userDist2 = 1000f; // far away |
3483 |
|
3484 |
if (userDist1 > userDist2) |
3485 |
{ |
3486 |
// switch them |
3487 |
Int64 tmpUserId = aUserIndexIds[j]; |
3488 |
aUserIndexIds[j] = aUserIndexIds[j + 1]; |
3489 |
aUserIndexIds[j + 1] = tmpUserId; |
3490 |
|
3491 |
reorderDone = switchDone = true; |
3492 |
} |
3493 |
} |
3494 |
|
3495 |
if (!switchDone) // check for sorted array |
3496 |
break; |
3497 |
} |
3498 |
|
3499 |
if (reorderDone) |
3500 |
{ |
3501 |
System.Text.StringBuilder sbUsersOrder = new System.Text.StringBuilder(); |
3502 |
sbUsersOrder.Append("Users reindexed: "); |
3503 |
|
3504 |
for (int i = 0; i < aUserIndexIds.Length; i++) |
3505 |
{ |
3506 |
if (aUserIndexIds [i] != 0) |
3507 |
{ |
3508 |
sbUsersOrder.Append(i).Append(":").Append(aUserIndexIds[i]).Append(" "); |
3509 |
} |
3510 |
} |
3511 |
|
3512 |
Debug.Log(sbUsersOrder.ToString()); |
3513 |
} |
3514 |
} |
3515 |
|
3516 |
} |
3517 |
|
3518 |
// Returns empty user slot for the given user Id |
3519 |
protected virtual int GetEmptyUserSlot(Int64 userId, int bodyIndex) |
3520 |
{ |
3521 |
// rearrange current users |
3522 |
RearrangeUserIndices(); |
3523 |
int uidIndex = -1; |
3524 |
|
3525 |
if (userDetectionOrder != UserDetectionOrder.Appearance) |
3526 |
{ |
3527 |
// add the new user, depending on the distance |
3528 |
Vector3 userPos = bodyFrame.bodyData[bodyIndex].position; |
3529 |
|
3530 |
float userDist = 0f; |
3531 |
if(userDetectionOrder == UserDetectionOrder.Distance) |
3532 |
userDist = Mathf.Abs(userPos.x) + Mathf.Abs(userPos.z); |
3533 |
else if(userDetectionOrder == UserDetectionOrder.LeftToRight) |
3534 |
userDist = userPos.x; |
3535 |
|
3536 |
for(int i = 0; i < aUserIndexIds.Length; i++) |
3537 |
{ |
3538 |
if(aUserIndexIds[i] == 0) |
3539 |
{ |
3540 |
// free user slot |
3541 |
uidIndex = i; |
3542 |
break; |
3543 |
} |
3544 |
else |
3545 |
{ |
3546 |
Int64 uidUserId = aUserIndexIds[i]; |
3547 |
Vector3 uidUserPos = GetUserPosition (uidUserId); |
3548 |
|
3549 |
float uidUserDist = 0; |
3550 |
if(userDetectionOrder == UserDetectionOrder.Distance) |
3551 |
uidUserDist = Mathf.Abs(uidUserPos.x) + Mathf.Abs(uidUserPos.z); |
3552 |
else if(userDetectionOrder == UserDetectionOrder.LeftToRight) |
3553 |
uidUserDist = uidUserPos.x; |
3554 |
|
3555 |
if(userDist <= uidUserDist) |
3556 |
{ |
3557 |
// current user is left to the compared one |
3558 |
for(int u = (aUserIndexIds.Length - 2); u >= i ; u--) |
3559 |
{ |
3560 |
aUserIndexIds[u + 1] = aUserIndexIds[u]; |
3561 |
|
3562 |
if (aUserIndexIds[u] != 0) |
3563 |
{ |
3564 |
Debug.Log(string.Format("Reindexing user {0} to {1}, ID: {2}.", u, u + 1, aUserIndexIds[u])); |
3565 |
} |
3566 |
} |
3567 |
|
3568 |
aUserIndexIds[i] = 0; // cleanup current index |
3569 |
uidIndex = i; |
3570 |
break; |
3571 |
} |
3572 |
} |
3573 |
} |
3574 |
|
3575 |
} |
3576 |
else |
3577 |
{ |
3578 |
// look for the 1st available slot |
3579 |
for(int i = 0; i < aUserIndexIds.Length; i++) |
3580 |
{ |
3581 |
if(aUserIndexIds[i] == 0) |
3582 |
{ |
3583 |
uidIndex = i; |
3584 |
break; |
3585 |
} |
3586 |
} |
3587 |
|
3588 |
// for(int i = aUserIndexIds.Length - 1; i >= 0; i--) |
3589 |
// { |
3590 |
// if(aUserIndexIds[i] == 0) |
3591 |
// { |
3592 |
// uidIndex = i; |
3593 |
// } |
3594 |
// else if(uidIndex >= 0) |
3595 |
// { |
3596 |
// break; |
3597 |
// } |
3598 |
// } |
3599 |
} |
3600 |
|
3601 |
return uidIndex; |
3602 |
} |
3603 |
|
3604 |
// releases the user slot. rearranges the remaining users. |
3605 |
protected virtual void FreeEmptyUserSlot(int uidIndex) |
3606 |
{ |
3607 |
aUserIndexIds[uidIndex] = 0; |
3608 |
|
3609 |
if (userDetectionOrder != UserDetectionOrder.Appearance) |
3610 |
{ |
3611 |
// rearrange the remaining users |
3612 |
for(int u = uidIndex; u < (aUserIndexIds.Length - 1); u++) |
3613 |
{ |
3614 |
aUserIndexIds[u] = aUserIndexIds[u + 1]; |
3615 |
|
3616 |
if (aUserIndexIds[u + 1] != 0) |
3617 |
{ |
3618 |
Debug.Log(string.Format("Reindexing user {0} to {1}, ID: {2}.", u + 1, u, aUserIndexIds[u + 1])); |
3619 |
} |
3620 |
} |
3621 |
|
3622 |
// make sure the last slot is free |
3623 |
aUserIndexIds[aUserIndexIds.Length - 1] = 0; |
3624 |
} |
3625 |
|
3626 |
// rearrange the remaining users |
3627 |
RearrangeUserIndices(); |
3628 |
} |
3629 |
|
3630 |
// Adds UserId to the list of users |
3631 |
protected virtual void CalibrateUser(Int64 userId, int bodyIndex) |
3632 |
{ |
3633 |
if(!alUserIds.Contains(userId)) |
3634 |
{ |
3635 |
if(CheckForCalibrationPose(userId, bodyIndex, playerCalibrationPose)) |
3636 |
{ |
3637 |
//int uidIndex = alUserIds.Count; |
3638 |
int uidIndex = GetEmptyUserSlot(userId, bodyIndex); |
3639 |
|
3640 |
if(uidIndex >= 0) |
3641 |
{ |
3642 |
aUserIndexIds[uidIndex] = userId; |
3643 |
} |
3644 |
else |
3645 |
{ |
3646 |
// no empty user-index slot |
3647 |
return; |
3648 |
} |
3649 |
|
3650 |
Debug.Log("Adding user " + uidIndex + ", ID: " + userId + ", Body: " + bodyIndex); |
3651 |
|
3652 |
dictUserIdToIndex[userId] = bodyIndex; |
3653 |
dictUserIdToTime[userId] = Time.time; |
3654 |
alUserIds.Add(userId); |
3655 |
|
3656 |
// set primary user-id, if there is none |
3657 |
if(liPrimaryUserId == 0 && aUserIndexIds.Length > 0) |
3658 |
{ |
3659 |
liPrimaryUserId = aUserIndexIds[0]; // userId |
3660 |
|
3661 |
if(liPrimaryUserId != 0) |
3662 |
{ |
3663 |
if(calibrationText != null && calibrationText.text != "") |
3664 |
{ |
3665 |
calibrationText.text = ""; |
3666 |
} |
3667 |
} |
3668 |
} |
3669 |
|
3670 |
// calibrates the respective avatar controllers |
3671 |
for(int i = 0; i < avatarControllers.Count; i++) |
3672 |
{ |
3673 |
AvatarController avatar = avatarControllers[i]; |
3674 |
|
3675 |
//if(avatar && avatar.playerIndex == uidIndex) |
3676 |
if(avatar && avatar.playerIndex == uidIndex && avatar.playerId == 0) |
3677 |
{ |
3678 |
avatar.playerId = userId; |
3679 |
avatar.SuccessfulCalibration(userId, false); |
3680 |
} |
3681 |
} |
3682 |
|
3683 |
// add the gestures to be detected by all users, if any |
3684 |
foreach(KinectGestures.Gestures gesture in playerCommonGestures) |
3685 |
{ |
3686 |
DetectGesture(userId, gesture); |
3687 |
} |
3688 |
|
3689 |
// notify all gesture listeners for the newly detected user |
3690 |
foreach(KinectGestures.GestureListenerInterface listener in gestureListeners) |
3691 |
{ |
3692 |
if(listener != null) |
3693 |
{ |
3694 |
listener.UserDetected(userId, uidIndex); |
3695 |
} |
3696 |
} |
3697 |
|
3698 |
ResetFilters(); |
3699 |
} |
3700 |
} |
3701 |
} |
3702 |
|
3703 |
// Remove a lost UserId |
3704 |
protected virtual void RemoveUser(Int64 userId) |
3705 |
{ |
3706 |
//int uidIndex = alUserIds.IndexOf(userId); |
3707 |
int uidIndex = Array.IndexOf(aUserIndexIds, userId); |
3708 |
Debug.Log("Removing user " + uidIndex + ", ID: " + userId + ", Body: " + dictUserIdToIndex[userId]); |
3709 |
|
3710 |
// reset the respective avatar controllers |
3711 |
for(int i = 0; i < avatarControllers.Count; i++) |
3712 |
{ |
3713 |
AvatarController avatar = avatarControllers[i]; |
3714 |
|
3715 |
//if(avatar && avatar.playerIndex >= uidIndex && avatar.playerIndex < alUserIds.Count) |
3716 |
if(avatar && avatar.playerId == userId) |
3717 |
{ |
3718 |
avatar.ResetToInitialPosition(); |
3719 |
avatar.playerId = 0; |
3720 |
} |
3721 |
} |
3722 |
|
3723 |
// notify all gesture listeners for losing this user |
3724 |
foreach(KinectGestures.GestureListenerInterface listener in gestureListeners) |
3725 |
{ |
3726 |
if(listener != null) |
3727 |
{ |
3728 |
listener.UserLost(userId, uidIndex); |
3729 |
} |
3730 |
} |
3731 |
|
3732 |
// clear gestures list for this user |
3733 |
ClearGestures(userId); |
3734 |
|
3735 |
// clear calibration data for this user |
3736 |
if(playerCalibrationData.ContainsKey(userId)) |
3737 |
{ |
3738 |
playerCalibrationData.Remove(userId); |
3739 |
} |
3740 |
|
3741 |
// clean up the outdated calibration data in the data dictionary |
3742 |
List<Int64> alCalDataKeys = new List<Int64>(playerCalibrationData.Keys); |
3743 |
|
3744 |
foreach(Int64 calUserID in alCalDataKeys) |
3745 |
{ |
3746 |
KinectGestures.GestureData gestureData = playerCalibrationData[calUserID]; |
3747 |
|
3748 |
if((gestureData.timestamp + 60f) < Time.realtimeSinceStartup) |
3749 |
{ |
3750 |
playerCalibrationData.Remove(calUserID); |
3751 |
} |
3752 |
} |
3753 |
|
3754 |
alCalDataKeys.Clear(); |
3755 |
|
3756 |
// remove user-id from the global users lists |
3757 |
dictUserIdToIndex.Remove(userId); |
3758 |
dictUserIdToTime.Remove(userId); |
3759 |
alUserIds.Remove(userId); |
3760 |
|
3761 |
if(uidIndex >= 0) |
3762 |
{ |
3763 |
FreeEmptyUserSlot(uidIndex); |
3764 |
} |
3765 |
|
3766 |
// if this was the primary user, update the primary user-id |
3767 |
if(liPrimaryUserId == userId) |
3768 |
{ |
3769 |
if(aUserIndexIds.Length > 0) |
3770 |
{ |
3771 |
liPrimaryUserId = aUserIndexIds[0]; |
3772 |
} |
3773 |
else |
3774 |
{ |
3775 |
liPrimaryUserId = 0; |
3776 |
} |
3777 |
} |
3778 |
|
3779 |
// for(int i = 0; i < avatarControllers.Count; i++) |
3780 |
// { |
3781 |
// AvatarController avatar = avatarControllers[i]; |
3782 |
// |
3783 |
// if(avatar && avatar.playerIndex >= uidIndex && avatar.playerIndex < alUserIds.Count) |
3784 |
// { |
3785 |
// avatar.SuccessfulCalibration(alUserIds[avatar.playerIndex]); |
3786 |
// } |
3787 |
// } |
3788 |
|
3789 |
if(alUserIds.Count == 0) |
3790 |
{ |
3791 |
Debug.Log("Waiting for users."); |
3792 |
|
3793 |
if(calibrationText != null) |
3794 |
{ |
3795 |
calibrationText.text = "WAITING FOR USERS"; |
3796 |
} |
3797 |
} |
3798 |
} |
3799 |
|
3800 |
// draws the skeleton in the given texture |
3801 |
private void DrawSkeleton(Texture2D aTexture, ref KinectInterop.BodyData bodyData) |
3802 |
{ |
3803 |
int jointsCount = sensorData.jointCount; |
3804 |
|
3805 |
for(int i = 0; i < jointsCount; i++) |
3806 |
{ |
3807 |
int parent = (int)sensorData.sensorInterface.GetParentJoint((KinectInterop.JointType)i); |
3808 |
|
3809 |
if(bodyData.joint[i].trackingState != KinectInterop.TrackingState.NotTracked && |
3810 |
bodyData.joint[parent].trackingState != KinectInterop.TrackingState.NotTracked) |
3811 |
{ |
3812 |
Vector2 posParent = KinectInterop.MapSpacePointToDepthCoords(sensorData, bodyData.joint[parent].kinectPos); |
3813 |
Vector2 posJoint = KinectInterop.MapSpacePointToDepthCoords(sensorData, bodyData.joint[i].kinectPos); |
3814 |
|
3815 |
if(posParent != Vector2.zero && posJoint != Vector2.zero) |
3816 |
{ |
3817 |
//Color lineColor = playerJointsTracked[i] && playerJointsTracked[parent] ? Color.red : Color.yellow; |
3818 |
KinectInterop.DrawLine(aTexture, (int)posParent.x, (int)posParent.y, (int)posJoint.x, (int)posJoint.y, Color.yellow); |
3819 |
} |
3820 |
} |
3821 |
} |
3822 |
|
3823 |
//aTexture.Apply(); |
3824 |
} |
3825 |
|
3826 |
// calculates orientations of the body joints |
3827 |
private void CalculateJointOrients(ref KinectInterop.BodyData bodyData) |
3828 |
{ |
3829 |
int jointCount = bodyData.joint.Length; |
3830 |
|
3831 |
for(int j = 0; j < jointCount; j++) |
3832 |
{ |
3833 |
int joint = j; |
3834 |
|
3835 |
KinectInterop.JointData jointData = bodyData.joint[joint]; |
3836 |
bool bJointValid = ignoreInferredJoints ? jointData.trackingState == KinectInterop.TrackingState.Tracked : jointData.trackingState != KinectInterop.TrackingState.NotTracked; |
3837 |
|
3838 |
if(bJointValid) |
3839 |
{ |
3840 |
int nextJoint = (int)sensorData.sensorInterface.GetNextJoint((KinectInterop.JointType)joint); |
3841 |
if(nextJoint != joint && nextJoint >= 0 && nextJoint < sensorData.jointCount) |
3842 |
{ |
3843 |
KinectInterop.JointData nextJointData = bodyData.joint[nextJoint]; |
3844 |
bool bNextJointValid = ignoreInferredJoints ? nextJointData.trackingState == KinectInterop.TrackingState.Tracked : nextJointData.trackingState != KinectInterop.TrackingState.NotTracked; |
3845 |
|
3846 |
Vector3 baseDir = KinectInterop.JointBaseDir[nextJoint]; |
3847 |
Vector3 jointDir = nextJointData.direction.normalized; |
3848 |
jointDir = new Vector3(jointDir.x, jointDir.y, -jointDir.z).normalized; |
3849 |
|
3850 |
Quaternion jointOrientNormal = jointData.normalRotation; |
3851 |
if(bNextJointValid) |
3852 |
{ |
3853 |
jointOrientNormal = Quaternion.FromToRotation(baseDir, jointDir); |
3854 |
} |
3855 |
|
3856 |
if ((joint == (int)KinectInterop.JointType.ShoulderLeft) || |
3857 |
(joint == (int)KinectInterop.JointType.ShoulderRight)) |
3858 |
{ |
3859 |
float angle = -bodyData.bodyTurnAngle; |
3860 |
Vector3 axis = jointDir; |
3861 |
Quaternion armTurnRotation = Quaternion.AngleAxis (angle, axis); |
3862 |
|
3863 |
jointData.normalRotation = armTurnRotation * jointOrientNormal; |
3864 |
} |
3865 |
else if ((joint == (int)KinectInterop.JointType.ElbowLeft) || |
3866 |
(joint == (int)KinectInterop.JointType.WristLeft) || |
3867 |
(joint == (int)KinectInterop.JointType.HandLeft)) |
3868 |
{ |
3869 |
if (bNextJointValid && jointData.direction != Vector3.zero && jointDir != Vector3.zero) |
3870 |
{ |
3871 |
Vector3 parJointDir = jointData.direction.normalized; |
3872 |
parJointDir = new Vector3 (parJointDir.x, parJointDir.y, -parJointDir.z).normalized; |
3873 |
|
3874 |
if (joint == (int)KinectInterop.JointType.WristLeft) |
3875 |
{ |
3876 |
// for wrist, take the finger direction into account, too |
3877 |
int fingerJoint = (int)sensorData.sensorInterface.GetNextJoint((KinectInterop.JointType)nextJoint); |
3878 |
|
3879 |
if (fingerJoint != joint && fingerJoint >= 0 && fingerJoint < sensorData.jointCount) |
3880 |
{ |
3881 |
KinectInterop.JointData fingerData = bodyData.joint [fingerJoint]; |
3882 |
if (fingerData.trackingState != KinectInterop.TrackingState.NotTracked) |
3883 |
{ |
3884 |
jointDir = (nextJointData.direction + fingerData.direction).normalized; |
3885 |
jointDir = new Vector3 (jointDir.x, jointDir.y, -jointDir.z).normalized; |
3886 |
} |
3887 |
} |
3888 |
} |
3889 |
|
3890 |
float parDotJoint = Vector3.Dot (parJointDir, jointDir); |
3891 |
//Debug.Log (joint + ": " + parDotJoint); |
3892 |
|
3893 |
if (Mathf.Abs(parDotJoint) <= 0.9f || |
3894 |
bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].trackingState == KinectInterop.TrackingState.NotTracked) |
3895 |
{ |
3896 |
Vector3 upDir = -Vector3.Cross (-parJointDir, jointDir).normalized; |
3897 |
Vector3 fwdDir = Vector3.Cross (-jointDir, upDir).normalized; |
3898 |
jointOrientNormal = Quaternion.LookRotation (fwdDir, upDir); |
3899 |
} |
3900 |
else |
3901 |
{ |
3902 |
KinectInterop.JointData shCenterData = bodyData.joint[(int)KinectInterop.JointType.SpineShoulder]; |
3903 |
|
3904 |
Vector3 spineDir = shCenterData.direction.normalized; |
3905 |
spineDir = new Vector3 (spineDir.x, spineDir.y, -spineDir.z).normalized; |
3906 |
|
3907 |
Vector3 fwdDir = Vector3.Cross (-jointDir, spineDir).normalized; |
3908 |
Vector3 upDir = Vector3.Cross (fwdDir, -jointDir).normalized; |
3909 |
jointOrientNormal = Quaternion.LookRotation (fwdDir, upDir); |
3910 |
} |
3911 |
|
3912 |
jointData.normalRotation = jointOrientNormal; |
3913 |
} |
3914 |
|
3915 |
// check for 'allowedHandRotations = None' |
3916 |
bool bRotated = (allowedHandRotations == AllowedRotations.None) && (joint != (int)KinectInterop.JointType.ElbowLeft); |
3917 |
if (bRotated) |
3918 |
{ |
3919 |
// in case of 'allowedHandRotations = None' take the parent's orientation |
3920 |
int prevJoint = (int)sensorData.sensorInterface.GetParentJoint((KinectInterop.JointType)joint); |
3921 |
|
3922 |
if (prevJoint != joint && prevJoint >= 0 && prevJoint < sensorData.jointCount) |
3923 |
{ |
3924 |
jointData.normalRotation = bodyData.joint[prevJoint].normalRotation; |
3925 |
} |
3926 |
} |
3927 |
|
3928 |
if (((allowedHandRotations == AllowedRotations.All && joint != (int)KinectInterop.JointType.ElbowLeft) || |
3929 |
(joint == (int)KinectInterop.JointType.HandLeft && !bRotated)) && |
3930 |
(sensorData.sensorIntPlatform == KinectInterop.DepthSensorPlatform.KinectSDKv2 || |
3931 |
sensorData.sensorIntPlatform == KinectInterop.DepthSensorPlatform.DummyK2)) |
3932 |
{ |
3933 |
KinectInterop.JointData thumbData = bodyData.joint [(int)KinectInterop.JointType.ThumbLeft]; |
3934 |
|
3935 |
int prevJoint = (int)sensorData.sensorInterface.GetParentJoint ((KinectInterop.JointType)joint); |
3936 |
KinectInterop.JointData prevJointData = bodyData.joint [prevJoint]; |
3937 |
|
3938 |
if (thumbData.trackingState != KinectInterop.TrackingState.NotTracked && |
3939 |
prevJointData.trackingState != KinectInterop.TrackingState.NotTracked) |
3940 |
{ |
3941 |
Vector3 rightDir = -jointDir; // -(handData.direction + handTipData.direction); |
3942 |
//rightDir = new Vector3 (rightDir.x, rightDir.y, -rightDir.z).normalized; |
3943 |
|
3944 |
Vector3 fwdDir = thumbData.direction.normalized; |
3945 |
fwdDir = new Vector3 (fwdDir.x, fwdDir.y, -fwdDir.z).normalized; |
3946 |
|
3947 |
if (joint == (int)KinectInterop.JointType.HandLeft) |
3948 |
{ |
3949 |
Vector3 prevBaseDir = -KinectInterop.JointBaseDir[prevJoint]; |
3950 |
Vector3 prevOrthoDir = new Vector3 (prevBaseDir.y, prevBaseDir.z, prevBaseDir.x); |
3951 |
fwdDir = prevJointData.normalRotation * prevOrthoDir; |
3952 |
//rightDir -= Vector3.Project(rightDir, fwdDir); |
3953 |
} |
3954 |
|
3955 |
if (rightDir != Vector3.zero && fwdDir != Vector3.zero) |
3956 |
{ |
3957 |
Vector3 upDir = Vector3.Cross (fwdDir, rightDir).normalized; |
3958 |
fwdDir = Vector3.Cross (rightDir, upDir).normalized; |
3959 |
|
3960 |
//jointData.normalRotation = Quaternion.LookRotation(fwdDir, upDir); |
3961 |
Quaternion jointOrientThumb = Quaternion.LookRotation (fwdDir, upDir); |
3962 |
jointOrientNormal = (joint == (int)KinectInterop.JointType.WristLeft) ? |
3963 |
Quaternion.RotateTowards (prevJointData.normalRotation, jointOrientThumb, 80f) : jointOrientThumb; |
3964 |
|
3965 |
jointData.normalRotation = jointOrientNormal; |
3966 |
//bRotated = true; |
3967 |
} |
3968 |
} |
3969 |
|
3970 |
bRotated = true; |
3971 |
} |
3972 |
|
3973 |
if (joint != (int)KinectInterop.JointType.ElbowLeft) |
3974 |
{ |
3975 |
// limit wrist and hand twist |
3976 |
int prevJoint = (int)sensorData.sensorInterface.GetParentJoint ((KinectInterop.JointType)joint); |
3977 |
KinectInterop.JointData prevJointData = bodyData.joint [prevJoint]; |
3978 |
|
3979 |
if (prevJointData.trackingState != KinectInterop.TrackingState.NotTracked) |
3980 |
{ |
3981 |
jointData.normalRotation = Quaternion.RotateTowards (prevJointData.normalRotation, jointData.normalRotation, 70f); |
3982 |
} |
3983 |
} |
3984 |
|
3985 |
} |
3986 |
else if ((joint == (int)KinectInterop.JointType.ElbowRight) || |
3987 |
(joint == (int)KinectInterop.JointType.WristRight) || |
3988 |
(joint == (int)KinectInterop.JointType.HandRight)) |
3989 |
{ |
3990 |
if (bNextJointValid && jointData.direction != Vector3.zero && jointDir != Vector3.zero) |
3991 |
{ |
3992 |
Vector3 parJointDir = jointData.direction.normalized; |
3993 |
parJointDir = new Vector3 (parJointDir.x, parJointDir.y, -parJointDir.z).normalized; |
3994 |
|
3995 |
if (joint == (int)KinectInterop.JointType.WristRight) |
3996 |
{ |
3997 |
// for wrist, take the finger direction into account, too |
3998 |
int fingerJoint = (int)sensorData.sensorInterface.GetNextJoint((KinectInterop.JointType)nextJoint); |
3999 |
|
4000 |
if (fingerJoint != joint && fingerJoint >= 0 && fingerJoint < sensorData.jointCount) |
4001 |
{ |
4002 |
KinectInterop.JointData fingerData = bodyData.joint [fingerJoint]; |
4003 |
if (fingerData.trackingState != KinectInterop.TrackingState.NotTracked) |
4004 |
{ |
4005 |
jointDir = (nextJointData.direction + fingerData.direction).normalized; |
4006 |
jointDir = new Vector3 (jointDir.x, jointDir.y, -jointDir.z).normalized; |
4007 |
} |
4008 |
} |
4009 |
} |
4010 |
|
4011 |
// estimate the joint orientation |
4012 |
float parDotJoint = Vector3.Dot (parJointDir, jointDir); |
4013 |
//Debug.Log (joint + ": " + parDotJoint); |
4014 |
|
4015 |
if (Mathf.Abs(parDotJoint) <= 0.9f || |
4016 |
bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].trackingState == KinectInterop.TrackingState.NotTracked) |
4017 |
{ |
4018 |
Vector3 upDir = -Vector3.Cross (parJointDir, jointDir).normalized; |
4019 |
Vector3 fwdDir = Vector3.Cross (jointDir, upDir).normalized; |
4020 |
jointOrientNormal = Quaternion.LookRotation (fwdDir, upDir); |
4021 |
} |
4022 |
else |
4023 |
{ |
4024 |
KinectInterop.JointData shCenterData = bodyData.joint[(int)KinectInterop.JointType.SpineShoulder]; |
4025 |
|
4026 |
Vector3 spineDir = shCenterData.direction.normalized; |
4027 |
spineDir = new Vector3 (spineDir.x, spineDir.y, -spineDir.z).normalized; |
4028 |
|
4029 |
Vector3 fwdDir = Vector3.Cross (jointDir, spineDir).normalized; |
4030 |
Vector3 upDir = Vector3.Cross (fwdDir, jointDir).normalized; |
4031 |
jointOrientNormal = Quaternion.LookRotation (fwdDir, upDir); |
4032 |
} |
4033 |
|
4034 |
jointData.normalRotation = jointOrientNormal; |
4035 |
} |
4036 |
|
4037 |
// check for 'allowedHandRotations = None' |
4038 |
bool bRotated = (allowedHandRotations == AllowedRotations.None) && (joint != (int)KinectInterop.JointType.ElbowRight); |
4039 |
if (bRotated) |
4040 |
{ |
4041 |
// in case of 'allowedHandRotations = None' take the parent's orientation |
4042 |
int prevJoint = (int)sensorData.sensorInterface.GetParentJoint((KinectInterop.JointType)joint); |
4043 |
|
4044 |
if (prevJoint != joint && prevJoint >= 0 && prevJoint < sensorData.jointCount) |
4045 |
{ |
4046 |
jointData.normalRotation = bodyData.joint[prevJoint].normalRotation; |
4047 |
} |
4048 |
} |
4049 |
|
4050 |
if (((allowedHandRotations == AllowedRotations.All && joint != (int)KinectInterop.JointType.ElbowRight) || |
4051 |
(joint == (int)KinectInterop.JointType.HandRight && !bRotated)) && |
4052 |
(sensorData.sensorIntPlatform == KinectInterop.DepthSensorPlatform.KinectSDKv2 || |
4053 |
sensorData.sensorIntPlatform == KinectInterop.DepthSensorPlatform.DummyK2)) |
4054 |
{ |
4055 |
KinectInterop.JointData thumbData = bodyData.joint [(int)KinectInterop.JointType.ThumbRight]; |
4056 |
|
4057 |
int prevJoint = (int)sensorData.sensorInterface.GetParentJoint ((KinectInterop.JointType)joint); |
4058 |
KinectInterop.JointData prevJointData = bodyData.joint [prevJoint]; |
4059 |
|
4060 |
if (thumbData.trackingState != KinectInterop.TrackingState.NotTracked && |
4061 |
prevJointData.trackingState != KinectInterop.TrackingState.NotTracked) |
4062 |
{ |
4063 |
Vector3 rightDir = jointDir; // handData.direction + handTipData.direction; |
4064 |
//rightDir = new Vector3 (rightDir.x, rightDir.y, -rightDir.z).normalized; |
4065 |
|
4066 |
Vector3 fwdDir = thumbData.direction.normalized; |
4067 |
fwdDir = new Vector3 (fwdDir.x, fwdDir.y, -fwdDir.z).normalized; |
4068 |
|
4069 |
if (joint == (int)KinectInterop.JointType.HandRight) |
4070 |
{ |
4071 |
Vector3 prevBaseDir = KinectInterop.JointBaseDir[prevJoint]; |
4072 |
Vector3 prevOrthoDir = new Vector3 (prevBaseDir.y, prevBaseDir.z, prevBaseDir.x); |
4073 |
fwdDir = prevJointData.normalRotation * prevOrthoDir; |
4074 |
//rightDir -= Vector3.Project(rightDir, fwdDir); |
4075 |
} |
4076 |
|
4077 |
if (rightDir != Vector3.zero && fwdDir != Vector3.zero) |
4078 |
{ |
4079 |
Vector3 upDir = Vector3.Cross (fwdDir, rightDir).normalized; |
4080 |
fwdDir = Vector3.Cross (rightDir, upDir).normalized; |
4081 |
|
4082 |
Quaternion jointOrientThumb = Quaternion.LookRotation (fwdDir, upDir); |
4083 |
jointOrientNormal = (joint == (int)KinectInterop.JointType.WristRight) ? |
4084 |
Quaternion.RotateTowards (prevJointData.normalRotation, jointOrientThumb, 80f) : jointOrientThumb; |
4085 |
|
4086 |
jointData.normalRotation = jointOrientNormal; |
4087 |
//bRotated = true; |
4088 |
} |
4089 |
} |
4090 |
|
4091 |
bRotated = true; |
4092 |
} |
4093 |
|
4094 |
if (joint != (int)KinectInterop.JointType.ElbowRight) |
4095 |
{ |
4096 |
// limit wrist and hand twist |
4097 |
int prevJoint = (int)sensorData.sensorInterface.GetParentJoint ((KinectInterop.JointType)joint); |
4098 |
KinectInterop.JointData prevJointData = bodyData.joint [prevJoint]; |
4099 |
|
4100 |
if (prevJointData.trackingState != KinectInterop.TrackingState.NotTracked) |
4101 |
{ |
4102 |
jointData.normalRotation = Quaternion.RotateTowards (prevJointData.normalRotation, jointData.normalRotation, 70f); |
4103 |
} |
4104 |
} |
4105 |
|
4106 |
} |
4107 |
else |
4108 |
{ |
4109 |
jointData.normalRotation = jointOrientNormal; |
4110 |
} |
4111 |
|
4112 |
if((joint == (int)KinectInterop.JointType.SpineMid) || |
4113 |
(joint == (int)KinectInterop.JointType.SpineShoulder) || |
4114 |
(joint == (int)KinectInterop.JointType.Neck)) |
4115 |
{ |
4116 |
Vector3 baseDir2 = Vector3.right; |
4117 |
Vector3 jointDir2 = Vector3.Lerp(bodyData.shouldersDirection, -bodyData.shouldersDirection, bodyData.turnAroundFactor); |
4118 |
jointDir2.z = -jointDir2.z; |
4119 |
|
4120 |
jointData.normalRotation *= Quaternion.FromToRotation(baseDir2, jointDir2); |
4121 |
} |
4122 |
else if((joint == (int)KinectInterop.JointType.SpineBase) || |
4123 |
(joint == (int)KinectInterop.JointType.HipLeft) || (joint == (int)KinectInterop.JointType.HipRight) || |
4124 |
(joint == (int)KinectInterop.JointType.KneeLeft) || (joint == (int)KinectInterop.JointType.KneeRight) || |
4125 |
(joint == (int)KinectInterop.JointType.AnkleLeft) || (joint == (int)KinectInterop.JointType.AnkleRight)) |
4126 |
{ |
4127 |
Vector3 baseDir2 = Vector3.right; |
4128 |
Vector3 jointDir2 = Vector3.Lerp(bodyData.hipsDirection, -bodyData.hipsDirection, bodyData.turnAroundFactor); |
4129 |
jointDir2.z = -jointDir2.z; |
4130 |
|
4131 |
jointData.normalRotation *= Quaternion.FromToRotation(baseDir2, jointDir2); |
4132 |
} |
4133 |
|
4134 |
if(joint == (int)KinectInterop.JointType.Neck && |
4135 |
sensorData != null && sensorData.sensorInterface != null) |
4136 |
{ |
4137 |
if(sensorData.sensorInterface.IsFaceTrackingActive() && |
4138 |
sensorData.sensorInterface.IsFaceTracked(bodyData.liTrackingID)) |
4139 |
{ |
4140 |
KinectInterop.JointData neckData = bodyData.joint[(int)KinectInterop.JointType.Neck]; |
4141 |
KinectInterop.JointData headData = bodyData.joint[(int)KinectInterop.JointType.Head]; |
4142 |
|
4143 |
if(neckData.trackingState == KinectInterop.TrackingState.Tracked && |
4144 |
headData.trackingState == KinectInterop.TrackingState.Tracked) |
4145 |
{ |
4146 |
Quaternion headRotation = Quaternion.identity; |
4147 |
if(sensorData.sensorInterface.GetHeadRotation(bodyData.liTrackingID, ref headRotation)) |
4148 |
{ |
4149 |
if (headRotation != Quaternion.identity) |
4150 |
{ |
4151 |
Vector3 rotAngles = headRotation.eulerAngles; |
4152 |
rotAngles.x = -rotAngles.x; |
4153 |
rotAngles.y = -rotAngles.y; |
4154 |
|
4155 |
bodyData.headOrientation = bodyData.headOrientation != Quaternion.identity ? |
4156 |
Quaternion.Slerp(bodyData.headOrientation, Quaternion.Euler(rotAngles), 10f * Time.deltaTime) : |
4157 |
Quaternion.Euler(rotAngles); |
4158 |
|
4159 |
//jointData.normalRotation = bodyData.headOrientation; |
4160 |
} |
4161 |
} |
4162 |
} |
4163 |
} |
4164 |
|
4165 |
if (bodyData.headOrientation != Quaternion.identity) |
4166 |
{ |
4167 |
jointData.normalRotation = bodyData.headOrientation; |
4168 |
} |
4169 |
} |
4170 |
|
4171 |
Vector3 mirroredAngles = jointData.normalRotation.eulerAngles; |
4172 |
mirroredAngles.y = -mirroredAngles.y; |
4173 |
mirroredAngles.z = -mirroredAngles.z; |
4174 |
|
4175 |
jointData.mirroredRotation = Quaternion.Euler(mirroredAngles); |
4176 |
} |
4177 |
else |
4178 |
{ |
4179 |
// get the orientation of the parent joint |
4180 |
int prevJoint = (int)sensorData.sensorInterface.GetParentJoint((KinectInterop.JointType)joint); |
4181 |
if(prevJoint != joint && prevJoint >= 0 && prevJoint < sensorData.jointCount && |
4182 |
joint != (int)KinectInterop.JointType.ThumbLeft && joint != (int)KinectInterop.JointType.ThumbRight) |
4183 |
{ |
4184 |
// if((allowedHandRotations == AllowedRotations.All) && |
4185 |
// (joint == (int)KinectInterop.JointType.ThumbLeft || |
4186 |
// joint == (int)KinectInterop.JointType.ThumbRight)) |
4187 |
// { |
4188 |
// Vector3 jointDir = jointData.direction; |
4189 |
// jointDir = new Vector3(jointDir.x, jointDir.y, -jointDir.z).normalized; |
4190 |
// |
4191 |
// Vector3 baseDir = KinectInterop.JointBaseDir[joint]; |
4192 |
// jointData.normalRotation = Quaternion.FromToRotation(baseDir, jointDir); |
4193 |
// |
4194 |
// Vector3 mirroredAngles = jointData.normalRotation.eulerAngles; |
4195 |
// mirroredAngles.y = -mirroredAngles.y; |
4196 |
// mirroredAngles.z = -mirroredAngles.z; |
4197 |
// |
4198 |
// jointData.mirroredRotation = Quaternion.Euler(mirroredAngles); |
4199 |
// } |
4200 |
// else |
4201 |
{ |
4202 |
jointData.normalRotation = bodyData.joint[prevJoint].normalRotation; |
4203 |
jointData.mirroredRotation = bodyData.joint[prevJoint].mirroredRotation; |
4204 |
} |
4205 |
} |
4206 |
else |
4207 |
{ |
4208 |
jointData.normalRotation = Quaternion.identity; |
4209 |
jointData.mirroredRotation = Quaternion.identity; |
4210 |
} |
4211 |
} |
4212 |
} |
4213 |
|
4214 |
bodyData.joint[joint] = jointData; |
4215 |
|
4216 |
if(joint == (int)KinectInterop.JointType.SpineBase) |
4217 |
{ |
4218 |
bodyData.normalRotation = jointData.normalRotation; |
4219 |
bodyData.mirroredRotation = jointData.mirroredRotation; |
4220 |
} |
4221 |
} |
4222 |
} |
4223 |
|
4224 |
// Estimates the current state of the defined gestures |
4225 |
private void CheckForGestures(Int64 UserId) |
4226 |
{ |
4227 |
if(!gestureManager || !playerGesturesData.ContainsKey(UserId) || !gesturesTrackingAtTime.ContainsKey(UserId)) |
4228 |
return; |
4229 |
|
4230 |
// check for gestures |
4231 |
if(Time.realtimeSinceStartup >= gesturesTrackingAtTime[UserId]) |
4232 |
{ |
4233 |
// get joint positions and tracking |
4234 |
int iAllJointsCount = sensorData.jointCount; |
4235 |
bool[] playerJointsTracked = new bool[iAllJointsCount]; |
4236 |
Vector3[] playerJointsPos = new Vector3[iAllJointsCount]; |
4237 |
|
4238 |
int[] aiNeededJointIndexes = gestureManager.GetNeededJointIndexes(instance); |
4239 |
int iNeededJointsCount = aiNeededJointIndexes.Length; |
4240 |
|
4241 |
for(int i = 0; i < iNeededJointsCount; i++) |
4242 |
{ |
4243 |
int joint = aiNeededJointIndexes[i]; |
4244 |
|
4245 |
if(joint >= 0) |
4246 |
{ |
4247 |
playerJointsTracked[joint] = IsJointTracked(UserId, joint); |
4248 |
playerJointsPos[joint] = GetJointPosition(UserId, joint); |
4249 |
|
4250 |
if (!playerJointsTracked[joint] && (joint == (int)KinectInterop.JointType.SpineShoulder) && |
4251 |
IsJointTracked(UserId, (int)KinectInterop.JointType.ShoulderLeft) && IsJointTracked(UserId, (int)KinectInterop.JointType.ShoulderRight)) |
4252 |
{ |
4253 |
playerJointsTracked[joint] = true; |
4254 |
playerJointsPos[joint] = (GetJointPosition(UserId, (int)KinectInterop.JointType.ShoulderLeft) + GetJointPosition(UserId, (int)KinectInterop.JointType.ShoulderRight)) / 2f; |
4255 |
} |
4256 |
} |
4257 |
} |
4258 |
|
4259 |
// check for gestures |
4260 |
List<KinectGestures.GestureData> gesturesData = playerGesturesData[UserId]; |
4261 |
|
4262 |
int listGestureSize = gesturesData.Count; |
4263 |
float timestampNow = Time.realtimeSinceStartup; |
4264 |
string sDebugGestures = string.Empty; // "Tracked Gestures:\n"; |
4265 |
|
4266 |
for(int g = 0; g < listGestureSize; g++) |
4267 |
{ |
4268 |
KinectGestures.GestureData gestureData = gesturesData[g]; |
4269 |
|
4270 |
if((timestampNow >= gestureData.startTrackingAtTime) && |
4271 |
!IsConflictingGestureInProgress(gestureData, ref gesturesData)) |
4272 |
{ |
4273 |
gestureManager.CheckForGesture(UserId, ref gestureData, Time.realtimeSinceStartup, |
4274 |
ref playerJointsPos, ref playerJointsTracked); |
4275 |
gesturesData[g] = gestureData; |
4276 |
|
4277 |
if(gestureData.complete) |
4278 |
{ |
4279 |
gesturesTrackingAtTime[UserId] = timestampNow + minTimeBetweenGestures; |
4280 |
} |
4281 |
|
4282 |
if(UserId == liPrimaryUserId) |
4283 |
{ |
4284 |
sDebugGestures += string.Format("{0} - state: {1}, time: {2:F1}, progress: {3}%\n", |
4285 |
gestureData.gesture, gestureData.state, |
4286 |
gestureData.timestamp, |
4287 |
(int)(gestureData.progress * 100 + 0.5f)); |
4288 |
} |
4289 |
} |
4290 |
} |
4291 |
|
4292 |
playerGesturesData[UserId] = gesturesData; |
4293 |
|
4294 |
if(gesturesDebugText && (UserId == liPrimaryUserId)) |
4295 |
{ |
4296 |
for(int i = 0; i < iNeededJointsCount; i++) |
4297 |
{ |
4298 |
int joint = aiNeededJointIndexes[i]; |
4299 |
|
4300 |
sDebugGestures += string.Format("\n {0}: {1}", (KinectInterop.JointType)joint, |
4301 |
playerJointsTracked[joint] ? playerJointsPos[joint].ToString() : ""); |
4302 |
} |
4303 |
|
4304 |
gesturesDebugText.text = sDebugGestures; |
4305 |
} |
4306 |
} |
4307 |
} |
4308 |
|
4309 |
private bool IsConflictingGestureInProgress(KinectGestures.GestureData gestureData, ref List<KinectGestures.GestureData> gesturesData) |
4310 |
{ |
4311 |
foreach(KinectGestures.Gestures gesture in gestureData.checkForGestures) |
4312 |
{ |
4313 |
int index = GetGestureIndex(gesture, ref gesturesData); |
4314 |
|
4315 |
if(index >= 0) |
4316 |
{ |
4317 |
if(gesturesData[index].progress > 0f) |
4318 |
return true; |
4319 |
} |
4320 |
} |
4321 |
|
4322 |
return false; |
4323 |
} |
4324 |
|
4325 |
// return the index of gesture in the list, or -1 if not found |
4326 |
private int GetGestureIndex(KinectGestures.Gestures gesture, ref List<KinectGestures.GestureData> gesturesData) |
4327 |
{ |
4328 |
int listSize = gesturesData.Count; |
4329 |
|
4330 |
for(int i = 0; i < listSize; i++) |
4331 |
{ |
4332 |
if(gesturesData[i].gesture == gesture) |
4333 |
return i; |
4334 |
} |
4335 |
|
4336 |
return -1; |
4337 |
} |
4338 |
|
4339 |
// check if the calibration pose is complete for given user |
4340 |
protected virtual bool CheckForCalibrationPose(Int64 UserId, int bodyIndex, KinectGestures.Gestures calibrationGesture) |
4341 |
{ |
4342 |
if(calibrationGesture == KinectGestures.Gestures.None) |
4343 |
return true; |
4344 |
if(!gestureManager) |
4345 |
return false; |
4346 |
|
4347 |
KinectGestures.GestureData gestureData = playerCalibrationData.ContainsKey(UserId) ? |
4348 |
playerCalibrationData[UserId] : new KinectGestures.GestureData(); |
4349 |
|
4350 |
// init gesture data if needed |
4351 |
if(gestureData.userId != UserId) |
4352 |
{ |
4353 |
gestureData.userId = UserId; |
4354 |
gestureData.gesture = calibrationGesture; |
4355 |
gestureData.state = 0; |
4356 |
gestureData.timestamp = Time.realtimeSinceStartup; |
4357 |
gestureData.joint = 0; |
4358 |
gestureData.progress = 0f; |
4359 |
gestureData.complete = false; |
4360 |
gestureData.cancelled = false; |
4361 |
} |
4362 |
|
4363 |
// get joint positions and tracking |
4364 |
int iAllJointsCount = sensorData.jointCount; |
4365 |
bool[] playerJointsTracked = new bool[iAllJointsCount]; |
4366 |
Vector3[] playerJointsPos = new Vector3[iAllJointsCount]; |
4367 |
|
4368 |
int[] aiNeededJointIndexes = gestureManager.GetNeededJointIndexes(instance); |
4369 |
int iNeededJointsCount = aiNeededJointIndexes.Length; |
4370 |
|
4371 |
for(int i = 0; i < iNeededJointsCount; i++) |
4372 |
{ |
4373 |
int joint = aiNeededJointIndexes[i]; |
4374 |
|
4375 |
if(joint >= 0) |
4376 |
{ |
4377 |
KinectInterop.JointData jointData = bodyFrame.bodyData[bodyIndex].joint[joint]; |
4378 |
|
4379 |
playerJointsTracked[joint] = jointData.trackingState != KinectInterop.TrackingState.NotTracked; |
4380 |
playerJointsPos[joint] = jointData.kinectPos; |
4381 |
|
4382 |
if (!playerJointsTracked[joint] && (joint == (int)KinectInterop.JointType.SpineShoulder)) |
4383 |
{ |
4384 |
KinectInterop.JointData lShoulderData = bodyFrame.bodyData[bodyIndex].joint[(int)KinectInterop.JointType.ShoulderLeft]; |
4385 |
KinectInterop.JointData rShoulderData = bodyFrame.bodyData[bodyIndex].joint[(int)KinectInterop.JointType.ShoulderRight]; |
4386 |
|
4387 |
if (lShoulderData.trackingState != KinectInterop.TrackingState.NotTracked && rShoulderData.trackingState != KinectInterop.TrackingState.NotTracked) |
4388 |
{ |
4389 |
playerJointsTracked[joint] = true; |
4390 |
playerJointsPos[joint] = (lShoulderData.kinectPos + rShoulderData.kinectPos) / 2f; |
4391 |
} |
4392 |
} |
4393 |
} |
4394 |
} |
4395 |
|
4396 |
// estimate the gesture progess |
4397 |
gestureManager.CheckForGesture(UserId, ref gestureData, Time.realtimeSinceStartup, |
4398 |
ref playerJointsPos, ref playerJointsTracked); |
4399 |
playerCalibrationData[UserId] = gestureData; |
4400 |
|
4401 |
// check if gesture is complete |
4402 |
if(gestureData.complete) |
4403 |
{ |
4404 |
gestureData.userId = 0; |
4405 |
playerCalibrationData[UserId] = gestureData; |
4406 |
|
4407 |
return true; |
4408 |
} |
4409 |
|
4410 |
return false; |
4411 |
} |
4412 |
|
4413 |
/// <summary> |
4414 |
/// Refreshs the gesture listeners' list. |
4415 |
/// </summary> |
4416 |
public void refreshGestureListeners() |
4417 |
{ |
4418 |
|
4419 |
// locate the available gesture listeners |
4420 |
gestureListeners.Clear(); |
4421 |
|
4422 |
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[]; |
4423 |
|
4424 |
foreach(MonoBehaviour monoScript in monoScripts) |
4425 |
{ |
4426 |
// if(typeof(KinectGestures.GestureListenerInterface).IsAssignableFrom(monoScript.GetType()) && |
4427 |
// monoScript.enabled) |
4428 |
if((monoScript is KinectGestures.GestureListenerInterface) && monoScript.enabled) |
4429 |
{ |
4430 |
//KinectGestures.GestureListenerInterface gl = (KinectGestures.GestureListenerInterface)monoScript; |
4431 |
gestureListeners.Add(monoScript); |
4432 |
} |
4433 |
} |
4434 |
|
4435 |
// locate Kinect gesture manager, if any |
4436 |
gestureManager = null; |
4437 |
foreach(MonoBehaviour monoScript in monoScripts) |
4438 |
{ |
4439 |
// if(typeof(KinectGestures).IsAssignableFrom(monoScript.GetType()) && |
4440 |
// monoScript.enabled) |
4441 |
if((monoScript is KinectGestures) && monoScript.enabled) |
4442 |
{ |
4443 |
// if skipRemoteAvatars - add only the local gesture managers |
4444 |
if( skipRemoteAvatars && monoScript is NetworkBehaviour ) |
4445 |
{ |
4446 |
if( false == ( monoScript as NetworkBehaviour ).isLocalPlayer ) |
4447 |
{ |
4448 |
Debug.Log( "KM: KinectGestures not registered because is not a local object!" ); |
4449 |
continue; // skip network objects from other clients -> they are controlled by the KM on other machine |
4450 |
} |
4451 |
} |
4452 |
|
4453 |
gestureManager = (KinectGestures)monoScript; |
4454 |
break; |
4455 |
} |
4456 |
} |
4457 |
} |
4458 |
|
4459 |
/// <summary> |
4460 |
/// Refreshs the avatar controllers' list. |
4461 |
/// </summary> |
4462 |
public void refreshAvatarControllers() |
4463 |
{ |
4464 |
// remove all users, filters and avatar controllers |
4465 |
avatarControllers.Clear(); |
4466 |
ClearKinectUsers(); |
4467 |
|
4468 |
// get the mono scripts. avatar controllers and gesture listeners are among them |
4469 |
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[]; |
4470 |
|
4471 |
// locate the available avatar controllers |
4472 |
foreach(MonoBehaviour monoScript in monoScripts) |
4473 |
{ |
4474 |
// if(typeof(AvatarController).IsAssignableFrom(monoScript.GetType()) && |
4475 |
// monoScript.enabled) |
4476 |
if((monoScript is AvatarController) && monoScript.enabled) |
4477 |
{ |
4478 |
// if skipRemoteAvatars - add only the local avatar controllers |
4479 |
if( skipRemoteAvatars && monoScript is NetworkBehaviour ) |
4480 |
{ |
4481 |
if( false == ( monoScript as NetworkBehaviour ).isLocalPlayer ) |
4482 |
{ |
4483 |
Debug.Log( "KM: AvatarController not registered because is not a local object!" ); |
4484 |
continue; // skip network objects from other clients -> there are controlled by kinect in other computer |
4485 |
} |
4486 |
} |
4487 |
|
4488 |
AvatarController avatar = (AvatarController)monoScript; |
4489 |
avatarControllers.Add(avatar); |
4490 |
} |
4491 |
} |
4492 |
} |
4493 |
|
4494 |
} |
4495 |
|