public LFloat sqrMagnitude; // 原点到直线的距离的平方 public EPAFace2D(SimplexPoint2D sp1, SimplexPoint2D sp2) { this.sp1 = sp1; this.sp2 = sp2; // 这里使用计算直线交点的方向求得原点到直线的最短向量 LVector2 D = -LGeometryUtil2D.GetPerpendicular2D(sp1.p, sp2.p, LVector2.zero); if (sp2.p.x == sp1.p.x) { LFloat x = sp1.p.x; this.direction = new LVector2(x, 0); this.sqrMagnitude = x * x; } else { if (D.x == 0) { LFloat y = sp1.p.y; this.direction = new LVector2(0, y); this.sqrMagnitude = y * y; } else { LFloat k1 = (sp2.p.y - sp1.p.y) / (sp2.p.x - sp1.p.x); LFloat b1 = (sp2.p.y * sp1.p.x - sp2.p.x * sp1.p.y) / (sp1.p.x - sp2.p.x); LFloat k2 = D.y / D.x; LFloat b2 = 0; LFloat y = (b1 * k2 - b2 * k1) / (k2 - k1); LFloat x = y / k2; this.direction = new LVector2(x, y); this.sqrMagnitude = x * x + y * y; } } }
/// <summary> /// <para>GJK判断是否碰撞</para> /// <para>1.找direction方向的最远点</para> /// <para>2.判断最远点是否与原点在同一方向(是否比原点还远),如果不是则未碰撞</para> /// <para>3.将最远点置入simplex,判断simplex是否包围了原点,如果是则碰撞,否则以离原点最近的simplex上的点到原点作为direction回到步骤1</para> /// </summary> /// <param name="objA"></param> /// <param name="objB"></param> private LContactConstraint2D CollisionDetect(LPhysicsObject2D objA, LPhysicsObject2D objB) { Vector2 a = objA.collider.GetFurtherPointInDirection(direction); Vector2 b = objB.collider.GetFurtherPointInDirection(-direction); SimplexPoint2D sp = new SimplexPoint2D(a, b); if (simplex.IsSameDirectionWithOrigin(sp, direction)) { simplex.Add(sp); if (simplex.DoSimplex2D()) { epa.Add(new EPAFace2D(simplex[0], simplex[1])); epa.Add(new EPAFace2D(simplex[1], simplex[2])); epa.Add(new EPAFace2D(simplex[2], simplex[0])); PenetrationDetect(objA, objB); return(ContactConstraintGenerate(objA, objB)); } else { direction = simplex.FindClosestPointToOriginDirection(); return(CollisionDetect(objA, objB)); } } else { return(null); } }
public void AddExpiredEdgePoint(SimplexPoint2D sp) { for (int i = points.Count - 1; i >= 0; i--) { if (points[i].Equals(sp)) { points.RemoveAt(i); return; } } points.Add(sp); }
/// <summary> /// 判断找到Support点是是否是沿找寻的方向 /// <para>如果是相反的方向,说明不会包含原点,退出GJK</para> /// </summary> /// <returns></returns> public bool IsSameDirectionWithOrigin(SimplexPoint2D sp, LVector2 direction) { bool certain = false; if (count == 0) { certain = true; } else if (count == 1 || count == 2) { certain = LVector2.Dot(sp.p, direction) >= 0; } return(certain); }
/// <summary> /// <para>EPA判断渗透方向和深度</para> /// <para>1.找到list中离原点最近的face</para> /// <para>2.如果与上一次找到的相同,直接返回</para> /// <para>3.否则找face.normal的最远点A</para> /// <para>4.移除使A看不到的原点的face,将(A, face.A)和(A, face.B)构建为新的face添加到list中,返回第一步</para> /// </summary> /// <returns></returns> private void PenetrationDetect(LPhysicsObject2D objA, LPhysicsObject2D objB) { EPAFace2D face = epa.FindClosestFace(); if (closestFace == null || Mathf.Abs(closestFace.sqrMagnitude - face.sqrMagnitude) > EPAFace2D.Threshold) { closestFace = face; safety.Add(closestFace); Vector2 a = objA.collider.GetFurtherPointInDirection(closestFace.direction); Vector2 b = objB.collider.GetFurtherPointInDirection(-closestFace.direction); SimplexPoint2D sp = new SimplexPoint2D(a, b); epa.ImportNewSupportPoint(sp); PenetrationDetect(objA, objB); } }
/// <summary> /// 引入一个新的support point,删除使得此点看不见原点的edge,将删除的edge的点保存在list中 /// 剔除list中的重复点,用list中的每个点与sp构建新的edge /// </summary> /// <param name="v1"></param> /// <param name="v2"></param> /// <param name="sp"></param> public void ImportNewSupportPoint(SimplexPoint2D sp) { for (int i = faces.Count - 1; i >= 0; i--) { if (LVector2.Dot(sp.p - faces[i].sp1.p, faces[i].direction) > 0) { AddExpiredEdgePoint(faces[i].sp1); AddExpiredEdgePoint(faces[i].sp2); faces.RemoveAt(i); } } for (int i = points.Count - 1; i >= 0; i--) { faces.Add(new EPAFace2D(sp, points[i])); } points.Clear(); }
public bool Equals(SimplexPoint2D sp) { return(ma == sp.a && mb == sp.b); }
/// <summary> /// 添加一个Support点 /// </summary> /// <param name="sp"></param> public void Add(SimplexPoint2D sp) { vertics[count] = sp; count += 1; }