private static QuadlinearCalculationResult LowCalculationResult(List <AnchorData> realData)
{
// Low level calculation tries to find a first match.
AnchorData[] anchorData = new AnchorData[4];
Vector3 res1, res2;
float d1, d2;
for (int i = 0; i < realData.Count; i++)
{
anchorData[0] = realData[i];
for (int j = i + 1; j < realData.Count; j++)
{
anchorData[1] = realData[j];
for (int k = j + 1; k < realData.Count; k++)
{
anchorData[2] = realData[k];
for (int l = k + 1; l < realData.Count; l++)
{
anchorData[3] = realData[l];
if (TrilinearCalculations.ThreePoint(anchorData[0].Position, anchorData[1].Position, anchorData[2].Position, anchorData[0].Distance, anchorData[1].Distance, anchorData[2].Distance, out res1, out res2))
{
d1 = (anchorData[3].Position - res1).sqrMagnitude;
d2 = (anchorData[3].Position - res2).sqrMagnitude;
if (Math.Abs(d1 - d2) < .000001f)
{
continue;
}
if (Math.Abs(d1 - anchorData[3].Distance) < Math.Abs(d2 - anchorData[3].Distance))
{
return(new QuadlinearCalculationResult
{
Position = res1,
CalculationLevel = CalculationLevel.Low,
CalculationOptions = realData.Count
});
}
else
{
return(new QuadlinearCalculationResult
{
Position = res2,
CalculationLevel = CalculationLevel.Low,
CalculationOptions = realData.Count
});
}
}
}
}
}
}
return(new QuadlinearCalculationResult
{
Errored = true,
CalculationLevel = CalculationLevel.Low,
CalculationOptions = realData.Count
});
}
internal static ICalculationResult Exact(AnchorData anchorData1, AnchorData anchorData2, AnchorData anchorData3, AnchorData anchorData4, CalculationLevel calculationLevel)
{
// The exact case does not NEED the calculation level as there are 4 points and calculation requires 4 points.
// The algorithm works by getting the result from 3 points and then picking the correct one from them by using the 4th point.
Vector3 res1, res2;
if (TrilinearCalculations.ThreePoint(anchorData1.Position, anchorData2.Position, anchorData3.Position, anchorData1.Distance, anchorData2.Distance, anchorData3.Distance, out res1, out res2))
{
float d1 = (anchorData4.Position - res1).sqrMagnitude;
float d2 = (anchorData4.Position - res2).sqrMagnitude;
if (Math.Abs(d1 - d2) < .000001f)
{
return(new TrilinearCalculationResult
{
Result1 = res1,
Result2 = res2
});
}
if (Math.Abs(d1 - anchorData4.Distance) < Math.Abs(d2 - anchorData4.Distance))
{
return(new QuadlinearCalculationResult
{
Position = res1,
CalculationLevel = calculationLevel,
CalculationOptions = 4
});
}
else
{
return(new QuadlinearCalculationResult
{
Position = res2,
CalculationLevel = calculationLevel,
CalculationOptions = 4
});
}
}
else
{
return(new QuadlinearCalculationResult
{
Errored = true,
CalculationLevel = calculationLevel,
CalculationOptions = 4
});
}
}
internal TrilinearCalculationResult(AnchorData anchorData1, AnchorData anchorData2, AnchorData anchorData3)
{
Vector3 result1, result2;
// Calculation is done by separate calculation function in a separate static class
if (TrilinearCalculations.ThreePoint(anchorData1.Position, anchorData2.Position, anchorData3.Position, anchorData1.Distance, anchorData2.Distance, anchorData3.Distance, out result1, out result2))
{
Result1 = result1;
Result2 = result2;
CalculationErrored = false;
}
else
{
Result1 = Result2 = new Vector2();
CalculationErrored = true;
}
}
private static ICalculationResult HighCalculationResult(List <AnchorData> realData)
{
// High calculation level works by getting all the options and calculating the average result.
List <Vector3> results = new List <Vector3>();
AnchorData[] anchorData = new AnchorData[4];
Vector3 res1, res2;
float d1, d2;
for (int i = 0; i < realData.Count; i++)
{
anchorData[0] = realData[i];
for (int j = i + 1; j < realData.Count; j++)
{
anchorData[1] = realData[j];
for (int k = j + 1; k < realData.Count; k++)
{
anchorData[2] = realData[k];
for (int l = k + 1; l < realData.Count; l++)
{
anchorData[3] = realData[l];
if (TrilinearCalculations.ThreePoint(anchorData[0].Position, anchorData[1].Position, anchorData[2].Position, anchorData[0].Distance, anchorData[1].Distance, anchorData[2].Distance, out res1, out res2))
{
d1 = (anchorData[3].Position - res1).sqrMagnitude;
d2 = (anchorData[3].Position - res2).sqrMagnitude;
if (Math.Abs(d1 - d2) < .000001f)
{
continue;
}
if (Math.Abs(d1 - anchorData[3].Distance) < Math.Abs(d2 - anchorData[3].Distance))
{
results.Add(res1);
}
else
{
results.Add(res2);
}
}
}
}
}
}
if (results.Count == 0)
{
return(new QuadlinearCalculationResult
{
Errored = true,
CalculationLevel = CalculationLevel.Low,
CalculationOptions = realData.Count
});
}
Vector3 position = new Vector3();
foreach (Vector3 res in results)
{
position += res;
}
return(new QuadlinearCalculationResult
{
Position = position / results.Count,
CalculationLevel = CalculationLevel.High,
CalculationOptions = realData.Count
});
}