        // Moves a value /current/ towards /target/.
        static public float MoveTowards(float current, float target, float maxDelta)
        {
            if (Mathf.Abs(target - current) <= maxDelta)
                return target;
            return current + Mathf.Sign(target - current) * maxDelta;
        }

        // Same as ::ref::MoveTowards but makes sure the values interpolate correctly when they wrap around 360 degrees.
        static public float MoveTowardsAngle(float current, float target, float maxDelta)
        {
            float deltaAngle = DeltaAngle(current, target);
            if (-maxDelta < deltaAngle && deltaAngle < maxDelta)
                return target;
            target = current + deltaAngle;
            return MoveTowards(current, target, maxDelta);
        }

        // Interpolates between /min/ and /max/ with smoothing at the limits.
        public static float SmoothStep(float from, float to, float t)
        {
            t = Mathf.Clamp01(t);
            t = -2.0F * t * t * t + 3.0F * t * t;
            return to * t + from * (1F - t);
        }

       public static float Gamma(float value, float absmax, float gamma)
        {
            bool negative = value < 0F;
            float absval = Abs(value);
            if (absval > absmax)
                return negative ? -absval : absval;

            float result = Pow(absval / absmax, gamma) * absmax;
            return negative ? -result : result;
        }

        // Compares two floating point values if they are similar.
        public static bool Approximately(float a, float b)
        {
            // If a or b is zero, compare that the other is less or equal to epsilon.
            // If neither a or b are 0, then find an epsilon that is good for
            // comparing numbers at the maximum magnitude of a and b.
            // Floating points have about 7 significant digits, so
            // 1.000001f can be represented while 1.0000001f is rounded to zero,
            // thus we could use an epsilon of 0.000001f for comparing values close to 1.
            // We multiply this epsilon by the biggest magnitude of a and b.
            return Abs(b - a) < Max(0.000001f * Max(Abs(a), Abs(b)), Epsilon * 8);
        }

    public static readonly float Pi        = Mathf.PI;
    public static readonly float TwoPi     = 2.0f * Mathf.PI;
    public static readonly float HalfPi    = Mathf.PI / 2.0f;
    public static readonly float ThirdPi   = Mathf.PI / 3.0f;
    public static readonly float QuarterPi = Mathf.PI / 4.0f;
    public static readonly float FifthPi   = Mathf.PI / 5.0f;
    public static readonly float SixthPi   = Mathf.PI / 6.0f;

    public static readonly float Sqrt2    = Mathf.Sqrt(2.0f);
    public static readonly float Sqrt2Inv = 1.0f / Mathf.Sqrt(2.0f);
    public static readonly float Sqrt3    = Mathf.Sqrt(3.0f);
    public static readonly float Sqrt3Inv = 1.0f / Mathf.Sqrt(3.0f);


    public static readonly float Epsilon = 1.0e-9f;

    // Returns a vector orthogonal to given vector.
    // If the given vector is a unit vector, the returned vector will also be a unit vector.
    public static Vector3 FindOrthogonal(Vector3 v)
    {
      if (v.x >= MathUtil.Sqrt3Inv) return new Vector3(v.y, -v.x, 0.0f);
      else        return new Vector3(0.0f, v.z, -v.y);
    }

    // Yields two extra vectors that form an orthogonal basis with the given vector.
    // If the given vector is a unit vector, the returned vectors will also be unit vectors.
    public static void FormOrthogonalBasis(Vector3 v, out Vector3 a, out Vector3 b)
    {
      a = FindOrthogonal(v);
      b = Vector3.Cross(a, v);
    }
	
        // Rotates the point /point/ with /rotation/.
        public static Vector3 operator*(Quaternion rotation, Vector3 point)
        {
            float x = rotation.x * 2F;
            float y = rotation.y * 2F;
            float z = rotation.z * 2F;
            float xx = rotation.x * x;
            float yy = rotation.y * y;
            float zz = rotation.z * z;
            float xy = rotation.x * y;
            float xz = rotation.x * z;
            float yz = rotation.y * z;
            float wx = rotation.w * x;
            float wy = rotation.w * y;
            float wz = rotation.w * z;

            Vector3 res;
            res.x = (1F - (yy + zz)) * point.x + (xy - wz) * point.y + (xz + wy) * point.z;
            res.y = (xy + wz) * point.x + (1F - (xx + zz)) * point.y + (yz - wx) * point.z;
            res.z = (xz - wy) * point.x + (yz + wx) * point.y + (1F - (xx + yy)) * point.z;
            return res;
        }


	//https://github.com/Unity-Technologies/UnityCsReference/blob/master/Runtime/Export/Math/Vector3.cs
        // Reflects a vector off the plane defined by a normal.
        public static Vector3 Reflect(Vector3 inDirection, Vector3 inNormal)
        {
            float factor = -2F * Dot(inNormal, inDirection);
            return new Vector3(factor * inNormal.x + inDirection.x,
                factor * inNormal.y + inDirection.y,
                factor * inNormal.z + inDirection.z);
        }


   	// Projects a vector onto another vector.
        public static Vector3 Project(Vector3 vector, Vector3 onNormal)
        {
            float sqrMag = Dot(onNormal, onNormal);
            if (sqrMag < Mathf.Epsilon)
                return zero;
            else
            {
                var dot = Dot(vector, onNormal);
                return new Vector3(onNormal.x * dot / sqrMag,
                    onNormal.y * dot / sqrMag,
                    onNormal.z * dot / sqrMag);
            }
        }

        // Projects a vector onto a plane defined by a normal orthogonal to the plane.
        public static Vector3 ProjectOnPlane(Vector3 vector, Vector3 planeNormal)
        {
            float sqrMag = Dot(planeNormal, planeNormal);
            if (sqrMag < Mathf.Epsilon)
                return vector;
            else
            {
                var dot = Dot(vector, planeNormal);
                return new Vector3(vector.x - planeNormal.x * dot / sqrMag,
                    vector.y - planeNormal.y * dot / sqrMag,
                    vector.z - planeNormal.z * dot / sqrMag);
            }
        }

        // Returns the angle in degrees between /from/ and /to/. This is always the smallest
        public static float Angle(Vector3 from, Vector3 to)
        {
            // sqrt(a) * sqrt(b) = sqrt(a * b) -- valid for real numbers
            float denominator = (float)Math.Sqrt(from.sqrMagnitude * to.sqrMagnitude);
            if (denominator < kEpsilonNormalSqrt)
                return 0F;

            float dot = Mathf.Clamp(Dot(from, to) / denominator, -1F, 1F);
            return ((float)Math.Acos(dot)) * Mathf.Rad2Deg;
        }

        // The smaller of the two possible angles between the two vectors is returned, therefore the result will never be greater than 180 degrees or smaller than -180 degrees.
        // If you imagine the from and to vectors as lines on a piece of paper, both originating from the same point, then the /axis/ vector would point up out of the paper.
        // The measured angle between the two vectors would be positive in a clockwise direction and negative in an anti-clockwise direction.
        public static float SignedAngle(Vector3 from, Vector3 to, Vector3 axis)
        {
            float unsignedAngle = Angle(from, to);

            float cross_x = from.y * to.z - from.z * to.y;
            float cross_y = from.z * to.x - from.x * to.z;
            float cross_z = from.x * to.y - from.y * to.x;
            float sign = Mathf.Sign(axis.x * cross_x + axis.y * cross_y + axis.z * cross_z);
            return unsignedAngle * sign;
        }

       // Returns a copy of /vector/ with its magnitude clamped to /maxLength/.
        public static Vector3 ClampMagnitude(Vector3 vector, float maxLength)
        {
            float sqrmag = vector.sqrMagnitude;
            if (sqrmag > maxLength * maxLength)
            {
                float mag = (float)Math.Sqrt(sqrmag);
                //these intermediate variables force the intermediate result to be
                //of float precision. without this, the intermediate result can be of higher
                //precision, which changes behavior.
                float normalized_x = vector.x / mag;
                float normalized_y = vector.y / mag;
                float normalized_z = vector.z / mag;
                return new Vector3(normalized_x * maxLength,
                    normalized_y * maxLength,
                    normalized_z * maxLength);
            }
            return vector;
        }



    public static Quaternion AngularVector(Vector3 v)
    {
      float len = v.magnitude;
      if (len < MathUtil.Epsilon)
        return Quaternion.identity;

      v /= len;

      float h = 0.5f * len;
      float s = Mathf.Sin(h);
      float c = Mathf.Cos(h);

      return new Quaternion(s * v.x, s * v.y, s * v.z, c);
    }

    public static float GetAngle(Quaternion q)
    {
      return 2.0f * Mathf.Acos(Mathf.Clamp(q.w, -1.0f, 1.0f));
    }

    public static Quaternion Pow(Quaternion q, float exp)
    {
      Vector3 axis = GetAxis(q);
      float angle = GetAngle(q);
      return AxisAngle(axis, angle * exp);
    }

    // v: derivative of q
    public static Quaternion Integrate(Quaternion q, Quaternion v, float dt)
    {
      return Pow(v, dt) * q;
    }


    // omega: angular velocity (direction is axis, magnitude is angle)
    // https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/
    // https://gafferongames.com/post/physics_in_3d/
    public static Quaternion Integrate(Quaternion q, Vector3 omega, float dt)
    {
      omega *= 0.5f;
      Quaternion p = (new Quaternion(omega.x, omega.y, omega.z, 0.0f)) * q;
      return Normalize(new Quaternion(q.x + p.x * dt, q.y + p.y * dt, q.z + p.z * dt, q.w + p.w * dt));
    }


        public const float kEpsilon = 0.000001F;

        // Is the dot product of two quaternions within tolerance for them to be considered equal?
        private static bool IsEqualUsingDot(float dot)
        {
            // Returns false in the presence of NaN values.
            return dot > 1.0f - kEpsilon;
        }


        // Returns the angle in degrees between two rotations /a/ and /b/.
        public static float Angle(Quaternion a, Quaternion b)
        {
            float dot = Dot(a, b);
            return IsEqualUsingDot(dot) ? 0.0f : Mathf.Acos(Mathf.Min(Mathf.Abs(dot), 1.0F)) * 2.0F * Mathf.Rad2Deg;
        }

        // Makes euler angles positive 0/360 with 0.0001 hacked to support old behaviour of QuaternionToEuler
        private static Vector3 Internal_MakePositive(Vector3 euler)
        {
            float negativeFlip = -0.0001f * Mathf.Rad2Deg;
            float positiveFlip = 360.0f + negativeFlip;

            if (euler.x < negativeFlip)
                euler.x += 360.0f;
            else if (euler.x > positiveFlip)
                euler.x -= 360.0f;

            if (euler.y < negativeFlip)
                euler.y += 360.0f;
            else if (euler.y > positiveFlip)
                euler.y -= 360.0f;

            if (euler.z < negativeFlip)
                euler.z += 360.0f;
            else if (euler.z > positiveFlip)
                euler.z -= 360.0f;

            return euler;
        }

        public static Quaternion RotateTowards(Quaternion from, Quaternion to, float maxDegreesDelta)
        {
            float angle = Quaternion.Angle(from, to);
            if (angle == 0.0f) return to;
            return SlerpUnclamped(from, to, Mathf.Min(1.0f, maxDegreesDelta / angle));
        }



    // swing-twist decomposition & interpolation
    // ------------------------------------------------------------------------

    public static void DecomposeSwingTwist
    (
      Quaternion q, 
      Vector3 twistAxis, 
      out Quaternion swing, 
      out Quaternion twist
    )
    {
      Vector3 r = new Vector3(q.x, q.y, q.z); // (rotaiton axis) * cos(angle / 2)

      // singularity: rotation by 180 degree
      if (r.sqrMagnitude < MathUtil.Epsilon)
      {
        Vector3 rotatedTwistAxis = q * twistAxis;
        Vector3 swingAxis = Vector3.Cross(twistAxis, rotatedTwistAxis);

        if (swingAxis.sqrMagnitude > MathUtil.Epsilon)
        {
          float swingAngle = Vector3.Angle(twistAxis, rotatedTwistAxis);
          swing = Quaternion.AngleAxis(swingAngle, swingAxis);
        }
        else
        {
          // more singularity: rotation axis parallel to twist axis
          swing = Quaternion.identity; // no swing
        }

        // always twist 180 degree on singularity
        twist = Quaternion.AngleAxis(180.0f, twistAxis);
        return;
      }

      // formula & proof: 
      // http://www.euclideanspace.com/maths/geometry/rotations/for/decomposition/
      Vector3 p = Vector3.Project(r, twistAxis);
      twist = new Quaternion(p.x, p.y, p.z, q.w);
      twist = Normalize(twist);
      swing = q * Quaternion.Inverse(twist);
    }

    public enum SterpMode
    {
      // non-constant angular velocity, faster
      // use if interpolating across small angles or constant angular velocity is not important
      Nlerp,

      // constant angular velocity, slower
      // use if interpolating across large angles and constant angular velocity is important
      Slerp, 
    };

    // same swing & twist parameters
    public static Quaternion Sterp
    (
      Quaternion a, 
      Quaternion b, 
      Vector3 twistAxis, 
      float t, 
      SterpMode mode = SterpMode.Slerp
    )
    {
      Quaternion swing;
      Quaternion twist;
      return Sterp(a, b, twistAxis, t, out swing, out twist, mode);
    }

    // same swing & twist parameters with individual interpolated swing & twist outputs
    public static Quaternion Sterp
    (
      Quaternion a, 
      Quaternion b, 
      Vector3 twistAxis, 
      float t, 
      out Quaternion swing, 
      out Quaternion twist, 
      SterpMode mode = SterpMode.Slerp
    )
    {
      return Sterp(a, b, twistAxis, t, t, out swing, out twist, mode);
    }

    // different swing & twist parameters
    public static Quaternion Sterp
    (
      Quaternion a, 
      Quaternion b, 
      Vector3 twistAxis, 
      float tSwing, 
      float tTwist, 
      SterpMode mode = SterpMode.Slerp
    )
    {
      Quaternion swing;
      Quaternion twist;
      return Sterp(a, b, twistAxis, tSwing, tTwist, out swing, out twist, mode);
    }

    // master sterp function
    public static Quaternion Sterp
    (
      Quaternion a, 
      Quaternion b, 
      Vector3 twistAxis, 
      float tSwing, 
      float tTwist, 
      out Quaternion swing, 
      out Quaternion twist, 
      SterpMode mode
    )
    {
      Quaternion q = b * Quaternion.Inverse(a);
      Quaternion swingFull;
      Quaternion twistFull;
      QuaternionUtil.DecomposeSwingTwist(q, twistAxis, out swingFull, out twistFull);

      switch (mode)
      {
        default:
        case SterpMode.Nlerp:
          swing = Quaternion.Lerp(Quaternion.identity, swingFull, tSwing);
          twist = Quaternion.Lerp(Quaternion.identity, twistFull, tTwist);
          break;
        case SterpMode.Slerp:
          swing = Quaternion.Slerp(Quaternion.identity, swingFull, tSwing);
          twist = Quaternion.Slerp(Quaternion.identity, twistFull, tTwist);
          break;
      }

      return twist * swing;
    }

    // ------------------------------------------------------------------------
    // end: swing-twist decomposition & interpolation


