Math bug fixed, vectors now made from head to hands, libraries added.…

… Performance still terrible.

SpatialController/SpatialController.sln

@@ -1,7 +1,7 @@
 
 Microsoft Visual Studio Solution File, Format Version 11.00
 # Visual Studio 2010
-Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "SpatialController", "TrackingNI\SpatialController.csproj", "{859CF4F2-ACA7-47DB-8B2F-0C6084D1E748}"
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "SpatialController", "SpatialController\SpatialController.csproj", "{859CF4F2-ACA7-47DB-8B2F-0C6084D1E748}"
 EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution

SpatialController/SpatialController/MainWindow.xaml.cs

@@ -15,6 +15,7 @@
 using System.Windows.Documents;
 using System.Windows.Input;
 using System.Windows.Media;
+using System.Windows.Media.Media3D;
 using System.Windows.Media.Imaging;
 using System.Windows.Navigation;
 using System.Windows.Shapes;
@@ -96,14 +97,17 @@ public MainWindow()
             worker = new BackgroundWorker();
             stop = false;
 
+            Device mock1 = new Device(new Vector3D(5, 5, 5));
+            Device mock2 = new Device(new Vector3D(-5, 5, -5));
+
             spatialWorker = new BackgroundWorker();
             if (File.Exists(SpatialController.CALIBRATION_DATA_FILE))
             {
-                spatialController = new SpatialController(ControllerStartup.FromFile, userGenerator);
+                spatialController = new SpatialController(ControllerStartup.FromFile, userGenerator, mock1, mock2);
             }
             else
             {
-                spatialController = new SpatialController(ControllerStartup.Calibrate, userGenerator);
+                spatialController = new SpatialController(ControllerStartup.Calibrate, userGenerator, mock1, mock2);
             }
 
             CompositionTarget.Rendering += new EventHandler(WorkerExec);
@@ -181,7 +185,7 @@ private void WorkerTick(object sender, DoWorkEventArgs e)
         {
             Dispatcher.BeginInvoke((Action)delegate
             {
-                imgDepth.Source = DepthImageSourceCorrected;
+                imgDepth.Source = DepthImageSource;
             });
         }
 
@@ -290,7 +294,7 @@ public ImageSource DepthImageSource
                 }
 
                 //DepthCorrection.Fix(ref depthBitmap, depthData.XRes, depthData.YRes);
-                skeletonDraw.DrawStickFigure(ref depthBitmap, depthGenerator, depthData, userGenerator);
+                skeletonDraw.DrawStickFigure(ref depthBitmap, depthGenerator, depthData, userGenerator, spatialController.RaysToBeAnimated);
 
                 return depthBitmap;
             }
@@ -328,7 +332,7 @@ public ImageSource DepthImageSourceCorrected
                 }
 
                 DepthCorrection.Fix(ref depthBitmapCorrected, depthData.XRes, depthData.YRes);
-                skeletonDraw.DrawStickFigure(ref depthBitmapCorrected, depthGenerator, depthData, userGenerator);
+                skeletonDraw.DrawStickFigure(ref depthBitmapCorrected, depthGenerator, depthData, userGenerator, spatialController.RaysToBeAnimated);
 
                 return depthBitmapCorrected;
             }

SpatialController/SpatialController/Ray3D.cs

@@ -4,9 +4,11 @@
 using System.Text;
 using System.Windows.Media.Media3D;
 
