Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- class Utils
- {
- public double[] ParseString(string str)
- {
- string[] splittedFile = str.Split(' ');
- double[] parsedValues = new double[splittedFile.Length];
- for (int i = 0; i < splittedFile.Length; i++)
- {
- double y = 0;
- if (splittedFile[i].Contains('s'))
- {
- string x = splittedFile[i].Replace("s", "");
- y = Math.Sqrt(double.Parse(x));
- }
- else
- {
- y = double.Parse(splittedFile[i]);
- }
- parsedValues[i] = y;
- }
- return parsedValues;
- }
- public static bool NearlyEqual(double a, double b, double epsilon)
- {
- const double MinNormal = 2.2250738585072014E-308d;
- double absA = Math.Abs(a);
- double absB = Math.Abs(b);
- double diff = Math.Abs(a - b);
- if (a.Equals(b))
- { // shortcut, handles infinities
- return true;
- }
- else if (a == 0 || b == 0 || absA + absB < MinNormal)
- {
- // a or b is zero or both are extremely close to it
- // relative error is less meaningful here
- return diff < (epsilon * MinNormal);
- }
- else
- { // use relative error
- return diff / (absA + absB) < epsilon;
- }
- }
- public static double[] FindXs(double a, double b, double r, double c, double d, double s)
- {
- var something = a * a * a - a * a * c + a * b * b - 2 * a * b * d - a * c * c + a * d * d - a * r * r + a * s * s + b * b * c - 2 * b * c * d + c * c * c + c * d * d + c * r * r - c * s * s;
- var other_thing = Math.Sqrt((-a * a + 2 * a * c - b * b + 2 * b * d - c * c - d * d + r * r + 2 * r * s + s * s) * (a * a - 2 * a * c + b * b - 2 * b * d + c * c + d * d - r * r + 2 * r * s - s * s));
- var denominator = 2 * (a * a - 2 * a * c + b * b - 2 * b * d + c * c + d * d);
- double[] result = { (something + other_thing * (d - b)) / denominator, (something + other_thing * (b - d)) / denominator };
- return result;
- }
- public static double FindY(double a, double b, double r, double c, double d, double s, double x)
- {
- return (a * a + b * b - c * c - d * d - r * r + s * s - 2 * x * (a - c)) / (2 * (b - d));
- }
- public static double[] FindPossibleSolutions(double a, double b, double r, double c, double d, double s)
- {
- var xs = FindXs(a, b, r, c, d, s);
- double[] results = {
- xs[0], FindY(a, b, r, c, d, s, xs[0]),
- xs[1], FindY(a, b, r, c, d, s, xs[1])
- };
- return results;
- }
- public static bool CheckSolution(double a, double b, double r, double x, double y)
- {
- return NearlyEqual((x - a) * (x - a) + (y - b) * (y - b), r * r, 1e-09);
- }
- public static double[] FindSolution(Radar radar1, Radar radar2, Radar radar3)
- {
- double[] solutions = FindPossibleSolutions(radar1.LatitudeCoord, radar1.LongitudeCoord, radar1.FirstMeasuredDistance, radar2.LatitudeCoord,
- radar2.LongitudeCoord, radar2.FirstMeasuredDistance);
- if (CheckSolution(radar3.LatitudeCoord, radar3.LongitudeCoord, radar3.FirstMeasuredDistance, solutions[0], solutions[1]))
- {
- double[] finalSolution = { solutions[0], solutions[1] };
- return finalSolution;
- }
- if (CheckSolution(radar3.LatitudeCoord, radar3.LongitudeCoord, radar3.FirstMeasuredDistance, solutions[2], solutions[3]))
- {
- double[] finalSolution = { solutions[2], solutions[3] };
- return finalSolution;
- }
- return null;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement