AppExperiments

using Leap.Unity.Infix;
using Leap.Unity.Swizzle;
using System;
using UnityEngine;

namespace Leap.Unity.Geometry {

  public static class Collision {

    #region Short-hand

    private static float Dot(Vector3 a, Vector3 b) {
      return Vector3.Dot(a, b);
    }

    private static float Dot(Vector2 a, Vector2 b) {
      return Vector2.Dot(a, b);
    }

    #endregion

    #region Box-Sphere

    public static bool DoesOverlap(Box box, Sphere sphere) {
      return DoesOverlap(sphere, box);
    }
    public static bool DoesOverlap(Sphere sphere, Box box) {
      var sphereCenter_world = sphere.matrix.MultiplyPoint3x4(Vector3.zero);
      var sphereCenter_box = box.matrix.inverse
                                .MultiplyPoint3x4(sphereCenter_world).Abs();

      float x = sphereCenter_box.x,
            y = sphereCenter_box.y,
            z = sphereCenter_box.z;

      float bx = box.radii.x.Abs(),
            by = box.radii.y.Abs(),
            bz = box.radii.z.Abs();

      float dx = x - Mathf.Min(bx, x),
            dy = y - Mathf.Min(by, y),
            dz = z - Mathf.Min(bz, z);

      if (dx == 0 && dy == 0 && dz == 0) return true;

      var boxSurfaceToSphereCenter_box = new Vector3(dx, dy, dz);
      var boxSurfaceToSphereCenter_world
        = box.matrix.MultiplyVector(boxSurfaceToSphereCenter_box);
      var boxSurfaceToSphereCenter_sphere
        = sphere.matrix.inverse.MultiplyVector(boxSurfaceToSphereCenter_world);

      return boxSurfaceToSphereCenter_sphere.sqrMagnitude < sphere.radius * sphere.radius;
    }

    #endregion

    #region Rect-Sphere

    /// <summary>
    /// Returns the distance between the closest points on the Rect and the Sphere,
    /// or 0 if the two overlap.
    /// </summary>
    public static float DistanceTo(Rect rect, Sphere sphere) {
      return DistanceBetween(sphere, rect);
    }
    /// <summary>
    /// Returns the distance between the closest points on the Rect and the Sphere,
    /// or 0 if the two overlap.
    /// </summary>
    public static float DistanceBetween(Sphere sphere, Rect rect) {
      var sphereCenter_world = sphere.matrix.MultiplyPoint3x4(Vector3.zero);
      var sphereCenter_rect = rect.matrix.inverse
                                  .MultiplyPoint3x4(sphereCenter_world).Abs();

      float x = sphereCenter_rect.x,
            y = sphereCenter_rect.y,
            z = sphereCenter_rect.z;

      float rx = rect.radii.x.Abs(),
            ry = rect.radii.y.Abs(),
            rz = 0f;

      float dx = x - Mathf.Min(rx, x),
            dy = y - Mathf.Min(ry, y),
            dz = z - Mathf.Min(rz, z);

      if (dx == 0 && dy == 0 && dz == 0) return 0f;

      var rectSurfaceToSphereCenter_rect = new Vector3(dx, dy, dz);
      var rectSurfaceToSphereCenter_world
        = rect.matrix.MultiplyVector(rectSurfaceToSphereCenter_rect);

      return Mathf.Max(0f, rectSurfaceToSphereCenter_world.magnitude - sphere.radius);
    }

    public static bool DoesOverlap(Rect rect, Sphere sphere) {
      return DoesOverlap(sphere, rect);
    }
    public static bool DoesOverlap(Sphere sphere, Rect rect) {
      var sphereCenter_world = sphere.matrix.MultiplyPoint3x4(Vector3.zero);
      var sphereCenter_rect = rect.matrix.inverse
                                  .MultiplyPoint3x4(sphereCenter_world).Abs();

      float x = sphereCenter_rect.x,
            y = sphereCenter_rect.y,
            z = sphereCenter_rect.z;

      float rx = rect.radii.x.Abs(),
            ry = rect.radii.y.Abs(),
            rz = 0f;

      float dx = x - Mathf.Min(rx, x),
            dy = y - Mathf.Min(ry, y),
            dz = z - Mathf.Min(rz, z);

      if (dx == 0 && dy == 0 && dz == 0) return true;

      var rectSurfaceToSphereCenter_rect = new Vector3(dx, dy, dz);
      var rectSurfaceToSphereCenter_world
        = rect.matrix.MultiplyVector(rectSurfaceToSphereCenter_rect);
      var rectSurfaceToSphereCenter_sphere
        = sphere.matrix.inverse.MultiplyVector(rectSurfaceToSphereCenter_world);

      return rectSurfaceToSphereCenter_sphere.sqrMagnitude < sphere.radius * sphere.radius;
    }

