Advertisement
Guest User

Untitled

a guest
Dec 6th, 2019
87
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 4.00 KB | None | 0 0
  1. class Utils
  2. {
  3. public double[] ParseString(string str)
  4. {
  5. string[] splittedFile = str.Split(' ');
  6. double[] parsedValues = new double[splittedFile.Length];
  7. for (int i = 0; i < splittedFile.Length; i++)
  8. {
  9. double y = 0;
  10. if (splittedFile[i].Contains('s'))
  11. {
  12. string x = splittedFile[i].Replace("s", "");
  13. y = Math.Sqrt(double.Parse(x));
  14. }
  15. else
  16. {
  17. y = double.Parse(splittedFile[i]);
  18. }
  19. parsedValues[i] = y;
  20. }
  21. return parsedValues;
  22. }
  23.  
  24.  
  25.  
  26.  
  27.  
  28. public static bool NearlyEqual(double a, double b, double epsilon)
  29. {
  30. const double MinNormal = 2.2250738585072014E-308d;
  31. double absA = Math.Abs(a);
  32. double absB = Math.Abs(b);
  33. double diff = Math.Abs(a - b);
  34.  
  35. if (a.Equals(b))
  36. { // shortcut, handles infinities
  37. return true;
  38. }
  39. else if (a == 0 || b == 0 || absA + absB < MinNormal)
  40. {
  41. // a or b is zero or both are extremely close to it
  42. // relative error is less meaningful here
  43. return diff < (epsilon * MinNormal);
  44. }
  45. else
  46. { // use relative error
  47. return diff / (absA + absB) < epsilon;
  48. }
  49. }
  50.  
  51. public static double[] FindXs(double a, double b, double r, double c, double d, double s)
  52. {
  53. 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;
  54. 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));
  55. var denominator = 2 * (a * a - 2 * a * c + b * b - 2 * b * d + c * c + d * d);
  56. double[] result = { (something + other_thing * (d - b)) / denominator, (something + other_thing * (b - d)) / denominator };
  57. return result;
  58. }
  59.  
  60. public static double FindY(double a, double b, double r, double c, double d, double s, double x)
  61. {
  62. return (a * a + b * b - c * c - d * d - r * r + s * s - 2 * x * (a - c)) / (2 * (b - d));
  63. }
  64.  
  65.  
  66. public static double[] FindPossibleSolutions(double a, double b, double r, double c, double d, double s)
  67. {
  68. var xs = FindXs(a, b, r, c, d, s);
  69. double[] results = {
  70. xs[0], FindY(a, b, r, c, d, s, xs[0]),
  71. xs[1], FindY(a, b, r, c, d, s, xs[1])
  72. };
  73.  
  74. return results;
  75. }
  76.  
  77.  
  78. public static bool CheckSolution(double a, double b, double r, double x, double y)
  79. {
  80. return NearlyEqual((x - a) * (x - a) + (y - b) * (y - b), r * r, 1e-09);
  81. }
  82.  
  83.  
  84. public static double[] FindSolution(Radar radar1, Radar radar2, Radar radar3)
  85. {
  86. double[] solutions = FindPossibleSolutions(radar1.LatitudeCoord, radar1.LongitudeCoord, radar1.FirstMeasuredDistance, radar2.LatitudeCoord,
  87. radar2.LongitudeCoord, radar2.FirstMeasuredDistance);
  88. if (CheckSolution(radar3.LatitudeCoord, radar3.LongitudeCoord, radar3.FirstMeasuredDistance, solutions[0], solutions[1]))
  89. {
  90. double[] finalSolution = { solutions[0], solutions[1] };
  91. return finalSolution;
  92. }
  93. if (CheckSolution(radar3.LatitudeCoord, radar3.LongitudeCoord, radar3.FirstMeasuredDistance, solutions[2], solutions[3]))
  94. {
  95. double[] finalSolution = { solutions[2], solutions[3] };
  96. return finalSolution;
  97. }
  98.  
  99. return null;
  100. }
  101. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement