/// <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);
}