    #endregion

    #region Rect-Segment3

    //[ThreadStatic]
    //private static LocalSegment3[] _rectEdgesBuffer = new LocalSegment3[4];

    public static float Intersect(LocalSegment3 segment, Rect rect) {
      return Intersect(rect, segment);
    }
    public static float Intersect(Rect rect, LocalSegment3 segment) {
      Vector3 closestPointOnSegment_unused, closestPointOnRect_unused;
      return Intersect(rect, segment,
        out closestPointOnRect_unused, out closestPointOnSegment_unused);
    }
    public static float Intersect(LocalSegment3 segment, Rect rect,
                                  out Vector3 closestPointOnSegment,
                                  out Vector3 closestPointOnRect) {
      return Intersect(rect, segment, out closestPointOnRect, out closestPointOnSegment);
    }
    public static float Intersect(Rect rect, LocalSegment3 segment,
                                  out Vector3 closestPointOnRect,
                                  out Vector3 closestPointOnSegment) {
      float closestSqrDist = float.PositiveInfinity;
      closestPointOnRect = closestPointOnSegment = default(Vector3);

      Vector3 segmentA_rect;
      bool aProjectsInside = rect.ContainsProjectedPoint(segment.a, out segmentA_rect);
      Vector3 segmentB_rect;
      bool bProjectsInside = rect.ContainsProjectedPoint(segment.b, out segmentB_rect);

      // Test if the segment intersects with the rect plane and is inside the rect.
      var plane_rect = rect.ToLocalPlane();
      var segment_rect = new LocalSegment3(segmentA_rect, segmentB_rect);
      Vector3 pointOnPlane_rect; float amountAlongSegment; bool isLineCoplanar;
      var intersectsPlane = Intersect(plane_rect, segment_rect,
                                      out pointOnPlane_rect, out amountAlongSegment,
                                      out isLineCoplanar);
      if (intersectsPlane) {
        var absPointOnPlane_rect = pointOnPlane_rect.Abs();
        var absRadii = rect.radii.Abs();
        if (absPointOnPlane_rect.x <= absRadii.x && absPointOnPlane_rect.y <= absRadii.y) {
          closestPointOnRect = rect.matrix.MultiplyPoint3x4(pointOnPlane_rect);
          closestPointOnSegment = segment.Evaluate(amountAlongSegment);
          return 0f;
        }
      }

      // Must test the rect segments and plane-projection distances to find the closest
      // pair of points and return their squared-distance.
      
      // Segment point A <> plane, when A projects inside rect.
      if (aProjectsInside) {
        var testPointRect = rect.matrix.MultiplyPoint3x4(segmentA_rect.WithZ(0f));
        var testSqrDist = (testPointRect - segment.a).sqrMagnitude;
        if (testSqrDist < closestSqrDist) {
          closestSqrDist = testSqrDist;
          closestPointOnRect = testPointRect;
          closestPointOnSegment = segment.a;
        }
      }

      // Segment point B <> plane, when B projects inside rect.
      if (bProjectsInside) {
        var testPointRect = rect.matrix.MultiplyPoint3x4(segmentB_rect.WithZ(0f));
        var testSqrDist = (testPointRect - segment.b).sqrMagnitude;
        if (testSqrDist < closestSqrDist) {
          closestSqrDist = testSqrDist;
          closestPointOnRect = testPointRect;
          closestPointOnSegment = segment.b;
        }
      }

      if (!aProjectsInside || !bProjectsInside) {
        // Closest point segment <> {AB, BC, CD, DA}.
        foreach (var rectSegment in rect.segments) {
          Vector3 testClosestPointRectSegment;
          Vector3 testClosestPointSegment;
          float unusedT1, unusedT2;
          var testSqrDist = Intersect(rectSegment, segment,
                                    out unusedT1, out unusedT2,
                                    out testClosestPointRectSegment,
                                    out testClosestPointSegment);

          // Debug
          //RuntimeGizmos.RuntimeGizmoDrawer drawer;
          //if (RuntimeGizmos.RuntimeGizmoManager.TryGetGizmoDrawer(out drawer)) {
          //  drawer.DrawLine(testClosestPointRectSegment, testClosestPointSegment);
          //}

          if (testSqrDist < closestSqrDist) {
            closestSqrDist = testSqrDist;
            closestPointOnRect = testClosestPointRectSegment;
            closestPointOnSegment = testClosestPointSegment;
          }
        }
      }

      return closestSqrDist;
    }