?/******************************************************************************/
/*
  Project - Unity CJ Lib
            https://github.com/TheAllenChou/unity-cj-lib
  
  Author  - Ming-Lun "Allen" Chou
  Web     - http://AllenChou.net
  Twitter - @TheAllenChou
*/
/******************************************************************************/

using UnityEngine;

namespace CjLib
{
  // push vector separates A from B
  public class Collision
  {
    public static bool SphereSphere(Vector3 centerA, float radiusA, Vector3 centerB, float radiusB)
    {
      Vector3 vec = centerA - centerB;
      float dd = vec.sqrMagnitude;
      float r = radiusA + radiusB;

      return dd <= r * r;
    }

    public static bool SphereSphere(Vector3 centerA, float radiusA, Vector3 centerB, float radiusB, out Vector3 push)
    {
      push = Vector3.zero;

      Vector3 vec = centerA - centerB;
      float dd = vec.sqrMagnitude;
      float r = radiusA + radiusB;

      if (dd > r * r)
      {
        return false;
      }

      float d = Mathf.Sqrt(dd);

      push = VectorUtil.NormalizeSafe(vec, Vector3.zero) * (r - d);
      return true;
    }

    public static bool SphereCapsule(Vector3 centerA, float radiusA, Vector3 headB, Vector3 tailB, float radiusB)
    {
      Vector3 segVec = tailB - headB;
      float segLenSqr = segVec.sqrMagnitude;
      if (segLenSqr < MathUtil.Epsilon)
        return SphereSphere(centerA, radiusA, 0.5f * (headB + tailB), radiusB);

      float segLenInv = 1.0f / Mathf.Sqrt(segLenSqr);
      Vector3 segDir = segVec * segLenInv;
      Vector3 headToA = centerA - headB;
      float t = Mathf.Clamp01(Vector3.Dot(headToA, segDir) * segLenInv);
      Vector3 closestB = Vector3.Lerp(headB, tailB, t);

      return SphereSphere(centerA, radiusA, closestB, radiusB);
    }

