Custom physics

        using UnityEngine;
using System.Collections;
using System.Collections.Generic;

public class CollisionCheck : ScriptableObject {

    struct Edge
    {
        public float Distance;
        public Vector2 Normal;
        public int index;
        public Vector2 Start;
        public Vector2 End;
    }

    // struct for contactpoints in the manifold
    public struct Contact_Point
    {
        public Vector2[] WorlCoords;
        public Vector2[] LocalCoords;
        public Vector2 Normal;
        public Vector2 Tangent;
        public float Penetration;
        public bool persistent;

        // contructor
        Contact_Point(bool per)
        {
            WorlCoords = new Vector2[2];
            LocalCoords = new Vector2[2];
            Normal = Vector2.zero;
            Tangent = Vector2.zero;
            Penetration = 0.0f;
            persistent = per;
        }

        // set contactpoint to be persistent
        public void SetPersistent(bool value)
        {
            persistent = value;
        }
    }

    // list that will contain all generated contact manifolds
    public List<Manifold> contacts = new List<Manifold>(); 

    List<Vector2[]> ConvexPoints = new List<Vector2[]>();
    Manifold contact = new Manifold();
    Contact_Point contact_point = new Contact_Point();
    Vector2[] Simplex = new Vector2[3];
    ColliderBase Object_A;
    ColliderBase Object_B;
    rigidBody _Body_1;
    rigidBody _Body_2;
    int SimplexSize = 0;

    public void SetColliders(ColliderBase obj_A, ColliderBase obj_B)
    {
        Object_A = obj_A;
        Object_B = obj_B;
        _Body_1 = obj_A._body;
        _Body_2 = obj_B._body;
    }

    
    public bool Collision(Manifold manifold = null)
    {
        // if both objects have inf mass don't check for collisions
        if (_Body_1 != null && _Body_2 != null)
        {
            if ((_Body_1.Mass == float.PositiveInfinity
                && _Body_2.Mass == float.PositiveInfinity))
                return false;
        }

        if (Object_A._obj == Object_B._obj)
            return false;

        if (!CheckCollision())
        {
            if(manifold != null && manifold.IsTrigger)
            {
                TriggerExit(Object_B, Object_A);
                TriggerExit(Object_A, Object_B);
            }
            return false;
        }
        else
        {
            // if we are rechecking the collision add the new contactpoint to the manifold and manage it
            // if it is a new collision add the contactpoint to a new manifold and add that manifold to the list
            if (manifold != null)
            {
                if (manifold.IsTrigger)
                {
                    if (manifold.Collider_A.IsTrigger)
                        TriggerStay(manifold.Collider_B, manifold.Collider_A);

                    if (manifold.Collider_B.IsTrigger)
                        TriggerStay(manifold.Collider_A, manifold.Collider_B);
                    return true;
                }

                else
                {
                    // get the additional contact data if we found out that objects are colliding
                    GetAdditionalContactData();
                    // validate previous contact points
                    manifold.Validate();
                    // add the new contactpoint
                    manifold.AddContact(contact_point);
                    // keep the most relevant points
                    manifold.Manage();

                    OnCollision(Object_A, Object_B);
                    OnCollision(Object_B, Object_A);
                    return true;
                }
            }
            else
            {
                // if one of the colliders is a trigger than the manifold is a trigger manifold s we don't have to resolve it
                if (!Object_A.IsTrigger && !Object_B.IsTrigger)
                {
                    // get the additional contact data if we found out that objects are colliding
                    GetAdditionalContactData();
                    AddManifold(contact_point);
                }
                else
                {
                    AddManifold(contact_point, true);
                    TriggerEnter(Object_B, Object_A);
                    TriggerEnter(Object_A, Object_B);
                }
                return true;
            }
        }
    }

