gjk算法

class Simplex
{
    private List<Vector2> m_PointList = new List<Vector2>();

    public void Add(Vector2 p)
    {
        m_PointList.Add(p);
    }

    public Vector2 Get(int index)
    {
        return m_PointList[index];
    }

    public void RemoveAt(int index)
    {
        m_PointList.RemoveAt(index);
    }

    public void Clear()
    {
        m_PointList.Clear();
    }

    //是否包含该点
    public bool IsPointIn(Vector2 p)
    {
        if (1 == m_PointList.Count)
            return Mathf.Approximately((p - m_PointList[0]).sqrMagnitude, 0);
        else if (2 == m_PointList.Count)
            return Shape2DHelper.IsPointOnSegment(p, m_PointList[0], m_PointList[1]);
        else if (3 == m_PointList.Count)
            return Shape2DHelper.IsPointInTriangle(p, m_PointList[0], m_PointList[1], m_PointList[2]);

        return false;
    }

    public int Count
    {
        get { return m_PointList.Count; }
    }
}

public interface Shape
{
    Vector2 GetVert(int index);
    int GetVertCount();
    Vector2 GetFarthestVertInDir(Vector2 dir);
}

public class GJK
{
    private Simplex m_Simplex = new Simplex();

    public bool Test(Shape s1, Shape s2)
    {
        m_Simplex.Clear();

        var dir = FindFirstDir();
        m_Simplex.Add(Support(s1, s2, dir));
        dir = -dir;
        while (true)
        {
            var p = Support(s1, s2, dir);
            if (Vector2.Dot(p, dir) < 0) //该方向上没有更远的点, 即无法跨越原点了
                return false;

            m_Simplex.Add(p);

            if (m_Simplex.IsPointIn(Vector2.zero)) //原点是否在单形体内
                return true;

            dir = FindNextDir();
        }
    }

    //一般初始方向随机取一个也不会有问题
    private Vector2 FindFirstDir()
    {
        return Vector2.right;
    }

    //找出形状1在dir方向上最远的点, 形状2在-dir方向上最远的点
    private Vector2 Support(Shape s1, Shape s2, Vector2 dir)
    {
        var a = s1.GetFarthestVertInDir(dir);
        var b = s2.GetFarthestVertInDir(-dir);
        return a - b;
    }

    private Vector2 FindNextDir()
    {
        if (2 == m_Simplex.Count) //1阶单纯形: 线段
        {
            var a = m_Simplex.Get(0);
            var b = m_Simplex.Get(1);
            var abPerp = Shape2DHelper.GetPerpendicularToOrigin(a, b);
            return Vector2.zero - abPerp; //原点到线段的垂线
        }

        if (3 == m_Simplex.Count) //2阶单纯形: 三角形
        {
            var a = m_Simplex.Get(0);
            var b = m_Simplex.Get(1);
            var c = m_Simplex.Get(2);
            var caPerp = Shape2DHelper.GetPerpendicularToOrigin(c, a);
            var cbPerp = Shape2DHelper.GetPerpendicularToOrigin(c, b);
            //使用离原点更近的线段
            if (caPerp.sqrMagnitude < cbPerp.sqrMagnitude)
            {
                m_Simplex.RemoveAt(1);
                return Vector2.zero - caPerp; //原点到线段的垂线
            }
            else
            {
                m_Simplex.RemoveAt(0);
                return Vector2.zero - cbPerp; //原点到线段的垂线
            }
        }

        //error
        return Vector2.zero;
    }

}

public class GJKPolygon : MonoBehaviour, Shape
{

    [SerializeField]
    private List<Vector2> m_VertList = new List<Vector2>();

    public GJKPolygon()
    {
        m_VertList.Add(new Vector2(1, 1));
        m_VertList.Add(new Vector2(1, -1));
        m_VertList.Add(new Vector2(-1, -1));
        m_VertList.Add(new Vector2(-1, 1));
    }

    public Vector2 GetVert(int index)
    {
        var vert = m_VertList[index];
        return vert + (Vector2)this.transform.position;
    }

    public int GetVertCount()
    {
        return m_VertList.Count;
    }

    public Vector2 GetFarthestVertInDir(Vector2 dir)
    {
        float maxDist = float.MinValue;
        int maxIndex = 0;
        for (int i = 0; i < m_VertList.Count; ++i)
        {
            float proj = Vector2.Dot(m_VertList[i], dir);
            if (proj > maxDist)
            {
                maxDist = proj;
                maxIndex = i;
            }
        }
        return GetVert(maxIndex);
    }

#if UNITY_EDITOR

    [NonSerialized]
    public bool m_IsHighlight = false;

    private void OnDrawGizmos()
    {
        if (m_VertList.Count < 3)
            return;

        if (m_IsHighlight)
            Gizmos.color = Color.red;

        var wVertex0 = GetVert(0);
        var lastWVertex = wVertex0;
        for (var i = 1; i < m_VertList.Count; ++i)
        {
            var curWVertex = GetVert(i);

            Gizmos.DrawLine(lastWVertex, curWVertex);
            lastWVertex = curWVertex;
        }
        Gizmos.DrawLine(lastWVertex, wVertex0);

        if (m_IsHighlight)
            Gizmos.color = Color.white;
    }
#endif


}

public class GJKTest : MonoBehaviour
{
    public GJKPolygon m_Polygon1;
    public GJKPolygon m_Polygon2;

    private GJK m_GJK = new GJK();

    void Update()
    {
        var result = m_GJK.Test(m_Polygon1, m_Polygon2);
        m_Polygon1.m_IsHighlight = result;
        m_Polygon2.m_IsHighlight = result;
    }
}