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 PXCMRectI32 ToPXCMRectI32(this Vector4D v)
{
var ret = new PXCMRectI32();
ret.x = (int)v.x;
ret.y = (int)v.y;
ret.w = (int)v.z;
ret.h = (int)v.w;
return ret;
}
Rectangle MakeRectangle(PXCMRectI32 rect)
{
Rectangle rectangle = new Rectangle()
{
Width = ScaleCameraXToCanvasX(rect.w),
Height = ScaleCameraXToCanvasX(rect.h),
Stroke = Brushes.Silver
};
Canvas.SetLeft(rectangle, ScaleCameraXToCanvasX(rect.x));
Canvas.SetTop(rectangle, ScaleCameraYToCanvasY(rect.y));
return(rectangle);
}
Rectangle MakeRectangle(PXCMRectI32 rect)
{
Rectangle rectangle = new Rectangle()
{
Width = rect.w,
Height = rect.h,
Stroke = Brushes.Silver
};
Canvas.SetLeft(rectangle, rect.x);
Canvas.SetTop(rectangle, rect.y);
return(rectangle);
}
public override void Work(Graphics g)
{
data = module.CreateOutput();
data.Update();
PXCMHandData.IHand hands = null;
data.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, 0, out hands);
if (hands == null)
{
return;
}
PXCMRectI32 rect = hands.QueryBoundingBoxImage();
Rectangle rectangle = new Rectangle(rect.x, rect.y, rect.w, rect.h); // Convert to Rectangle
g.DrawRectangle(pen, rectangle); // Draw
}
private void CreateFaceObject(PXCMFaceData.Face face, ref FaceObject fObj)
{
PXCMFaceData.DetectionData detection = face.QueryDetection();
if (detection != null)
{
PXCMRectI32 faceRect = new PXCMRectI32();
detection.QueryBoundingRect(out faceRect);
PXCMFaceData.RecognitionData recogData = face.QueryRecognition();
int userID = -1;
if (recogData != null)
{
userID = recogData.QueryUserID();
}
//Registered Face
if (userID > 0)
{
//Get Face by USER-ID
fObj = this.GetRecognizedFaceByID(userID);
//Due to pre-loaded DB, FaceObject can be null
if (fObj == null)
{
fObj = new FaceObject();
}
}
float faceDistance;
detection.QueryFaceAverageDepth(out faceDistance);
faceDistance /= 1000;
Rectangle rect = new Rectangle(faceRect.x, faceRect.y, faceRect.w, faceRect.h);
//Point faceLoc = faceCamConfig.GetFaceLoc(rect);
Point faceLoc = faceCamConfig.Location;
fObj.ID = userID;
fObj.boundingBox = rect;
fObj.cellLocation = faceLoc;
fObj.registerTime = DateTime.Now;
fObj.lastSeen = DateTime.Now;
fObj.distance = faceDistance;
}
}
public void SetDetectionRect(PXCMRectI32 _Rect)
{
myfaceCube.transform.position = new Vector3(_Rect.x + 100f, -1 * _Rect.y - 120f) * 0.05f;
myfaceCube.transform.localScale = new Vector3(_Rect.w, _Rect.h) * 0.05f;
}
public override bool Process(Trigger trigger)
{
trigger.ErrorDetected = false;
if (!SenseToolkitManager.Instance.IsSenseOptionSet(SenseOption.SenseOptionID.Face))
{
trigger.ErrorDetected = true;
return false;
}
if (!(trigger is TrackTrigger))
{
trigger.ErrorDetected = true;
return false;
}
// make sure we have valid values
if (RealWorldBoxDimensions.x <= 0)
{
RealWorldBoxDimensions.x = 1;
}
if (RealWorldBoxDimensions.y <= 0)
{
RealWorldBoxDimensions.y = 1;
}
if (RealWorldBoxDimensions.z <= 0)
{
RealWorldBoxDimensions.z = 1;
}
if (SenseToolkitManager.Instance.Initialized
&&
SenseToolkitManager.Instance.FaceModuleOutput != null)
{
if (SenseToolkitManager.Instance.FaceModuleOutput.QueryNumberOfDetectedFaces() == 0)
{
((TrackTrigger)trigger).Position = Vector3.zero;
return false;
}
PXCMFaceData.Face singleFaceOutput = null;
singleFaceOutput = SenseToolkitManager.Instance.FaceModuleOutput.QueryFaceByIndex(FaceIndex);
bool success = false;
if (singleFaceOutput != null && singleFaceOutput.QueryUserID() >= 0)
{
// Process Tracking
if (trigger is TrackTrigger)
{
TrackTrigger specificTrigger = (TrackTrigger)trigger;
var landmarksData = singleFaceOutput.QueryLandmarks();
bool hasLandmarks = false;
if (landmarksData != null)
{
PXCMFaceData.LandmarkPoint outpt = null;
bool hasPoint = landmarksData.QueryPoint(landmarksData.QueryPointIndex(LandmarkToTrack), out outpt);
if (hasPoint)
{
hasLandmarks = outpt.confidenceWorld != 0;
}
}
if (!hasLandmarks && useBoundingBox)
{
PXCMRectI32 rect = new PXCMRectI32();
if (singleFaceOutput.QueryDetection().QueryBoundingRect(out rect))
{
float depth;
singleFaceOutput.QueryDetection().QueryFaceAverageDepth(out depth);
float bbCenterX = (rect.x + rect.w / 2);
float bbCenterY = (rect.y + rect.h / 2);
Vector3 vec = new Vector3();
if (_pos_ijz == null)
{
_pos_ijz = new PXCMPoint3DF32[1] { new PXCMPoint3DF32() };
}
_pos_ijz[0].x = bbCenterX;
_pos_ijz[0].y = bbCenterY;
_pos_ijz[0].z = depth;
if (_pos3d == null)
{
_pos3d = new PXCMPoint3DF32[1] { new PXCMPoint3DF32() };
}
SenseToolkitManager.Instance.Projection.ProjectDepthToCamera(_pos_ijz, _pos3d);
Vector3 position = new Vector3();
vec.x = _pos3d[0].x / 10f;
vec.y = _pos3d[0].y / 10f;
vec.z = _pos3d[0].z / 10f;
// Clamp and normalize to the Real World Box
TrackingUtilityClass.ClampToRealWorldInputBox(ref vec, RealWorldBoxCenter, RealWorldBoxDimensions);
TrackingUtilityClass.Normalize(ref vec, RealWorldBoxCenter, RealWorldBoxDimensions);
if (!float.IsNaN(vec.x) && !float.IsNaN(vec.y) && !float.IsNaN(vec.z))
{
specificTrigger.Position = vec;
return true;
}
}
else
{
specificTrigger.Position = Vector3.zero;
return false;
}
}
else if (landmarksData == null && !useBoundingBox)
{
specificTrigger.Position = Vector3.zero;
return false;
}
else
{
int landmarkId = landmarksData.QueryPointIndex(LandmarkToTrack);
PXCMFaceData.LandmarkPoint point = null;
landmarksData.QueryPoint(landmarkId, out point);
// Translation
if (point != null)
{
Vector3 vec = new Vector3();
vec.x = -point.world.x * 100f;
vec.y = point.world.y * 100f;
vec.z = point.world.z * 100f;
if (vec.x + vec.y + vec.z == 0)
{
specificTrigger.Position = Vector3.zero;
return false;
}
// Clamp and normalize to the Real World Box
TrackingUtilityClass.ClampToRealWorldInputBox(ref vec, RealWorldBoxCenter, RealWorldBoxDimensions);
TrackingUtilityClass.Normalize(ref vec, RealWorldBoxCenter, RealWorldBoxDimensions);
if (!float.IsNaN(vec.x) && !float.IsNaN(vec.y) && !float.IsNaN(vec.z))
{
specificTrigger.Position = vec;
success = true;
}
}
//Rotation
PXCMFaceData.PoseData poseData = singleFaceOutput.QueryPose();
if (success && poseData != null)
{
PXCMFaceData.PoseEulerAngles angles;
if (poseData.QueryPoseAngles(out angles))
{
if (!float.IsNaN(angles.pitch) && !float.IsNaN(angles.yaw) && !float.IsNaN(angles.roll))
{
Quaternion q = Quaternion.Euler(-angles.pitch, angles.yaw, -angles.roll);
specificTrigger.RotationQuaternion = q;
return true;
}
}
}
}
}
}
}
return false;
}
private void writeSignificantToFile(Hashtable iHashTable, PXCMRectI32 iBoundingRect, float iAverageDepth, PXCMFaceData.LandmarkPoint[] iLandmarkPoints, PXCMFaceData.PoseEulerAngles iEulerAngles, PXCMFaceData.PoseQuaternion iQuaternionAngles)
{
//write the timestamp
string toWrite = DateTime.Now.ToString("yyyyMMddHHmmssfff"); //First timestamp
double highPerfElapsed;
if (numLinesWritten == 0)
{
toWrite += ",0"; //high perf timestamp is 0 to indicate first ever line written
highPerformanceTimer.Start();
}
else
{
//stop the timer, then get the elapsed time with the Duration internal function
highPerformanceTimer.Stop();
highPerfElapsed = highPerformanceTimer.Duration;
//restart the timer, only missed timings are the time taken to set 1 value
//Likely to build up error over time
highPerformanceTimer.Start();
//write to the string builder
toWrite += "," + totalHighPerfTimeElapsed.ToString();
totalHighPerfTimeElapsed += highPerfElapsed;
//Console.WriteLine(totalHighPerfTimeElapsed);
//Console.WriteLine(DateTime.Now.Second);
}
//write current stim code then reset it
toWrite += "," + stimcode;
stimcode = 0;
//add all the expressiondata
var enumListFunction = Enum.GetNames(typeof(PXCMFaceData.ExpressionsData.FaceExpression));
for (int i = 0; i < enumListFunction.Length; i++)
{
toWrite += "," + iHashTable[enumListFunction[i]];
}
//add the bounding rectangle data
toWrite += "," + iBoundingRect.h + "," + iBoundingRect.w + "," + iBoundingRect.x + "," + iBoundingRect.y;
//add average depth data
toWrite += "," + iAverageDepth;
// add all landmarkpoints data
if (iLandmarkPoints != null)
{
for (int i = 0; i < iLandmarkPoints.Length; i++)
{
toWrite += "," + iLandmarkPoints[i].image.x + "," + iLandmarkPoints[i].image.y;
}
}
//add euler angles
toWrite += "," + iEulerAngles.pitch + "," + iEulerAngles.roll + "," + iEulerAngles.yaw;
//add quaternion angles
toWrite += "," + iQuaternionAngles.w + "," + iQuaternionAngles.x + "," + iQuaternionAngles.y + "," + iQuaternionAngles.z;
file.WriteLine(toWrite); //actually write the entire string to file
numLinesWritten++; //increase number of lines written counter
}
public override bool Process(Trigger trigger)
{
trigger.ErrorDetected = false;
if (!SenseToolkitManager.Instance.IsSenseOptionSet(SenseOption.SenseOptionID.Face))
{
trigger.ErrorDetected = true;
return(false);
}
if (!(trigger is TrackTrigger))
{
trigger.ErrorDetected = true;
return(false);
}
// make sure we have valid values
if (RealWorldBoxDimensions.x <= 0)
{
RealWorldBoxDimensions.x = 1;
}
if (RealWorldBoxDimensions.y <= 0)
{
RealWorldBoxDimensions.y = 1;
}
if (RealWorldBoxDimensions.z <= 0)
{
RealWorldBoxDimensions.z = 1;
}
if (SenseToolkitManager.Instance.Initialized
&&
SenseToolkitManager.Instance.FaceModuleOutput != null)
{
if (SenseToolkitManager.Instance.FaceModuleOutput.QueryNumberOfDetectedFaces() == 0)
{
((TrackTrigger)trigger).Position = Vector3.zero;
return(false);
}
PXCMFaceData.Face singleFaceOutput = null;
singleFaceOutput = SenseToolkitManager.Instance.FaceModuleOutput.QueryFaceByIndex(FaceIndex);
bool success = false;
if (singleFaceOutput != null && singleFaceOutput.QueryUserID() >= 0)
{
// Process Tracking
if (trigger is TrackTrigger)
{
TrackTrigger specificTrigger = (TrackTrigger)trigger;
var landmarksData = singleFaceOutput.QueryLandmarks();
bool hasLandmarks = false;
if (landmarksData != null)
{
PXCMFaceData.LandmarkPoint outpt = null;
bool hasPoint = landmarksData.QueryPoint(landmarksData.QueryPointIndex(LandmarkToTrack), out outpt);
if (hasPoint)
{
hasLandmarks = outpt.confidenceWorld != 0;
}
}
if (!hasLandmarks && useBoundingBox)
{
PXCMRectI32 rect = new PXCMRectI32();
if (singleFaceOutput.QueryDetection() != null && singleFaceOutput.QueryDetection().QueryBoundingRect(out rect))
{
float depth;
singleFaceOutput.QueryDetection().QueryFaceAverageDepth(out depth);
float bbCenterX = (rect.x + rect.w / 2);
float bbCenterY = (rect.y + rect.h / 2);
Vector3 vec = new Vector3();
if (_pos_ijz == null)
{
_pos_ijz = new PXCMPoint3DF32[1] {
new PXCMPoint3DF32()
};
}
_pos_ijz[0].x = bbCenterX;
_pos_ijz[0].y = bbCenterY;
_pos_ijz[0].z = depth;
if (_pos3d == null)
{
_pos3d = new PXCMPoint3DF32[1] {
new PXCMPoint3DF32()
};
}
SenseToolkitManager.Instance.Projection.ProjectDepthToCamera(_pos_ijz, _pos3d);
Vector3 position = new Vector3();
vec.x = _pos3d[0].x / 10f;
vec.y = _pos3d[0].y / 10f;
vec.z = _pos3d[0].z / 10f;
// Clamp and normalize to the Real World Box
TrackingUtilityClass.ClampToRealWorldInputBox(ref vec, RealWorldBoxCenter, RealWorldBoxDimensions);
TrackingUtilityClass.Normalize(ref vec, RealWorldBoxCenter, RealWorldBoxDimensions);
if (!float.IsNaN(vec.x) && !float.IsNaN(vec.y) && !float.IsNaN(vec.z))
{
specificTrigger.Position = vec;
return(true);
}
}
else
{
specificTrigger.Position = Vector3.zero;
return(false);
}
}
else if (landmarksData == null && !useBoundingBox)
{
specificTrigger.Position = Vector3.zero;
return(false);
}
else
{
int landmarkId = landmarksData.QueryPointIndex(LandmarkToTrack);
PXCMFaceData.LandmarkPoint point = null;
landmarksData.QueryPoint(landmarkId, out point);
// Translation
if (point != null)
{
Vector3 vec = new Vector3();
vec.x = -point.world.x * 100f;
vec.y = point.world.y * 100f;
vec.z = point.world.z * 100f;
if (vec.x + vec.y + vec.z == 0)
{
specificTrigger.Position = Vector3.zero;
return(false);
}
// Clamp and normalize to the Real World Box
TrackingUtilityClass.ClampToRealWorldInputBox(ref vec, RealWorldBoxCenter, RealWorldBoxDimensions);
TrackingUtilityClass.Normalize(ref vec, RealWorldBoxCenter, RealWorldBoxDimensions);
if (!float.IsNaN(vec.x) && !float.IsNaN(vec.y) && !float.IsNaN(vec.z))
{
specificTrigger.Position = vec;
success = true;
}
}
//Rotation
PXCMFaceData.PoseData poseData = singleFaceOutput.QueryPose();
if (success && poseData != null)
{
PXCMFaceData.PoseEulerAngles angles;
if (poseData.QueryPoseAngles(out angles))
{
if (!float.IsNaN(angles.pitch) && !float.IsNaN(angles.yaw) && !float.IsNaN(angles.roll))
{
Quaternion q = Quaternion.Euler(-angles.pitch, angles.yaw, -angles.roll);
specificTrigger.RotationQuaternion = q;
return(true);
}
}
}
}
}
}
}
return(false);
}