    public bool CheckCollision()
    {
        SimplexSize = 0;
        Simplex = new Vector2[3];
        Vector2 direction = Object_A._obj.transform.position - Object_B._obj.transform.position;

        // calc support point and add it as first point i our simplex
        Simplex[0] = SupportPoint(direction);
        SimplexSize++;
        // negate search direction
        direction = -direction;

        //2D GJK
        while (true)
        {
            // add point to simplex searched in the direction calculated at the end of the loop
            Simplex[SimplexSize] = SupportPoint(direction);
            SimplexSize++;

            // check if last added point in simplex passed the origin if not than the simplex will never contain the origin and thus there is no collision
            if (Vector2.Dot(Simplex[SimplexSize - 1], direction) <= 0.0f)
            {
                return false;
            }

            // if it does pass the point check if the simplex contains the origin
            // if so then there is collision if not repeat process
            else
            {
                if (OriginInSimplex(ref direction))
                {
                    return true;
                }
            }
        }
    }

    // check if the simplex contains the origin
    bool OriginInSimplex(ref Vector2 Direction)
    {
        Vector2 Point_A = Simplex[SimplexSize - 1];
        // direction point A to origin
        Vector2 PointToOrigin = -Point_A;

        switch (SimplexSize)
        {
            // simplex is a line
            case 2:
                {
                    Vector2 point_B = Simplex[0];
                    Vector2 AB = point_B - Point_A;

                    Vector2 Normal = new Vector2(-AB.y, AB.x);//Vector3.Cross(Vector3.Cross(AB, PointToOrigin), AB);

                    Direction = Normal;
                    if (Vector2.Dot(Direction, PointToOrigin) < 0.0f)
                    {
                        Direction = -Direction;
                    }
                    return false;
                }
            // simplex is a triangle
            case 3:
                {
                    // check if point is in triangle 
                    Vector2 Point_B = Simplex[1];
                    Vector2 Point_C = Simplex[0];

                    Vector2 AB = Point_B - Point_A;
                    Vector2 AC = Point_C - Point_A;

                    // direction perpendicular to AB
                    Direction = new Vector2(-AB.y, AB.x);//Vector3.Cross(Vector3.Cross(AB, PointToOrigin), AB);

                    // find on what side of AB the origin is and set seach direction to that direction
                    if (Vector2.Dot(Direction, Point_C) > 0.0f)
                    {
                        Direction = -Direction;
                    }

                    // check if C is furthest point in origin direction
                    if (Vector2.Dot(Direction, PointToOrigin) > 0.0f)
                    {
                        // remove point C and find a new point A
                        Simplex[0] = Point_B;
                        Simplex[1] = Point_A;
                        SimplexSize--;
                        return false;
                    }

                    // direction perpendicular to AC
                    Direction = new Vector3(-AC.y, AC.x);//Vector3.Cross(Vector3.Cross(AC, PointToOrigin), AC);

                    if (Vector2.Dot(Direction, Point_B) > 0.0f)
                    {
                        Direction = -Direction;
                    }


                    if (Vector2.Dot(Direction, PointToOrigin) > 0.0f)
                    {
                        // remove point B and find a new point A
                        Simplex[0] = Point_C;
                        Simplex[1] = Point_A;
                        SimplexSize--;
                        return false;
                    }
                    return true;
                }
            default:
                return false;
        }
    }

    // get a support point in the murkowski hull
    Vector2 SupportPoint(Vector2 Direction)
    {
        Vector2 p1 = Object_A.FurthestPoint(Direction, Object_A._obj);
        Vector2 p2 = Object_B.FurthestPoint(-Direction, Object_B._obj);

        // store the points to be used for closest point calculation
        // save all original points
        Vector2[] temp = new Vector2[3];
        temp[0] = p1;
        temp[1] = p2;
        temp[2] = p1 - p2;
        ConvexPoints.Add(temp);

        return p1 - p2;
    }

