Permalink
Browse files

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

… Performance still terrible.
  • Loading branch information...
sterlingswigart committed May 4, 2011
1 parent 393ed0d commit 91e33e0b4e56d5c28a9478fe0d68665d15c9979c
@@ -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
@@ -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;
}

Some generated files are not rendered by default. Learn more.

Oops, something went wrong.

Some generated files are not rendered by default. Learn more.

Oops, something went wrong.
@@ -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 + ")";
+ }
}
}
@@ -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();
@@ -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("=============================");
}
}
}
@@ -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" />
Binary file not shown.
Binary file not shown.
Binary file not shown.

0 comments on commit 91e33e0

Please sign in to comment.