Foxtails-asal_testchamber001/New Unity Project/Assets/Scripts/CharacterController/CECollision.cs

using UnityEngine;
using System.Collections;

namespace CharacterEngine.Primitives
{
    public struct CECollision
    {
        public Vector2 impulse;
        public Vector2 point;
        public Vector2 normal;

        /// <summary>
        /// RESOLVE THE COLLISION AND RETURN A COLLISION VALUE
        /// a = Collider wich is dynamic 
        /// b = static Collider
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        public static CECollision ResolveCollision (CECollider a, CECollider b)
        {
            Vector2 closestPoint = Vector2.zero;
            CECollision collision = new CECollision();
            for (int i = 0; i < 4; i++)
            {
                Vector2 currentPoint = a.GetPoint(i);
                if (b.ContainsPoint(currentPoint))
                {
                    closestPoint = ClosestPointInBounds(b, currentPoint);

                    collision.impulse = closestPoint - currentPoint;
                    collision.normal = collision.impulse.normalized;
                    collision.point = closestPoint;

                    return collision;
                }
            }

            for (int i = 0; i < 4; i++)
            {
                Vector2 currentPoint = b.GetPoint(i);
                if (a.ContainsPoint(currentPoint))
                {
                    closestPoint = ClosestPointInBounds(a, currentPoint);

                    collision.impulse = (closestPoint - currentPoint) * -1;
                    collision.normal = collision.impulse.normalized;
                    collision.point = closestPoint;

                    return collision;
                }
            }

            //IF REACH HERE DIDNT RESOLVE SHIT
            Debug.Log("Could not resolve collision");
            return collision;
        }

        /// <summary>
        /// GET CLOSEST CONTACT POINT
        /// </summary>
        /// <param name="coll"></param>
        /// <param name="point"></param>
        /// <returns></returns>
        public static Vector2 ClosestPointInBounds (CECollider coll, Vector2 point)
        {
            Vector2 smallerDelta = Vector2.zero;
            for (int i = 0; i < 4; i++)
            {
                Vector2 cpis = ClosestVectorToSegment(point, coll.GetPoint(CECollider.edges[i, 0]), coll.GetPoint(CECollider.edges[i, 1]));
                Vector2 delta = cpis - point;

                if (smallerDelta == Vector2.zero) smallerDelta = delta;
                else
                {
                    if (delta.sqrMagnitude < smallerDelta.sqrMagnitude)
                        smallerDelta = delta;
                }
            }

            return point + smallerDelta;
        }

        public static Vector2 ClosestVectorToSegment (Vector2 p, Vector2 a, Vector2 b)
        {
            Vector2 segmentNormal = Vector3.Cross((a- b).normalized, Vector3.forward);
            Vector2 distanceToPointA = a - p;
            Vector2 distanceToPointB = b - p;

            return p + (Vector2)Vector3.Project(distanceToPointA, segmentNormal);
        }

    }
}