    // calculates the closest point on the simplex to the origin
    void GetClosestPoint(Vector2[] Point_A, Vector2[] Point_B)
    {
        Vector2 L = Point_B[2] - Point_A[2];
        contact_point.LocalCoords = new Vector2[2];
        contact_point.WorlCoords = new Vector2[2];

        if (L == Vector2.zero)
        {
            contact_point.WorlCoords[0] = Point_A[0];
            contact_point.WorlCoords[0] = Point_A[1];
            return;
        }

        float Labda_2 = Vector2.Dot(-L, Point_A[2]) / Vector2.Dot(L, L);
        float Labda_1 = 1 - Labda_2;

        if (Labda_1 < 0)
        {
            contact_point.WorlCoords[0] = Point_B[0];
            contact_point.WorlCoords[0] = Point_B[1];
            return;
        }
        else if (Labda_2 < 0)
        {
            contact_point.WorlCoords[0] = Point_A[0];
            contact_point.WorlCoords[0] = Point_A[1];
            return;
        }

        // contact point on body A in world coords and local coords
        contact_point.WorlCoords[0] = Labda_1 * Point_A[0] + Labda_2 * Point_B[0];
        contact_point.WorlCoords[1] = Labda_1 * Point_A[1] + Labda_2 * Point_B[1];

        // convert world space to local space
        contact_point.LocalCoords[0] = Object_A._obj.transform.worldToLocalMatrix.MultiplyPoint3x4(contact_point.WorlCoords[0]);
        contact_point.LocalCoords[1] = Object_B._obj.transform.worldToLocalMatrix.MultiplyPoint3x4(contact_point.WorlCoords[1]);

    }

    Edge FindClosestEdge(ref List<Vector2> Simplex)
    {
        Edge closest = new Edge();
        closest.Distance = float.MaxValue;

        for (int i = 0; i < Simplex.Count; i++)
        {
            // calculate the next point in the simplex

            // below line is the same as 
            //
            // if (i + 1 == Simplex.Cont) 
            // { j = i + 1} 
            // else j = 0
            int j = i + 1 == Simplex.Count ? 0 : i + 1;

            // set 2 vectors to the simplex points choosen
            Vector2 a = Simplex[i];
            Vector2 b = Simplex[j];

            // get line A to B and Origin to A
            Vector2 e = b - a;
            Vector2 OA = a;

            // get the normal from line AB
            Vector2 normal = new Vector2(-e.y, e.x);//Vector3.Cross(Vector3.Cross(e, OA), e);

            // normalize the normal
            normal.Normalize();

            float d = Vector2.Dot(normal, a);

            // check if any point a is closer than the current closest point
            if (d < closest.Distance)
            {
                closest.Start = Simplex[i];
                closest.End = Simplex[j];
                // set values
                closest.Distance = d;
                closest.Normal = normal;
                closest.index = j;
            }
        }
        return closest;
    }

    // get additional contact data using the EPA algorithm
    public void GetAdditionalContactData()
    {
        float Tolerance = 0.00005f;
        List<Vector2> s = new List<Vector2>();
        for (int i = 0; i < 3; i++)
        {
            s.Add(Vector2.zero);
            s[i] = Simplex[i];
        }
        int j = 0;
        while (true)
        {
            
            Edge E = FindClosestEdge(ref s);
            // add new support point with as search direction the normal from the closest edge
            Vector2 P = SupportPoint(E.Normal);

            // check if distance from P is similar to the normal if so than we found the closest edge
            float d = Vector2.Dot(P, E.Normal);
            j += 1;
            if (d - E.Distance < Tolerance)
            {
                // closest edge is made up by subbort points Simplex[i], Simplex[j]
                // get the original points that created these support points
                Vector2[] point_a = new Vector2[3];
                Vector2[] point_b = new Vector2[3];
                // search through list of calculated support point to find suport points that make up the closest edge
                for (int x = 0; x < ConvexPoints.Count; x++)
                {
                    if (ConvexPoints[x][2] == E.Start)
                    {
                        point_a = ConvexPoints[x];
                    }
                    if (ConvexPoints[x][2] == E.End)
                    {
                        point_b = ConvexPoints[x];
                    }
                }

                // get point of deepest penetration for this opject (closest points)
                GetClosestPoint(point_a, point_b);

                contact_point.Normal = E.Normal;
                contact_point.Penetration = d;
                // get tangets so we can resolve friction
                contact_point.Tangent = GetTangent(contact_point);
                ConvexPoints.Clear();
                return;
            }
            else
            {
                // insert the support point that got closer to the origin between the points that made up the closest edge
                s.Insert(E.index, P);
            }
        }
    }

