public override bool Process(Trigger trigger)
{
trigger.ErrorDetected = false;
if (!SenseToolkitManager.Instance.IsSenseOptionSet(SenseOption.SenseOptionID.Hand))
{
trigger.ErrorDetected = true;
Debug.LogError("Hand Analysis Module Not Set");
return(false);
}
if (!(trigger is TrackTrigger))
{
trigger.ErrorDetected = true;
return(false);
}
bool success = 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.HandDataOutput != null)
{
PXCMHandData.IHand data = null;
if (SenseToolkitManager.Instance.HandDataOutput.QueryNumberOfHands() > 0 &&
((ContinuousTracking && SenseToolkitManager.Instance.HandDataOutput.QueryHandDataById(_uniqueID, out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
||
SenseToolkitManager.Instance.HandDataOutput.QueryHandData(WhichHand, HandIndex, out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
)
{
// Process Tracking
_uniqueID = data.QueryUniqueId();
PXCMHandData.JointData jointData;
data.QueryTrackedJoint(TrackedJoint, out jointData);
TrackTrigger specificTrigger = (TrackTrigger)trigger;
PXCMPoint3DF32 point = jointData.positionWorld;
Vector3 position = new Vector3(point.x, point.y, point.z);
position.x *= -100;
position.y *= 100;
position.z *= 100;
if (position.x + position.y + position.z == 0)
{
return(false);
}
TrackingUtilityClass.ClampToRealWorldInputBox(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
TrackingUtilityClass.Normalize(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
if (!float.IsNaN(position.x) && !float.IsNaN(position.y) && !float.IsNaN(position.z))
{
specificTrigger.Position = position;
}
else
{
return(false);
}
Quaternion q = new Quaternion(jointData.globalOrientation.x, jointData.globalOrientation.y,
jointData.globalOrientation.z, jointData.globalOrientation.w);
q = q * _zInvert * _yInvert;
specificTrigger.RotationQuaternion = q;
success = true;
}
}
else
{
return(false);
}
return(success);
}
public override bool Process(Trigger trigger)
{
trigger.ErrorDetected = false;
if (!SenseToolkitManager.Instance.IsSenseOptionSet(SenseOption.SenseOptionID.VideoDepthStream))
{
trigger.ErrorDetected = true;
Debug.LogError("Blob Module Not Set");
return(false);
}
if (!(trigger is EventTrigger))
{
trigger.ErrorDetected = true;
return(false);
}
EventTrigger specificTrigger = (EventTrigger)trigger;
specificTrigger.Source = this.name;
bool success = false;
if (SenseToolkitManager.Instance.Initialized && SenseToolkitManager.Instance.BlobDataOutput != null)
{
int numberOfBlobsDetected = SenseToolkitManager.Instance.BlobDataOutput.QueryNumberOfBlobs();
if (numberOfBlobsDetected >= NumberOfBlobs)
{
int blobsRightlyDetected = 0;
PXCMBlobData.IBlob iblob;
// For each detected blob, check that it is in our real world box
for (int i = 0; i < numberOfBlobsDetected; i++)
{
SenseToolkitManager.Instance.BlobDataOutput.QueryBlobByAccessOrder(i, PXCMBlobData.AccessOrderType.ACCESS_ORDER_NEAR_TO_FAR, out iblob);
// Process Tracking
PXCMPoint3DF32 trackedPoint = iblob.QueryExtremityPoint(BlobPointToTrack);
Vector3 position = new Vector3();
position.x = -trackedPoint.x / 10;
position.y = trackedPoint.y / 10;
position.z = trackedPoint.z / 10;
TrackingUtilityClass.ClampToRealWorldInputBox(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
TrackingUtilityClass.Normalize(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
if (!float.IsNaN(position.x) && !float.IsNaN(position.y) && !float.IsNaN(position.z))
{
if (position.x > 0 && position.x < 1 &&
position.y > 0 && position.y < 1 &&
position.z > 0 && position.z < 1)
{
blobsRightlyDetected++;
if (blobsRightlyDetected == NumberOfBlobs)
{
break;
}
}
}
else
{
_lastFrameDetected = false;
return(false);
}
}
if (blobsRightlyDetected < NumberOfBlobs)
{
if (_lastFrameDetected)
{
success = true;
}
_lastFrameDetected = false;
}
else
{
_lastFrameDetected = true;
}
}
else
{
if (_lastFrameDetected)
{
success = true;
}
_lastFrameDetected = false;
}
}
else
{
return(false);
}
return(success);
}
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);
}
bool success = 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)
{
return(false);
}
PXCMFaceData.Face singleFaceOutput = null;
singleFaceOutput = SenseToolkitManager.Instance.FaceModuleOutput.QueryFaceByIndex(FaceIndex);
if (singleFaceOutput != null && singleFaceOutput.QueryUserID() >= 0)
{
// Process Tracking
if (trigger is TrackTrigger)
{
TrackTrigger specificTrigger = (TrackTrigger)trigger;
var landmarksData = singleFaceOutput.QueryLandmarks();
if (landmarksData == null)
{
return(false);
}
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 * 100;
vec.y = point.world.y * 100;
vec.z = point.world.z * 100;
if (vec.x + vec.y + vec.z == 0)
{
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 (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;
success = true;
}
}
}
}
}
return(success);
}
return(success);
}
public override bool Process(Trigger trigger)
{
trigger.ErrorDetected = false;
if (!SenseToolkitManager.Instance.IsSenseOptionSet(SenseOption.SenseOptionID.Object))
{
trigger.ErrorDetected = true;
Debug.LogError("Object Tracking Module Not Set");
return(false);
}
if (!(trigger is TrackTrigger))
{
trigger.ErrorDetected = true;
return(false);
}
bool success = 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;
}
var tracker = SenseToolkitManager.Instance.SenseManager.QueryTracker();
if (SenseToolkitManager.Instance.Initialized && tracker != null)
{
if (_trackerID < 0)
{
if (tracker.Set2DTrackFromFile(Tracker2DPath, out _trackerID, WidthInMM, HeightInMM, QualityThershold, IsExtensible) < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
trigger.ErrorDetected = true;
Debug.LogError("Cannot set 2D image. Make sure it is a valid image (png)");
return(false);
}
}
PXCMTracker.TrackingValues trackingValues;
if (tracker.QueryNumberTrackingValues() > 0)
{
if (tracker.QueryTrackingValues(_trackerID, out trackingValues) < pxcmStatus.PXCM_STATUS_NO_ERROR || trackingValues.state != PXCMTracker.ETrackingState.ETS_TRACKING)
{
return(false);
}
// Process Tracking
if (trigger is TrackTrigger)
{
TrackTrigger specificTrigger = (TrackTrigger)trigger;
Vector3 position = new Vector3(trackingValues.translation.x, trackingValues.translation.y, trackingValues.translation.z);
position.x *= -0.1f;
position.y *= 0.1f;
position.z *= 0.1f;
TrackingUtilityClass.ClampToRealWorldInputBox(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
TrackingUtilityClass.Normalize(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
if (!float.IsNaN(position.x) && !float.IsNaN(position.y) && !float.IsNaN(position.z))
{
specificTrigger.Position = position;
}
else
{
return(false);
}
Quaternion q = new Quaternion(trackingValues.rotation.x, -trackingValues.rotation.y,
-trackingValues.rotation.z, trackingValues.rotation.w);
specificTrigger.RotationQuaternion = q;
success = true;
}
}
}
else
{
return(false);
}
return(success);
}
public override bool Process(Trigger trigger)
{
trigger.ErrorDetected = false;
if (!SenseToolkitManager.Instance.IsSenseOptionSet(SenseOption.SenseOptionID.VideoDepthStream))
{
trigger.ErrorDetected = true;
Debug.LogError("Blob Analysis Module Not Set");
return(false);
}
if (!(trigger is TrackTrigger))
{
trigger.ErrorDetected = true;
return(false);
}
bool success = 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 != null && SenseToolkitManager.Instance.Initialized && SenseToolkitManager.Instance.BlobExtractor != null && SenseToolkitManager.Instance.ImageDepthOutput != null)
{
// Setting max distance for this rule and process the image
PXCMBlobExtractor.BlobData _blobData = new PXCMBlobExtractor.BlobData();
SenseToolkitManager.Instance.BlobExtractor.SetMaxDistance(MaxDistance * 10);
var sts = SenseToolkitManager.Instance.BlobExtractor.ProcessImage(SenseToolkitManager.Instance.ImageDepthOutput);
if (sts >= pxcmStatus.PXCM_STATUS_NO_ERROR && SenseToolkitManager.Instance.BlobExtractor.QueryNumberOfBlobs() > 0)
{
if (BlobIndex >= SenseToolkitManager.Instance.BlobExtractor.QueryNumberOfBlobs())
{
return(false);
}
PXCMImage.ImageInfo info = SenseToolkitManager.Instance.ImageDepthOutput.QueryInfo();
info.format = PXCMImage.PixelFormat.PIXEL_FORMAT_Y8;
PXCMImage new_image = SenseToolkitManager.Instance.SenseManager.session.CreateImage(info);
// Process Tracking
SenseToolkitManager.Instance.BlobExtractor.QueryBlobData(BlobIndex, new_image, out _blobData);
new_image.Dispose();
TrackTrigger specificTrigger = (TrackTrigger)trigger;
PXCMPointI32 trackedPoint = BlobUtilityClass.GetTrackedPoint(_blobData, BlobPointToTrack);
PXCMImage.ImageData data;
SenseToolkitManager.Instance.ImageDepthOutput.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH_F32, out data);
if (_depthArray == null)
{
_depthArray = new float[SenseToolkitManager.Instance.ImageDepthOutput.info.width * SenseToolkitManager.Instance.ImageDepthOutput.info.height];
}
data.ToFloatArray(0, _depthArray);
float depth = _depthArray[(int)trackedPoint.x + (int)trackedPoint.y * SenseToolkitManager.Instance.ImageDepthOutput.info.width];
if (_pos_uvz == null)
{
_pos_uvz = new PXCMPoint3DF32[1] {
new PXCMPoint3DF32()
};
}
_pos_uvz[0].x = trackedPoint.x;
_pos_uvz[0].y = trackedPoint.y;
_pos_uvz[0].z = depth;
if (_pos3d == null)
{
_pos3d = new PXCMPoint3DF32[1] {
new PXCMPoint3DF32()
};
}
SenseToolkitManager.Instance.Projection.ProjectDepthToCamera(_pos_uvz, _pos3d);
Vector3 position = new Vector3();
position.x = -_pos3d[0].x / 10;
position.y = _pos3d[0].y / 10;
position.z = _pos3d[0].z / 10;
SenseToolkitManager.Instance.ImageDepthOutput.ReleaseAccess(data);
TrackingUtilityClass.ClampToRealWorldInputBox(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
TrackingUtilityClass.Normalize(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
if (!float.IsNaN(position.x) && !float.IsNaN(position.y) && !float.IsNaN(position.z))
{
specificTrigger.Position = position;
}
else
{
return(false);
}
success = true;
}
}
else
{
return(false);
}
return(success);
}
public override bool Process(Trigger trigger)
{
trigger.ErrorDetected = false;
if (!SenseToolkitManager.Instance.IsSenseOptionSet(SenseOption.SenseOptionID.Hand))
{
Debug.LogError("Hand Analysis Module Not Set");
trigger.ErrorDetected = true;
return(false);
}
if (!(trigger is TranslationTrigger))
{
trigger.ErrorDetected = true;
return(false);
}
bool success = 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.HandDataOutput != null)
{
PXCMHandData.IHand data = null;
if (SenseToolkitManager.Instance.HandDataOutput.QueryNumberOfHands() > 0
&&
(
(ContinuousTracking && SenseToolkitManager.Instance.HandDataOutput.QueryHandDataById(_uniqueID, out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
||
SenseToolkitManager.Instance.HandDataOutput.QueryHandData(WhichHand, HandIndex, out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
)
{
_uniqueID = data.QueryUniqueId();
// Process Translation
if (trigger is TranslationTrigger)
{
TranslationTrigger specificTrigger = (TranslationTrigger)trigger;
PXCMHandData.JointData jointData;
data.QueryTrackedJoint(TrackedJoint, out jointData);
Vector3 vecPos = new Vector3(-(jointData.positionWorld.x), (jointData.positionWorld.y), jointData.positionWorld.z);
vecPos.x *= 100;
vecPos.y *= 100;
vecPos.z *= 100;
TrackingUtilityClass.ClampToRealWorldInputBox(ref vecPos, RealWorldBoxCenter, RealWorldBoxDimensions);
TrackingUtilityClass.Normalize(ref vecPos, RealWorldBoxCenter, RealWorldBoxDimensions);
if (specificTrigger.Restart)
{
specificTrigger.Restart = false;
_referencePosition = vecPos;
return(false);
}
specificTrigger.Translation = vecPos - _referencePosition;
Vector3 passCriteria = new Vector3();
passCriteria.x = Mathf.Abs(vecPos.x) - Mathf.Abs(Thresholds.x);
passCriteria.y = Mathf.Abs(vecPos.y) - Mathf.Abs(Thresholds.y);
passCriteria.z = Mathf.Abs(vecPos.z) - Mathf.Abs(Thresholds.z);
if (passCriteria.x > 0 || passCriteria.y > 0 || passCriteria.z > 0)
{
_referencePosition = vecPos;
success = true;
}
}
}
}
else
{
return(false);
}
return(success);
}
public override bool Process(Trigger trigger)
{
trigger.ErrorDetected = false;
if (!SenseToolkitManager.Instance.IsSenseOptionSet(SenseOption.SenseOptionID.VideoDepthStream))
{
trigger.ErrorDetected = true;
Debug.LogError("Depth Stream Not Set");
return(false);
}
if (!(trigger is EventTrigger))
{
trigger.ErrorDetected = true;
return(false);
}
EventTrigger specificTrigger = (EventTrigger)trigger;
specificTrigger.Source = this.name;
bool success = false;
if (SenseToolkitManager.Instance.Initialized && SenseToolkitManager.Instance.BlobExtractor != null && SenseToolkitManager.Instance.ImageDepthOutput != null)
{
// Setting max distance for this rule and process the image
SenseToolkitManager.Instance.BlobExtractor.SetMaxDistance(MaxDistance * 10);
var sts = SenseToolkitManager.Instance.BlobExtractor.ProcessImage(SenseToolkitManager.Instance.ImageDepthOutput);
if (sts == pxcmStatus.PXCM_STATUS_NO_ERROR && SenseToolkitManager.Instance.BlobExtractor.QueryNumberOfBlobs() >= NumberOfBlobs)
{
int numberOfBlobsDetected = SenseToolkitManager.Instance.BlobExtractor.QueryNumberOfBlobs();
int blobsRightlyDetected = 0;
PXCMBlobExtractor.BlobData blobData = new PXCMBlobExtractor.BlobData();
// For each detected blob, project the tracked point to the real world and
// check that it is in our real world box
for (int i = 0; i < numberOfBlobsDetected; i++)
{
PXCMImage.ImageInfo info = SenseToolkitManager.Instance.ImageDepthOutput.QueryInfo();
info.format = PXCMImage.PixelFormat.PIXEL_FORMAT_Y8;
PXCMImage new_image = SenseToolkitManager.Instance.SenseManager.session.CreateImage(info);
// Process Tracking
SenseToolkitManager.Instance.BlobExtractor.QueryBlobData(i, new_image, out blobData);
new_image.Dispose();
PXCMPointI32 trackedPoint = BlobUtilityClass.GetTrackedPoint(blobData, BlobPointToTrack);
PXCMImage.ImageData data;
SenseToolkitManager.Instance.ImageDepthOutput.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH_F32, out data);
if (_depthArray == null)
{
_depthArray = new float[SenseToolkitManager.Instance.ImageDepthOutput.info.width * SenseToolkitManager.Instance.ImageDepthOutput.info.height];
}
data.ToFloatArray(0, _depthArray);
float depth = _depthArray[(int)trackedPoint.x + (int)trackedPoint.y * SenseToolkitManager.Instance.ImageDepthOutput.info.width];
if (_pos_uvz == null)
{
_pos_uvz = new PXCMPoint3DF32[1] {
new PXCMPoint3DF32()
};
}
_pos_uvz[0].x = trackedPoint.x;
_pos_uvz[0].y = trackedPoint.y;
_pos_uvz[0].z = depth;
if (_pos3d == null)
{
_pos3d = new PXCMPoint3DF32[1] {
new PXCMPoint3DF32()
};
}
SenseToolkitManager.Instance.Projection.ProjectDepthToCamera(_pos_uvz, _pos3d);
Vector3 position = new Vector3();
position.x = -_pos3d[0].x / 10;
position.y = _pos3d[0].y / 10;
position.z = _pos3d[0].z / 10;
SenseToolkitManager.Instance.ImageDepthOutput.ReleaseAccess(data);
TrackingUtilityClass.ClampToRealWorldInputBox(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
TrackingUtilityClass.Normalize(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
if (!float.IsNaN(position.x) && !float.IsNaN(position.y) && !float.IsNaN(position.z))
{
if (position.x > 0 && position.x < 1 &&
position.y > 0 && position.y < 1 &&
position.z > 0 && position.z < 1)
{
blobsRightlyDetected++;
if (blobsRightlyDetected == NumberOfBlobs)
{
break;
}
}
}
else
{
_lastFrameDetected = false;
return(false);
}
}
if (blobsRightlyDetected >= NumberOfBlobs)
{
if (!_lastFrameDetected)
{
success = true;
}
_lastFrameDetected = true;
}
else
{
_lastFrameDetected = false;
}
}
else
{
_lastFrameDetected = false;
}
}
else
{
return(false);
}
return(success);
}
public override bool Process(Trigger trigger)
{
trigger.ErrorDetected = false;
if (!SenseToolkitManager.Instance.IsSenseOptionSet(SenseOption.SenseOptionID.Blob))
{
trigger.ErrorDetected = true;
Debug.LogError("Blob Module Not Set");
return(false);
}
if (!(trigger is TrackTrigger))
{
trigger.ErrorDetected = true;
return(false);
}
bool success = 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 != null && SenseToolkitManager.Instance.Initialized && SenseToolkitManager.Instance.BlobDataOutput != null)
{
int numberOfBlobsDetected = SenseToolkitManager.Instance.BlobDataOutput.QueryNumberOfBlobs();
PXCMBlobData.IBlob iblob;
// GZ: do we need to set this one more time ???
// Setting max distance for this rule and process the image
// SenseToolkitManager.Instance.BlobExtractor.SetMaxDistance(MaxDistance * 10);
if (numberOfBlobsDetected > 0)
{
if (BlobIndex >= numberOfBlobsDetected)
{
return(false);
}
// Process Tracking
SenseToolkitManager.Instance.BlobDataOutput.QueryBlobByAccessOrder(BlobIndex, PXCMBlobData.AccessOrderType.ACCESS_ORDER_NEAR_TO_FAR, out iblob);
TrackTrigger specificTrigger = (TrackTrigger)trigger;
PXCMPoint3DF32 trackedPoint = iblob.QueryExtremityPoint(BlobPointToTrack);
Vector3 position = new Vector3();
position.x = -trackedPoint.x / 10;
position.y = trackedPoint.y / 10;
position.z = trackedPoint.z / 10;
TrackingUtilityClass.ClampToRealWorldInputBox(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
TrackingUtilityClass.Normalize(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
if (!float.IsNaN(position.x) && !float.IsNaN(position.y) && !float.IsNaN(position.z))
{
specificTrigger.Position = position;
}
else
{
return(false);
}
success = true;
}
}
else
{
return(false);
}
return(success);
}
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);
}