Vector2 Trilateration(EstimoteUnityBeacon b1, EstimoteUnityBeacon b2, EstimoteUnityBeacon b3)
{
Vector2 position = new Vector2();
//get the custom beacons to get data from Unity
CustomBeacon c1 = FindCustomBeacon(b1);
CustomBeacon c2 = FindCustomBeacon(b2);
CustomBeacon c3 = FindCustomBeacon(b3);
//get estimated distances from each estimoteBeacon
float d1 = GetBeaconDistance(b1.RSSI);
float d2 = GetBeaconDistance(b2.RSSI);
float d3 = GetBeaconDistance(b3.RSSI);
//temp variables set 1
float A = (c1.transform.position.x * c1.transform.position.x) + (c1.transform.position.y * c1.transform.position.y) - (d1 * d1);
float B = (c2.transform.position.x * c2.transform.position.x) + (c2.transform.position.y * c2.transform.position.y) - (d2 * d2);
float C = (c3.transform.position.x * c3.transform.position.x) + (c3.transform.position.y * c3.transform.position.y) - (d3 * d3);
//temp variable set 2
float x32 = c3.transform.position.x - c2.transform.position.x;
float x13 = c1.transform.position.x - c3.transform.position.x;
float x21 = c2.transform.position.x - c1.transform.position.x;
float y32 = c3.transform.position.y - c2.transform.position.y;
float y13 = c1.transform.position.y - c3.transform.position.y;
float y21 = c2.transform.position.y - c1.transform.position.y;
//use temp variabales in trilateration equation to get position
position.x = (A * y32) + (B * y13) + (C * y21);
position.x /= (2 * ((c1.transform.position.x * y32) + (c2.transform.position.x * y13) + (c3.transform.position.x * y21)));
position.y = (A * x32) + (B * x13) + (C * x21);
position.y /= (2 * ((c1.transform.position.y * y32) + (c2.transform.position.y * y13) + (c3.transform.position.y * y21)));
return(position);
}
//return true if found
public bool SelectBeacon(CustomBeacon _beacon)
{
DeselectBeacon();
for (int i = 0; i < estimoteBeacons.Count; i++)
{
if (estimoteBeacons[i].Major == _beacon.major && estimoteBeacons[i].Minor == _beacon.minor)
{
selectedBeacon = estimoteBeacons[i];
_beacon.SelectBeacon();
return(true);
}
}
return(false);
//selectedBeacon = null;
}