    public static bool SphereCapsule(Vector3 centerA, float radiusA, Vector3 headB, Vector3 tailB, float radiusB, out Vector3 push)
    {
      push = Vector3.zero;

      Vector3 segVec = tailB - headB;
      float segLenSqr = segVec.sqrMagnitude;
      if (segLenSqr < MathUtil.Epsilon)
        return SphereSphere(centerA, radiusA, 0.5f * (headB + tailB), radiusB, out push);

      float segLenInv = 1.0f / Mathf.Sqrt(segLenSqr);
      Vector3 segDir = segVec * segLenInv;
      Vector3 headToA = centerA - headB;
      float t = Mathf.Clamp01(Vector3.Dot(headToA, segDir) * segLenInv);
      Vector3 closestB = Vector3.Lerp(headB, tailB, t);

      return SphereSphere(centerA, radiusA, closestB, radiusB, out push);
    }

    public static bool SphereBox(Vector3 centerOffsetA, float radiusA, Vector3 halfExtentB)
    {
      Vector3 closestOnB = 
        new Vector3
        (
          Mathf.Clamp(centerOffsetA.x, -halfExtentB.x, halfExtentB.x), 
          Mathf.Clamp(centerOffsetA.y, -halfExtentB.y, halfExtentB.y), 
          Mathf.Clamp(centerOffsetA.z, -halfExtentB.z, halfExtentB.z)
        );

      Vector3 vec = centerOffsetA - closestOnB;
      float dd = vec.sqrMagnitude;
      
      return dd <= radiusA * radiusA;
    }