+using xn;
+
 namespace TrackingNI
 {
-    class Ray3D
+    public class Ray3D
     {
         private const double MAX_CALIBRATION_DISTANCE = 10.0;
         private const double MAX_COMMAND_DISTANCE = 10.0;
@@ -88,12 +90,14 @@ public Vector3D intersectionWith(Ray3D other)
         // and the given is in front of the line; this returns false otherwise.
         public bool closeTo(Vector3D otherPoint)
         {
+            Console.Write("Point at (" + otherPoint.X + "," + otherPoint.Y + "," + otherPoint.Z + ")");
+
             // This is based on the algorithm above, simplified due to the second entity
             // just being a point rather than a line.
-            Vector3D p1 = p0;
-            Vector3D p2 = p1;
+            Vector3D p1 = this.p0;
+            Vector3D p2 = this.p1;
             Vector3D p3 = otherPoint;
-
+            
             Vector3D p31 = p3 - p1;
             Vector3D p21 = p2 - p1;
 
@@ -105,7 +109,9 @@ public bool closeTo(Vector3D otherPoint)
             Vector3D resultingPoint = new Vector3D((float)(p1.X + mu * p21.X),
                     (float)(p1.Y + mu * p21.Y), (float)(p1.Z + mu * p21.Z));
 
-            return distance3D(p3, resultingPoint) <= MAX_COMMAND_DISTANCE && this.pointsToward(p3);
+            double distance = distance3D(p3, resultingPoint);
+            Console.Write("Distance (ray to point) = " + distance);
+            return distance <= MAX_COMMAND_DISTANCE && this.pointsToward(p3);
         }
 
         // Returns whether the line points toward p--simply, whether p is closer to
@@ -122,5 +128,29 @@ private static double distance3D(Vector3D p0, Vector3D p1)
             double dz = p0.Z - p1.Z;
             return Math.Sqrt(dx * dx + dy * dy + dz * dz);
         }
+
+        public SkeletonJointPosition SkeletonJointPosition0()
+        {
+            SkeletonJointPosition skp = new SkeletonJointPosition();
+            skp.position.X = (float)p0.X;
+            skp.position.Y = (float)p0.Y;
+            skp.position.Z = (float)p0.Z;
+            return skp;
+        }
+
+        public SkeletonJointPosition SkeletonJointPosition1()
+        {
+            SkeletonJointPosition skp = new SkeletonJointPosition();
+            skp.position.X = (float)p1.X;
+            skp.position.Y = (float)p1.Y;
+            skp.position.Z = (float)p1.Z;
+            return skp;
+        }
+
+        public override String ToString()
+        {
+            return "FROM (" + p0.X + "," + p0.Y + "," + p0.Z + ") "
+                    + "TO (" + p1.X + "," + p1.Y + "," + p1.Z + ")";
+        }
     }
 }

SpatialController/SpatialController/SkeletonDraw.cs

