/// <summary> /// 算法来自 game physics cookbook /// </summary> /// <param name="line"></param> /// <param name="circle"></param> /// <returns></returns> public static bool Line2dWithCircle2d(Line2d line, Circle2d circle) { Vector2L ab = line.m_point2 - line.m_point1; FloatL t = Vector2L.Dot(circle.m_pos - line.m_point1, ab) / Vector2L.Dot(ab, ab); Vector2L closestPoint = line.m_point1 + ab * t; return((circle.m_pos - closestPoint).sqrMagnitude < circle.m_radius * circle.m_radius); }
static bool linesCross(Vector2L v0, Vector2L v1, Vector2L t0, Vector2L t1, ref Vector2L intersectionPoint) { if (v1 == t0 || v0 == t0 || v1 == t1 || v0 == t1) { return(false); } Vector2L vnormal = v1 - v0; vnormal = b2Cross(1.0f, vnormal); FloatL v0d = Vector2L.Dot(vnormal, v0); FloatL t0d = Vector2L.Dot(vnormal, t0); FloatL t1d = Vector2L.Dot(vnormal, t1); if (t0d > v0d && t1d > v0d) { return(false); } if (t0d < v0d && t1d < v0d) { return(false); } Vector2L tnormal = t1 - t0; tnormal = b2Cross(1.0f, tnormal); t0d = Vector2L.Dot(tnormal, t0); v0d = Vector2L.Dot(tnormal, v0); FloatL v1d = Vector2L.Dot(tnormal, v1); if (v0d > t0d && v1d > t0d) { return(false); } if (v0d < t0d && v1d < t0d) { return(false); } intersectionPoint = v0 + ((t0d - v0d) / (v1d - v0d)) * (v1 - v0); return(true); }
public static FloatL Angle(Vector2L from, Vector2L to) { return(FixPointMath.Acos(FixPointMath.Clamp(Vector2L.Dot(from.normalized, to.normalized), -1, 1)) * 57.29578d); }
/// <summary> /// 根据Separating Axis Theorem理论实现 /// https://www.sevenson.com.au/actionscript/sat/ /// https://github.com/sevdanski/SAT_AS3 /// </summary> /// <param name="convex1"></param> /// <param name="convex2"></param> /// <returns></returns> public static CollisionInfo Circle2dWithConvex2d(Circle2d circleA, Convex2d polygonA, bool flip, bool docalc) { CollisionInfo result = new CollisionInfo(); if (flip) { result.shapeA = polygonA; result.shapeB = circleA; } else { result.shapeA = circleA; result.shapeB = polygonA; } result.shapeAContained = true; result.shapeBContained = true; // get the offset Vector2L vOffset = new Vector2L(polygonA.m_pos.x - circleA.m_pos.x, polygonA.m_pos.y - circleA.m_pos.y); // get the vertices List <Vector2L> p1 = polygonA.GetRotateList(); // find the closest point Vector2L closestPoint = new Vector2L(); FloatL minDist = FloatL.MaxValue; foreach (var iter in p1) { FloatL currentDist = (circleA.m_pos - (polygonA.m_pos + iter)).sqrMagnitude; if (currentDist < minDist) { minDist = currentDist; closestPoint = polygonA.m_pos + iter; } } // make a normal of this vector Vector2L vAxis = closestPoint - circleA.m_pos; vAxis.Normalize(); // project polygon A FloatL min0 = Vector2L.Dot(vAxis, p1[0]); FloatL max0 = min0; for (int j = 1; j < p1.Count; ++j) { FloatL t = Vector2L.Dot(vAxis, p1[j]); if (t < min0) { min0 = t; } if (t > max0) { max0 = t; } } // project circle A FloatL min1 = Vector2L.Dot(vAxis, Vector2L.zero); FloatL max1 = min1 + circleA.m_radius; min1 -= circleA.m_radius; // shift polygonA's projected points FloatL sOffset = Vector2L.Dot(vAxis, vOffset); min0 += sOffset; max0 += sOffset; // test for intersections FloatL d0 = min0 - max1; FloatL d1 = min1 - max0; if (d0 > 0 || d1 > 0) { // gap found return(null); } FloatL shortestDist = FloatL.MaxValue; if (docalc) { FloatL distmin = (max1 - min0) * -1; //Math.min(dist0, dist1); if (flip) { distmin *= -1; } FloatL distminAbs = (distmin < 0) ? distmin * -1 : distmin; // check for containment if (!flip) { if (max0 > max1 || min0 < min1) { result.shapeAContained = false; } if (max1 > max0 || min1 < min0) { result.shapeBContained = false; } } else { if (max0 < max1 || min0 > min1) { result.shapeAContained = false; } if (max1 < max0 || min1 > min0) { result.shapeBContained = false; } } // this distance is shorter so use it... result.distance = distmin; result.vector = vAxis; // shortestDist = distminAbs; } // loop through all of the axis on the first polygon for (int i = 0; i < p1.Count; i++) { // find the axis that we will project onto vAxis = GetAxisNormal(p1, i); // project polygon A min0 = Vector2L.Dot(vAxis, p1[0]); max0 = min0; // for (int j = 1; j < p1.Count; j++) { FloatL t = Vector2L.Dot(vAxis, p1[j]); if (t < min0) { min0 = t; } if (t > max0) { max0 = t; } } // project circle A min1 = Vector2L.Dot(vAxis, new Vector2L(0, 0)); max1 = min1 + circleA.m_radius; min1 -= circleA.m_radius; // shift polygonA's projected points sOffset = Vector2L.Dot(vAxis, vOffset); min0 += sOffset; max0 += sOffset; // test for intersections d0 = min0 - max1; d1 = min1 - max0; if (d0 > 0 || d1 > 0) { // gap found return(null); } if (docalc) { // check for containment if (!flip) { if (max0 > max1 || min0 < min1) { result.shapeAContained = false; } if (max1 > max0 || min1 < min0) { result.shapeBContained = false; } } else { if (max0 < max1 || min0 > min1) { result.shapeAContained = false; } if (max1 < max0 || min1 > min0) { result.shapeBContained = false; } } FloatL distmin = (max1 - min0) * -1; if (flip) { distmin *= -1; } FloatL distminAbs = (distmin < 0) ? distmin * -1 : distmin; if (distminAbs < shortestDist) { // this distance is shorter so use it... result.distance = distmin; result.vector = vAxis; // shortestDist = distminAbs; } } } // if you are here then no gap was found return(result); }
/// <summary> /// 根据Separating Axis Theorem理论实现 /// https://www.sevenson.com.au/actionscript/sat/ /// https://github.com/sevdanski/SAT_AS3 /// </summary> /// <param name="convex1"></param> /// <param name="convex2"></param> /// <returns></returns> public static CollisionInfo Convex2dWithConvex2d(Convex2d polygonA, Convex2d polygonB, bool flip, bool docalc) { CollisionInfo result = new CollisionInfo(); result.shapeA = (flip) ? polygonB : polygonA; result.shapeB = (flip) ? polygonA : polygonB; result.shapeAContained = true; result.shapeBContained = true; // get the vertices List <Vector2L> p1 = polygonA.GetRotateList(); List <Vector2L> p2 = polygonB.GetRotateList(); // get the offset Vector2L vOffset = new Vector2L(polygonA.m_pos.x - polygonB.m_pos.x, polygonA.m_pos.y - polygonB.m_pos.y); FloatL shortestDist = FloatL.MaxValue; // loop through all of the axis on the first polygon for (int i = 0; i < p1.Count; ++i) { // find the axis that we will project onto Vector2L vAxis = GetAxisNormal(p1, i); // project polygon A FloatL min0 = Vector2L.Dot(vAxis, p1[0]); FloatL max0 = min0; for (int j = 1; j < p1.Count; ++j) { FloatL t = Vector2L.Dot(vAxis, p1[j]); if (t < min0) { min0 = t; } if (t > max0) { max0 = t; } } // project polygon B FloatL min1 = Vector2L.Dot(vAxis, p2[0]); FloatL max1 = min1; for (int j = 1; j < p2.Count; ++j) { FloatL t = Vector2L.Dot(vAxis, p2[j]); if (t < min1) { min1 = t; } if (t > max1) { max1 = t; } } // shift polygonA's projected points FloatL sOffset = Vector2L.Dot(vAxis, vOffset); min0 += sOffset; max0 += sOffset; // test for intersections FloatL d0 = min0 - max1; FloatL d1 = min1 - max0; if (d0 > 0 || d1 > 0) { // gap found return(null); } if (docalc) { // check for containment if (!flip) { if (max0 > max1 || min0 < min1) { result.shapeAContained = false; } if (max1 > max0 || min1 < min0) { result.shapeBContained = false; } } else { if (max0 < max1 || min0 > min1) { result.shapeAContained = false; } if (max1 < max0 || min1 > min0) { result.shapeBContained = false; } } FloatL distmin = (max1 - min0) * -1; if (flip) { distmin *= -1; } FloatL distminAbs = (distmin < 0) ? distmin * -1 : distmin; if (distminAbs < shortestDist) { // this distance is shorter so use it... result.distance = distmin; result.vector = vAxis; shortestDist = distminAbs; } } } // if you are here then no gap was found return(result); }