    // add new manifold
    public void AddManifold(Contact_Point point, bool Trigger = false)
    {
        contact = new Manifold();
        // add contact to manifold
        contact.AddContact(point);
        // set the bodys that are colliding
        contact.SetBodys(Object_A, Object_B);
        // add manifold to list
        contacts.Add(contact);

        contact.IsTrigger = Trigger;
    }

    // get the tangent vector for linear velocity
    public Vector2 GetTangent(Contact_Point point)
    {
        Vector2 rA = point.WorlCoords[0] - (Vector2)Object_A._obj.transform.position;
        Vector2 rB = point.WorlCoords[1] - (Vector2)Object_B._obj.transform.position;

        Vector2 Relative_v = _Body_2.getLinearVelocity() + new Vector2(-_Body_2.getAngularVelocity() * rB.y, _Body_2.getAngularVelocity() * rB.x)
                           - (_Body_1.getLinearVelocity()) - new Vector2(-_Body_1.getAngularVelocity() * rA.y, _Body_1.getAngularVelocity() * rA.x);

        Vector2 tangent = Relative_v - (Vector2.Dot(Relative_v, point.Normal) * point.Normal);
        tangent.Normalize();
        return tangent;
    }

    // gets the scripts on the object that derive from collisions and run ontrigger enter method
    // target is the obj the scripts should run the function on and trigger is the trigger they are entering
    void TriggerEnter(ColliderBase target, ColliderBase trigger)
    {
        Collision[] scripts;
        scripts = target._obj.GetComponents<Collision>();
        for (int i = 0; i < scripts.Length; i++)
        {
            scripts[i].Trigger(trigger);
        }
    }

    // gets the scripts on the object that derive from collisions and run ontriggerstay method
    void TriggerStay(ColliderBase target, ColliderBase trigger)
    {
        Collision[] scripts;
        scripts = target._obj.GetComponents<Collision>();
        for (int i = 0; i < scripts.Length; i++)
        {
            scripts[i].TriggerStay(trigger);
        }
    }

    // gets the scripts on the object that derive from collisions and run ontriggerstay method
    void TriggerExit(ColliderBase target, ColliderBase trigger)
    {
        Collision[] scripts;
        scripts = target._obj.GetComponents<Collision>();
        for (int i = 0; i < scripts.Length; i++)
        {
            scripts[i].TriggerExit(trigger);
        }
    }

    void OnCollision(ColliderBase target, ColliderBase trigger)
    {
        Collision[] scripts;
        scripts = target._obj.GetComponents<Collision>();
        for (int i = 0; i < scripts.Length; i++)
        {
            scripts[i].OnCollision(trigger);
        }
    }
}



       

        using UnityEngine;
using System.Collections;
using System.Collections.Generic;

public class Manifold {

    public rigidBody object_A;
    public rigidBody object_B;
    public ColliderBase Collider_A;
    public ColliderBase Collider_B;
    public bool IsTrigger;
    public bool Persistent;
    List<CollisionCheck.Contact_Point> contacts = new List<CollisionCheck.Contact_Point>();
    
    public Manifold()
    {
    }

