프로젝트

일반

사용자정보

통계
| 개정판:

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