// Returns the point halfway between the lines at their closest points.
// If the lines are more than MAX_CALIBRATION_DISTANCE apart, this returns
// an empty point.
public Vector3D intersectionWith(Ray3D other)
{
// Algorithm is ported from the C algorithm of
// Paul Bourke at http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/.
// Port to C# done by Ronald Hulthuizen.
Vector3D p1 = this.p0;
Vector3D p2 = this.p1;
Vector3D p3 = other.p0;
Vector3D p4 = other.p1;
Vector3D p13 = p1 - p3;
Vector3D p43 = p4 - p3;
if (p43.LengthSquared < Double.Epsilon)
return new Vector3D();
Vector3D p21 = p2 - p1;
if (p21.LengthSquared < Double.Epsilon) // FIXME: p21 = {0,0,0}
return new Vector3D();
// Dot product multiplication.
double d1343 = p13.X * (double)p43.X + (double)p13.Y * p43.Y + (double)p13.Z * p43.Z;
double d4321 = p43.X * (double)p21.X + (double)p43.Y * p21.Y + (double)p43.Z * p21.Z;
double d1321 = p13.X * (double)p21.X + (double)p13.Y * p21.Y + (double)p13.Z * p21.Z;
double d4343 = p43.X * (double)p43.X + (double)p43.Y * p43.Y + (double)p43.Z * p43.Z;
double d2121 = p21.X * (double)p21.X + (double)p21.Y * p21.Y + (double)p21.Z * p21.Z;
double denom = d2121 * d4343 - d4321 * d4321;
if (Math.Abs(denom) < Double.Epsilon)
return new Vector3D();
double numer = d1343 * d4321 - d1321 * d4343;
double mua = numer / denom;
double mub = (d1343 + d4321 * (mua)) / d4343;
// Endpoints of shortest line connecting the two lines.
Vector3D resultSegmentPoint1 = new Vector3D((float)(p1.X + mua * p21.X),
(float)(p1.Y + mua * p21.Y), (float)(p1.Z + mua * p21.Z));
Vector3D resultSegmentPoint2 = new Vector3D((float)(p3.X + mub * p43.X),
(float)(p3.Y + mub * p43.Y), (float)(p3.Z + mub * p43.Z));
double distance = distance3D(resultSegmentPoint1, resultSegmentPoint2);
if (distance > MAX_CALIBRATION_DISTANCE)
return new Vector3D();
// Find the point half of that distance down the line by adding the vectors
// and finding the halfway point of the resulting vector.
return (resultSegmentPoint1 + resultSegmentPoint2) / 2;
}
// Calibrate the locations of devices in the room, saving calibration
// data in CALIBRATION_DATA_FILE. Use only right hand for pointing.
private void calibrate(int user)
{
List <byte> nodes = Device.getNodes();
Ray3D[] firstRays = new Ray3D[nodes.Count];
Device.turnOffAll();
SpeakAndWriteToPrompt("Please stand about 10 feet away from the kinect on the right"
+ " side of the field of view, but leave room for pointing off to the right.");
SpeakAndWriteToPrompt("When a light turns on, please point to it until it turns off.");
//Thread.Sleep((SEC_FOR_RELOCATION - SEC_BETWEEN_CALIBRATIONS) * 1000);
for (int i = 0; i < nodes.Count; i++)
{
//Thread.Sleep(SEC_BETWEEN_CALIBRATIONS * 1000);
firstRays[i] = calibrateDeviceOnePosition(user, nodes[i]);
}
SpeakAndWriteToPrompt("Please stand about 5 feet away from the kinect on the left"
+ " side of the field of view, but leave room for pointing off to the left.");
SpeakAndWriteToPrompt("Once again, when a light turns on, please point to it until it turns off.");
//Thread.Sleep((SEC_FOR_RELOCATION - SEC_BETWEEN_CALIBRATIONS) * 1000);
for (int i = 0; i < nodes.Count; i++)
{
//Thread.Sleep(SEC_BETWEEN_CALIBRATIONS * 1000);
devices[i] = new Device(firstRays[i].intersectionWith(calibrateDeviceOnePosition(user, nodes[i])), nodes[i]);
}
saveCalibrationToFile(devices);
calibrated = true;
SpeakAndWriteToPrompt("Calibration has been completed. After a few seconds, you should be able"
+ " to point to lights to turn them on!");
}
private void checkUserGestures(int id)
{
SkeletonJointPosition head = new SkeletonJointPosition();
SkeletonJointPosition leftHand = new SkeletonJointPosition();
SkeletonJointPosition rightHand = new SkeletonJointPosition();
SkeletonJointPosition leftShoulder = new SkeletonJointPosition();
SkeletonJointPosition rightShoulder = new SkeletonJointPosition();
head = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.Head);
leftHand = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.LeftHand);
rightHand = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.RightHand);
leftShoulder = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.LeftShoulder);
rightShoulder = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.RightShoulder);
OpenNI.Point3D headPoint = head.Position;
OpenNI.Point3D leftPoint = leftHand.Position;
OpenNI.Point3D rightPoint = rightHand.Position;
Ray3D leftPointer = new Ray3D(headPoint.X, headPoint.Y, headPoint.Z,
leftPoint.X, leftPoint.Y, leftPoint.Z);
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);
if (VerticallyClose(leftPoint, rightPoint))
{
// Handle dimming.
if (FirstAboveSecond(leftPoint, headPoint) && FirstAboveSecond(rightPoint, headPoint))
{
Console.Write("Beginning dim down!");
dimmingDown = true;
dimmingStartY = leftPoint.Y;
}
else if (dimmingDown)
{
int dimPercent = (int)((dimmingStartY - leftPoint.Y) / TOTAL_DIMMING_DISTANCE);
if (dimPercent < 0)
{
dimPercent = 0;
}
else if (dimPercent > 100)
{
dimPercent = 100;
}
Device.dimAllToPercent(dimPercent);
}
else if (VerticallyClose(leftPoint, leftShoulder.Position) &&
VerticallyClose(leftShoulder.Position, rightShoulder.Position) &&
VerticallyClose(rightShoulder.Position, rightPoint))
{
Console.Write("Beginning dim up!");
dimmingUp = true;
dimmingStartY = leftPoint.Y;
}
else if (dimmingUp)
{
int dimPercent = (int)((leftPoint.Y - dimmingStartY) / TOTAL_DIMMING_DISTANCE);
if (dimPercent < 0)
{
dimPercent = 0;
}
else if (dimPercent > 100)
{
dimPercent = 100;
}
Device.dimAllToPercent(dimPercent);
}
}
else
{
// Allow pointing.
dimmingDown = false;
dimmingUp = false;
}
if (!dimmingUp && !dimmingDown)
{
foreach (Device d in devices)
{
if (leftPointer.closeTo(d.position) || rightPointer.closeTo(d.position))
{
d.isInFocus();
}
}
}
Console.Write("=============================");
}
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);
this.depthGenerator = depthGenerator;
int nXRes = data.XRes;
int nYRes = data.YRes;
// TODO: Fix these.
/*foreach (Ray3D ray in rays)
{
if (ray != null)
{
int[] p0 = ray.point0();
int[] p1 = ray.point1();
DrawTheLine(ref image, p0, p1);
}
}*/
int[] users = userGenerator.GetUsers();
foreach (int user in users)
{
if (userGenerator.SkeletonCapability.IsTracking(user))
{
DrawSingleUser(ref image, user, userGenerator, corner);
}
}
}
private void checkUserGestures(int id)
{
SkeletonJointPosition head = new SkeletonJointPosition();
SkeletonJointPosition leftHand = new SkeletonJointPosition();
SkeletonJointPosition rightHand = new SkeletonJointPosition();
SkeletonJointPosition leftShoulder = new SkeletonJointPosition();
SkeletonJointPosition rightShoulder = new SkeletonJointPosition();
head = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.Head);
leftHand = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.LeftHand);
rightHand = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.RightHand);
leftShoulder = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.LeftShoulder);
rightShoulder = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.RightShoulder);
OpenNI.Point3D headPoint = head.Position;
OpenNI.Point3D leftPoint = leftHand.Position;
OpenNI.Point3D rightPoint = rightHand.Position;
Ray3D leftPointer = new Ray3D(headPoint.X, headPoint.Y, headPoint.Z,
leftPoint.X, leftPoint.Y, leftPoint.Z);
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);
if (VerticallyClose(leftPoint, rightPoint))
{
// Handle dimming.
if (FirstAboveSecond(leftPoint, headPoint) && FirstAboveSecond(rightPoint, headPoint))
{
Console.Write("Beginning dim down!");
dimmingDown = true;
dimmingStartY = leftPoint.Y;
}
else if (dimmingDown)
{
int dimPercent = (int)((dimmingStartY - leftPoint.Y) / TOTAL_DIMMING_DISTANCE);
if (dimPercent < 0) dimPercent = 0;
else if (dimPercent > 100) dimPercent = 100;
Device.dimAllToPercent(dimPercent);
}
else if (VerticallyClose(leftPoint, leftShoulder.Position)
&& VerticallyClose(leftShoulder.Position, rightShoulder.Position)
&& VerticallyClose(rightShoulder.Position, rightPoint))
{
Console.Write("Beginning dim up!");
dimmingUp = true;
dimmingStartY = leftPoint.Y;
}
else if (dimmingUp)
{
int dimPercent = (int)((leftPoint.Y - dimmingStartY) / TOTAL_DIMMING_DISTANCE);
if (dimPercent < 0) dimPercent = 0;
else if (dimPercent > 100) dimPercent = 100;
Device.dimAllToPercent(dimPercent);
}
}
else
{
// Allow pointing.
dimmingDown = false;
dimmingUp = false;
}
if (!dimmingUp && !dimmingDown)
foreach (Device d in devices)
if (leftPointer.closeTo(d.position) || rightPointer.closeTo(d.position))
d.isInFocus();
Console.Write("=============================");
}
// Calibrate the locations of devices in the room, saving calibration
// data in CALIBRATION_DATA_FILE. Use only right hand for pointing.
private void calibrate(int user)
{
List<byte> nodes = Device.getNodes();
Ray3D[] firstRays = new Ray3D[nodes.Count];
Device.turnOffAll();
SpeakAndWriteToPrompt("Please stand about 10 feet away from the kinect on the right"
+ " side of the field of view, but leave room for pointing off to the right.");
SpeakAndWriteToPrompt("When a light turns on, please point to it until it turns off.");
//Thread.Sleep((SEC_FOR_RELOCATION - SEC_BETWEEN_CALIBRATIONS) * 1000);
for (int i = 0; i < nodes.Count; i++)
{
//Thread.Sleep(SEC_BETWEEN_CALIBRATIONS * 1000);
firstRays[i] = calibrateDeviceOnePosition(user, nodes[i]);
}
SpeakAndWriteToPrompt("Please stand about 5 feet away from the kinect on the left"
+ " side of the field of view, but leave room for pointing off to the left.");
SpeakAndWriteToPrompt("Once again, when a light turns on, please point to it until it turns off.");
//Thread.Sleep((SEC_FOR_RELOCATION - SEC_BETWEEN_CALIBRATIONS) * 1000);
for (int i = 0; i < nodes.Count; i++)
{
//Thread.Sleep(SEC_BETWEEN_CALIBRATIONS * 1000);
devices[i] = new Device(firstRays[i].intersectionWith(calibrateDeviceOnePosition(user, nodes[i])), nodes[i]);
}
saveCalibrationToFile(devices);
calibrated = true;
SpeakAndWriteToPrompt("Calibration has been completed. After a few seconds, you should be able"
+ " to point to lights to turn them on!");
}