    //function that will resolve the collision
    public void Resolve()
    {
        // if the manifold is a trigger manifold don't resolve
        if(IsTrigger)
        {
            return;
        }

        for (int i = 0; i < contacts.Count; i++)
        {

            float e;

            // get the distances from centre of mass to point of deepest penetration
            Vector2 rA = contacts[i].WorlCoords[0] - (Vector2)object_A.transform.position;
            Vector2 rB = contacts[i].WorlCoords[1] - (Vector2)object_B.transform.position;

            // calculate relative velocity
            Vector2 relativeV = object_B.getLinearVelocity() + new Vector2(-object_B.getAngularVelocity() * rB.y, object_B.getAngularVelocity() * rB.x)
                              - (object_A.getLinearVelocity()) - new Vector2(-object_A.getAngularVelocity() * rA.y, object_A.getAngularVelocity() * rA.x);

            e = Mathf.Min(object_A.Resitution, object_B.Resitution);
            // calculate the velocity on N
            float relativeN = Vector2.Dot(relativeV, contacts[i].Normal);

            if (relativeN > 0)
            {
                return;
            }
            
            
            float raCrossN = rA.x * contacts[i].Normal.y - rA.y * contacts[i].Normal.x;
            float rbCrossN = rB.x * contacts[i].Normal.y - rB.y * contacts[i].Normal.x;

            // calculate the impulse
            float j = -(1.0f + e) * relativeN;
            j /=  object_A.GetInvMass() + object_B.GetInvMass() + (raCrossN * raCrossN) * object_A.GetInvInertia() + (rbCrossN * rbCrossN) * object_B.GetInvInertia();
            j /= contacts.Count;
            if (j < 2.0f)
            {
                e = 0;
                j = -(1.0f + e) * relativeN;
                j /= object_A.GetInvMass() + object_B.GetInvMass() + (raCrossN * raCrossN) * object_A.GetInvInertia() + (rbCrossN * rbCrossN) * object_B.GetInvInertia();
                j /= contacts.Count;
            }
            Vector2 impulse = j * contacts[i].Normal;

            // apply the impulse
            object_A.ApplyImpulse(-impulse, rA);
            object_B.ApplyImpulse(impulse, rB);

            // calculate the relative velocty with the new impulse applied
            relativeV = object_B.getLinearVelocity() + new Vector2(-object_B.getAngularVelocity() * rB.y, object_B.getAngularVelocity() * rB.x)
                              - (object_A.getLinearVelocity()) - new Vector2(-object_A.getAngularVelocity() * rA.y, object_A.getAngularVelocity() * rA.x);

            Vector2 Tangent = contacts[i].Tangent;
            /*friction*/
            // the friction is the force that also applies the torque when a ball rolls down a hill
            // friction impuls that needs to be overcome
            float jfriction = -Vector2.Dot(relativeV, Tangent);
            jfriction /= object_A.GetInvMass() + object_B.GetInvMass() + (raCrossN * raCrossN) * object_A.GetInvInertia() + (rbCrossN * rbCrossN) * object_B.GetInvInertia();
            jfriction /= contacts.Count;
            // coulomb law :F_friction <= frictionconstant * F_normal
            // F_normal = j. in this case we check impulses so the impuls along noral N = f_normal

            // calculate the friction constant by getting the 2 friction variables from the 2 objects
            // this calculation can be anything
            float frictionconstant = Mathf.Sqrt(object_A.StaticFriction * object_A.StaticFriction + object_B.StaticFriction * object_B.StaticFriction);
            Vector2 FrictionImpulse;

            // check if the activation energy has been exeeded and apply friction accordingly
            // if not then the frition impuls is as big as our normal impulse
            // else apply the dynamic friction
            if (Mathf.Abs(jfriction) < float.Epsilon)
                return;

            if (Mathf.Abs(jfriction) < j * frictionconstant)
            {
                FrictionImpulse = jfriction * Tangent;
            }
            else
            {
                frictionconstant = Mathf.Sqrt(object_A.DynamicFriction * object_A.DynamicFriction + object_B.DynamicFriction * object_B.DynamicFriction);
                FrictionImpulse = -j * Tangent * frictionconstant;
            }

            // apply the friction impulse according to the collider
                object_A.ApplyImpulse(-FrictionImpulse, rA);
                object_B.ApplyImpulse(FrictionImpulse, rB);

            
        }
    }

    // validates contactpoints to check if they are persistent
    public void Validate()
    {
        for (int i = contacts.Count - 1; i >= 0; i--)
        {
            // calculate the new global positions from the local points that was collididing
            Vector2[] newGlobalPosition = new Vector2[2] {(Vector2)object_A.transform.position + contacts[i].LocalCoords[0],
                                                          (Vector2)object_B.transform.position + contacts[i].LocalCoords[1]};

            // new line from points of deepest penetration with the new global position of those points
            Vector2 AB = newGlobalPosition[1] - newGlobalPosition[0];

            // calculate the difference between the new gl0bal posisiotn of the contactpoint and the worldposition from when it was created
            Vector2 GlobalDifferenceA = contacts[i].WorlCoords[0] - newGlobalPosition[0];
            Vector2 GlobalDifferenceB = contacts[i].WorlCoords[1] - newGlobalPosition[1];

            // bool to check if the point is still penetrating
            bool StillPenetrating = Vector2.Dot(contacts[i].Normal, AB) <= 0.0f;

            // bools to check if the points haven't moved to much
            bool ACloseEnough = GlobalDifferenceA.sqrMagnitude < 0.0001f;
            bool BCloseEnough = GlobalDifferenceB.sqrMagnitude < 0.0001f;

            // if the contactpoints are still penetrating and if they didn't move to much than keep the points
            if (ACloseEnough && BCloseEnough && StillPenetrating)
            {
                contacts[i].SetPersistent(true);
            }
            else
            {
                contacts.Remove(contacts[i]);
            }
        }
    }

