/// <summary>
/// Give any 3 valid anchors then return the x, y position
/// </summary>
private bool loc_trilateration(class_Anchors anchorA, class_Anchors anchorB, class_Anchors anchorC)
{
Single x, y, x1, y1, x2, y2, x3, y3;
Single ca, cb, cc, cd, ce, cf, cg, ch, ci;
bool v1, v2, v3;
short count;
// calculate trilateration
ca = anchorB.X - anchorA.X;
cb = anchorB.Y - anchorA.Y;
cc = (Single)(Math.Pow(anchorA.Range, 2) - Math.Pow(anchorB.Range, 2) - Math.Pow(anchorA.X, 2) + Math.Pow(anchorB.X, 2) - Math.Pow(anchorA.Y, 2) + Math.Pow(anchorB.Y, 2));
cd = anchorB.X - anchorC.X;
ce = anchorB.Y - anchorC.Y;
cf = (Single)(Math.Pow(anchorC.Range, 2) - Math.Pow(anchorB.Range, 2) - Math.Pow(anchorC.X, 2) + Math.Pow(anchorB.X, 2) - Math.Pow(anchorC.Y, 2) + Math.Pow(anchorB.Y, 2));
cg = anchorA.X - anchorC.X;
ch = anchorA.Y - anchorC.Y;
ci = (Single)(Math.Pow(anchorC.Range, 2) - Math.Pow(anchorA.Range, 2) - Math.Pow(anchorC.X, 2) + Math.Pow(anchorA.X, 2) - Math.Pow(anchorC.Y, 2) + Math.Pow(anchorA.Y, 2));
if (cd * cb == ca * ce)
{
x1 = 0;
y1 = 0;
v1 = false;
}
else
{
y1 = 0.5f * (cd * cc - ca * cf) / (cd * cb - ca * ce);
if (ca == 0) x1 = (cf - 2 * ce * y1) / (2 * cd);
else x1 = (cc - 2 * cb * y1) / (2 * ca);
v1 = true;
}
if (cg * cb == ca * ch)
{
x2 = 0;
y2 = 0;
v2 = false;
}
else
{
y2 = 0.5f * (cg * cc - ca * ci) / (cg * cb - ca * ch);
if (ca == 0) x2 = (ci - 2 * ch * y2) / (2 * cg);
else x2 = (cc - 2 * cb * y2) / (2 * ca);
v2 = true;
}
if (cg * ce == cd * ch)
{
x3 = 0;
y3 = 0;
v3 = false;
}
else
{
y3 = 0.5f * (cg * cf - cd * ci) / (cg * ce - cd * ch);
if (cd == 0) x3 = (ci - 2 * ch * y3) / (2 * cg);
else x3 = (cf - 2 * ce * y3) / (2 * cd);
v3 = true;
}
x = 0;
y = 0;
count = 0;
if (v1)
{
x = x + x1;
y = y + y1;
count++;
}
if (v2)
{
x = x + x2;
y = y + y2;
count++;
}
if (v3)
{
x = x + x3;
y = y + y3;
count++;
}
if (count > 0)
{
x = x / count;
y = y / count;
}
else return false;
myTag.X = x;
myTag.Y = y;
myTag.Avg.X = x;
myTag.Avg.Y = y;
for (count = 0; count < myTag_Old_X.Length; count++)
{
myTag_Old_X[count] = x;
myTag_Old_Y[count] = y;
}
return true;
}
private bool loc_initial()
{
class_Anchors[] candidate3 = new class_Anchors[3];
Single[] tmpSingleA = new Single[6];
int anchor_count = 0;
for (i = 0; i < 3; i++)
{
candidate3[i] = anchor[i];
}
for (i = 0; i < anchor.Count; i++)
{
if (anchor[i].Reliability >= candidate3[0].Reliability)
{
candidate3[2] = candidate3[1];
candidate3[1] = candidate3[0];
candidate3[0] = anchor[i];
anchor_count++;
continue;
}
else if (anchor[i].Reliability >= candidate3[1].Reliability)
{
candidate3[2] = candidate3[1];
candidate3[1] = anchor[i];
anchor_count++;
continue;
}
else if (anchor[i].Reliability >= candidate3[2].Reliability)
{
candidate3[2] = anchor[i];
anchor_count++;
continue;
}
}
//Log.Debug("Brian", "count= " + anchor_count.ToString());
if (anchor_count < 3)
{
Log.Debug(Tag, "Searching for beacon...\r\n");
bBeaconFind = false;
return false;
}
else
{
if (!loc_trilateration(candidate3[0], candidate3[1], candidate3[2]))
{
return false;
}
Log.Debug("Brian", "Beacon found!!");
bBeaconFind = true;
myEKF = new class_EKFL5(myTag.X, myTag.Y, 0f, anchor.Count);
for (i = 0; i < anchor.Count; i++)
{
myEKF.Anchor[i] = anchor[i];
}
return true;
}
}
private bool ReadConfig(string ff)
{
if (File.Exists(ff))
{
StreamReader sr = new StreamReader(ff);
int tmpInt, count1 = 0;
string line;
string[] linesplit;
//var adapter = new ArrayAdapter<string>(this, Android.Resource.Layout.SimpleSpinnerDropDownItem);
//adapter.Add("Favorite");
while (!sr.EndOfStream)
{
line = sr.ReadLine();
linesplit = line.Split('=');
if (linesplit[0] == "mapwidth") cfg.MapWidth = Convert.ToUInt16(linesplit[1]);
else if (linesplit[0] == "mapheight") cfg.MapHeight = Convert.ToUInt16(linesplit[1]);
else if (linesplit[0] == "mapeast") cfg.MapEast = Convert.ToInt16(linesplit[1]);
else if (linesplit[0] == "gridwidth") cfg.GridWidth = Convert.ToUInt16(linesplit[1]);
else if (linesplit[0] == "gridheight") cfg.GridHeight = Convert.ToUInt16(linesplit[1]);
else if (linesplit[0] == "agent_max_speed") cfg.MaxSpeed = Convert.ToUInt16(linesplit[1]);
else if (linesplit[0] == "anchor")
{
class_Anchors tmpanchor = new class_Anchors(Convert.ToInt32(linesplit[1]), Convert.ToSingle(linesplit[2]), Convert.ToSingle(linesplit[3]), Convert.ToSingle(linesplit[4]));
anchor.Add(tmpanchor);
}
else if (linesplit[0] == "favorite")
{
//favorite[count1].Note = linesplit[1];
//favorite[count1].X = Convert.ToInt16(linesplit[2]);
//favorite[count1].Y = Convert.ToInt16(linesplit[3]);
//adapter.Add(favorite[count1].Note);
count1++;
}
}
sr.Close();
//spinnerTarget.Adapter = adapter;
Log.Debug("Brian", "anchor num=" + anchor.Count.ToString());
return true;
}
else
return false;
}