    #endregion

    #region Plane-Segment3

    // https://stackoverflow.com/a/18543221 for line-plane intersection.

    /// <summary>
    /// Returns true if the line segment intersects with the plane, or false otherwise.
    /// 
    /// Specify output parameters to receive more detailed information about the
    /// intersection, such as the point at which it occurs and whether the line lies on
    /// the plane.
    /// </summary>
    public static bool Intersect(LocalPlane plane, LocalSegment3 line) {
      Vector3 pointOnPlane_unused;
      float amountAlongSegment_unused;
      bool isLineCoplanar_unused;
      return Intersect(plane, line,
                       out pointOnPlane_unused,
                       out amountAlongSegment_unused,
                       out isLineCoplanar_unused);
    }


    /// <summary>
    /// Returns true if the line segment intersects with the plane, or false otherwise.
    /// 
    /// If intersectInfiniteLine is specified, returns true if the line defined by the
    /// line segment intersects with the plane, or false otherwise.
    /// 
    /// If this method returns true, no out parameters are set to NaN, and reasonable
    /// defaults are chosen in edge-cases (such as a coplanar line). If this method
    /// returns false, some or all output parameters may have no reasonable value and
    /// are set to NaN.
    /// 
    /// pointOnPlane is the point along the line defined by the segment that intersects
    /// with the plane. If the line segment is parallel to the plane and _on_ the plane,
    /// pointOnPlane will be set to the center of the line segment as a convenience.
    /// Otherwise, it will be set to a Vector3 containing all NaNs.
    /// 
    /// amountAlongSegment is the normalized amount from A to B along the line segment
    /// that intersects with the plane. The line segment intersects with the plane only
    /// if this value is between 0 and 1 (inclusive). If the line segment is parallel to
    /// the plane and _on_ the plane, amountAlongSegment will be set to 0.5f. Otherwise,
    /// amountAlongSegment will be set to NaN.
    /// 
    /// isLineCoplanar is true if the line defined by the line segment is wholly on
    /// the plane, or false otherwise.
    /// </summary>
    public static bool Intersect(LocalPlane plane, LocalSegment3 line,
                                 out Vector3 pointOnPlane,
                                 out float amountAlongSegment,
                                 out bool isLineCoplanar,
                                 bool intersectInfiniteLine = false) {
      var planePosition = plane.position;
      var planeNormal = plane.normal;

      var a = line.a;
      var b = line.b;

      var u = b - a;
      var uDotN = planeNormal.Dot(u);
      if (uDotN.Abs() > float.Epsilon) {
        isLineCoplanar = false;

        var w = a - planePosition;
        amountAlongSegment = -(planeNormal.Dot(w)) / uDotN;
        pointOnPlane = a + u * amountAlongSegment;

        return intersectInfiniteLine
               || (amountAlongSegment >= 0f && amountAlongSegment <= 1f);
      }
      else {
        var pa = a - planePosition;
        if (planeNormal.Dot(pa).Abs() < float.Epsilon) {
          isLineCoplanar = true;
        }
        else {
          isLineCoplanar = false;
        }

        if (isLineCoplanar) {
          pointOnPlane = (a + b) / 2f;
          amountAlongSegment = 0.5f;
          return true;
        }
        else {
          pointOnPlane = new Vector3(float.NaN, float.NaN, float.NaN);
          amountAlongSegment = float.NaN;
          return false;
        }
      }
    }

    #endregion

    #region Segment-Segment

    /// <summary>
    /// Returns 2 times the signed triangle area. The result is positive if abc is
    /// counter-clockwise, negative if abc is clockwise, and zero if abc is degenerate.
    /// </summary>
    private static float Signed2DTriArea(Vector2 a, Vector2 b, Vector2 c) {
      return (a.x - c.x) * (b.y - c.y)
           - (a.y - c.y) * (b.x - c.x);
    }

