private void RecogMove(PXCMHandData.IHand hand, PXCMPointF32 mcp)
{
int side = (int)hand.QueryBodySide() - 1;
if (side < 0)
{
return;
}
hand.QueryTrackedJoint(PXCMHandData.JointType.JOINT_MIDDLE_TIP, out middleData[side]);
if (oldMiddleData[side] == null)
{
oldMiddleData[side] = middleData[side];
return;
}
//Console.WriteLine(middleData[side].speed.x);
var distance = Math.Pow(
Math.Pow(middleData[side].positionWorld.x - oldMiddleData[side].positionWorld.x, 2)
+ Math.Pow(middleData[side].positionWorld.y - oldMiddleData[side].positionWorld.y, 2)
+ Math.Pow((middleData[side].positionWorld.z - oldMiddleData[side].positionWorld.z) * 1000, 2),
0.5);
oldMiddleData[side] = middleData[side];
//Console.Write(middleData[side].positionImage.x + ":" + oldMiddleData[side].positionImage.x + ":");
//Console.WriteLine(middleData[side].positionImage.x - oldMiddleData[side].positionImage.x);
//oldMiddleData = middleData;
}
public static PXCMPointF32 ToPXCMPointF32(this Vector2D v)
{
var ret = new PXCMPointF32();
ret.x = (float)v.x;
ret.y = (float)v.y;
return ret;
}
public float PositionChange(PXCMPointF32 image, PXCMPointF32 lastimage)
{
float ans = 0.0F;
ans = (float)Math.Sqrt((image.x - lastimage.x) * (image.x - lastimage.x) + (image.y - lastimage.y) * (image.y - lastimage.y));
return(ans);
}
public Vector2 ProcessSmoothing(SmoothingTypes type, float factor, Vector2 vec)
{
Init();
if (_dataSmoothing == null)
{
SenseToolkitManager.Instance.SenseManager.session.CreateImpl <PXCMDataSmoothing>(out _dataSmoothing);
}
if (_smoother2D == null || _type2D != type || factor != _factor2D)
{
if (_smoother2D != null)
{
_smoother2D.Dispose();
}
CreateSmootherType(type, factor, out _smoother2D);
_type2D = type;
_factor2D = factor;
}
PXCMPointF32 point = new PXCMPointF32()
{
x = vec.x, y = vec.y
};
_smoother2D.AddSample(point);
point = _smoother2D.GetSample();
return(new Vector2(point.x, point.y));
}
public bool ProjectRealWorldToImage(PXCMPoint3DF32[] pos3d, out PXCMPointF32[] pos2d)
{
pos2d = null;
if (!initialized || pos3d == null)
{
return(false);
}
pos2d = new PXCMPointF32[pos3d.Length];
return(ProjectRealWorldToImageC(instance, pos3d.Length, pos3d, pos2d));
}
public bool MapDepthToColorCoordinates(PXCMPoint3DF32[] pos2d, out PXCMPointF32[] posc)
{
posc = null;
if (!initialized || pos2d == null)
{
return(false);
}
posc = new PXCMPointF32[pos2d.Length];
return(MapDepthToColorCoordinatesC(instance, pos2d.Length, pos2d, posc));
}
public byte[] DepthToColorCoordinatesByFunction(PXCMImage color, PXCMImage depth, int dots, out int cwidth, out int cheight)
{
/* Retrieve the color pixels */
byte[] cpixels = color.GetRGB32Pixels(out cwidth, out cheight);
if (projection == null || cpixels == null)
{
return(cpixels);
}
/* Retrieve the depth pixels and uvmap */
PXCMImage.ImageData ddata;
UInt16[] dpixels;
bool isdepth = (depth.info.format == PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH);
if (depth.AcquireAccess(PXCMImage.Access.ACCESS_READ, out ddata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Int32 dpitch = ddata.pitches[0] / sizeof(Int16); /* aligned width */
Int32 dwidth = (Int32)depth.info.width;
Int32 dheight = (Int32)depth.info.height;
dpixels = ddata.ToUShortArray(0, isdepth ? dpitch * dheight : dpitch * dheight * 3);
depth.ReleaseAccess(ddata);
/* Projection Calculation */
PXCMPoint3DF32[] dcords = new PXCMPoint3DF32[dwidth * dheight];
for (Int32 y = 0, k = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++, k++)
{
dcords[k].x = x;
dcords[k].y = y;
dcords[k].z = isdepth ? dpixels[y * dpitch + x] : dpixels[3 * (y * dpitch + x) + 2];
}
}
PXCMPointF32[] ccords = new PXCMPointF32[dwidth * dheight];
projection.MapDepthToColor(dcords, ccords);
/* Draw dots onto the color pixels */
for (Int32 y = 0, k = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++, k++)
{
UInt16 d = isdepth ? dpixels[y * dpitch + x] : dpixels[3 * (y * dpitch + x) + 2];
if (d == invalid_value)
{
continue; // no mapping based on unreliable depth values
}
Int32 xx = (Int32)ccords[k].x, yy = (Int32)ccords[k].y;
PlotXY(cpixels, xx, yy, cwidth, cheight, dots, 2);
}
}
}
return(cpixels);
}
Ellipse MakeEllipse(PXCMPointF32 point)
{
Ellipse ellipse = new Ellipse()
{
Width = LANDMARK_ELLIPSE_WIDTH,
Height = LANDMARK_ELLIPSE_WIDTH,
Fill = Brushes.Orange
};
Canvas.SetLeft(ellipse, point.x);
Canvas.SetTop(ellipse, point.y);
return(ellipse);
}
Ellipse MakeEllipse(PXCMPointF32 point)
{
Ellipse ellipse = new Ellipse()
{
Width = LANDMARK_ELLIPSE_WIDTH,
Height = LANDMARK_ELLIPSE_WIDTH,
Fill = Brushes.Orange
};
Canvas.SetLeft(ellipse, ScaleCameraXToCanvasX(point.x) - (LANDMARK_ELLIPSE_WIDTH / 2.0));
Canvas.SetTop(ellipse, ScaleCameraYToCanvasY(point.y) - (LANDMARK_ELLIPSE_WIDTH / 2.0));
return(ellipse);
}
/// <summary> アイコンの当たり判定の確認 </summary>
private void IconHitCheck(PXCMPointF32 p)
{
//Console.WriteLine((imageColor.Width / 5) * 2);
//x座標がアイコン領域外ならreturn
if (p.x < 0 || p.x > (imageColor.Width / 5) * 2)
{
return;
}
for (int mode = 0; mode < 5; mode++)
{
if ((imageColor.Height / 5) * mode < p.y && p.y < (imageColor.Height / 5) * (mode + 1))
{
Dispatcher.BeginInvoke(
new Action(() =>
{
switch (mode)
{
case MODE_WHOLE:
OnWholeTone.IsChecked = true;
break;
case MODE_QUARTER:
OnQuarterTone.IsChecked = true;
break;
case MODE_ARPEGGIO:
OnArpeggio.IsChecked = true;
break;
case MODE_DELAY:
OnDelay.IsChecked = true;
break;
case MODE_FREE:
OnFree.IsChecked = true;
break;
default:
break;
}
}
));
break;
}
}
}
// 手のデータを更新する
private void UpdateHandFrame()
{
// 手のデータを更新する
handData.Update();
// データを初期化する
CanvasFaceParts.Children.Clear();
// 検出した手の数を取得する
var numOfHands = handData.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
// 手を取得する
PXCMHandData.IHand hand;
var sts = handData.QueryHandData(
PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out hand);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// 指の関節を列挙する
for (int j = 0; j < PXCMHandData.NUMBER_OF_JOINTS; j++)
{
// 指のデータを取得する
PXCMHandData.JointData jointData;
sts = hand.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// Depth座標系をカラー座標系に変換する
var depthPoint = new PXCMPoint3DF32[1];
var colorPoint = new PXCMPointF32[1];
depthPoint[0].x = jointData.positionImage.x;
depthPoint[0].y = jointData.positionImage.y;
depthPoint[0].z = jointData.positionWorld.z * 1000;
projection.MapDepthToColor(depthPoint, colorPoint);
AddEllipse(CanvasFaceParts,
new Point(colorPoint[0].x, colorPoint[0].y),
5, Brushes.Green);
}
}
}
/// <summary> 指のデータを取得する </summary>
private void GetFingerData(PXCMHandData.IHand hand, PXCMHandData.JointType jointType)
{
PXCMHandData.JointData jointData;
var sts = hand.QueryTrackedJoint(jointType, out jointData);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
// Depth座標系をカラー座標系に変換する
var depthPoint = new PXCMPoint3DF32[1];
var colorPoint = new PXCMPointF32[1];
depthPoint[0].x = jointData.positionImage.x;
depthPoint[0].y = jointData.positionImage.y;
depthPoint[0].z = jointData.positionWorld.z * 1000;
projection.MapDepthToColor(depthPoint, colorPoint);
var masp = hand.QueryMassCenterImage();
var mdp = new PXCMPoint3DF32[1];
var mcp = new PXCMPointF32[1];
mdp[0].x = masp.x;
mdp[0].y = masp.y;
mdp[0].z = hand.QueryMassCenterWorld().z * 1000;
projection.MapDepthToColor(mdp, mcp);
//Console.WriteLine(mcp[0].x);
AddEllipse(new Point(mcp[0].x, mcp[0].y), 10, Brushes.Red, 1);
colorPoint = mcp;
//AddEllipse(new Point(colorPoint[0].x, colorPoint[0].y), 5, Brushes.White, 1);
if (ensembleTimer.IsEnabled)
{
DetectTap(hand, mcp[0]);
}
//if (ensembleTimer.IsEnabled) RecogMove(hand,mcp[0]);
//RecogMove(hand,mcp[0]);
}
public void ProcessFrame(PXCMCapture.Sample sample)
{
this.jointPositions.Clear();
if (this.handData.Update().Succeeded())
{
foreach (var handId in this.GetHandIdentifiersInFrame())
{
if (!this.currentHandId.HasValue ||
(this.currentHandId.Value == handId))
{
this.currentHandId = handId;
PXCMHandData.IHand handInfo;
if (this.handData.QueryHandDataById(handId, out handInfo).IsSuccessful())
{
PXCMHandData.JointData jointData;
foreach (var jointType in jointTypes)
{
if (handInfo.QueryTrackedJoint(jointType, out jointData).IsSuccessful())
{
var scaledPosition = new PXCMPointF32(
(float)jointData.positionImage.x / sample.depth.info.width,
(float)jointData.positionImage.y / sample.depth.info.height);
this.jointPositions[jointType] = scaledPosition;
}
}
}
}
}
}
if (this.jointPositions.Count() == 0)
{
this.currentHandId = null;
}
}
// Update is called once per frame
void Update()
{
if (!init)
{
OnEnable();
}
if (SenseToolkitManager.Instance.Initialized &&
SenseToolkitManager.Instance.ImageRgbOutput != null &&
SenseToolkitManager.Instance.ImageDepthOutput != null)
{
PXCMImage.ImageData depthData;
int width = SenseToolkitManager.Instance.ImageDepthOutput.info.width;
int height = SenseToolkitManager.Instance.ImageDepthOutput.info.height;
// 1. Get the depth image
SenseToolkitManager.Instance.ImageDepthOutput.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH_F32, out depthData);
if (_depthArray == null)
{
_depthArray = new float[width * height];
}
depthData.ToFloatArray(0, _depthArray);
PXCMPoint3DF32[] pos_uvz = new PXCMPoint3DF32[1] {
new PXCMPoint3DF32()
{
x = 0, y = 0, z = 0
}
};
// 2. Find a "good" pixel (not too far)
int x = width / 4;
int y = height / 4;
int xx = 5;
int yy = 5;
while (pos_uvz[0].z == 0 || (pos_uvz[0].z / 10) > MaxDepthThreshold)
{
x += xx;
if (x >= 3 * width / 4)
{
x = width / 4;
y += yy;
if (y >= 3 * height / 4)
{
SenseToolkitManager.Instance.ImageDepthOutput.ReleaseAccess(depthData);
return;
}
}
pos_uvz[0].x = x;
pos_uvz[0].y = y;
pos_uvz[0].z = _depthArray[x + y * width];
}
SenseToolkitManager.Instance.ImageDepthOutput.ReleaseAccess(depthData);
// 3. Projet it to the color image
PXCMPointF32[] pos_ij = new PXCMPointF32[1] {
new PXCMPointF32()
};
var sts = SenseToolkitManager.Instance.Projection.MapDepthToColor(pos_uvz, pos_ij);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
// 4. Project it to the Real World coordinates
PXCMPoint3DF32[] pos3d = new PXCMPoint3DF32[1] {
new PXCMPoint3DF32()
};
sts = SenseToolkitManager.Instance.Projection.ProjectDepthToCamera(pos_uvz, pos3d);
pos3d[0].x /= -10;
pos3d[0].y /= 10;
pos3d[0].z /= 10;
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR || pos3d[0].x == 0 || pos3d[0].y == 0)
{
return;
}
Vector3 vec = new Vector3();
vec.x = pos3d[0].x;
vec.y = pos3d[0].y;
vec.z = pos3d[0].z;
// 5. Normalize the color pixel location
Vector3 vecScreen = new Vector3();
vecScreen.x = (SenseToolkitManager.Instance.ImageRgbOutput.info.width - pos_ij[0].x) / SenseToolkitManager.Instance.ImageRgbOutput.info.width;
vecScreen.y = (SenseToolkitManager.Instance.ImageRgbOutput.info.height - pos_ij[0].y) / SenseToolkitManager.Instance.ImageRgbOutput.info.height;
// 6. Calibrate the world point to the screen point
Calibrate(vec, vecScreen);
}
}
// 手のデータを更新する
private void UpdateHandFrame()
{
// 手のデータを更新する
handData.Update();
// データを初期化する
CanvasFaceParts.Children.Clear();
// 検出した手の数を取得する
var numOfHands = handData.QueryNumberOfHands();
for ( int i = 0; i < numOfHands; i++ ) {
// 手を取得する
PXCMHandData.IHand hand;
var sts = handData.QueryHandData(
PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out hand );
if ( sts < pxcmStatus.PXCM_STATUS_NO_ERROR ) {
continue;
}
// 指の関節を列挙する
for ( int j = 0; j < PXCMHandData.NUMBER_OF_JOINTS; j++ ) {
// 指のデータを取得する
PXCMHandData.JointData jointData;
sts = hand.QueryTrackedJoint( (PXCMHandData.JointType)j, out jointData );
if ( sts < pxcmStatus.PXCM_STATUS_NO_ERROR ) {
continue;
}
// Depth座標系をカラー座標系に変換する
var depthPoint = new PXCMPoint3DF32[1];
var colorPoint = new PXCMPointF32[1];
depthPoint[0].x = jointData.positionImage.x;
depthPoint[0].y = jointData.positionImage.y;
depthPoint[0].z = jointData.positionWorld.z * 1000;
projection.MapDepthToColor( depthPoint, colorPoint );
AddEllipse( CanvasFaceParts,
new Point( colorPoint[0].x, colorPoint[0].y ),
5, Brushes.Green );
}
}
}
private Point3D ToPoint3D(PXCMPointF32 p)
{
return new Point3D(p.x, p.y);
}
private static extern bool ProjectRealWorldToImageC(IntPtr pp, Int32 npoints, PXCMPoint3DF32[] pos3d, PXCMPointF32[] pos2d);
private void UpdateHandFrame()
{
if (this.handData == null)
{
return;
}
this.handData.Update();
//ピクセルデータを初期化する
Array.Clear(this.imageBuffer, 0, this.imageBuffer.Length);
// 検出した手の数を取得する
var numOfHands = this.handData.QueryNumberOfHands();
FOutHandID.SliceCount = 0;
for (int i = 0; i < numOfHands; i++)
{
int handID = -1;
pxcmStatus sts = this.handData.QueryHandId(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, i, out handID);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
FLogger.Log(LogType.Debug, "手のIDの取得に失敗しました");
continue;
}
FOutHandID.SliceCount = 1 + i;
FOutHandID[i] = handID;
// 手を取得する
PXCMHandData.IHand hand;
sts = this.handData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out hand);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
FLogger.Log(LogType.Debug, "手のデータの取得に失敗しました");
continue;
}
PXCMImage.ImageData data;
if (FInMode[0] == Mode.Mask)
{
// 手の画像を取得する
sts = hand.QuerySegmentationImage(out this.image);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
FLogger.Log(LogType.Debug, "手の画像の取得に失敗しました");
continue;
}
if (this.image != null)
{
this.invalidate = true;
}
// マスク画像を取得する
sts = this.image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_Y8, out data);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
FLogger.Log(LogType.Debug, "マスク画像の取得に失敗しました");
continue;
}
// マスク画像のサイズはDepthに依存
// 手は2つまで
var info = this.image.QueryInfo();
// マスク画像をバイト列に変換する
var buffer = data.ToByteArray(0, data.pitches[0] * info.height);
for (int j = 0; j < info.height * info.width; ++j)
{
if (buffer[j] != 0)
{
var index = j * BYTE_PER_PIXEL;
// 手のインデックスで色を決める
// ID = 0 : 127
// ID = 1 : 254
var value = (byte)((i + 1) * 127);
imageBuffer[index + 0] = value;
imageBuffer[index + 1] = value;
imageBuffer[index + 2] = value;
imageBuffer[index + 3] = 255;
}
}
this.image.ReleaseAccess(data);
}
// 指の関節を列挙する
int jointCount = PXCMHandData.NUMBER_OF_JOINTS;
FOutJointPositionWorld.SliceCount = jointCount * (1 + i);
FOutJointPositionImage.SliceCount = jointCount * (1 + i);
for (int j = 0; j < jointCount; j++)
{
int sliceIndex = i * jointCount + j;
PXCMHandData.JointData jointData;
sts = hand.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
FOutJointPositionWorld[sliceIndex] = new Vector3D(0.0f, 0.0f, 0.0f);
FOutJointPositionImage[sliceIndex] = new Vector3D(0.0f, 0.0f, 0.0f);
continue;
}
if (FInMode[0] == Mode.Color)
{
// Depth座標系をカラー座標系に変換する
var depthPoint = new PXCMPoint3DF32[1];
var colorPoint = new PXCMPointF32[1];
depthPoint[0].x = jointData.positionImage.x;
depthPoint[0].y = jointData.positionImage.y;
depthPoint[0].z = jointData.positionWorld.z * 1000;
projection.MapDepthToColor(depthPoint, colorPoint);
Vector3D posWorld = new Vector3D(colorPoint[0].x, jointData.positionWorld.y, jointData.positionWorld.z);
Vector3D posImage = new Vector3D(colorPoint[0].x, colorPoint[0].y, 0.0f);
FOutJointPositionWorld[sliceIndex] = posWorld;
FOutJointPositionImage[sliceIndex] = posImage;
}
else
{
Vector3D posWorld = new Vector3D(jointData.positionWorld.x, jointData.positionWorld.y, jointData.positionWorld.z);
Vector3D posImage = new Vector3D(jointData.positionImage.x, jointData.positionImage.y, jointData.positionImage.z);
FOutJointPositionWorld[sliceIndex] = posWorld;
FOutJointPositionImage[sliceIndex] = posImage;
}
}
// 手の重心を表示する
var center = hand.QueryMassCenterWorld();
Vector3D centerPosition = new Vector3D(center.x, center.y, center.z);
FOutMassCenter[0] = centerPosition;
}
}
public static float[] toFloatArray(this PXCMPointF32 pf32)
{
return(new float[] { pf32.x, pf32.y });
}
private Point3D ToPoint3D(PXCMPointF32 p)
{
return(new Point3D(p.x, p.y));
}
private void ProcessingThread()
{
// Start AcquireFrame/ReleaseFrame loop
while (senseManager.AcquireFrame(true).IsSuccessful())
{
PXCMCapture.Sample sample = senseManager.QuerySample();
#region face
// Retrieve gesture data
face = senseManager.QueryFace();
if (face != null)
{
// Retrieve the most recent processed data
faceData = face.CreateOutput();
faceData.Update();
if (faceData.QueryNumberOfDetectedFaces() > 0)
{
var face = faceData.QueryFaceByIndex(0);
var pose = face.QueryPose();
if (pose != null)
{
PXCMFaceData.PoseEulerAngles angles;
pose.QueryPoseAngles(out angles);
if (angles != null)
{
double correctedYaw = (angles.yaw + 20) / 40;
correctedYaw = correctedYaw < 1 ? correctedYaw : 1;
correctedYaw = correctedYaw > 0 ? correctedYaw : 0;
correctedYaw = Math.Round(correctedYaw, 2);
correctedYaw = correctedYaw - correctedYaw % .02;
double correctedPitch = (-angles.pitch - 9) / 8;
correctedPitch = correctedPitch < 1 ? correctedPitch : 1;
correctedPitch = correctedPitch > 0 ? correctedPitch : 0;
correctedPitch = Math.Round(correctedPitch, 2);
correctedPitch = correctedPitch - correctedPitch % .02;
Nullable <PXCMPointF32> point = new PXCMPointF32();
Dispatcher.Invoke(() =>
{
double maxX = App.Current.MainWindow.RenderSize.Width;
float currentX = Convert.ToSingle(maxX * correctedYaw);
double maxY = App.Current.MainWindow.RenderSize.Height;
float currentY = Convert.ToSingle(maxY * correctedPitch);
point = new PXCMPointF32(currentX, currentY);
});
point = smoother2D.SmoothValue(point.Value);
point = smoother2D2.SmoothValue(point.Value);
SetCursorPosition(Convert.ToInt32(point.Value.x), Convert.ToInt32(point.Value.y));
}
}
var expressions = face.QueryExpressions();
if (expressions != null)
{
PXCMFaceData.ExpressionsData.FaceExpressionResult result;
expressions.QueryExpression(PXCMFaceData.ExpressionsData.FaceExpression.EXPRESSION_MOUTH_OPEN, out result);
if (result != null && result.intensity > 35)
{
Click();
}
}
}
}
#endregion
#region hand
hand = senseManager.QueryHand();
if (hand != null)
{
using (var handData = hand.CreateOutput())
{
handData.Update();
var handCount = handData.QueryNumberOfHands();
for (int i = 0; i < handCount; i++)
{
PXCMHandData.IHand handInfo;
handData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_RIGHT_HANDS, i, out handInfo);
if (handInfo != null && handInfo.HasTrackedJoints())
{
if (handInfo.QueryBodySide() == PXCMHandData.BodySideType.BODY_SIDE_RIGHT)
{
PXCMHandData.FingerData finger;
handInfo.QueryFingerData(PXCMHandData.FingerType.FINGER_INDEX, out finger);
Nullable <PXCMPointF32> mouseLocation = null;
if (finger.foldedness > 85)
{
PXCMHandData.JointData joint;
handInfo.QueryTrackedJoint(PXCMHandData.JointType.JOINT_INDEX_TIP, out joint);
double appX = 0;
double appY = 0;
Dispatcher.Invoke(() =>
{
appX = App.Current.MainWindow.RenderSize.Width;
appY = App.Current.MainWindow.RenderSize.Height;
});
var xScaled = (joint.positionImage.x - 580) * -1 * (appX / 640) * 1.2;
var yScaled = (joint.positionImage.y - 60) * (appY / 480) * 1.6;
mouseLocation = smoother2D.SmoothValue(new PXCMPointF32(Convert.ToSingle(xScaled), Convert.ToSingle(yScaled)));
SetCursorPosition(Convert.ToInt32(mouseLocation.Value.x), Convert.ToInt32(mouseLocation.Value.y));
}
handInfo.QueryFingerData(PXCMHandData.FingerType.FINGER_PINKY, out finger);
if (finger.foldedness > 85)
{
if (DateTime.Now - lastClick > new TimeSpan(0, 0, 2))
{
Click();
}
}
}
}
}
}
}
#endregion
// Update the user interface
//UpdateUI(colorBitmap);
// Release the frame
if (faceData != null)
{
faceData.Dispose();
}
senseManager.ReleaseFrame();
}
}
private void DetectTap(PXCMHandData.IHand hand, PXCMPointF32 mcp)
{
PXCMHandData.JointData midData;
PXCMHandData.JointData cntData;
//指のデータをとってくる(depth)
//ユーザの右手のデータ
if (hand.QueryBodySide() == PXCMHandData.BodySideType.BODY_SIDE_LEFT)
{
hand.QueryTrackedJoint(PXCMHandData.JointType.JOINT_CENTER, out cntData);
RightCenter = cntData.positionWorld;
hand.QueryTrackedJoint(PXCMHandData.JointType.JOINT_MIDDLE_TIP, out midData);
RightMiddle = midData.positionWorld;
}
//ユーザの左手のデータ
if (hand.QueryBodySide() == PXCMHandData.BodySideType.BODY_SIDE_RIGHT)
{
hand.QueryTrackedJoint(PXCMHandData.JointType.JOINT_CENTER, out cntData);
LeftCenter = cntData.positionWorld;
hand.QueryTrackedJoint(PXCMHandData.JointType.JOINT_MIDDLE_TIP, out midData);
LeftMiddle = midData.positionWorld;
}
if (mcp.y < 0)
{
mcp.y = 0;
}
if (mcp.y > ColorImage.Height)
{
mcp.y = (float)ColorImage.Height;
}
//Console.WriteLine(rsw.ElapsedMilliseconds);
//if文の条件を記述(前の指のデータと比較)
// ユーザの右手でタップ
if (-RightMiddle.z + preRightMiddle.z > 0.02 && // 1F(約1/60秒)あたりの深度の変化が0.02m以上
Math.Pow(Math.Pow(RightMiddle.x - preRightMiddle.x, 2) // 指先の速度が1.8m/s以上
+ Math.Pow(RightMiddle.y - preRightMiddle.y, 2)
+ Math.Pow(RightMiddle.z * 1000 - preRightMiddle.z * 1000, 2), 0.5) > 0.03 &&
Math.Pow(Math.Pow(RightCenter.x - preRightCenter.x, 2) // 手のひらの速度が0.6m/s以上
+ Math.Pow(RightCenter.y - preRightCenter.y, 2)
+ Math.Pow(RightCenter.z * 1000 - preRightCenter.z * 1000, 2), 0.5) > 0.01 &&
rsw.ElapsedMilliseconds > 300
)
{
//tap音を出力
midi.OnNote(currentFreqs[(int)(((ColorImage.Height - mcp.y) / (ColorImage.Height + 0.01)) * currentFreqs.Length)]);
rsw.Restart();
}
//var c = ColorImage.Height - y;
//var e = c / ColorImage.Height;
//var d = e * currentFreqs.Length;
//Index.Text = y + "\n" + d + "\n" + (int)d;
//Index.Text = mcp.y.ToString();
//Chord.Text = e.ToString();
// ユーザの左手でタップ
if (-LeftMiddle.z + preLeftMiddle.z > 0.02 && // 1F(約1/60秒)あたりの深度の変化が0.02m以上
Math.Pow(Math.Pow(LeftMiddle.x - preLeftMiddle.x, 2) // 指先の速度が1.8m/s以上
+ Math.Pow(LeftMiddle.y - preLeftMiddle.y, 2)
+ Math.Pow(LeftMiddle.z * 1000 - preLeftMiddle.z * 1000, 2), 0.5) > 0.03 &&
Math.Pow(Math.Pow(LeftCenter.x - preLeftCenter.x, 2) // 手のひらの速度が0.6m/s以上
+ Math.Pow(LeftCenter.y - preLeftCenter.y, 2)
+ Math.Pow(LeftCenter.z * 1000 - preLeftCenter.z * 1000, 2), 0.5) > 0.01 &&
lsw.ElapsedMilliseconds > 300
)
{
//tap音を出力
//midi.OnNote(currentFreqs[(int)(((ColorImage.Height - mcp.y) / (ColorImage.Height + 0.01)) * currentFreqs.Length)]);
lsw.Restart();
}
//Console.WriteLine("RightCenter.x:" + RightCenter.x);
//Console.WriteLine("preRightCenter.x:" + preRightCenter.x);
//Console.WriteLine();
//Console.WriteLine("RightMiddle.x:" + RightMiddle.x);
//Console.WriteLine("preRightMiddle.x:" + preRightMiddle.x);
//前の指のデータに今の指のデータを上書き
//plc,preLeftMiddle,preRightCenter,preRightMiddle
// 上手くいかなければディープコピーする．
preRightCenter.x = RightCenter.x;
preRightCenter.y = RightCenter.y;
preRightCenter.z = RightCenter.z;
preRightMiddle.x = RightMiddle.x;
preRightMiddle.y = RightMiddle.y;
preRightMiddle.z = RightMiddle.z;
preLeftCenter.x = LeftCenter.x;
preLeftCenter.y = LeftCenter.y;
preLeftCenter.z = LeftCenter.z;
preLeftMiddle.x = LeftMiddle.x;
preLeftMiddle.y = LeftMiddle.y;
preLeftMiddle.z = LeftMiddle.z;
}
public byte[] DepthToColorCoordinatesByFunction(PXCMImage color, PXCMImage depth, int dots, out int cwidth, out int cheight)
{
/* Retrieve the color pixels */
byte[] cpixels = RenderStreams.GetRGB32Pixels(color, out cwidth, out cheight);
if (projection == null || cpixels == null)
{
return(cpixels);
}
/* Retrieve the depth pixels and uvmap */
PXCMImage.ImageData ddata;
UInt16[] dpixels;
bool isdepth = (depth.info.format == PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH);
if (depth.AcquireAccess(PXCMImage.Access.ACCESS_READ, out ddata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
int dwidth = ddata.pitches[0] / sizeof(short); /* aligned width */
int dheight = (int)depth.info.height;
dpixels = ddata.ToUShortArray(0, isdepth ? dwidth * dheight : dwidth * dheight * 3);
depth.ReleaseAccess(ddata);
//----
//var myImg = ddata;
var confirmResult = MessageBox.Show("Save image?",
"Confirm",
MessageBoxButtons.YesNo);
if (confirmResult == DialogResult.Yes)
{
PXCMImage.ImageData MyImgData;
//myImg.AcquireAccess(PXCMImage.Access.ACCESS_READ, out MyImgData);
var bmp = ddata.ToBitmap(0, 640, 480);
//myImg.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out MyImgData);
// WriteableBitmap WBmp = MyImgData.ToWritableBitmap(0, 640, 480, 96, 96);
//var bmp = BitmapFromWriteableBitmap(WBmp);
bmp.Save("bmpfile.bmp");
}
else
{
// If 'No', do something here.
}
//-----
/* Projection Calculation */
PXCMPoint3DF32[] dcords = new PXCMPoint3DF32[dwidth * dheight];
for (int y = 0, k = 0; y < dheight; y++)
{
for (int x = 0; x < dwidth; x++, k++)
{
dcords[k].x = x;
dcords[k].y = y;
dcords[k].z = isdepth ? dpixels[k] : dpixels[3 * k + 2];
}
}
PXCMPointF32[] ccords = new PXCMPointF32[dwidth * dheight];
projection.MapDepthToColor(dcords, ccords);
/* Draw dots onto the color pixels */
for (int y = 0, k = 0; y < dheight; y++)
{
for (int x = 0; x < dwidth; x++, k++)
{
UInt16 d = isdepth ? dpixels[k] : dpixels[3 * k + 2];
if (d == invalid_value)
{
continue; // no mapping based on unreliable depth values
}
int xx = (int)ccords[k].x, yy = (int)ccords[k].y;
PlotXY(cpixels, xx, yy, cwidth, cheight, dots, 2);
}
}
}
return(cpixels);
}
/// <summary> 指のデータを取得する </summary>
private bool GetFingerData(PXCMHandData.IHand hand, PXCMHandData.JointType jointType)
{
PXCMHandData.JointData jointData;
var sts = hand.QueryTrackedJoint(jointType, out jointData);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return(false);
}
// Depth座標系をカラー座標系に変換する
var depthPoint = new PXCMPoint3DF32[1];
var colorPoint = new PXCMPointF32[1];
depthPoint[0].x = jointData.positionImage.x;
depthPoint[0].y = jointData.positionImage.y;
depthPoint[0].z = jointData.positionWorld.z * 1000;
projection.MapDepthToColor(depthPoint, colorPoint);
var masp = hand.QueryMassCenterImage();
var mdp = new PXCMPoint3DF32[1];
var mcp = new PXCMPointF32[1];
mdp[0].x = masp.x;
mdp[0].y = masp.y;
mdp[0].z = hand.QueryMassCenterWorld().z * 1000;
projection.MapDepthToColor(mdp, mcp);
//Console.WriteLine(mcp[0].x);
AddEllipse(new Point(mcp[0].x, mcp[0].y), 10, Brushes.Red, 1);
colorPoint = mcp;
//ユーザの右手に対して演奏領域の当たり判定確認
if (hand.QueryBodySide() == PXCMHandData.BodySideType.BODY_SIDE_LEFT)
{
for (int i = 0; i < 5; i++)
{
if ((imageColor.Height / 5) * i <= colorPoint[0].y && colorPoint[0].y < (imageColor.Height / 5) * (i + 1))
{
if (16 - i != NowRange)
{
NowRange = 16 - i;
PivotList.Dispatcher.BeginInvoke(
new Action(() =>
{
PivotList.SelectedItem = NowRange;
}
));
}
}
}
}
//ユーザの左手に対してアイコンの当たり判定の確認
if (hand.QueryBodySide() == PXCMHandData.BodySideType.BODY_SIDE_RIGHT)
{
IconHitCheck(colorPoint[0]);
}
AddEllipse(new Point(colorPoint[0].x, colorPoint[0].y), 5, Brushes.White, 1);
return(true);
}
public bool ProjectRealWorldToImage(PXCMPoint3DF32[] pos3d, out PXCMPointF32[] pos2d)
{
pos2d=null;
if (!initialized || pos3d==null) return false;
pos2d=new PXCMPointF32[pos3d.Length];
return ProjectRealWorldToImageC(instance,pos3d.Length,pos3d,pos2d);
}
public Vector2 ProcessSmoothing(SmoothingTypes type, float factor, Vector2 vec)
{
Init();
if (_dataSmoothing == null)
{
SenseToolkitManager.Instance.SenseManager.session.CreateImpl<PXCMDataSmoothing>(out _dataSmoothing);
}
if (_smoother2D == null || _type2D != type || factor != _factor2D)
{
if (_smoother2D != null)
{
_smoother2D.Dispose();
}
CreateSmootherType(type, factor, out _smoother2D);
_type2D = type;
_factor2D = factor;
}
PXCMPointF32 point = new PXCMPointF32(){x = vec.x, y = vec.y};
_smoother2D.AddSample(point);
point = _smoother2D.GetSample();
return new Vector2(point.x, point.y);
}
private static extern bool MapDepthToColorCoordinatesC(IntPtr pp, Int32 npoints, PXCMPoint3DF32[] pos2d, PXCMPointF32[] posc);
// Update is called once per frame
void Update ()
{
if (!init)
{
OnEnable();
}
if (SenseToolkitManager.Instance.Initialized &&
SenseToolkitManager.Instance.ImageRgbOutput != null &&
SenseToolkitManager.Instance.ImageDepthOutput != null)
{
PXCMImage.ImageData depthData;
int width = SenseToolkitManager.Instance.ImageDepthOutput.info.width;
int height = SenseToolkitManager.Instance.ImageDepthOutput.info.height;
// 1. Get the depth image
SenseToolkitManager.Instance.ImageDepthOutput.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH_F32, out depthData);
if (_depthArray == null)
{
_depthArray = new float[width * height];
}
depthData.ToFloatArray(0,_depthArray);
PXCMPoint3DF32[] pos_uvz = new PXCMPoint3DF32[1]{new PXCMPoint3DF32(){x = 0, y= 0 , z = 0}};
// 2. Find a "good" pixel (not too far)
int x = width / 4;
int y = height / 4;
int xx = 5;
int yy = 5;
while (pos_uvz[0].z == 0 || (pos_uvz[0].z / 10) > MaxDepthThreshold)
{
x += xx;
if ( x >= 3 * width / 4 )
{
x = width / 4 ;
y += yy;
if (y >= 3 * height / 4)
{
SenseToolkitManager.Instance.ImageDepthOutput.ReleaseAccess(depthData);
return;
}
}
pos_uvz[0].x = x;
pos_uvz[0].y = y;
pos_uvz[0].z = _depthArray[x + y * width];
}
SenseToolkitManager.Instance.ImageDepthOutput.ReleaseAccess(depthData);
// 3. Projet it to the color image
PXCMPointF32[] pos_ij = new PXCMPointF32[1]{new PXCMPointF32()};
var sts = SenseToolkitManager.Instance.Projection.MapDepthToColor( pos_uvz, pos_ij);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
// 4. Project it to the Real World coordinates
PXCMPoint3DF32[] pos3d = new PXCMPoint3DF32[1]{new PXCMPoint3DF32()};
sts = SenseToolkitManager.Instance.Projection.ProjectDepthToCamera( pos_uvz, pos3d );
pos3d[0].x /= -10;
pos3d[0].y /= 10;
pos3d[0].z /= 10;
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR || pos3d[0].x == 0 || pos3d[0].y == 0)
{
return;
}
Vector3 vec = new Vector3();
vec.x = pos3d[0].x;
vec.y = pos3d[0].y;
vec.z = pos3d[0].z;
// 5. Normalize the color pixel location
Vector3 vecScreen = new Vector3();
vecScreen.x = (SenseToolkitManager.Instance.ImageRgbOutput.info.width - pos_ij[0].x) / SenseToolkitManager.Instance.ImageRgbOutput.info.width;
vecScreen.y = (SenseToolkitManager.Instance.ImageRgbOutput.info.height - pos_ij[0].y) / SenseToolkitManager.Instance.ImageRgbOutput.info.height;
// 6. Calibrate the world point to the screen point
Calibrate(vec, vecScreen);
}
}
/// <summary>
/// HandTrackingで取得したHandDataを受け取り
/// 画面にHandDataの情報を表示する
/// </summary>
private void DisplayHandTrackingData(PXCMHandData handData)
{
// SegmentationImageの情報格納用の変数
int segWbStride = 0;
Int32Rect segWbRect = new Int32Rect(0, 0, 0, 0);
Byte[] segImageBuffer = null;
// HandJointの情報格納用の変数
List<List<PXCMHandData.JointData>> handJointsList = new List<List<PXCMHandData.JointData>>();
for (int handIndex = 0; handIndex < handData.QueryNumberOfHands(); handIndex++)
{
// IHandDataを取得
PXCMHandData.IHand iHandData;
if (handData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, handIndex, out iHandData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
// SegmentationImageを取得
PXCMImage image;
iHandData.QuerySegmentationImage(out image); // 取得出来る画像は8bitGrayスケール画像 手の部分が0xff(白) 背景が0x00(黒)
// Imageから画像データを取得
PXCMImage.ImageData data = null ;
pxcmStatus sts = image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_Y8, out data);
if (sts == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
// ImageDataが取得できたらWritableBitmapに書き込むためにバイト配列に変換する
int length = data.pitches[0] * image.info.height;
Byte[] tmpBuffer = data.ToByteArray(0, length);
// ストライドと描画領域を取得
segWbStride = data.pitches[0];
segWbRect = new Int32Rect(0, 0, image.info.width, image.info.height);
// Imageデータのアクセスを終了する
image.ReleaseAccess(data);
// HandSegmentationImageは複数ある可能性があるためすでにバイト配列を取得ずみの場合は重ね合わせる
if (segImageBuffer == null)
{
// まだない場合は、そのまま使用する
Array.Resize(ref segImageBuffer, tmpBuffer.Length);
tmpBuffer.CopyTo(segImageBuffer, 0);
}
else
{
// 既にひとつの手の情報がある場合は手の白部分(0xff)のみ重ね合わせる
for (int i=0; i<segImageBuffer.Length; i++)
{
segImageBuffer[i] = (byte)(segImageBuffer[i] | tmpBuffer[i]);
}
}
}
// TODO:後で取得してみる
//iHandData.QueryBoundingBoxImage // 手の領域
//iHandData.QueryMassCenterImage // 2D Image coordinatesでの手の中心座標
//iHandData.QueryMassCenterWorld // 3D World Coordinatesでの手の中心座標
//iHandData.QueryExtremityPoint // TODO:Extremitiesモードで取得してみる
// 1つの手のJointを入れるListを生成
List<PXCMHandData.JointData> jointList = new List<PXCMHandData.JointData>();
// 手のJoint座標を取得してListに格納
for (int jointIndex = 0; jointIndex < Enum.GetNames(typeof(PXCMHandData.JointType)).Length; jointIndex++)
{
// 手の1つのJoint座標を取得
PXCMHandData.JointData jointData;
iHandData.QueryTrackedJoint((PXCMHandData.JointType)jointIndex, out jointData);
jointList.Add(jointData);
}
// 作成した1つの手のJoint座標リストをListに格納
handJointsList.Add(jointList);
}
}
// SegmentationImageデータをバイト配列にしたものをWriteableBitmapに書き込む
if (segImageBuffer != null)
{
m_HandSegmentWBitmap.Dispatcher.BeginInvoke
(
new Action(() =>
{
m_HandSegmentWBitmap.WritePixels(segWbRect, segImageBuffer, segWbStride, 0);
}
));
}
// HandJointの座標を画面に表示
if (handJointsList.Count > 0)
{
m_ColorWBitmap.Dispatcher.BeginInvoke
(
new Action(() =>
{
foreach (List<PXCMHandData.JointData> jointList in handJointsList)
{
foreach (PXCMHandData.JointData joint in jointList)
{
PXCMPoint3DF32[] depthPoint = new PXCMPoint3DF32[1];
depthPoint[0].x = joint.positionImage.x;
depthPoint[0].y = joint.positionImage.y;
depthPoint[0].z = joint.positionWorld.z * 1000; // mmとpixcelを合わす
PXCMPointF32[] colorPoint = new PXCMPointF32[1];
pxcmStatus status = m_Projection.MapDepthToColor(depthPoint, colorPoint);
// 指の位置を描画
m_ColorWBitmap.FillEllipseCentered((int)colorPoint[0].x,
(int)colorPoint[0].y,
10, 10, Colors.YellowGreen);
}
}
}
));
}
m_HandJointWBitmap.Dispatcher.BeginInvoke
(
new Action(() =>
{
m_HandJointWBitmap.Clear();
foreach (List<PXCMHandData.JointData> jointList in handJointsList)
{
foreach (PXCMHandData.JointData joint in jointList)
{
m_HandJointWBitmap.FillEllipse(
(int)joint.positionImage.x, (int)joint.positionImage.y,
(int)joint.positionImage.x + 6, (int)joint.positionImage.y + 6, Colors.YellowGreen);
}
}
}
));
}
public byte[] DepthToColorCoordinatesByFunction(PXCMImage color, PXCMImage depth, int dots, out int cwidth, out int cheight)
{
/* Retrieve the color pixels */
byte[] cpixels = color.GetRGB32Pixels(out cwidth, out cheight);
if (projection == null || cpixels == null) return cpixels;
/* Retrieve the depth pixels and uvmap */
PXCMImage.ImageData ddata;
UInt16[] dpixels;
bool isdepth = (depth.info.format == PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH);
if (depth.AcquireAccess(PXCMImage.Access.ACCESS_READ, out ddata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Int32 dpitch = ddata.pitches[0] / sizeof(Int16); /* aligned width */
Int32 dwidth = (Int32)depth.info.width;
Int32 dheight = (Int32)depth.info.height;
dpixels = ddata.ToUShortArray(0, isdepth ? dpitch * dheight : dpitch * dheight * 3);
depth.ReleaseAccess(ddata);
/* Projection Calculation */
PXCMPoint3DF32[] dcords = new PXCMPoint3DF32[dwidth * dheight];
for (Int32 y = 0, k = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++, k++)
{
dcords[k].x = x;
dcords[k].y = y;
dcords[k].z = isdepth ? dpixels[y * dpitch + x] : dpixels[3 * (y * dpitch + x) + 2];
}
}
PXCMPointF32[] ccords = new PXCMPointF32[dwidth * dheight];
projection.MapDepthToColor(dcords, ccords);
/* Draw dots onto the color pixels */
for (Int32 y = 0, k = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++, k++)
{
UInt16 d = isdepth ? dpixels[y * dpitch + x] : dpixels[3 * (y * dpitch + x) + 2];
if (d == invalid_value) continue; // no mapping based on unreliable depth values
Int32 xx = (Int32)ccords[k].x, yy = (Int32)ccords[k].y;
PlotXY(cpixels, xx, yy, cwidth, cheight, dots, 2);
}
}
}
return cpixels;
}
public static Point3D ToPoint3D(this PXCMPointF32 p)
{
return(new Point3D(p.x, p.y));
}
public bool MapDepthToColorCoordinates(PXCMPoint3DF32[] pos2d, out PXCMPointF32[] posc)
{
posc=null;
if (!initialized || pos2d==null) return false;
posc=new PXCMPointF32[pos2d.Length];
return MapDepthToColorCoordinatesC(instance,pos2d.Length,pos2d,posc);
}
private static void ProcessHands(PXCMHandData handData)
{
// Querying how many hands were detected
int numberOfHands = handData.QueryNumberOfHands();
//Console.WriteLine("{0} hand(s) were detected.", numberOfHands);
// Querying the information about detected hands
for (int i = 0; i < numberOfHands; i++)
{
// Querying hand id
int handId;
pxcmStatus queryHandIdStatus = handData.QueryHandId(PXCMHandData.AccessOrderType.ACCESS_ORDER_NEAR_TO_FAR, i, out handId);
if (queryHandIdStatus != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Console.WriteLine("Failed to query the hand Id.");
continue;
}
//Console.WriteLine("Hand id: {0}", handId);
// Querying the hand data
PXCMHandData.IHand hand;
pxcmStatus queryHandStatus = handData.QueryHandDataById(handId, out hand);
if (queryHandStatus == pxcmStatus.PXCM_STATUS_NO_ERROR && hand != null)
{
// Querying the body side (Left/Right)
PXCMHandData.BodySideType bodySide = hand.QueryBodySide();
//Console.WriteLine("Body Side: {0}", bodySide);
// Querying the hand openness
int handOpenness = hand.QueryOpenness();
//Console.WriteLine("Hand openness: {0}", handOpenness);
SendMqttMessage(String.Format("red?{0}", (255 * handOpenness) / 100));
// Querying Hand 2D Position
PXCMPointF32 massCenterImage = hand.QueryMassCenterImage();
//Console.WriteLine("Hand position on image: {0} | {1}", massCenterImage.x, massCenterImage.y);
PXCMPoint4DF32 palmOrientation = hand.QueryPalmOrientation();
rotationHelper.SetFromQuaternion(palmOrientation);
PXCMPoint3DF32 rotationEuler = rotationHelper.QueryEulerAngles();
double angleInDegrees = (180 * rotationEuler.y / Math.PI) + 180;
Console.WriteLine("Angle in degrees: {0}", angleInDegrees);
SendMqttMessage(String.Format("red?{0}", (int)angleInDegrees));
// Console.WriteLine("Rotation x:{0},y:{1},z:{2}", rotationEuler.x, rotationEuler.y, rotationEuler.z);
// Querying Hand 3D Position
PXCMPoint3DF32 massCenterWorld = hand.QueryMassCenterWorld();
//Console.WriteLine("Hand position on world: {0} | {1} | {2}", massCenterWorld.x, massCenterWorld.y, massCenterWorld.z);
/*
* // Querying Hand Joints
* if (hand.HasTrackedJoints())
* {
* foreach (PXCMHandData.JointType jointType in Enum.GetValues(typeof(PXCMHandData.JointType)))
* {
* PXCMHandData.JointData jointData;
* pxcmStatus queryStatus = hand.QueryTrackedJoint(jointType, out jointData);
*
* if (queryStatus == pxcmStatus.PXCM_STATUS_NO_ERROR && jointData != null)
* {
* // Printing joint label and tracking confidence
* Console.WriteLine("Joint {0} with confidence {1}", jointType, jointData.confidence);
*
* // Printing the 2D position (image)
* Console.WriteLine("\t2D Position: {0} | {1}", jointData.positionImage.x, jointData.positionImage.y);
*
* // Printing the 3D position (depth)
* Console.WriteLine("\t3D Position: {0} | {1} | {2}", jointData.positionWorld.x, jointData.positionWorld.y, jointData.positionWorld.z);
* }
* }
* }*/
}
//Console.WriteLine("----------");
//Console.Clear();
}
}
