gjk算法代码
效果
1) 可以显示gjk的每一步执行情况，步骤可以在Inspector上调
2) 步骤的信息包括：原点到最近边的垂线(蓝色线)，support点用的方向(右上角的绿色箭头), support点用的shape上的点(在shape上用绿线连接)(原点到最新support点的绿色连线和这跟线是一样长度)
3) shape上红色方块标出的是shape的起始顶点
[Serializable] public struct SupportPoint { public Vector2 m_Point; public int m_ShapeAVertIndex; public int m_ShapeBVertIndex; }
单形体
public class Simplex { private List<SupportPoint> m_PointList = new List<SupportPoint>(); public void Add(Vector2 p, int shapeAVertIndex, int shapeBVertIndex) { var sp = new SupportPoint(); sp.m_Point = p; sp.m_ShapeAVertIndex = shapeAVertIndex; sp.m_ShapeBVertIndex = shapeBVertIndex; m_PointList.Add(sp); } public Vector2 Get(int index) { return m_PointList[index].m_Point; } public SupportPoint GetSupportPoint(int index) { return m_PointList[index]; } public void Insert(int index, Vector2 p, int shapeAVertIndex, int shapeBVertIndex) { var sp = new SupportPoint(); sp.m_Point = p; sp.m_ShapeAVertIndex = shapeAVertIndex; sp.m_ShapeBVertIndex = shapeBVertIndex; m_PointList.Insert(index, sp); } 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].m_Point).sqrMagnitude, 0); else if (2 == m_PointList.Count) return Shape2DHelper.IsPointOnSegment(p, m_PointList[0].m_Point, m_PointList[1].m_Point); else if (3 == m_PointList.Count) return Shape2DHelper.IsPointInTriangle(p, m_PointList[0].m_Point, m_PointList[1].m_Point, m_PointList[2].m_Point); return false; } public int Count { get { return m_PointList.Count; } } }
public interface Shape { Vector2 GetFarthestVertInDir(Vector2 dir, out int vertIndex); }
public partial class GJK { public const float Epsilon = 0.000001f; //浮点数误差 private Simplex m_Simplex = new Simplex(); //单行体 private int m_MaxIterCount = 20; //防止死循环的 private List<StepCmd> m_StepCmdList; //调试用: 用于展示每一步的执行情况 public GJK(List<StepCmd> list) { m_StepCmdList = list; } //测试是否发生碰撞 public bool Test(Shape s1, Shape s2) { m_Simplex.Clear(); m_StepCmdList.Clear(); int shapeAVertIndex = 0; int shapeBVertIndex = 0; var dir = FindFirstDir(); var supportPoint = Support(s1, s2, dir, out shapeAVertIndex, out shapeBVertIndex); m_Simplex.Add(supportPoint, shapeAVertIndex, shapeBVertIndex); AddStepCmd(dir); dir = -dir; for (int i = 0; i < m_MaxIterCount; ++i) { if (dir.sqrMagnitude < Epsilon) // 方向接近于0，说明原点就在边上 return true; supportPoint = Support(s1, s2, dir, out shapeAVertIndex, out shapeBVertIndex); if (Vector2.Dot(supportPoint, dir) < Epsilon) //该方向上没有更远的点, 即无法跨越原点了 return false; m_Simplex.Add(supportPoint, shapeAVertIndex, shapeBVertIndex); AddStepCmd(dir); if (m_Simplex.IsPointIn(Vector2.zero)) //原点是否在单形体内return true; dir = FindNextDir(); } return false; } //一般初始方向随机取一个也不会有问题 private Vector2 FindFirstDir() { return Vector2.right; } //找出形状1在dir方向上最远的点, 形状2在-dir方向上最远的点 private Vector2 Support(Shape s1, Shape s2, Vector2 dir, out int shapeAVertIndex, out int shapeBVertIndex) { var a = s1.GetFarthestVertInDir(dir, out shapeAVertIndex); var b = s2.GetFarthestVertInDir(-dir, out shapeBVertIndex); //Debug.Log($"Support: dir: {dir}; {a}, {b}"); var result = a - b; return result; } 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); var topStepCmd = m_StepCmdList[m_StepCmdList.Count - 1]; topStepCmd.m_Perp = abPerp; 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); var topStepCmd = m_StepCmdList[m_StepCmdList.Count - 1]; topStepCmd.m_Perp = caPerp; return Vector2.zero - caPerp; //原点到线段的垂线 } else { m_Simplex.RemoveAt(0); var topStepCmd = m_StepCmdList[m_StepCmdList.Count - 1]; topStepCmd.m_Perp = cbPerp; return Vector2.zero - cbPerp; //原点到线段的垂线 } } //error: 不可能执行到这边 return Vector2.zero; } private void AddStepCmd(Vector2 dir) { var cmd = new StepCmd(); cmd.m_Dir = dir; cmd.m_SimplexPointList = new List<SupportPoint>(); for (int i = 0; i < m_Simplex.Count; ++i) { var sp = m_Simplex.GetSupportPoint(i); cmd.m_SimplexPointList.Add(sp); } m_StepCmdList.Add(cmd); }
}
public class GJKPolygon : MonoBehaviour, Shape { [SerializeField] private List<Transform> m_VertList = new List<Transform>(); public Vector2 GetVert(int index) { var trans = m_VertList[index]; return trans.position; } public int GetVertCount() { return m_VertList.Count; } public Vector2 GetFarthestVertInDir(Vector2 dir, out int vertIndex) { vertIndex = 0; Vector2 maxVert = GetVert(0); float maxDist = Vector2.Dot(maxVert, dir); for (int i = 1; i < m_VertList.Count; ++i) { var vert = GetVert(i); float proj = Vector2.Dot(vert, dir); if (proj > maxDist) { maxDist = proj; maxVert = vert; vertIndex = i; } } return maxVert; } #if UNITY_EDITOR [NonSerialized] public bool m_IsHighlight = false; private Vector3 m_PointSize = Vector3.zero; private void OnDrawGizmos() { if (null == m_VertList || m_VertList.Count < 3) return; for (var i = 0; i < m_VertList.Count; ++i) { if (null == m_VertList[i]) return; } if (m_IsHighlight) Gizmos.color = Color.red; for (var i = 0; i < m_VertList.Count; ++i) { var a = GetVert(i); var b = GetVert((i+1) % m_VertList.Count); if (0 == i) DrawPoint(a, ref m_PointSize); //第1个点 Gizmos.DrawLine(a, b); } Gizmos.color = Color.white; } public static void DrawPoint(Vector2 point, ref Vector3 pointSize) { float handleSize = HandleUtility.GetHandleSize(point) * 0.1f; pointSize.Set(handleSize, handleSize, 0.01f); Gizmos.DrawCube(point, pointSize); } #endif }
//显示调试图形所需的数据 [Serializable] public class StepCmd { public Vector2 m_Dir; //这次找Support点, 取的方向 public List<SupportPoint> m_SimplexPointList; //这一步迭代时的Support点copy public Vector2 m_Perp; //原点到单形体边的垂足 }
测试代码
public class GJKTest : MonoBehaviour { public GJKPolygon m_Polygon1; public GJKPolygon m_Polygon2; private GJK m_GJK; [Range(0, 20)] public int m_Step = 0; private List<GJK.StepCmd> m_StepCmdList = new List<GJK.StepCmd>(); public GJK.StepCmd m_CurStepCmd; private Vector3 m_CubeSize = new Vector3(0.02f, 0.02f, 0.01f); void Start() { m_GJK = new GJK(m_StepCmdList); } void Update() { if (m_Polygon1 && m_Polygon2) { var result = m_GJK.Test(m_Polygon1, m_Polygon2); m_Polygon1.m_IsHighlight = result; m_Polygon2.m_IsHighlight = result; } } #if UNITY_EDITOR public Vector2 m_DirStartPos = new Vector2(3, 3); private void OnDrawGizmos() { m_CurStepCmd = null; if (m_StepCmdList.Count > 0) { m_Step = Mathf.Min(m_StepCmdList.Count - 1, Mathf.Max(m_Step, 0)); var stepCmd = m_StepCmdList[m_Step]; m_CurStepCmd = stepCmd; var lastSupportPoint = new GJK.SupportPoint(); if (1 == stepCmd.m_SimplexPointList.Count) { var p1 = stepCmd.m_SimplexPointList[0]; DrawPoint(p1.m_Point, ref m_CubeSize); Gizmos.color = Color.green; lastSupportPoint = p1; Gizmos.color = Color.blue; } else if (2 == stepCmd.m_SimplexPointList.Count) { var p1 = stepCmd.m_SimplexPointList[0]; var p2 = stepCmd.m_SimplexPointList[1]; Gizmos.DrawLine(p1.m_Point, p2.m_Point); Gizmos.color = Color.blue; lastSupportPoint = p2; Gizmos.DrawLine(stepCmd.m_Perp, Vector2.zero); //原点到最近边的垂线 } else { for (int i = 0; i < stepCmd.m_SimplexPointList.Count; ++i) { var p1 = stepCmd.m_SimplexPointList[i]; var p2 = stepCmd.m_SimplexPointList[(i + 1) % stepCmd.m_SimplexPointList.Count]; lastSupportPoint = p1; Gizmos.DrawLine(p1.m_Point, p2.m_Point); } Gizmos.color = Color.blue; Gizmos.DrawLine(stepCmd.m_Perp, Vector2.zero); //原点到最近边的垂线 } Gizmos.color = Color.green; Gizmos.DrawLine(lastSupportPoint.m_Point, Vector2.zero); //构成support点的shape上的点 var shapeAVert = m_Polygon1.GetVert(lastSupportPoint.m_ShapeAVertIndex); var shapeBVert = m_Polygon2.GetVert(lastSupportPoint.m_ShapeBVertIndex); Gizmos.DrawLine(shapeAVert, shapeBVert); //寻找support点时用的方向 Gizmos.color = Color.green; DrawArrow(m_DirStartPos, stepCmd.m_Dir.normalized, 0.8f); Gizmos.color = Color.white; } } public static void DrawPoint(Vector2 point, ref Vector3 pointCubeSize) { float handleSize = HandleUtility.GetHandleSize(point) * 0.1f; pointCubeSize.Set(handleSize, handleSize, 0.01f); Gizmos.DrawCube(point, pointCubeSize); } public static void DrawArrow(Vector2 start, Vector2 dir, float len, float size = 0.2f) { var end = start + dir * len; Gizmos.DrawLine(start, end); var arrowSize = new Vector2(size, 0); Vector2 arrowLineDirCCW = Quaternion.Euler(0, 0, 150) * dir * 0.2f; Gizmos.DrawLine(end, end + arrowLineDirCCW); Vector2 arrowLineDirCW = Quaternion.Euler(0, 0, -150) * dir * 0.2f; Gizmos.DrawLine(end, end + arrowLineDirCW); } #endif }
参考
物理引擎学习03-GJK碰撞检测算法基础_gjk算法-CSDN博客
2D凸多边形碰撞检测算法（二） - GJK（上） - 知乎 (zhihu.com)
2D凸多边形碰撞检测算法（二） - GJK（下） - 知乎 (zhihu.com)
