context: /*Automated Rodent Tracker - A program to automatically track rodents
  Copyright(C) 2015 Brett Michael Hewitt
  
  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.
  
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the
  GNU General Public License for more details.
  
  You should have received a copy of the GNU General Public License
  along with this program.If not, see<http://www.gnu.org/licenses/>.*/
  
  using System;
  using System.Collections.Generic;
  using System.Drawing;
  using System.Linq;
  using AutomatedRodentTracker.Classes;
  
  namespace AutomatedRodentTracker.Extensions
  {
      public static class MathExtension
      {
          public static double Deg2Rad;
          public static double Rad2Deg;
  
          static MathExtension()
          {
              double pi = Math.PI;
              Deg2Rad = pi/180d;
              Rad2Deg = 180/pi;
          }
  
          public static int ClosestPowerOfTwo(int x)
          {
              x--;
              x |= (x >> 1);
              x |= (x >> 2);
              x |= (x >> 4);
              x |= (x >> 8);
              x |= (x >> 16);
  
              return (x + 1);
          }
  
          public static float[] NormalizeData(IEnumerable<float> data, float min, float max)
          {
              float dataMax = data.Max();
              float dataMin = data.Min();
              float range = dataMax - dataMin;
  
              return data.Select(d => (d - dataMin) / range).Select(n => ((1 - n) * min + n * max)).ToArray();
          }
  
          public static float[] NormalizeData(IEnumerable<double> data, float min, float max)
          {
              double dataMax = data.Max();
              double dataMin = data.Min();
              double range = dataMax - dataMin;
  
              return data.Select(d => (d - dataMin) / range).Select(n => (float)((1 - n) * min + n * max)).ToArray();
          }
  
          //public static float GetAreaFromPoints(Point[] points)
          //{
          //    return Math.Abs(points.Take(points.Length - 1).Select((p, i2) => (points[i2 + 1].X - p.X) * (points[i2 + 1].Y + p.Y)).Sum() / 2);
          //}
  
          public static float PolygonArea(Point[] polygon)
          {
              int i, j;
              float area = 0;
  
              for (i = 0; i < polygon.Length; i++)
              {
                  j = (i + 1) % polygon.Length;
  
                  area += polygon[i].X * polygon[j].Y;
                  area -= polygon[i].Y * polygon[j].X;
              }
  
              area /= 2;
              return (area < 0 ? -area : area);
          }
  
          public static float PolygonArea(PointF[] polygon)
          {
              int i, j;
              float area = 0;
  
              for (i = 0; i < polygon.Length; i++)
              {
                  j = (i + 1) % polygon.Length;
  
                  area += polygon[i].X * polygon[j].Y;
                  area -= polygon[i].Y * polygon[j].X;
              }
  
              area /= 2;
              return (area < 0 ? -area : area);
          }
  
          public static PointSideVector FindSide(Point p1, Point p2, Point pointToCheck)
          {
              float result = (p2.X - p1.X) * (pointToCheck.Y - p1.Y) - (p2.Y - p1.Y) * (pointToCheck.X - p1.X);
  
              if (result < 0)
              {
                  return PointSideVector.Below;
              }
  
              if (result > 0)
              {
                  return PointSideVector.Above;
              }
  
              return PointSideVector.On;
          }
  
          public static PointSideVector FindSide(PointF p1, PointF p2, PointF pointToCheck)
          {
              float result = (p2.X - p1.X) * (pointToCheck.Y - p1.Y) - (p2.Y - p1.Y) * (pointToCheck.X - p1.X);
  
              if (result < 0)
              {
                  return PointSideVector.Below;
              }
  
              if (result > 0)
              {
                  return PointSideVector.Above;
              }
  
              return PointSideVector.On;
          }
  
          public static bool PolygonContainsPoint(Point[] polygon, Point point)
          {
              bool result = false;
              int j = polygon.Count() - 1;
              for (int i = 0; i < polygon.Count(); i++)
              {
                  if (polygon[i].Y < point.Y && polygon[j].Y >= point.Y || polygon[j].Y < point.Y && polygon[i].Y >= point.Y)
                  {
                      if (polygon[i].X + (point.Y - polygon[i].Y) / (polygon[j].Y - polygon[i].Y) * (polygon[j].X - polygon[i].X) < point.X)
                      {
                          result = !result;
                      }
                  }
                  j = i;
              }
              return result;
          }
  
          public static bool PolygonContainsPoint(Point[] polygon, PointF point)
          {
              bool result = false;
              int j = polygon.Count() - 1;
              for (int i = 0; i < polygon.Count(); i++)
              {
                  if (polygon[i].Y < point.Y && polygon[j].Y >= point.Y || polygon[j].Y < point.Y && polygon[i].Y >= point.Y)
                  {
                      if (polygon[i].X + (point.Y - polygon[i].Y) / (polygon[j].Y - polygon[i].Y) * (polygon[j].X - polygon[i].X) < point.X)
                      {
                          result = !result;
                      }
                  }
                  j = i;
              }
              return result;
          }
  
          /// <summary>
          /// Rotates one point around another
          /// </summary>
          /// <param name="pointToRotate">The point to rotate.</param>
          /// <param name="centerPoint">The centre point of rotation.</param>
          /// <param name="angleInDegrees">The rotation angle in degrees.</param>
          /// <returns>Rotated point</returns>
          public static Point RotatePoint(Point pointToRotate, Point centerPoint, double angleInDegrees)
          {
              double angleInRadians = angleInDegrees * (Math.PI / 180);
              double cosTheta = Math.Cos(angleInRadians);
              double sinTheta = Math.Sin(angleInRadians);
              return new Point
              {
                  X =
                      (int)
                      (cosTheta * (pointToRotate.X - centerPoint.X) -
                      sinTheta * (pointToRotate.Y - centerPoint.Y) + centerPoint.X),
                  Y =
                      (int)
                      (sinTheta * (pointToRotate.X - centerPoint.X) +
                      cosTheta * (pointToRotate.Y - centerPoint.Y) + centerPoint.Y)
              };
          }
  
          /// <summary>
          /// Finds the the first intersection point of a line and polygon 
          /// </summary>
          /// <param name="ps1">The first point of the line</param>
          /// <param name="pe1">The second point of the line</param>
          /// <param name="polygon">THe polygon</param>
          /// <returns>The intersection point, PointF.Empty if no intersection found</returns>
          public static PointF PolygonLineIntersectionPoint(PointF ps1, PointF pe1, PointF[] polygon)
          {
              for (int i = 1; i < polygon.Length; i++)
              {
                  PointF ps2 = polygon[i - 1];
                  PointF pe2 = polygon[i];
  
                  PointF intersectionPoint = LineIntersectionPoint(ps1, pe1, ps2, pe2);
  
                  if (intersectionPoint != Point.Empty)
                  {
                      return intersectionPoint;
                  }
              }
  
              return PointF.Empty;
          }
  
          /// <summary>
          /// Finds the the first intersection point of a line and polygon 
          /// </summary>
          /// <param name="ps1">The first point of the line</param>
          /// <param name="pe1">The second point of the line</param>
          /// <param name="polygon">THe polygon</param>
          /// <returns>The intersection point, PointF.Empty if no intersection found</returns>
          public static PointF PolygonLineIntersectionPoint(PointF ps1, PointF pe1, Point[] polygon)
          {
              for (int i = 1; i < polygon.Length; i++)
              {
                  PointF ps2 = polygon[i - 1];
                  PointF pe2 = polygon[i];
  
                  PointF intersectionPoint = LineIntersectionPoint(ps1, pe1, ps2, pe2);
  
                  if (!intersectionPoint.IsEmpty)
                  {
                      return intersectionPoint;
                  }
              }
  
              return PointF.Empty;
          }
  
          public static PointF LineIntersectionPoint(PointF ps1, PointF pe1, PointF ps2, PointF pe2)
          {
              // Get A,B,C of first line - points : ps1 to pe1
              float A1 = pe1.Y - ps1.Y;
              float B1 = ps1.X - pe1.X;
              float C1 = A1 * ps1.X + B1 * ps1.Y;
  
              // Get A,B,C of second line - points : ps2 to pe2
              float A2 = pe2.Y - ps2.Y;
              float B2 = ps2.X - pe2.X;
              float C2 = A2 * ps2.X + B2 * ps2.Y;
  
              // Get delta and check if the lines are parallel
              float delta = A1 * B2 - A2 * B1;
  
              if (delta == 0)
              {
                  return PointF.Empty;
              }
  
              //Check distances
              Line line1 = new Line(ps1, pe1);
              Line line2 = new Line(ps2, pe2);
  
              if (IntersectionOf(line1, line2) == Intersection.None)
              {
                  return PointF.Empty;
              }
  
              // now return the Vector2 intersection point
              return new PointF((B2 * C1 - B1 * C2) / delta, (A1 * C2 - A2 * C1) / delta);
          }
  
          public static Intersection IntersectionOf(Line line1, Line line2)
          {
              //  Fail if either line segment is zero-length.
              if (line1.X1 == line1.X2 && line1.Y1 == line1.Y2 || line2.X1 == line2.X2 && line2.Y1 == line2.Y2)
                  return Intersection.None;
  
              if (line1.X1 == line2.X1 && line1.Y1 == line2.Y1 || line1.X2 == line2.X1 && line1.Y2 == line2.Y1)
                  return Intersection.Intersection;
              if (line1.X1 == line2.X2 && line1.Y1 == line2.Y2 || line1.X2 == line2.X2 && line1.Y2 == line2.Y2)
                  return Intersection.Intersection;
  
              //  (1) Translate the system so that point A is on the origin.
              line1.X2 -= line1.X1; line1.Y2 -= line1.Y1;
              line2.X1 -= line1.X1; line2.Y1 -= line1.Y1;
              line2.X2 -= line1.X1; line2.Y2 -= line1.Y1;
  
              //  Discover the length of segment A-B.
              float distAB = (float)Math.Sqrt(line1.X2 * line1.X2 + line1.Y2 * line1.Y2);
  
              //  (2) Rotate the system so that point B is on the positive X axis.
              float theCos = line1.X2 / distAB;
              float theSin = line1.Y2 / distAB;
              float newX = line2.X1 * theCos + line2.Y1 * theSin;
              line2.Y1 = line2.Y1 * theCos - line2.X1 * theSin;
              line2.X1 = newX;
              newX = line2.X2 * theCos + line2.Y2 * theSin;
              line2.Y2 = line2.Y2 * theCos - line2.X2 * theSin;
              line2.X2 = newX;
  
              //  Fail if segment C-D doesn't cross line A-B.
              if (line2.Y1 < 0 && line2.Y2 < 0 || line2.Y1 >= 0 && line2.Y2 >= 0)
                  return Intersection.None;
  
              //  (3) Discover the position of the intersection point along line A-B.
              double posAB = line2.X2 + (line2.X1 - line2.X2) * line2.Y2 / (line2.Y2 - line2.Y1);
  
              //  Fail if segment C-D crosses line A-B outside of segment A-B.
              if (posAB < 0 || posAB > distAB)
                  return Intersection.None;
  
              //  (4) Apply the discovered position to line A-B in the original coordinate system.
              return Intersection.Intersection;
          }
  
          /// <summary>
          /// Find the minimum distance between a line and a point
          /// </summary> 
          /// <param name="p1">Line point 1</param>
          /// <param name="p2">Line point 2</param>
          /// <param name="pt">Point to test</param>
          /// <returns>The minimum distance between a point and a line segment</returns>
          public static double MinDistanceFromLineToPoint(PointF p1, PointF p2, PointF pt)
          {
              float dx = p2.X - p1.X;
              float dy = p2.Y - p1.Y;
              
              if ((dx == 0) && (dy == 0))
              {
                  dx = pt.X - p1.X;
                  dy = pt.Y - p1.Y;
                  return Math.Sqrt(dx * dx + dy * dy);
              }
  
              float t = ((pt.X - p1.X) * dx + (pt.Y - p1.Y) * dy) / (dx * dx + dy * dy);
  
              if (t < 0)
              {
                  dx = pt.X - p1.X;
                  dy = pt.Y - p1.Y;
              }
              else if (t > 1)
              {
                  dx = pt.X - p2.X;
                  dy = pt.Y - p2.Y;
              }
              else
              {
                  PointF closest = new PointF(p1.X + t * dx, p1.Y + t * dy);
                  dx = pt.X - closest.X;
                  dy = pt.Y - closest.Y;
              }
  
              return Math.Sqrt(dx * dx + dy * dy);
          }
  
          public static double MinDistanceFromLineToPoint(PointF p1, PointF p2, PointF pt, out PointF closestPoint)
          {
              closestPoint = pt;
              float dx = p2.X - p1.X;
              float dy = p2.Y - p1.Y;
  
              if ((dx == 0) && (dy == 0))
              {
                  dx = pt.X - p1.X;
                  dy = pt.Y - p1.Y;
                  return Math.Sqrt(dx * dx + dy * dy);
              }
  
              float t = ((pt.X - p1.X) * dx + (pt.Y - p1.Y) * dy) / (dx * dx + dy * dy);
  
              if (t < 0)
              {
                  dx = pt.X - p1.X;
                  dy = pt.Y - p1.Y;
              }
              else if (t > 1)
              {
                  dx = pt.X - p2.X;
                  dy = pt.Y - p2.Y;
              }
              else
              {
                  closestPoint = new PointF(p1.X + t * dx, p1.Y + t * dy);
                  dx = pt.X - closestPoint.X;
                  dy = pt.Y - closestPoint.Y;
              }
  
              return Math.Sqrt(dx * dx + dy * dy);
          }
  
          public static double FindDistanceBetweenSegments(PointF p1, PointF p2, PointF p3, PointF p4)
          {
              // See if the segments intersect.
              bool lines_intersect, segments_intersect;
              PointF intersection;
  
              FindIntersection(p1, p2, p3, p4, out lines_intersect, out segments_intersect, out intersection);
              if (segments_intersect)
              {
                  // They intersect.
                  //close1 = intersection;
                  //close2 = intersection;
                  return 0;
              }
  
              // Find the other possible distances.
              PointF closest;
              double best_dist = double.MaxValue, test_dist;
  
              // Try p1.
              test_dist = FindDistanceToSegment(p1, p3, p4, out closest);
              if (test_dist < best_dist)
              {
                  best_dist = test_dist;
                  //close1 = p1;
                  //close2 = closest;
              }
  
              // Try p2.
              test_dist = FindDistanceToSegment(p2, p3, p4, out closest);
              if (test_dist < best_dist)
              {
                  best_dist = test_dist;
                  //close1 = p2;
                  //close2 = closest;
              }
  
              // Try p3.
              test_dist = FindDistanceToSegment(p3, p1, p2, out closest);
              if (test_dist < best_dist)
              {
                  best_dist = test_dist;
                  //close1 = closest;
                  //close2 = p3;
              }
  
              // Try p4.
              test_dist = FindDistanceToSegment(p4, p1, p2, out closest);
              if (test_dist < best_dist)
              {
                  best_dist = test_dist;
                  //close1 = closest;
                  //close2 = p4;
              }
  
              return best_dist;
          }
  
          public static double FindDistanceToSegment(PointF pt, PointF p1, PointF p2, out PointF closest)
          {
              float dx = p2.X - p1.X;
              float dy = p2.Y - p1.Y;
              if ((dx == 0) && (dy == 0))
              {
                  // It's a point not a line segment.
                  closest = p1;
  
gt:                 dx = pt.X - p1.X;