    /// <summary>
    /// Returns whether seg1 and seg2 overlap. If they do, computes and outputs the
    /// intersection t value along seg1 and outputs the intersection point p. Otherwise,
    /// outputs default values.
    /// </summary>
    public static bool Intersect(LocalSegment2 seg1, LocalSegment2 seg2,
                                 out float t, out Vector2 p) {
      Vector2 a = seg1.a, b = seg1.b, c = seg2.a, d = seg2.b;

      // Sign of areas correspond to which side of ab the points c and d are.
      float a1 = Signed2DTriArea(a, b, d); // Compute winding of abd (+ or -).
      float a2 = Signed2DTriArea(a, b, c); // To intersect, must have opposite sign.

      // If c and d are on different sides of ab, areas have different signs.
      // Also verify that a1 and a2 are non-zero (areas are zero for degenerate triangles).
      if (a1 * a2 < 0f && a1 != 0f && a2 != 0f) {
        // Compute signs for a and b with respect to cd.
        float a3 = Signed2DTriArea(c, d, a); // Winding of cda (+ or -).
        // Since area is constant, a1 - a2 = a3 - a4, or a4 = a3 + a2 - a1
        // float a4 = Signed2DTriArea(c, d, b);
        float a4 = a3 + a2 - a1;
        // The points a and b are on different sides of cd if areas have different signs.
        if (a3 * a4 < 0f) {
          // The segments intersect. Find intersection point along L(t) = a + t * (b - a).
          // Given the height h1 of a over cd and height h2 of b over cd,
          // t = h1 / (h1 - h2) = (b*h1/2) / (b*h1/2 - b*h2/2) = a3 / (a3 - a4),
          // where b (the base of the triangles cda and cdb, i.e., the length of cd)
          // cancels out.
          t = a3 / (a3 - a4);
          p = a + t * (b - a);
          return true;
        }
      }

      // Segments don't intersect, or are collinear.
      t = default(float); p = default(Vector2);
      return false;
    }
    
    /// <summary>
    /// Computes the squared distance between two 3D line segments, outputting the amount
    /// along each segment (0 to 1) corresponding to the closest points (t1 and t2), and
    /// outputting the closest points themselves (c1 and c2).
    /// </summary>
    public static float Intersect(LocalSegment3 seg1,   LocalSegment3 seg2,
                                  out float     t1,     out float     t2,
                                  out Vector3   c1,     out Vector3   c2) {
      Vector3 d1 = seg1.b - seg1.a; // Direction vector of seg1.
      Vector3 d2 = seg2.b - seg2.a; // Direction vector of seg2.
      Vector3 r = seg1.a - seg2.a;
      float a = Dot(d1, d1); // Squared length of seg1.
      float e = Dot(d2, d2); // Squared length of seg2.
      float f = Dot(d2, r);

      // Check if either or both segments degenerate into points.
      if (a <= float.Epsilon && e <= float.Epsilon) {
        // Both segments degenerate into points.
        t1 = t2 = 0f;
        c1 = seg1.a; c2 = seg2.a;
        return Dot(c1 - c2, c1 - c2);
      }
      if (a <= float.Epsilon) {
        // First segment degenerates into a point.
        t1 = 0f;
        t2 = (f / e).Clamped01();
      }
      else {
        float c = Dot(d1, r);
        if (e <= float.Epsilon) {
          // Second segment degenerates into a point.
          t2 = 0f;
          t1 = (-c / a).Clamped01();
        }
        else {
          // General, non-degenerate case.
          float b = Dot(d1, d2);
          float denom = a*e - b*b; // Always non-negative.

          // If the segments are not parallel, compute the closest point on seg1 to seg2
          // and clamp to seg1. Otherwise, pick an arbitrary t1 (here 0).
          if (denom != 0f) {
            t1 = ((b*f - c*e) / denom).Clamped01();
          }
          else {
            t1 = 0f;
          }
          // Compute the point on seg2 closest to t1.
          float t2nom = b*t1 + f; // avoid dividing e until later.

          // If t2 in [0, 1], we're done. Otherwise, recompute t1 for the new value of
          // t2 and clamp to [0, 1].
          if (t2nom < 0f) {
            t2 = 0f;
            t1 = (-c / a).Clamped01();
          }
          else if (t2nom > e) {
            t2 = 1f;
            t1 = ((b - c) / a).Clamped01();
          }
          else {
            t2 = t2nom / e;
          }
        }
      }

      c1 = seg1.a + d1 * t1;
      c2 = seg2.a + d2 * t2;
      return (c1 - c2).sqrMagnitude;
    }

