public static AxonPoint CalculatePoint(int x, int y, HeadsetModel headset) { var polar = CoordinateSystem.PixelToPolar(x, y, headset); var bestPhi0 = ClosestWholePhi0(polar); for (int i = 0; i < 5; i++) { var deviation = .55f / Mathf.Pow(10, i); bestPhi0 = ClosestPhi0(polar, bestPhi0 - deviation, bestPhi0 + deviation, 11); } return(new AxonPoint() { phi0 = bestPhi0, rho = polar.x, b = AxonModel.CalculateB(bestPhi0), c = AxonModel.CalculateC(bestPhi0) }); }
private static float ClosestPhi0(Vector2 polar, float from, float to, int resolution) { var increment = (to - from) / resolution; var closestPhi0 = float.MaxValue; var closestPhi = float.MaxValue; for (int i = 0; i < resolution; i++) { var estPhi0 = from + increment * i; estPhi0 = AuxMath.Overflow(estPhi0, -180, 180); var estPhi = AxonModel.CalculatePhi(estPhi0, polar.x); if (Mathf.Abs(polar.y - estPhi) < Mathf.Abs(polar.y - closestPhi)) { closestPhi = estPhi; closestPhi0 = estPhi0; } } return(closestPhi0); }