@@ -18,7 +18,8 @@ public class SkeletonDraw
     {
         private DepthGenerator depthGenerator;
 
-        public void DrawStickFigure(ref WriteableBitmap image, DepthGenerator depthGenerator, DepthMetaData data, UserGenerator userGenerator)
+        public void DrawStickFigure(ref WriteableBitmap image, DepthGenerator depthGenerator, DepthMetaData data,
+                UserGenerator userGenerator, Ray3D[] rays)
         {
             Point3D corner = new Point3D(data.XRes, data.YRes, data.ZRes);
             corner = depthGenerator.ConvertProjectiveToRealWorld(corner);
@@ -27,6 +28,16 @@ public void DrawStickFigure(ref WriteableBitmap image, DepthGenerator depthGener
             int nXRes = data.XRes;
             int nYRes = data.YRes;
 
+            foreach (Ray3D ray in rays)
+            {
+                if (ray != null)
+                {
+                    SkeletonJointPosition skp1 = ray.SkeletonJointPosition0();
+                    SkeletonJointPosition skp2 = ray.SkeletonJointPosition1();
+                    DrawTheLine(ref image, ref skp1, ref skp2);
+                }
+            }
+
             uint[] users = userGenerator.GetUsers();
             foreach (uint user in users)
             {
@@ -56,7 +67,6 @@ public void DrawSingleUser(ref WriteableBitmap image, uint id, UserGenerator use
             DrawStickLine(ref image, id, userGenerator, SkeletonJoint.RightKnee, SkeletonJoint.RightFoot, corner);
             DrawHeadAndHands(ref image, id, userGenerator, depthGenerator);
 
-
             SkeletonJointPosition leftShoulder = new SkeletonJointPosition();
             SkeletonJointPosition rightShoulder = new SkeletonJointPosition();
             SkeletonJointPosition neck = new SkeletonJointPosition();

SpatialController/SpatialController/SpatialController.cs

@@ -29,13 +29,33 @@ class SpatialController
         private UserGenerator userGenerator;
         private Device[] devices;
 
-        public SpatialController(ControllerStartup startupType, UserGenerator userGenerator)
+        private object animationLock;
+        private Ray3D[] raysToBeAnimated; // Note: Only works for one user.
+
+        public Ray3D[] RaysToBeAnimated
         {
-            this.calibrated = false;
+            get { lock (animationLock) { return raysToBeAnimated; } }
+        }
+
+        public SpatialController(ControllerStartup startupType, UserGenerator userGenerator, params Device[] devices)
+        {
+            this.raysToBeAnimated = new Ray3D[2]; // Left and right, in this case.
             this.userGenerator = userGenerator;
-            this.devices = new Device[9]; // Null entries until calibrated.
+            this.animationLock = new object();
 
-            Console.Write("Starting SpatialController!\n");
+            int numRealDevices = 0; // TODO: List devices on the Z-wave network.
+            if (numRealDevices == 0)
+                this.calibrated = true; // We know where they are!
+            else
+                this.calibrated = false;
+
+            this.devices = new Device[numRealDevices + devices.Length];
+            for (int i = 0; i < devices.Length; i++)
+            {
+                this.devices[i] = devices[i];
+            }
+
+            Console.Write("Starting SpatialController!");
 
             switch (startupType)
             {
@@ -55,6 +75,7 @@ public SpatialController(ControllerStartup startupType, UserGenerator userGenera
         // data in CALIBRATION_DATA_FILE. Use only right hand for pointing.
         private void calibrate(uint user)
         {
+            // TODO: Only calibrate real devices (since mock ones already have positions).
             int numDevices = 10/* TODO: number of devices */;
             Device[] devices = new Device[numDevices];
 
@@ -130,21 +151,26 @@ private void initFromFile()
         // targets.
         public void checkGestures()
         {
-            Console.Write("Called checkGestures().\n");
+            Console.Write("Called checkGestures().");
 
             uint[] users = userGenerator.GetUsers();
             foreach (uint user in users)
             {
-                if (!calibrated)
-                {
-                    // Use the first user to calibrate system.
-                    calibrate(user);
-                    break;
-                }
                 if (userGenerator.GetSkeletonCap().IsTracking(user))
                 {
+                    if (!calibrated)
+                    {
+                        // Use the first user to calibrate system.
+                        calibrate(user);
+                        break;
+                    }
                     checkUserGestures(user);
                 }
+                else
+                {
+                    for (int i = 0; i < raysToBeAnimated.Length; i++)
+                        raysToBeAnimated[i] = null;
+                }
             }
         }
 
@@ -167,6 +193,15 @@ private void checkUserGestures(uint id)
             Ray3D rightPointer = new Ray3D(headPoint.X, headPoint.Y, headPoint.Z,
                     rightPoint.X, rightPoint.Y, rightPoint.Z);
 
+            lock (animationLock)
+            {
+                raysToBeAnimated[0] = leftPointer;
+                raysToBeAnimated[1] = rightPointer;
+            }
+
+            Console.Write("Left vector: " + leftPointer);
+            Console.Write("Right vector: " + rightPointer);
+
             foreach (Device d in devices)
             {
                 if (leftPointer.closeTo(d.position) || rightPointer.closeTo(d.position))
@@ -177,6 +212,7 @@ private void checkUserGestures(uint id)
                     // device's action is.
                 }
             }
+            Console.Write("=============================");
         }
     }
 }

SpatialController/SpatialController/SpatialController.csproj

@@ -38,7 +38,10 @@
   </PropertyGroup>
   <ItemGroup>
     <Reference Include="OpenNI.net">
-      <HintPath>..\..\..\KinectTesting\KinectTesting\lib\OpenNI.net.dll</HintPath>
+      <HintPath>libs\OpenNI.net.dll</HintPath>
+    </Reference>
+    <Reference Include="OpenZWaveDotNet">
+      <HintPath>libs\OpenZWaveDotNet.dll</HintPath>
     </Reference>
     <Reference Include="System" />
     <Reference Include="System.Data" />