public static IntermediateDevice GetCompleteIntermediateDevice(Device device)
{
if (device == null)
{
return null;
}
IntermediateDevice intermediateDevice = new IntermediateDevice();
intermediateDevice.identifier = device.Identifier;
intermediateDevice.orientation = device.Orientation;
intermediateDevice.location = device.Location;
// We only want to return devices for which all of the properties are known
if (intermediateDevice.isComplete)
return intermediateDevice;
else
return null;
}
public void ZeroDeviceOrientationTest()
{
Tracker tracker = new Tracker() {Location = new System.Windows.Point(2,4)};
Device device = new Device() {Location = new System.Windows.Point(7,7)};
tracker.ZeroDeviceOrientation(device);
System.Diagnostics.Debug.WriteLine(device.Orientation.Value);
device.Location = new System.Windows.Point(-7, 7);
tracker.ZeroDeviceOrientation(device);
System.Diagnostics.Debug.WriteLine(device.Orientation.Value);
device.Location = new System.Windows.Point(-7, -7);
tracker.ZeroDeviceOrientation(device);
System.Diagnostics.Debug.WriteLine(device.Orientation.Value);
device.Location = new System.Windows.Point(7, -7);
tracker.ZeroDeviceOrientation(device);
System.Diagnostics.Debug.WriteLine(device.Orientation.Value);
}
/// <summary>
/// Computes the position of all 4 corners of a device using size and orientation of a device.
/// </summary>
/// <param name="device"></param>
/// <returns>A list of points of all 4 corners of a device.</returns>
public List<Point> getCornersOfShape(Device device)
{
List<Point> returnPoints = new List<Point>();
List<Point> intPoints = new List<Point>();
Point deviceLocation = device.Location.Value;
intPoints.Add(new Point((double)(deviceLocation.X + device.Width / 2), (double)(deviceLocation.Y + device.Height / 2)));
intPoints.Add(new Point((double)(deviceLocation.X + device.Width / 2), (double)(deviceLocation.Y - device.Height / 2)));
intPoints.Add(new Point((double)(deviceLocation.X - device.Width / 2), (double)(deviceLocation.Y - device.Height / 2)));
intPoints.Add(new Point((double)(deviceLocation.X - device.Width / 2), (double)(deviceLocation.Y + device.Height / 2)));
double angle;
// Check if the device's orientation is not null
if (device.Orientation != null)
{
// This will help when we consider sending to moving devices that change its
// orientation dynamically. The choice of 270 is for consistency with the
// current code that handles the special case of a tabletop facing away
// from the kinect
angle = ((Double)device.Orientation - 90);
angle = angle * Math.PI / 180;
}
else
{
// No changes neccessary
return intPoints;
}
foreach (Point point in intPoints)
{
double xValue = (point.X - deviceLocation.X) * Math.Cos(angle) - (point.Y - deviceLocation.Y) * Math.Sin(angle) + deviceLocation.X;
double yValue = (point.Y - deviceLocation.Y) * Math.Cos(angle) + (point.X - deviceLocation.X) * Math.Sin(angle) + deviceLocation.Y;
Point newPoint = new Point(xValue, yValue);
returnPoints.Add(newPoint);
}
return returnPoints;
}
public void GetDeviceInfoTest()
{
Setup();
//Server.Route(Routes.GetDeviceInfoRoute, delegate(IARequest request, IAResponse response)
//{
// if(request.Parameters["identifier"].Equals(Client.OwnIdentifier))
// {
// // This string derived from the actual MSEAPI JSON serializer. Use real data for all tests!
// response.SetBodyWithString("{\"identifier\":\"" + Client.OwnIdentifier + "\",\"orientation\":99.55555,\"location\":\"2.22716139629483,3.0686103105545\"}");
// }
//});
Device testDevice = new Device
{ Identifier = "testDevice",
Location = new Point(2.227, 3.0686),
Orientation = 99.55555
};
Server.Locator.Devices.Add(testDevice);
Server.Start();
Client.Start();
WaitForConnections();
Client.Locator.locate(new MSEDevice() { Identifier= "testDevice"}, delegate(MSEDevice successDevice)
{
Assert.AreEqual(testDevice.Orientation.Value, successDevice.Orientation.Value, 0.001, "Orientation not equal");
Assert.AreEqual(testDevice.Location.Value, successDevice.Location.Value, "Location not equal");
Assert.AreEqual(testDevice.Identifier, successDevice.Identifier, "Identifier not equal");
doneWaitingForResponse = true;
},
delegate(Exception exception)
{
Assert.Fail();
});
WaitForResponse();
Teardown();
}
public void onFOVChanged(Device device)
{
Tracker tracker = (Tracker)device;
updateFOV(tracker);
updateRange(tracker);
}
/// <summary>
/// Returns the device's orientation in the locator's coordinate space, under the assumption that the device is pointed toward the tracker.
/// For use with a calibration button/function on the device, that the user will trigger while holding the device so that it faces the tracker.
/// Also sets the device's orientation to the calculated value.
/// </summary>
/// <param name="device"></param>
/// <returns></returns>
public double ZeroDeviceOrientation(Device device)
{
if (!device.Location.HasValue)
throw new ArgumentException("Tracker.ZeroDeviceOrientation : device Location was null. ");
// find the device's orientation with respect to the tracker's location
// i.e., find the angle formed by two lines: x axis and the line between the tracker and the device
device.Orientation = Math.Atan2(device.Location.Value.Y - this.Location.Value.Y, device.Location.Value.X - this.Location.Value.X) * 180/Math.PI;
return device.Orientation.Value;
}
// To use data received from the Kinect, or other tracker, the coordinates need to be translated from the Kinect's coordinate space, to the tracker's coordinate space, to the locator's coordinate space.
// In the Locator's coordinate space, the tracker is at the position and orientation stored in the tracker's instance variables
// In the Tracker's coordinate space, it is at the origin and with orientation 0 (facing down the X axis).
// In the Kinect's coordinate space, it is at the origin, and faces down the Z axis.
// Left-right movement in front of the camera is mapped to the X axis, and vertical movement is mapped to the Y axis.
// So, the Kinect's Z axis corresponds to the Tracker's X axis, and the Kinect's X axis corresponds to the Tracker's Y axis.
// To translate from the Kinect's coordinate space to the Tracker's coordinate space, you can create a Vector(SkeletonPoint.Z, SkeletonPoint.X).
// To translate from the Tracker's coordinate space to the locator's coordinate space, use this class's methods.
/// <summary>
/// Accepts a point in the Tracker's coordinate space, translates it into the room's coordinate space using the Tracker's position and orientation, and updates the location of the device with it.
/// </summary>
/// <param name="device"></param>
/// <param name="vector"></param>
public void UpdatePositionForDevice(Device device, Vector vector)
{
Vector updatedPosition = Util.TranslateFromCoordinateSpace(vector, Orientation, new Vector(Location.Value.X, Location.Value.Y));
device.Location = new Point(updatedPosition.X, updatedPosition.Y);
}
/// <summary>
/// Computes the devices within the field of view of the observer alongside the intersection point
/// with each of these devices. Returns an empty dictionary if FieldOfView or Location are null on the observer.
/// </summary>
/// <param name="observer"></param>
/// <returns>Devices in the field of view of the observer and their intersection points.</returns>
public List<Device> GetDevicesInView(Device observer)
{
List<Device> returnDevices = new List<Device>();
Line obseverLineOfSight = new Line(observer.Location, observer.Orientation);
List<Device> devicesInView = GetDevicesInFront(observer);
foreach (Device target in devicesInView)
{
if (target.Width == null || target.Width == null)
continue;
List<Line> sides = getLinesOfShape(target);
List<Point?> intersectionPoints = new List<Point?>();
foreach (Line side in sides)
{
Point? intPoint = Line.getIntersectionPoint(obseverLineOfSight, side);
if (intPoint != null)
intersectionPoints.Add(intPoint);
}
if (intersectionPoints.Count == 0)
continue;
Point? nearestPoint = intersectionPoints[0];
double shortestDistance = Line.getDistanceBetweenPoints((Point)observer.Location, (Point)nearestPoint);
foreach (Point point in intersectionPoints)
{
double distance = Line.getDistanceBetweenPoints((Point)observer.Location, point);
if (distance < shortestDistance)
{
nearestPoint = point;
shortestDistance = distance;
}
}
Point ratioOnScreen = GetRatioPositionOnScreen(target, (Point)nearestPoint);
target.intersectionPoint["x"] = ratioOnScreen.X;
target.intersectionPoint["y"] = ratioOnScreen.Y;
returnDevices.Add(target);
}
return returnDevices;
}
public bool Equals(Device d)
{
return (d.Identifier == this.Identifier);
}
private static Device FindNearestDevice(Device observer, List<Device> deviceList)
{
if (deviceList.Count == 0)
return null;
else
{
Device nearest = null;
//First, find a device with a location to compare against
foreach (Device device in deviceList)
{
if (device != observer && device.Location.HasValue)
{
nearest = device;
}
}
if (nearest == null)
return null;
//Find the device with the least distance to the observer
foreach (Device device in deviceList)
{
if (device != observer && device.Location.HasValue &&
Util.DistanceBetweenPoints(device.Location.Value, observer.Location.Value) < Util.DistanceBetweenPoints(nearest.Location.Value, observer.Location.Value))
{
nearest = device;
}
}
return nearest;
}
}
/// <summary>
/// This function takes a point located in the room and turns it into an intersection point on the target device.
/// For example if an intersection point is on the bottom left corner of a device, this function will return (0,0) or if
/// the intersection point is on the top right corner, it will return (1,1).
/// </summary>
/// <param name="target"></param>
/// <param name="intersection"></param>
/// <returns>A point </returns>
public Point GetRatioPositionOnScreen(Device target, Point intersection)
{
List<Point> cornersOfShape = getCornersOfShape(target);
Double distance1 = Line.getDistanceBetweenPoints(intersection,cornersOfShape[0]);
Double distance2 = Line.getDistanceBetweenPoints(intersection,cornersOfShape[1]);
Double distance3 = Line.getDistanceBetweenPoints(cornersOfShape[0],cornersOfShape[1]);
if (Math.Abs(distance3 - (distance1 + distance2)) < 0.01)
{
Double xRatio = 1;
Double yRatio = distance1 / distance3;
//Double xRatio = distance1 / distance3;
//Double yRatio = 0;
return new Point(xRatio, yRatio);
}
distance1 = Line.getDistanceBetweenPoints(intersection, cornersOfShape[2]);
distance2 = Line.getDistanceBetweenPoints(intersection, cornersOfShape[1]);
distance3 = Line.getDistanceBetweenPoints(cornersOfShape[1], cornersOfShape[2]);
if (Math.Abs(distance3 - (distance1 + distance2)) < 0.01)
{
Double yRatio = 1;
Double xRatio = distance1 / distance3;
//Double xRatio = 1;
//Double yRatio = distance2 / distance3;
return new Point(xRatio, yRatio);
}
distance1 = Line.getDistanceBetweenPoints(intersection, cornersOfShape[3]);
distance2 = Line.getDistanceBetweenPoints(intersection, cornersOfShape[2]);
distance3 = Line.getDistanceBetweenPoints(cornersOfShape[2], cornersOfShape[3]);
if (Math.Abs(distance3 - (distance1 + distance2)) < 0.01)
{
Double xRatio = 0;
Double yRatio = distance1 / distance3;
//Double xRatio = distance1 / distance3;
//Double yRatio = 1;
return new Point(xRatio, yRatio);
}
distance1 = Line.getDistanceBetweenPoints(intersection, cornersOfShape[3]);
distance2 = Line.getDistanceBetweenPoints(intersection, cornersOfShape[0]);
distance3 = Line.getDistanceBetweenPoints(cornersOfShape[3], cornersOfShape[0]);
if (Math.Abs(distance3 - (distance1 + distance2)) < 0.01)
{
Double yRatio = 0;
Double xRatio = distance1 / distance3;
//Double xRatio = 0;
//Double yRatio = distance2 / distance3;
return new Point(xRatio, yRatio);
}
return new Point(-1, -1);
}
public Device GetNearestDeviceWithinRange(Device observer, double distance)
{
List<Device> devicesInView = GetDevicesWithinRange(observer, distance);
return FindNearestDevice(observer, devicesInView);
}
public Device GetNearestDeviceInView(Device observer)
{
List<Device> devicesInView = GetDevicesInView(observer);
return FindNearestDevice(observer, devicesInView);
}
/// <summary>
/// Uses the the size and orientation of the device to compute all 4 line equations a device.
/// </summary>
/// <param name="device"></param>
/// <returns>The line euqations of all 4 sides of a device</returns>
public List<Line> getLinesOfShape(Device device)
{
List<Line> returnLines = new List<Line>();
List<Point> corners = getCornersOfShape(device);
Line topSide = new Line(corners[0], corners[1]);
Line rightSide = new Line(corners[1], corners[2]);
Line bottomSide = new Line(corners[2], corners[3]);
Line leftSide = new Line(corners[3], corners[0]);
returnLines.Add(topSide);
returnLines.Add(rightSide);
returnLines.Add(bottomSide);
returnLines.Add(leftSide);
return returnLines;
}
public List<Device> GetDevicesWithinRange(Device observer, double distance)
{
List<Device> returnDevices = new List<Device>();
if (!observer.Location.HasValue)
return returnDevices;
foreach (Device device in _devices)
{
if (device == observer)
continue;
else if (device.Location.HasValue && Util.DistanceBetweenPoints(observer.Location.Value, device.Location.Value) < distance)
{
returnDevices.Add(device);
}
}
return returnDevices;
}
public void onLocationChanged(Device device)
{
if (device.Location.HasValue)
{
this.Dispatcher.Invoke(new Action(delegate()
{
Point newPoint = DrawingResources.ConvertFromMetersToDisplayCoordinates(device.Location.Value, MainWindow.SharedCanvas);
Canvas.SetLeft(this, newPoint.X);
Canvas.SetTop(this, newPoint.Y);
}));
}
}
public void onOrientationChanged(Device device)
{
if (device.Orientation != null)
{
this.Dispatcher.Invoke(new Action(delegate()
{
// We are using RotateTransform now to make things easier. Everything should be drawn pointing downwards (270 degrees);
LeftLine.RenderTransform = new RotateTransform((device.Orientation.Value * -1) + 270, 50, 15);
RightLine.RenderTransform = new RotateTransform((device.Orientation.Value * -1) + 270, 50, 15);
NearTriangle.RenderTransform = new RotateTransform((device.Orientation.Value * -1) + 270, 50, 15);
FarLine.RenderTransform = new RotateTransform((device.Orientation.Value * -1) + 270, 50, 15);
}));
}
}
public void onLocationChanged(Device device)
{
PairableDevice pairableDevice = (PairableDevice)device;
//Dispatch UI Changes to Main Thread
Application.Current.Dispatcher.BeginInvoke(new Action(() =>
{
if (pairableDevice.Location.HasValue)
{
if (iaDevice.SupportedRoutes.Contains(Routes.GetLocationRoute))
{
MyDisplayState = DisplayState.LocatedAndOnStackPanel;
}
Point newPoint = DrawingResources.ConvertFromMetersToDisplayCoordinates(pairableDevice.Location.Value, MainWindow.SharedCanvas);
// InnerBorder.Width / 2 is to make it so that the point that the DeviceControl is drawn at is actually the center of the Border
Canvas.SetLeft(this, newPoint.X - (InnerBorder.Width / 2));
Canvas.SetTop(this, newPoint.Y - (InnerBorder.Height / 2));
}
else if (iaDevice.SupportedRoutes.Contains(Routes.GetLocationRoute))
{
MyDisplayState = DisplayState.UnlocatedAndOnStackPanel;
}
}));
}
public void onOrientationChanged(Device device)
{
PairableDevice pairableDevice = (PairableDevice)device;
// Draw two lines to serve as field of view indicators
double topAngle = Util.NormalizeAngle(pairableDevice.Orientation.Value + pairableDevice.FieldOfView.Value);
double topX = Math.Cos(topAngle * Math.PI / 180);
double topY = Math.Sin(topAngle * Math.PI / 180);
double bottomAngle = Util.NormalizeAngle(pairableDevice.Orientation.Value - pairableDevice.FieldOfView.Value);
double bottomX = Math.Cos(bottomAngle * Math.PI / 180);
double bottomY = Math.Sin(bottomAngle * Math.PI / 180);
Point newLeft = DrawingResources.ConvertPointToProperLength(new Point(topX, topY), DrawingResources.DEVICE_FOV_LENGTH);
Point newRight = DrawingResources.ConvertPointToProperLength(new Point(bottomX, bottomY), DrawingResources.DEVICE_FOV_LENGTH);
//Dispatch UI Changes to Main Thread
Application.Current.Dispatcher.BeginInvoke(new Action(() =>
{
LeftLine.X2 = newLeft.X;
LeftLine.Y2 = -newLeft.Y;
RightLine.X2 = newRight.X;
RightLine.Y2 = -newRight.Y;
if (pairableDevice.PairingState == PairingState.Paired)
{
LeftLine.Visibility = System.Windows.Visibility.Visible;
RightLine.Visibility = System.Windows.Visibility.Visible;
}
}));
}
/* Depricated!!!
/// <summary>
/// Computes the devices within the field of view of the observer. Returns an empty list if FieldOfView or Location are null on the observer.
/// </summary>
/// <param name="observer"></param>
/// <returns>Devices in the field of view of the observer.</returns>
public List<Device> GetDevicesInView(Device observer)
{
List<Device> returnDevices = new List<Device>();
//(CB - Should we throw an exception here? Rather then just returning an empty list?)
if (observer.Location == null || observer.Orientation == null)
return returnDevices;
if (observer.FieldOfView == 0.0)
return returnDevices;
// We imagine the field of view as two vectors, pointing away from the observing device. Targets between the vectors are in view.
// We will use angles to represent these vectors.
double leftFieldOfView = Util.NormalizeAngle(observer.Orientation.Value + observer.FieldOfView.Value / 2);
double rightFieldOfView = Util.NormalizeAngle(observer.Orientation.Value - observer.FieldOfView.Value / 2);
foreach (Device target in _devices)
{
if (target == observer || !target.Location.HasValue)
continue;
// Atan2 is the inverse tangent function, given lengths for the opposite and adjacent legs of a right triangle, it returns the angle
double angle = Util.NormalizeAngle(Math.Atan2(target.Location.Value.Y - observer.Location.Value.Y, target.Location.Value.X - observer.Location.Value.X) * 180 / Math.PI);
// Ordinarily, the angle defining the left boundary of the field of view will be larger than the angle for the right.
// For example, if our device has an orientation of 90.0 and a field of view of 15 degrees, then the left and right FoV vectors are at 97.5 and 82.5 degrees.
// In this case, the target must be at an angle between left and right to be in view.
if (leftFieldOfView > rightFieldOfView && angle < leftFieldOfView && angle > rightFieldOfView)
{
returnDevices.Add(target);
}
// If the field of view includes the X axis, then the left field of view will be smaller than the right field of view.
// For example, if our device has an orientation of 0.0 and a field of view of 15 degrees, then the left FoV vector will be at 7.5 degrees,
// and the right FoV will be at 352.5 degrees.
else if (leftFieldOfView < rightFieldOfView)
{
if (angle < leftFieldOfView || angle > rightFieldOfView)
returnDevices.Add(target);
}
}
return returnDevices;
}
* */
/// <summary>
/// Computes the devices that are in front of the observer device. Returns an empty list if FieldOfView or Location are null on the observer.
/// </summary>
/// <param name="observer"></param>
/// <returns>Devices in the field of view of the observer.</returns>
public List<Device> GetDevicesInFront(Device observer)
{
List<Device> returnDevices = new List<Device>();
//(CB - Should we throw an exception here? Rather then just returning an empty list?)
if (observer.Location == null || observer.Orientation == null)
return returnDevices;
if (observer.FieldOfView == 0.0)
return returnDevices;
// We imagine the field of view as two vectors, pointing away from the observing device. Targets between the vectors are in view.
// We will use angles to represent these vectors.
double leftFieldOfView = Util.NormalizeAngle(observer.Orientation.Value + 30);
double rightFieldOfView = Util.NormalizeAngle(observer.Orientation.Value - 30);
foreach (Device target in _devices)
{
if (target == observer || !target.Location.HasValue)
continue;
// Atan2 is the inverse tangent function, given lengths for the opposite and adjacent legs of a right triangle, it returns the angle
double angle = Util.NormalizeAngle(Math.Atan2(target.Location.Value.Y - observer.Location.Value.Y, target.Location.Value.X - observer.Location.Value.X) * 180 / Math.PI);
// Ordinarily, the angle defining the left boundary of the field of view will be larger than the angle for the right.
// For example, if our device has an orientation of 90.0 and a field of view of 15 degrees, then the left and right FoV vectors are at 97.5 and 82.5 degrees.
// In this case, the target must be at an angle between left and right to be in view.
if (leftFieldOfView > rightFieldOfView && angle < leftFieldOfView && angle > rightFieldOfView)
{
returnDevices.Add(target);
}
// If the field of view includes the X axis, then the left field of view will be smaller than the right field of view.
// For example, if our device has an orientation of 0.0 and a field of view of 15 degrees, then the left FoV vector will be at 7.5 degrees,
// and the right FoV will be at 352.5 degrees.
else if (leftFieldOfView < rightFieldOfView)
{
if (angle < leftFieldOfView || angle > rightFieldOfView)
returnDevices.Add(target);
}
}
return returnDevices;
}
