public float getDist(Vector3DF point1, Vector3DF point2)
{
float deltaX = point1.X - point2.X;
float deltaY = point1.Y - point2.Y;
float deltaZ = point1.Z - point2.Z;
float distance = (float)Math.Sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ);
return distance;
}
public bool Equals(Vector3DF other)
{
if (X != other.X)
{
return(false);
}
if (Y != other.Y)
{
return(false);
}
return(Z == other.Z);
}
/// <summary>
/// Helper API to retrieve head points structure from a given skeleton instance
/// </summary>
/// <param name="skeletonOfInterest">
/// skeleton from which head points are to be extracted
/// </param>
/// <returns>
/// HeadPoints that can be passed to Start/Continue tracking APIs
/// </returns>
private static Vector3DF[] GetHeadPointsFromSkeleton(Skeleton skeletonOfInterest)
{
Vector3DF[] headPoints = null;
if (skeletonOfInterest != null && skeletonOfInterest.TrackingState == SkeletonTrackingState.Tracked)
{
headPoints = new Vector3DF[2];
SkeletonPoint sp0 = skeletonOfInterest.Joints[JointType.ShoulderCenter].Position;
headPoints[0] = new Vector3DF(sp0.X, sp0.Y, sp0.Z);
SkeletonPoint sp1 = skeletonOfInterest.Joints[JointType.Head].Position;
headPoints[1] = new Vector3DF(sp1.X, sp1.Y, sp1.Z);
}
return(headPoints);
}
public double getDist(Vector3DF point1, Vector3DF point2)
{
double X1 = point1.X;
double Y1 = point1.Y;
double Z1 = point1.Z;
double X2 = point2.X;
double Y2 = point2.Y;
double Z2 = point2.Z;
double deltaX = X1 - X2;
double deltaY = Y1 - Y2;
double deltaZ = Z1 - Z2;
double distance = Math.Sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ);
return distance;
}
private PointF[] GetProjected3DShape(
CameraConfig videoCameraConfig,
float zoomFactor,
Point viewOffset,
IntPtr shapeUnitCoeffPtr,
uint shapeUnitCoeffCount,
IntPtr animUnitCoeffPtr,
uint animUnitCoeffCount,
float scale,
Vector3DF rotation,
Vector3DF translation)
{
this.CheckPtrAndThrow();
PointF[] faceModelProjected3DShape = null;
uint vertexCount = this.VertexCount;
IntPtr faceModel3DVerticesPtr = IntPtr.Zero;
if (shapeUnitCoeffPtr == IntPtr.Zero || shapeUnitCoeffCount == 0)
{
throw new ArgumentException("Invalid shape unit co-efficients", "shapeUnitCoeffPtr");
}
if (animUnitCoeffPtr == IntPtr.Zero || animUnitCoeffCount == 0)
{
throw new ArgumentException("Invalid animation unit co-efficients", "animUnitCoeffPtr");
}
if (vertexCount > 0)
{
try
{
faceModel3DVerticesPtr = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(Vector3DF)) * (int)vertexCount);
this.faceTrackingModelPtr.GetProjectedShape(
videoCameraConfig,
zoomFactor,
viewOffset,
shapeUnitCoeffPtr,
shapeUnitCoeffCount,
animUnitCoeffPtr,
animUnitCoeffCount,
scale,
ref rotation,
ref translation,
faceModel3DVerticesPtr,
vertexCount);
faceModelProjected3DShape = new PointF[vertexCount];
for (int i = 0; i < (int)vertexCount; i++)
{
IntPtr faceModel3DVerticesIthPtr;
if (IntPtr.Size == 8)
{
// 64bit
faceModel3DVerticesIthPtr = new IntPtr(faceModel3DVerticesPtr.ToInt64() + (i * Marshal.SizeOf(typeof(PointF))));
}
else
{
// 32bit
faceModel3DVerticesIthPtr = new IntPtr(faceModel3DVerticesPtr.ToInt32() + (i * Marshal.SizeOf(typeof(PointF))));
}
faceModelProjected3DShape[i] = (PointF)Marshal.PtrToStructure(faceModel3DVerticesIthPtr, typeof(PointF));
}
}
finally
{
if (faceModel3DVerticesPtr != IntPtr.Zero)
{
Marshal.FreeHGlobal(faceModel3DVerticesPtr);
}
}
}
return faceModelProjected3DShape;
}
/// <summary>
/// Helper API to retrieve head points structure from a given skeleton instance
/// </summary>
/// <param name="skeletonOfInterest">
/// skeleton from which head points are to be extracted
/// </param>
/// <returns>
/// HeadPoints that can be passed to Start/Continue tracking APIs
/// </returns>
private static Vector3DF[] GetHeadPointsFromSkeleton(Skeleton skeletonOfInterest)
{
Vector3DF[] headPoints = null;
if (skeletonOfInterest != null && skeletonOfInterest.TrackingState == SkeletonTrackingState.Tracked)
{
headPoints = new Vector3DF[2];
SkeletonPoint sp0 = skeletonOfInterest.Joints[JointType.ShoulderCenter].Position;
headPoints[0] = new Vector3DF(sp0.X, sp0.Y, sp0.Z);
SkeletonPoint sp1 = skeletonOfInterest.Joints[JointType.Head].Position;
headPoints[1] = new Vector3DF(sp1.X, sp1.Y, sp1.Z);
}
return headPoints;
}
/// <summary>
/// Updates the face tracking information for this skeleton
/// </summary>
//public static System.IO.Ports.SerialPort serialPort1;
//private System.IO.Ports.SerialPort serialPort1;
//serialPort1 = new SerialPort();
internal void OnFrameReady(KinectSensor kinectSensor, ColorImageFormat colorImageFormat, byte[] colorImage, DepthImageFormat depthImageFormat, short[] depthImage, Skeleton skeletonOfInterest)
{
this.skeletonTrackingState = skeletonOfInterest.TrackingState;
if (this.skeletonTrackingState != SkeletonTrackingState.Tracked)
{
// nothing to do with an untracked skeleton.
return;
}
if (this.faceTracker == null)
{
try
{
this.faceTracker = new FaceTracker(kinectSensor);
}
catch (InvalidOperationException)
{
// During some shutdown scenarios the FaceTracker
// is unable to be instantiated. Catch that exception
// and don't track a face.
Debug.WriteLine("AllFramesReady - creating a new FaceTracker threw an InvalidOperationException");
this.faceTracker = null;
}
}
/*System.ComponentModel.IContainer components = new System.ComponentModel.Container();
serialPort1 = new System.IO.Ports.SerialPort(components); // Creating the new object.
serialPort1.PortName = "COM3"; //+ numCom.Value.ToString(); // Setting what port number.
serialPort1.BaudRate = 9600; // Setting baudrate.
serialPort1.DtrEnable = true; // Enable the Data Terminal Ready
serialPort1.Open(); // Open the port for use.*/
if (this.faceTracker != null)
{
FaceTrackFrame frame = this.faceTracker.Track(
colorImageFormat, colorImage, depthImageFormat, depthImage, skeletonOfInterest);
this.lastFaceTrackSucceeded = frame.TrackSuccessful;
if (this.lastFaceTrackSucceeded)
{
if (faceTriangles == null)
{
// only need to get this once. It doesn't change.
faceTriangles = frame.GetTriangles();
}
this.facePoints = frame.GetProjected3DShape();
rotation = frame.Rotation;
float yRotation = frame.Rotation.Y;
Debug.WriteLine(yRotation);
if ((yRotation <= -25))
{
Debug.WriteLine("a");
//port.Write("a");
if (!serialPort1.IsOpen)
{
try
{
serialPort1.Open();
serialPort1.Write("a");
serialPort1.Close();
}
catch
{
MessageBox.Show("There was an error. Please make sure that the correct port was selected, and the device, plugged in.");
}
}
//serialPort1.Write("1");
}
if ((yRotation > -25) && (yRotation < -10))
{
Debug.WriteLine("b");
//port.Write("a");
if (!serialPort1.IsOpen)
{
try
{
serialPort1.Open();
serialPort1.Write("b");
serialPort1.Close();
}
catch
{
MessageBox.Show("There was an error. Please make sure that the correct port was selected, and the device, plugged in.");
}
}
//serialPort1.Write("1");
}
if ((yRotation >= -10) && (yRotation < 10))
{
Debug.WriteLine("c");
//port.Write("a");
if (!serialPort1.IsOpen)
{
try
{
serialPort1.Open();
serialPort1.Write("c");
serialPort1.Close();
}
catch
{
MessageBox.Show("There was an error. Please make sure that the correct port was selected, and the device, plugged in.");
}
}
//serialPort1.Write("1");
}
if ((yRotation >= 10) && (yRotation < 20))
{
Debug.WriteLine("d");
//port.Write("a");
if (!serialPort1.IsOpen)
{
try
{
serialPort1.Open();
serialPort1.Write("d");
serialPort1.Close();
}
catch
{
MessageBox.Show("There was an error. Please make sure that the correct port was selected, and the device, plugged in.");
}
}
//serialPort1.Write("1");
}
if ((yRotation >= 20) && (yRotation < 30))
{
Debug.WriteLine("e");
//port.Write("a");
if (!serialPort1.IsOpen)
{
try
{
serialPort1.Open();
serialPort1.Write("e");
serialPort1.Close();
}
catch
{
MessageBox.Show("There was an error. Please make sure that the correct port was selected, and the device, plugged in.");
}
}
//serialPort1.Write("1");
}
if ((yRotation >= 30))
{
Debug.WriteLine("f");
//port.Write("a");
if (!serialPort1.IsOpen)
{
try
{
serialPort1.Open();
serialPort1.Write("f");
serialPort1.Close();
}
catch
{
MessageBox.Show("There was an error. Please make sure that the correct port was selected, and the device, plugged in.");
}
}
//serialPort1.Write("1");
}
}
}
}
public bool Equals(Vector3DF other)
{
if (X != other.X)
{
return false;
}
if (Y != other.Y)
{
return false;
}
return Z == other.Z;
}
private PointF[] GetProjected3DShape(
CameraConfig videoCameraConfig,
float zoomFactor,
Point viewOffset,
IntPtr shapeUnitCoeffPtr,
uint shapeUnitCoeffCount,
IntPtr animUnitCoeffPtr,
uint animUnitCoeffCount,
float scale,
Vector3DF rotation,
Vector3DF translation)
{
this.CheckPtrAndThrow();
PointF[] faceModelProjected3DShape = null;
uint vertexCount = this.VertexCount;
IntPtr faceModel3DVerticesPtr = IntPtr.Zero;
if (shapeUnitCoeffPtr == IntPtr.Zero || shapeUnitCoeffCount == 0)
{
throw new ArgumentException("Invalid shape unit co-efficients", "shapeUnitCoeffPtr");
}
if (animUnitCoeffPtr == IntPtr.Zero || animUnitCoeffCount == 0)
{
throw new ArgumentException("Invalid animation unit co-efficients", "animUnitCoeffPtr");
}
if (vertexCount > 0)
{
try
{
faceModel3DVerticesPtr = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(Vector3DF)) * (int)vertexCount);
this.faceTrackingModelPtr.GetProjectedShape(
videoCameraConfig,
zoomFactor,
viewOffset,
shapeUnitCoeffPtr,
shapeUnitCoeffCount,
animUnitCoeffPtr,
animUnitCoeffCount,
scale,
ref rotation,
ref translation,
faceModel3DVerticesPtr,
vertexCount);
faceModelProjected3DShape = new PointF[vertexCount];
for (int i = 0; i < (int)vertexCount; i++)
{
IntPtr faceModel3DVerticesIthPtr;
if (IntPtr.Size == 8)
{
// 64bit
faceModel3DVerticesIthPtr = new IntPtr(faceModel3DVerticesPtr.ToInt64() + (i * Marshal.SizeOf(typeof(PointF))));
}
else
{
// 32bit
faceModel3DVerticesIthPtr = new IntPtr(faceModel3DVerticesPtr.ToInt32() + (i * Marshal.SizeOf(typeof(PointF))));
}
faceModelProjected3DShape[i] = (PointF)Marshal.PtrToStructure(faceModel3DVerticesIthPtr, typeof(PointF));
}
}
finally
{
if (faceModel3DVerticesPtr != IntPtr.Zero)
{
Marshal.FreeHGlobal(faceModel3DVerticesPtr);
}
}
}
return(faceModelProjected3DShape);
}
private double getDistance(Vector3DF v1, Vector3DF v2)
{
return Math.Sqrt((v1.X-v2.X)*(v1.X-v2.X)+(v1.Y-v2.Y)*(v1.Y-v2.Y)+(v1.Z-v2.Z)*(v1.Z-v2.Z));
}
public void Merge(Face faceNew)
{
foreach (Guid ad in faceNew.SecondsFacing.Keys) {
if (!Double.IsNaN(faceNew.SecondsFacing[ad])) {
if (Double.IsNaN(this.SecondsFacing[ad])) {
this.SecondsFacing[ad] = 0;
}
this.SecondsFacing[ad] += faceNew.SecondsFacing[ad];
}
}
if (this.IsFacingAd && faceNew.IsFacingAd) {
double newSeconds = (faceNew.LastTimeFacingAd - this.LastTimeFacingAd).TotalSeconds;
if (Double.IsNaN(this.SecondsFacing[AdView.currentAd.id]))
this.SecondsFacing[AdView.currentAd.id] = 0;
this.SecondsFacing[AdView.currentAd.id] += newSeconds;
}
double secs = (faceNew.LastVectorTime - this.LastVectorTime).TotalSeconds;
if (faceNew.Frame != null && secs > 0.15) {
Vector3DF newVector = getVector(faceNew.Frame);
this.Velocity = getDistance(this.Vector, newVector) / secs;
this.Vector = newVector;
this.LastVectorTime = faceNew.LastVectorTime;
}
this.Frame = faceNew.Frame;
// just added this stuff, need to check
}