    public static bool SphereBox(Vector3 centerOffsetA, float radiusA, Vector3 halfExtentB, out Vector3 push)
    {
      push = Vector3.zero;

      Vector3 closestOnB = 
        new Vector3
        (
          Mathf.Clamp(centerOffsetA.x, -halfExtentB.x, halfExtentB.x), 
          Mathf.Clamp(centerOffsetA.y, -halfExtentB.y, halfExtentB.y), 
          Mathf.Clamp(centerOffsetA.z, -halfExtentB.z, halfExtentB.z)
        );

      Vector3 vec = centerOffsetA - closestOnB;
      float dd = vec.sqrMagnitude;
      
      if (dd > radiusA * radiusA)
      {
        return false;
      }

      int numInBoxAxes = 
          ((centerOffsetA.x < -halfExtentB.x || centerOffsetA.x > halfExtentB.x) ? 0 : 1)
        + ((centerOffsetA.y < -halfExtentB.y || centerOffsetA.y > halfExtentB.y) ? 0 : 1)
        + ((centerOffsetA.z < -halfExtentB.z || centerOffsetA.z > halfExtentB.z) ? 0 : 1);

      switch (numInBoxAxes)
      {
      case 0: // hit corner
      case 1: // hit edge
      case 2: // hit face
        {
          push = VectorUtil.NormalizeSafe(vec, Vector3.right) * (radiusA - Mathf.Sqrt(dd));
        }
        break;
      case 3: // inside
        {
          Vector3 penetration = 
            new Vector3
            (
              halfExtentB.x - Mathf.Abs(centerOffsetA.x) + radiusA, 
              halfExtentB.y - Mathf.Abs(centerOffsetA.y) + radiusA, 
              halfExtentB.z - Mathf.Abs(centerOffsetA.z) + radiusA
            );

          if (penetration.x < penetration.y)
          {
            if (penetration.x < penetration.z)
              push = new Vector3(Mathf.Sign(centerOffsetA.x) * penetration.x, 0.0f, 0.0f);
            else
              push = new Vector3(0.0f, 0.0f, Mathf.Sign(centerOffsetA.z) * penetration.z);
          }
          else
          {
            if (penetration.y < penetration.z)
                push = new Vector3(0.0f, Mathf.Sign(centerOffsetA.y) * penetration.y, 0.0f);
            else
                push = new Vector3(0.0f, 0.0f, Mathf.Sign(centerOffsetA.z) * penetration.z);
          }
        }
        break;
      }

      return true;
    }
  }
}


 // Infinite Line Intersection (line1 is p1-p2 and line2 is p3-p4)
        internal static bool LineIntersection(Vector2 p1, Vector2 p2, Vector2 p3, Vector2 p4, ref Vector2 result)
        {
            float bx = p2.x - p1.x;
            float by = p2.y - p1.y;
            float dx = p4.x - p3.x;
            float dy = p4.y - p3.y;
            float bDotDPerp = bx * dy - by * dx;
            if (bDotDPerp == 0)
            {
                return false;
            }
            float cx = p3.x - p1.x;
            float cy = p3.y - p1.y;
            float t = (cx * dy - cy * dx) / bDotDPerp;

            result.x = p1.x + t * bx;
            result.y = p1.y + t * by;
            return true;
        }

        // Line Segment Intersection (line1 is p1-p2 and line2 is p3-p4)
        internal static bool LineSegmentIntersection(Vector2 p1, Vector2 p2, Vector2 p3, Vector2 p4, ref Vector2 result)
        {
            float bx = p2.x - p1.x;
            float by = p2.y - p1.y;
            float dx = p4.x - p3.x;
            float dy = p4.y - p3.y;
            float bDotDPerp = bx * dy - by * dx;
            if (bDotDPerp == 0)
            {
                return false;
            }
            float cx = p3.x - p1.x;
            float cy = p3.y - p1.y;
            float t = (cx * dy - cy * dx) / bDotDPerp;
            if (t < 0 || t > 1)
            {
                return false;
            }
            float u = (cx * by - cy * bx) / bDotDPerp;
            if (u < 0 || u > 1)
            {
                return false;
            }

            result.x = p1.x + t * bx;
            result.y = p1.y + t * by;
            return true;
        }

        // Calculates the ::ref::Lerp parameter between of two values.
        public static float InverseLerp(float a, float b, float value)
        {
            if (a != b)
                return Clamp01((value - a) / (b - a));
            else
                return 0.0f;
        }

        // Calculates the shortest difference between two given angles.
        public static float DeltaAngle(float current, float target)
        {
            float delta = Mathf.Repeat((target - current), 360.0F);
            if (delta > 180.0F)
                delta -= 360.0F;
            return delta;
        }

        // PingPongs the value t, so that it is never larger than length and never smaller than 0.
        public static float PingPong(float t, float length)
        {
            t = Repeat(t, length * 2F);
            return length - Mathf.Abs(t - length);
        }

      // Loops the value t, so that it is never larger than length and never smaller than 0.
        public static float Repeat(float t, float length)
        {
            return Clamp(t - Mathf.Floor(t / length) * length, 0.0f, length);
        }


        [uei.ExcludeFromDocs]
        public static float SmoothDampAngle(float current, float target, ref float currentVelocity, float smoothTime, float maxSpeed)
        {
            float deltaTime = Time.deltaTime;
            return SmoothDampAngle(current, target, ref currentVelocity, smoothTime, maxSpeed, deltaTime);
        }

        [uei.ExcludeFromDocs]
        public static float SmoothDampAngle(float current, float target, ref float currentVelocity, float smoothTime)
        {
            float deltaTime = Time.deltaTime;
            float maxSpeed = Mathf.Infinity;
            return SmoothDampAngle(current, target, ref currentVelocity, smoothTime, maxSpeed, deltaTime);
        }

        // Gradually changes an angle given in degrees towards a desired goal angle over time.
        public static float SmoothDampAngle(float current, float target, ref float currentVelocity, float smoothTime, [uei.DefaultValue("Mathf.Infinity")]  float maxSpeed, [uei.DefaultValue("Time.deltaTime")]  float deltaTime)
        {
            target = current + DeltaAngle(current, target);
            return SmoothDamp(current, target, ref currentVelocity, smoothTime, maxSpeed, deltaTime);
        }