void moveHands()
{
for (int i = 0; i < hands.Count; i++)
{
if (i < handData.QueryNumberOfHands())
{
hands[i].SetActive(true);
PXCMHandData.IHand ihandData;
if (handData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, i, out ihandData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
PXCMPoint3DF32 handPos = ihandData.QueryMassCenterWorld();
PXCMPoint4DF32 handRot = ihandData.QueryPalmOrientation();
float handRadius = ihandData.QueryPalmRadiusWorld();
PXCMRectI32 handBounds = ihandData.QueryBoundingBoxImage();
hands[i].transform.position = new Vector3(-handPos.x * 25, handPos.y * 25, -handPos.z * 25 + 10);
depthText.text = "Hand Depth: " + -Mathf.RoundToInt(hands[i].transform.position.z * 100) / 100.0f;
hands[i].transform.rotation = new Quaternion(handRot.x, handRot.y, handRot.z, handRot.w);
hands[i].transform.localScale = new Vector3(handBounds.h, handBounds.h, handBounds.h * 2) * 0.0075f;
}
}
else
{
hands[i].SetActive(false);
}
}
}
public static PXCMPoint4DF32 ToPXCMPoint4DF32(this Vector4D v)
{
var ret = new PXCMPoint4DF32();
ret.x = (float)v.x;
ret.y = (float)v.y;
ret.z = (float)v.z;
ret.w = (float)v.w;
return ret;
}
/* Displaying current frames hand joints */
private void TrackJoints(PXCMHandData handOutput)
{
//Get hand by time of appearence
if (handOutput.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, 0, out handData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
PXCMPoint4DF32 palmOrientation = handData.QueryPalmOrientation();
handData.QueryTrackedJoint(PXCMHandData.JointType.JOINT_THUMB_TIP, out ThumbJointData);
handData.QueryTrackedJoint(PXCMHandData.JointType.JOINT_INDEX_TIP, out IndexJointData);
handData.QueryTrackedJoint(PXCMHandData.JointType.JOINT_CENTER, out PalmCenterJointData);
/* get joint distance */
Vector3 thumb = new Vector3(ThumbJointData.positionWorld.x, ThumbJointData.positionWorld.y, ThumbJointData.positionWorld.z);
Vector3 index = new Vector3(IndexJointData.positionWorld.x, IndexJointData.positionWorld.y, IndexJointData.positionWorld.z);
Vector3 palm = new Vector3(PalmCenterJointData.positionWorld.x, PalmCenterJointData.positionWorld.y, PalmCenterJointData.positionWorld.z);
switch (rotationType)
{
case RotationType.FingerDelta:
//Rotate along y-axis
///Compare the difference between the z values and determine if is great than the threshold
float diff = CompareFloats(index.z, palm.z);
if (Mathf.Abs(diff) > 0.02f)
{
//begin to rotate using the difference thershold as a multiplier for rotation speed.
logo.transform.Rotate(Time.deltaTime * (diff * 500), 0, 0);
}
//rotate along x-axis
diff = CompareFloats(thumb.z, palm.z);
if (Mathf.Abs(diff) > 0.02f)
{
Debug.Log(diff.ToString());
//begin to rotate using the difference thershold as a multiplier for rotation speed.
logo.transform.Rotate(0, Time.deltaTime * (diff * 500), 0);
}
//rotate along x-axis
diff = CompareFloats(thumb.z, index.z);
if (Mathf.Abs(diff) > 0.02f)
{
Debug.Log(diff.ToString());
//begin to rotate using the difference thershold as a multiplier for rotation speed.
logo.transform.Rotate(0, 0, Time.deltaTime * (diff * 500));
}
break;
case RotationType.handleDelta:
//logo.transform.localRotation = new Quaternion(-palmOrientation.w, -palmOrientation.z, -palmOrientation.y, -palmOrientation.x);
logo.transform.Rotate(index.y * Time.deltaTime * 750, index.x * Time.deltaTime * 750, 0);
break;
case RotationType.Pinch:
pinch_dist = Vector3.Distance(index, thumb);
if (HandlePinchResult(pinch_dist))
{
//set the initial position
if (pinchPos == Vector3.zero)
{
pinchPos = index;
}
//get the vector between the initial position and the current index postion
Vector3 temp = pinchPos - index;
logo.transform.Rotate(-temp.y * Time.deltaTime * 750, -temp.x * Time.deltaTime * 750, -temp.z * Time.deltaTime * 750);
}
break;
default:
Debug.Log("something went wrong");
break;
}
}
}
private void ShowTrackingValue(PXCMTracker.TrackingValues arrData)
{
// for the middleware being a left hand coordinate system.
float depthnum = 630;
//correction values for translation and movement dependent on z values
float depthcorrectionfactor = (arrData.translation.z / depthnum);
arrData.translation.x = arrData.translation.x / depthcorrectionfactor;
arrData.translation.y = arrData.translation.y / depthcorrectionfactor;
var translation = arrData.translation;
// オブジェクトのカメラ座標(画面の中心が原点)
arrData.translation.y = -arrData.translation.y;
arrData.translation.x += COLOR_WIDTH / 2;
arrData.translation.y += COLOR_HEIGHT / 2;
// 中心点を表示する
AddEllipse(CanvasPoint,
new Point(arrData.translation.x, arrData.translation.y),
5, Brushes.Blue, -1);
//3x1 points for scaling
PXCMPoint3DF32 trans1;
PXCMPoint3DF32 trans2;
PXCMPoint3DF32 trans3;
//changes the size of the arrows
int scalefactor = 150000; //150000
float height = scalefactor / translation.z;
float width = scalefactor / translation.z;
float depth = scalefactor / translation.z;
//creates the point vectors
trans1.x = width;
trans2.x = 0;
trans3.x = 0;
trans1.y = 0;
trans2.y = height;
trans3.y = 0;
trans1.z = 0;
trans2.z = 0;
trans3.z = depth;
PXCMPoint4DF32 rot = arrData.rotation;
//put into 3x1 vector point for matrix multiplication purposes
PXCMPoint3DF32 q;
q.x = rot.x;
q.y = rot.y;
q.z = rot.z;
float s = rot.w;
// 画像の検出角度(向き)を求める
double heading;
double attitude;
double bank;
float sqw = s * s;
float sqx = q.x * q.x;
float sqy = q.y * q.y;
float sqz = q.z * q.z;
float unit = sqx + sqy + sqz + sqw;
float check = q.x * q.y + q.z * s;
float rad2deg = 180 / (float)Math.PI;
heading = Math.Atan2(2 * q.y * s - 2 * q.x * q.z, sqx - sqy - sqz + sqw) * rad2deg;
attitude = Math.Asin(2.0 * check / unit) * rad2deg;
bank = Math.Atan2(2 * q.x * s - 2 * q.y * q.z, -sqx + sqy - sqz + sqw) * rad2deg;
// 向きの方向に伸ばした点(向きの線の終点)を計算する
PXCMPoint3DF32 prime1;
PXCMPoint3DF32 prime2;
PXCMPoint3DF32 prime3;
float[][] rotmat = new float[3][];
rotmat[0] = new float[3];
rotmat[1] = new float[3];
rotmat[2] = new float[3];
//rotation matrix using quaternion values
rotmat[0][0] = (1 - (2 * (q.y * q.y)) - (2 * (q.z * q.z)));
rotmat[0][1] = ((2 * q.x * q.y) - (2 * s * q.z));
rotmat[0][2] = ((2 * q.x * q.z) + (2 * s * q.y));
rotmat[1][0] = ((2 * q.x * q.y) + (2 * s * q.z));
rotmat[1][1] = (1 - (2 * q.x * q.x) - (2 * q.z * q.z));
rotmat[1][2] = ((2 * q.y * q.z) - (2 * s * q.x));
rotmat[2][0] = ((2 * q.x * q.z) - (2 * s * q.y));
rotmat[2][1] = ((2 * q.y * q.z) + (2 * s * q.x));
rotmat[2][2] = (1 - (2 * q.x * q.x) - (2 * q.y * q.y));
//rotation for x point
prime1.x = (rotmat[0][0] * trans1.x) +
(rotmat[0][1] * trans1.y) +
(rotmat[0][2] * trans1.z);
prime1.y = (rotmat[1][0] * trans1.x) +
(rotmat[1][1] * trans1.y) +
(rotmat[1][2] * trans1.z);
prime1.z = (rotmat[2][0] * trans1.x) +
(rotmat[1][2] * trans1.y) +
(rotmat[2][2] * trans1.z);
//rotation for y point
prime2.x = (rotmat[0][0] * trans2.x) +
(rotmat[0][1] * trans2.y) +
(rotmat[0][2] * trans2.z);
prime2.y = (rotmat[1][0] * trans2.x) +
(rotmat[1][1] * trans2.y) +
(rotmat[1][2] * trans2.z);
prime2.z = (rotmat[2][0] * trans2.x) +
(rotmat[1][2] * trans2.y) +
(rotmat[2][2] * trans2.z);
//rotation for z point
prime3.x = (rotmat[0][0] * trans3.x) +
(rotmat[0][1] * trans3.y) +
(rotmat[0][2] * trans3.z);
prime3.y = (rotmat[1][0] * trans3.x) +
(rotmat[1][1] * trans3.y) +
(rotmat[1][2] * trans3.z);
prime3.z = (rotmat[2][0] * trans3.x) +
(rotmat[1][2] * trans3.y) +
(rotmat[2][2] * trans3.z);
// 各座標の向きを表示する
AddLine(CanvasPoint, new Point(arrData.translation.x, arrData.translation.y),
new Point(arrData.translation.x + prime1.x, arrData.translation.y - prime1.y),
Brushes.Red);
AddLine(CanvasPoint, new Point(arrData.translation.x, arrData.translation.y),
new Point(arrData.translation.x + prime2.x, arrData.translation.y - prime2.y),
Brushes.Green);
AddLine(CanvasPoint, new Point(arrData.translation.x, arrData.translation.y),
new Point(arrData.translation.x + prime3.x, arrData.translation.y - prime3.y),
Brushes.Blue);
}
// Update is called once per frame
void Update () {
Status = Alert.ALERT_ARM_TRACKED;
if (SenseToolkitManager.Instance.PointCloud != null)
{
int _gridSize = 3;
int width = SenseToolkitManager.Instance.ImageDepthOutput.info.width/_gridSize;
int height = SenseToolkitManager.Instance.ImageDepthOutput.info.height/_gridSize;
//Debug.LogWarning("width:"+width.ToString());
// Build vertices and UVs
if (_vertices == null)
{
_vertices = new Vector3[width * height];
}
PXCMHandData _handData;
PXCMHandData.IHand _iHand;
PXCMHandData.JointData WristData;
if (SenseToolkitManager.Instance.Initialized && SenseToolkitManager.Instance.HandDataOutput != null)
{
_handData = SenseToolkitManager.Instance.HandDataOutput;
if (_handData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_FIXED,0,out _iHand) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (_iHand.QueryTrackedJoint ((PXCMHandData.JointType)0, out WristData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
x1 = -WristData.positionWorld.x*100f;
y1 = WristData.positionWorld.y*100f;
z1 = WristData.positionWorld.z*100f;
}
if (_iHand.QueryTrackedJoint ((PXCMHandData.JointType)0, out WristData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
WristRotation = WristData.localRotation;
}else{
Status = Alert.ALERT_HAND_NOT_DETECTED;
return ;
}
}
int i = 0;
for (int y=0; y < height; y++)
{
for (int x=0; x < width - 1; x++)
{
int j = y * width * _gridSize * _gridSize + x * _gridSize;
_vertices [i].x = SenseToolkitManager.Instance.PointCloud[j].x / 10;
_vertices [i].y = SenseToolkitManager.Instance.PointCloud[j].y / 10;
_vertices [i].z = -SenseToolkitManager.Instance.PointCloud[j].z / 10;
i++;
}
}
if (!FindShoulder(height,width)) return ;
if (!FindElbow(height,width)) return ;
}
if (Status == Alert.ALERT_ARM_TRACKED)
Status = CheckStatus ((float)x1,(float) y1,(float) z1,(float) x2, (float)y2,(float) z2,(float) x3,(float) y3, (float)z3);
}
private string PopulateFingerRotationDataToString(PXCMPoint4DF32 finger)
{
string result = Math.Round(finger.w, 3).ToString(ci) + "," + Math.Round(finger.x, 3).ToString(ci) + "," + Math.Round(finger.y, 3).ToString(ci) + "," + Math.Round(finger.z, 3).ToString(ci);
return(result);
}
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();
}
}
