private float getDistanceWithDepthSpacePoint(Kinect.DepthSpacePoint p, Kinect.DepthSpacePoint q, ushort[] depthData)
{
Kinect.CameraSpacePoint P = _Mapper.MapDepthPointToCameraSpace(p, depthData[Pos2Idx((int)p.X, (int)p.Y)]);
Kinect.CameraSpacePoint Q = _Mapper.MapDepthPointToCameraSpace(q, depthData[Pos2Idx((int)q.X, (int)q.Y)]);
return(Mathf.Sqrt((P.X - Q.X) * (P.X - Q.X) + (P.Y - Q.Y) * (P.Y - Q.Y) + (P.Z - Q.Z) * (P.Z - Q.Z)));
}