    // makes sure we only have a max of two contact points at the same time
    public void Manage()
    {
        // find contact that pentrates the most
        CollisionCheck.Contact_Point deepest = new CollisionCheck.Contact_Point();
        float deepestPenetration = -float.MaxValue;
        for (int i = 0; i < contacts.Count; i++)
        {
            if (contacts[i].Penetration > deepestPenetration)
            {
                deepestPenetration = contacts[i].Penetration;
                deepest = contacts[i];
            }
        }

        //find the second contact furthest from the deepest one
        CollisionCheck.Contact_Point furthest_1 = new CollisionCheck.Contact_Point();
        float dist_sqr = -float.MaxValue;
        for (int i = 0; i < contacts.Count; i++)
        {
            float dist = (contacts[i].LocalCoords[0] - deepest.LocalCoords[0]).sqrMagnitude;
            if (dist > dist_sqr)
            {
                dist_sqr = dist;
                furthest_1 = contacts[i];
            }
        }

        // clear manifold and add to most usefull points
        contacts.Clear();
        contacts.Add(deepest);
        contacts.Add(furthest_1);

        // check manifold for persistent contactpoints
        Persistent = false;
        for (int i = 0; i < contacts.Count; i++)
        {
            if (contacts[i].persistent)
                Persistent = true;
        }
    }

    // add contactpoint to the manifold
    public void AddContact(CollisionCheck.Contact_Point point)
    {
        if (contacts.Count == 0)
        {
            contacts.Add(point);
            return;
        }
        // check if the points are far enough appart to be added as a new contact point
        for (int i = 0; i < contacts.Count; i++)
        {
            // lines created by newly added point and points in manifold
            Vector2 DifferenceA = point.WorlCoords[0] - contacts[i].WorlCoords[0];
            Vector2 DifferenceB = point.WorlCoords[1] - contacts[i].WorlCoords[1];

            // bool  values to check if the points are far enough appart
            bool AFarEnough = DifferenceA.sqrMagnitude > 0.0001f;
            bool BFarEnough = DifferenceB.sqrMagnitude > 0.0001f;

            if (AFarEnough && BFarEnough)
            {
                point.persistent = false;
                contacts.Add(point);
                return;
            }
        }
    }

    public void SetBodys(ColliderBase obj_A, ColliderBase obj_B)
    {
        Collider_A = obj_A;
        Collider_B = obj_B;

        object_A = obj_A._body;
        object_B = obj_B._body;
    }

    public void CorrectPosition()
    {
        if(IsTrigger)
            return;

        float percent = 0.5f;
        float slop = 0.05f;

        for (int i = 0; i < contacts.Count; i++)
        {
            Vector2 correction = Mathf.Max(contacts[i].Penetration - slop, 0.0f) / (object_A.GetInvMass() + object_B.GetInvMass()) * percent * contacts[i].Normal;
            object_A.transform.position -= object_A.GetInvMass() * (Vector3)correction;
            object_B.transform.position += object_B.GetInvMass() * (Vector3)correction;
        }
    }

    public CollisionCheck.Contact_Point GetContactPoint(int index)
    {
        return contacts[index];
    }

    public int GetListSize()
    {
        return contacts.Count;
    }

    public Vector2 GetTangent(Vector2 normal, rigidBody A, rigidBody B)
    {
        Vector2 Relative_v = B.getLinearVelocity() - A.getLinearVelocity();
        Vector2 tangent = Relative_v - (Vector2.Dot(Relative_v, normal) * normal);
        tangent.Normalize();
        return tangent;
    }
}