/// <summary>
/// Két, a középpontjával és sugarával adott körnek számolja ki a metszéspontjait, ha lézetnek, vagy létezik
/// </summary>
/// <param name="P0">Az első kör középpontja</param>
/// <param name="r0">Az első kör sugara</param>
/// <param name="P1">A második kör középpontja</param>
/// <param name="r1">A második kör sugara</param>
/// <returns>Az Intersection objektum a metszéspontokat reprezentálja, melyben egy lista tárolja, hogy hány metszéspontja van a két körnek</returns>
public static Intersection CalculateIntersection(Point P0, double r0, Point P1, double r1)
{
double d, a, h;
Point P2, isPoint1, isPoint2;
d = LocationCalculator.Distance(P0, P1);
if (d > r0 + r1)
{
return(new Intersection(new List <Point>())); // Két külön körről van szó
}
if (d < Math.Abs(r0 - r1))
{
return(new Intersection(new List <Point>())); // Az egyik kör a másikon belül van
}
if (d == 0 && r0 == r1)
{
return(new Intersection(null)); // A két kör éppen egybeesik
}
//TODO: megvizsgálni, hogy két metszéspont lesz, egy, végtelen, vagy nem érintkeznek
a = (Math.Pow(r0, 2) - Math.Pow(r1, 2) + Math.Pow(d, 2)) / (2 * d);
h = Math.Sqrt(Math.Pow(r0, 2) - Math.Pow(a, 2));
P2 = new Point(P0.X + a * (P1.X - P0.X) / d, P0.Y + a * (P1.Y - P0.Y) / d);
isPoint1 = new Point(P2.X + h * (P1.Y - P0.Y) / d, P2.Y - h * (P1.X - P0.X) / d);
isPoint2 = new Point(P2.X - h * (P1.Y - P0.Y) / d, P2.Y + h * (P1.X - P0.X) / d);
// Ha a két kör éppen érinti egymást egy ponton (d = r1 + r2), akkor is két pont lesz a metszet eredménye, de ezek egybeesnek
List <Point> solutions = new List <Point>();
solutions.Add(isPoint1);
solutions.Add(isPoint2);
return(new Intersection(solutions));
}
/// <summary>
/// Első eset: a két távolság által jelzett kör messze van egymástól:
/// Megoldás: mindkettőt arányosan növeljük
/// </summary>
protected bool CheckFarDistances(NearbyBluetoothTag tag1, NearbyBluetoothTag tag2)
{
if (LocationCalculator.Distance(tag1.Origo, tag2.Origo) > tag1.AveragePredictedDistanceFromTag + tag2.AveragePredictedDistanceFromTag)
{
double deviance = LocationCalculator.Distance(tag1.Origo, tag2.Origo) - (tag1.AveragePredictedDistanceFromTag + tag2.AveragePredictedDistanceFromTag);
deviance += BIAS; // Az eltérés torzítása, így biztosan lesz metszéspont
double max = tag1.AveragePredictedDistanceFromTag + tag2.AveragePredictedDistanceFromTag;
double rate1 = tag1.AveragePredictedDistanceFromTag / max;
double rate2 = tag2.AveragePredictedDistanceFromTag / max;
tag1.SetAveragePredictedDistance(tag1.AveragePredictedDistanceFromTag + deviance * rate1);
tag2.SetAveragePredictedDistance(tag2.AveragePredictedDistanceFromTag + deviance * rate2);
//TEST
double dist = LocationCalculator.Distance(tag1.Origo, tag2.Origo) - (tag1.AveragePredictedDistanceFromTag + tag2.AveragePredictedDistanceFromTag);
if (dist > 0.0)
{
dist = 0; /* throw new Exception("Calculation error: distance must be 0.0"); */
}
//TEST
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// !!! Ez csak pontos esetben működik !!!
///
/// Minden metszet minden pontját megvizsgáljuk, és az elsőhöz viszonyítjuk.
/// Feltesszük, hogy minden metszet tartalmaz egy pontot, amely közös a többivel, így az első is.
/// Ezért azt keressük, hogy az első metszet első, vagy második pontja a közös pont.
/// </summary>
public override Point CalculateCommonPoint(List <Intersection> intersections)
{
double minDist = LocationCalculator.Distance(intersections[0].Points[0], intersections[1].Points[0]);
Point commonPoint = intersections[0].Points[0];
for (int pointIdx = 0; pointIdx < 2; pointIdx++)
{
Point ip1 = intersections[0].Points[pointIdx];
for (int i = 1; i < intersections.Count; i++)
{
for (int otherPointIdx = 0; otherPointIdx < 2; otherPointIdx++)
{
Point ip2 = intersections[i].Points[otherPointIdx];
double d = LocationCalculator.Distance(ip1, ip2);
if (d < minDist)
{
minDist = d;
commonPoint = ip1;
if (pointIdx == 1)
{
return(commonPoint); // Ha az első pont nem jó, és találtunk kisebb távolságot, akkor ez lesz a közös pont
}
}
}
}
}
return(commonPoint);
}
/// <summary>
/// Az egyik tag távolságából felírható kör tartalmazza-e a másik tag távolsága alapján felírható kört (average predicted distance alapján)
/// </summary>
public bool Includes(NearbyBluetoothTag tag)
{
if (this.avgPredictedDistance > LocationCalculator.Distance(this.origo, tag.origo) + tag.avgPredictedDistance)
{
return(true);
}
return(false);
}
private Point getClosestPoint(Point P, List <Point> IntersectionPoints)
{
int mini = 0;
Point intersectionPoint;
double mindist = LocationCalculator.Distance(P, IntersectionPoints[0]);
double dist;
for (int i = 1; i < IntersectionPoints.Count; i++)
{
intersectionPoint = IntersectionPoints[i];
dist = LocationCalculator.Distance(P, intersectionPoint);
if (dist < mindist)
{
mini = i;
mindist = dist;
}
}
return(IntersectionPoints[mini]);
}
public double DeviceMotion(Point DevicePosition)
{
double distanceFromTag = LocationCalculator.Distance(origo, DevicePosition);
label_distance.Content = Math.Round(distanceFromTag, 2);
if (distanceFromTag > zoneRadius)
{
if (hasDevice)
{
background.Children.Remove(deviceDistance);
background.Children.Remove(line_radius);
background.Children.Remove(predictedDistance);
background.Children.Remove(label_prediction);
background.Children.Remove(avgPredictedDistance);
background.Children.Remove(label_avg_prediction);
hasDevice = false;
contactEventHandler.DeviceLeft(origo);
}
}
else if (!hasDevice)
{
background.Children.Add(deviceDistance);
background.Children.Add(line_radius);
background.Children.Add(predictedDistance);
background.Children.Add(label_prediction);
background.Children.Add(avgPredictedDistance);
background.Children.Add(label_avg_prediction);
predictedDistance.Visibility = Visibility.Hidden;
label_prediction.Visibility = Visibility.Hidden;
hasDevice = true;
contactEventHandler.DeviceAppear(origo, distanceFromTag, mac);
}
Canvas.SetLeft(deviceDistance, origo.X - distanceFromTag);
Canvas.SetTop(deviceDistance, origo.Y - distanceFromTag);
deviceDistance.Width = distanceFromTag * 2;
deviceDistance.Height = distanceFromTag * 2;
line_radius.X1 = origo.X;
line_radius.Y1 = origo.Y;
line_radius.X2 = DevicePosition.X;
line_radius.Y2 = DevicePosition.Y;
return(distanceFromTag);
}
/// <summary>
/// Második eset: az egyik kör a másikon belül helyezkedik el:
/// Megoldás: a nagyobb kört arányosan csökkentjük, a kisebb kört pedig arányosan növeljük
/// </summary>
protected bool CheckInclude(NearbyBluetoothTag tag1, NearbyBluetoothTag tag2)
{
NearbyBluetoothTag bigger, smaller;
if (tag1.Includes(tag2))
{
bigger = tag1;
smaller = tag2;
}
else if (tag2.Includes(tag1))
{
bigger = tag2;
smaller = tag1;
}
else
{
return(false);
}
double deviance = bigger.AveragePredictedDistanceFromTag - (LocationCalculator.Distance(bigger.Origo, smaller.Origo) + smaller.AveragePredictedDistanceFromTag);
deviance += BIAS; // Az eltérés torzítása, így biztosan lesz metszéspont
double max = bigger.AveragePredictedDistanceFromTag + smaller.AveragePredictedDistanceFromTag;
double brate = bigger.AveragePredictedDistanceFromTag / max;
double srate = smaller.AveragePredictedDistanceFromTag / max;
bigger.SetAveragePredictedDistance(bigger.AveragePredictedDistanceFromTag - deviance * brate);
smaller.SetAveragePredictedDistance(smaller.AveragePredictedDistanceFromTag + deviance * srate);
//TEST
double dist = bigger.AveragePredictedDistanceFromTag - (LocationCalculator.Distance(bigger.Origo, smaller.Origo) + smaller.AveragePredictedDistanceFromTag);
if (dist > 0.0)
{
dist = 0; /* throw new Exception("Calculation error: distance must be 0.0"); */
}
//TEST
return(true);
}
public LocationResult CalculateLocation(List <NearbyBluetoothTag> NearbyDistances, LocationResult LastLocation)
{
List <NearbyBluetoothTag> Distances = NearbyDistances.ToList <NearbyBluetoothTag>();
Distances.RemoveAll(tag => tag.AverageRSSI == 0.0);
if (Distances.Count == 0)
{
return(new LocationResult(new Point(-1, -1), Precision.NoTag));
}
if (Distances.Count == 1)
{
return(new LocationResult(Distances[0].Origo, Precision.OneTag, Distances[0].AveragePredictedDistanceFromTag)); // 1 vagy semennyi kör esetén nem tudunk pozíciót meghatározni
}
if (Distances.Count == 2) // 2 kör esetén a metszet két pontja közti felezőpont kell
{
ForceAllIntersections(Distances);
Intersection points = Intersection.CalculateIntersection(Distances[0].Origo, Distances[0].AveragePredictedDistanceFromTag, Distances[1].Origo, Distances[1].AveragePredictedDistanceFromTag);
return(new LocationResult(LocationCalculator.Midpoint(points.Points[0], points.Points[1]), Precision.TwoTag, LocationCalculator.Distance(points.Points[0], points.Points[1]) / 2));
}
return(CalculateCommonPoint(Distances, LastLocation));
}
protected override LocationResult CalculateCommonPoint(List <NearbyBluetoothTag> Distances, LocationResult LastLocation)
{
List <Point> adjacentPoints = new List <Point>(); // ebben lesznek az egyes metszetek legközelebbi pontjai után kiválogatott szomszédos pontok
// kiszámoljuk minden kör, minden másik körhöz mért metszéspontját
List <Intersection> intersectionPoints = GetIntersections(Distances); // ebben lesznek két-két kör metszéspontjai
// eddig jó
for (int i = 0; i < intersectionPoints.Count; i++)
{
for (int j = i + 1; j < intersectionPoints.Count; j++)
{
if (i == j)
{
continue;
}
adjacentPoints.Add(LocationCalculator.ClosestPoint(intersectionPoints[i], intersectionPoints[j]));
}
}
return(new LocationResult(LocationCalculator.PointAverage(adjacentPoints), Precision.ThreeOrMoreTag));
}
/// <summary>
/// !!! Ez csak pontos esetben működik !!!
///
/// Ez a módszer nem vizsgálja meg, csak az első két metszet pontjait.
/// Azon a feltevésen alapszik, hogy minden metszet két pontja közül az egyik közös.
/// Így elég, ha két metszés megvizsgálunk, és megkeressük a közös pontot.
/// </summary>
public override Point CalculateCommonPoint(List <Intersection> intersections)
{
List <Point> mainIntersectPoints = intersections[0].Points;
List <Point> comparsionIntersectPoints = intersections[1].Points;
double mindist = LocationCalculator.Distance(mainIntersectPoints[0], comparsionIntersectPoints[0]);
Point commonPoint = mainIntersectPoints[0];
for (int i = 0; i < mainIntersectPoints.Count; i++)
{
for (int j = 0; j < comparsionIntersectPoints.Count; j++)
{
double dist = LocationCalculator.Distance(mainIntersectPoints[i], comparsionIntersectPoints[j]);
if (dist < mindist)
{
mindist = dist;
commonPoint = mainIntersectPoints[i];
}
}
}
return(commonPoint);
}
/// <summary>
/// Ez működik általános esetre, amikor nincs pontos metszéspont
///
/// A legközelebbi 3 metszéspont átlagpontját adjuk vissza
/// Válasszuk ki a metszéspontok távolságai közül a legrövidebb hármat
/// Elméletileg a 3 távolság 6 metszéspontja lényegében 3 pont, amit keresünk, ezek átlaga kell
/// </summary>
public override Point CalculateCommonPoint(List <Intersection> intersections)
{
List <IntersectDistance> idistances = new List <IntersectDistance>(); // A metszéspontok távolságai
IntersectDistance idist;
for (int i = 0; i < intersections.Count; i++)
{
for (int j = 0; j < intersections.Count; j++)
{
if (i != j)
{
// Egy metszet két metszéspontjának távolságát nem is vesszük figyelembe
idist = new IntersectDistance(intersections[i].Points[0], intersections[j].Points[1]);
if (!idistances.Contains(idist))
{
idistances.Add(idist);
}
idist = new IntersectDistance(intersections[i].Points[0], intersections[j].Points[0]);
if (!idistances.Contains(idist))
{
idistances.Add(idist);
}
idist = new IntersectDistance(intersections[i].Points[1], intersections[j].Points[1]);
if (!idistances.Contains(idist))
{
idistances.Add(idist);
}
}
}
}
//---------------------új megoldás:--------------------
// Az összes metszéspontot ebben a listában tároljuk
List <Point> intersectPoints = new List <Point>();
//Ebben a listában tároljuk el a k legközelebbi metszéspontokat
List <Point> closestPoints = new List <Point>();
IntersectDistance firstDistance = getMinimumDistance(idistances);
closestPoints.Add(firstDistance.P1);
closestPoints.Add(firstDistance.P2);
foreach (Intersection i in intersections)
{
foreach (Point p in i.Points)
{
if (!p.Equals(firstDistance.P1) && !p.Equals(firstDistance.P2))
{
intersectPoints.Add(p);
}
}
}
Point candidate1 = getClosestPoint(firstDistance.P1, intersectPoints);
Point candidate2 = getClosestPoint(firstDistance.P2, intersectPoints);
closestPoints.Add(getThirdPoint(candidate1, candidate2, firstDistance));
return(LocationCalculator.PointAverage(closestPoints));
}
private double getDistance(Point p1, Point p2, Point p3)
{
return(LocationCalculator.Distance(p1, p2) + LocationCalculator.Distance(p2, p3) + LocationCalculator.Distance(p1, p3));
}
public IntersectDistance(Point P1, Point P2)
{
this.P1 = P1;
this.P2 = P2;
this.Distance = LocationCalculator.Distance(P1, P2);
}
