public Edge createEdge(Vector2 a, Vector2 b) { Edge e = new Edge(); e.a = a; e.b = b; e.normal = GJKTool.getPerpendicularToOrigin(a, b); float lengthSq = e.normal.sqrMagnitude; // 单位化边 if (lengthSq > 0.00001f) { e.distance = Mathf.Sqrt(lengthSq); e.normal *= 1.0f / e.distance; } else { // 如果距离原点太近,用数学的方法来得到直线的垂线 // 方向可以随便取,刚好另外一边是反着来的 Vector2 v = a - b; v.Normalize(); e.normal = new Vector2(-v.y, v.x); } return(e); }
public Vector2 findNextDirection() { if (simplex.count() == 2) { Vector2 crossPoint = GJKTool.getClosestPointToOrigin(simplex.get(0), simplex.get(1)); // 取靠近原点方向的向量 return(Vector2.zero - crossPoint); } else if (simplex.count() == 3) { Vector2 crossOnCA = GJKTool.getClosestPointToOrigin(simplex.get(2), simplex.get(0)); Vector2 crossOnCB = GJKTool.getClosestPointToOrigin(simplex.get(2), simplex.get(1)); // 保留距离原点近的,移除较远的那个点 if (crossOnCA.sqrMagnitude < crossOnCB.sqrMagnitude) { simplex.remove(1); return(Vector2.zero - crossOnCA); } else { simplex.remove(0); return(Vector2.zero - crossOnCB); } } else { // 不应该执行到这里 return(new Vector2(0, 0)); } }
void UpdateSelection() { if (Input.GetMouseButtonDown(0)) { Vector2 mousePos = Camera.main.ScreenToWorldPoint(Input.mousePosition); //Debug.Log("mousePos: " + mousePos); selectedIndex = -1; for (int i = 0; i < shapes.Count; ++i) { if (GJKTool.contains(shapes[i].vertices, mousePos)) { selectedIndex = i; break; } } if (selectedIndex >= 0) { var t = colliders[selectedIndex].transform; Vector2 position = t.position; selectedOffset = mousePos - position; float mouseAngle = Mathf.Atan2(selectedOffset.y, selectedOffset.x) * Mathf.Rad2Deg; selectedAngle = mouseAngle - t.eulerAngles.z; } } else if (Input.GetMouseButtonUp(0)) { selectedIndex = -1; } else if (Input.GetMouseButton(0)) { if (selectedIndex < 0) { return; } Transform t = colliders[selectedIndex].transform; Vector2 mousePos = Camera.main.ScreenToWorldPoint(Input.mousePosition); if (selectedOffset.magnitude < 0.4f) // 移动 { Vector3 position = t.position; position.x = mousePos.x - selectedOffset.x; position.y = mousePos.y - selectedOffset.y; t.position = position; } else // 旋转 { Vector2 position = t.position; Vector2 mouseDir = mousePos - position; float mouseAngle = Mathf.Atan2(mouseDir.y, mouseDir.x) * Mathf.Rad2Deg; float angle = mouseAngle - selectedAngle; t.eulerAngles = new Vector3(0, 0, angle); } } }
/// 按步骤分解,碰撞检测 public IEnumerator queryStepByStep(Shape shapeA, Shape shapeB) { this.shapeA = shapeA; this.shapeB = shapeB; simplex.clear(); isCollision = false; direction = Vector2.zero; closestOnA = Vector2.zero; closestOnB = Vector2.zero; simplexEdge.clear(); currentEpaEdge = null; penetrationVector = Vector2.zero; yield return(null); direction = findFirstDirection(); simplex.add(support(direction)); simplex.add(support(-direction)); yield return(null); direction = -GJKTool.getClosestPointToOrigin(simplex.get(0), simplex.get(1)); for (int i = 0; i < maxIterCount; ++i) { // 方向接近于0,说明原点就在边上 if (direction.sqrMagnitude < epsilon) { isCollision = true; break; } SupportPoint p = support(direction); // 新点与之前的点重合了。也就是沿着dir的方向,已经找不到更近的点了。 if (GJKTool.sqrDistance(p.point, simplex.get(0)) < epsilon || GJKTool.sqrDistance(p.point, simplex.get(1)) < epsilon) { isCollision = false; break; } simplex.add(p); yield return(null); // 单形体包含原点了 if (simplex.contains(Vector2.zero)) { isCollision = true; break; } direction = findNextDirection(); } yield return(null); if (!isCollision) { ComputeClosetPoint(); } else { // EPA算法计算穿透向量 simplexEdge.initEdges(simplex); for (int i = 0; i < maxIterCount; ++i) { Edge e = simplexEdge.findClosestEdge(); currentEpaEdge = e; penetrationVector = e.normal * e.distance; yield return(null); Vector2 point = support(e.normal).point; float distance = Vector2.Dot(point, e.normal); if (distance - e.distance < epsilon) { penetrationVector = e.normal * distance; break; } simplexEdge.insertEdgePoint(e, point); yield return(null); } } }
public bool contains(Vector2 point) { return(GJKTool.contains(points, point)); }