    #region ARCHIVE

    // 2D intersection:
    // https://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
    // https://ideone.com/PnPJgb

    /// <summary>
    /// Returns true if the two segments are not parallel or colinear and intersect in 2D,
    /// ignoring the segments' Z components.
    /// </summary>
    public static bool Intersect2D(LocalSegment3 segment0, LocalSegment3 segment1,
                                   out Vector2 intersectionPoint,
                                   out float amountAlongSegment0,
                                   out float amountAlongSegment1) {

      Vector2 p = segment0.a, q = segment0.b, r = segment1.a, s = segment1.b;

      // t = (q − p) × s / (r × s)
      // u = (q − p) × r / (r × s)

      var denom = Fake2DCross(r, s);

      if (denom == 0) {
        // Lines are collinear or parallel.
        amountAlongSegment0 = float.NaN;
        amountAlongSegment1 = float.NaN;
        intersectionPoint = new Vector2(float.NaN, float.NaN);
        return false;
      }

      var tNumer = Fake2DCross(q - p, s);
      var uNumer = Fake2DCross(q - p, r);

      amountAlongSegment0 = tNumer / denom;
      amountAlongSegment1 = uNumer / denom;

      if (   amountAlongSegment0 < 0 || amountAlongSegment0 > 1
          || amountAlongSegment1 < 0 || amountAlongSegment1 > 1) {
        // Line segments do not intersect within their ranges.
        intersectionPoint = default(Vector2);
        return false;
      }

      intersectionPoint = p + r * amountAlongSegment0;
      return true;
    }

    private static float Fake2DCross(Vector2 a, Vector2 b) {
      return a.x * b.y - a.y * b.x;
    }

    #endregion

    #endregion

    #region Point-Segment

    /// <summary>
    /// Returns the squared-distance of the point p from the segment.
    /// </summary>
    public static float SqrDistPointSegment(LocalSegment3 segment, Vector3 p) {
      return SqrDistPointSegment(segment.a, segment.b, p);
    }
    /// <summary>
    /// Returns the squared-distance of the point c from the segment defined by a, b.
    /// </summary>
    public static float SqrDistPointSegment(Vector3 a, Vector3 b, Vector3 c) {
      Vector3 ab = b - a, ac = c - a, bc = c - b;
      float e = Dot(ac, ab);
      // Handle cases where C projects outside AB.
      if (e <= 0f) return Dot(ac, ac);
      float f = Dot(ab, ab);
      if (e >= f) return Dot(bc, bc);
      // Handle cases where C projects onto AB.
      return Dot(ac, ac) - e * e / f;
    }

    /// <summary>
    /// Returns the closest point to point p on the segment.
    /// </summary>
    public static Vector3 ClosestPtPointSegment(LocalSegment3 segment, Vector3 p) {
      float unusedT;
      return ClosestPtPointSegment(segment.a, segment.b, p, out unusedT);
    }
    /// <summary>
    /// Returns the closest point to point p on the segment. Also outputs the
    /// parameterization from 0 to 1 along ab that results in the closest point:
    /// closestPt(t) = a + t*(b - a).
    /// </summary>
    public static Vector3 ClosestPtPointSegment(LocalSegment3 segment, Vector3 p,
                                                out float t) {
      return ClosestPtPointSegment(segment.a, segment.b, p, out t);
    }
    /// <summary>
    /// Returns the closest point to point c on the segment ab. Also outputs the
    /// parameterization from 0 to 1 along ab that results in the closest point:
    /// closestPt(t) = a + t*(b - a).
    /// </summary>
    public static Vector3 ClosestPtPointSegment(Vector3 a, Vector3 b, Vector3 c,
                                                out float t) {
      var ab = b - a;
      // Project c onto ab, computing parameterized position of the point: a + t*(b - a)
      t = Dot(c - a, ab) / Dot(ab, ab);
      // If outside the segment, clamp the parameterization to the closest endpoint.
      if (t < 0f) t = 0f;
      if (t > 1f) t = 1f;
      // Compute and output the projected position from the clamped parameterization.
      return a + t * ab;
    }

    #endregion

  }

}