//http://geomalgorithms.com/
//https://stackoverflow.com/questions/1073336/circle-line-segment-collision-detection-algorithm
public static bool TestRayCircle(LVector2 cPos, LFloat cR, LVector2 rB, LVector2 rDir, ref LFloat t)
{
var d = rDir;
var f = rB - cPos;
var a = LVector2.Dot(d, d);
var b = 2 * LVector2.Dot(f, d);
var c = LVector2.Dot(f, f) - cR * cR;
var discriminant = b * b - 4 * a * c;
if (discriminant < 0)
{
// no intersection
return(false);
}
else
{
discriminant = LMath.Sqrt(discriminant);
var t1 = (-b - discriminant) / (2 * a);
var t2 = (-b + discriminant) / (2 * a);
if (t1 >= 0)
{
t = t1;
return(true);
}
if (t2 >= 0)
{
t = t2;
return(true);
}
return(false);
}
}
/// <summary>
/// 判断是否包围了原点
/// <para>如果没有包含，且点数超过2时，只保留连线离原点最近的两个点</para>
/// </summary>
/// <returns></returns>
public bool DoSimplex2D()
{
bool certain = false;
if (count == 1 ||
count == 2)
{
certain = false;
}
else if (count == 3)
{
LVector2 p02 = LGeometryUtil2D.GetPerpendicular2D(vertics[0].p, vertics[2].p, vertics[1].p);
if (LVector2.Dot(-vertics[0].p, p02) >= 0)
{
LVector2 p12 = LGeometryUtil2D.GetPerpendicular2D(vertics[1].p, vertics[2].p, vertics[0].p);
if (LVector2.Dot(-vertics[1].p, p12) >= 0)
{
certain = true;
}
else
{
RemoveIndex(0);
}
}
else
{
RemoveIndex(1);
}
}
return(certain);
}
public static bool TestAABBOBB(LVector2 posA, LFloat rA, LVector2 sizeA, LVector2 posB, LFloat rB, LVector2 sizeB,
LVector2 upB)
{
var diff = posA - posB;
var allRadius = rA + rB;
//circle 判定
if (diff.sqrMagnitude > allRadius * allRadius)
{
return(false);
}
var absUPX = LMath.Abs(upB.x); //abs(up dot aabb.right)
var absUPY = LMath.Abs(upB.y); //abs(right dot aabb.right)
{
//轴 投影 AABBx
var distX = absUPX * sizeB.y + absUPY * sizeB.x;
if (LMath.Abs(diff.x) > distX + sizeA.x)
{
return(false);
}
//轴 投影 AABBy
//absUPX is abs(right dot aabb.up)
//absUPY is abs(up dot aabb.up)
var distY = absUPY * sizeB.y + absUPX * sizeB.x;
if (LMath.Abs(diff.y) > distY + sizeA.y)
{
return(false);
}
}
{
var right = new LVector2(upB.y, -upB.x);
var diffPObbX = LVector2.Dot(diff, right);
var diffPObbY = LVector2.Dot(diff, upB);
//absUPX is abs(aabb.up dot right )
//absUPY is abs(aabb.right dot right)
//轴 投影 OBBx
var distX = absUPX * sizeA.y + absUPY * sizeA.x;
if (LMath.Abs(diffPObbX) > distX + sizeB.x)
{
return(false);
}
//absUPX is abs(aabb.right dot up )
//absUPY is abs(aabb.up dot up)
//轴 投影 OBBy
var distY = absUPY * sizeA.y + absUPX * sizeA.x;
if (LMath.Abs(diffPObbY) > distY + sizeB.y)
{
return(false);
}
}
return(true);
}
/// <summary>
/// 计算v1v2的垂线，pointTo决定垂线的指向(返回向量未归一化)
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <param name="pointTo"></param>
public static LVector2 GetPerpendicular2D(LVector2 v1, LVector2 v2, LVector2 pointTo)
{
LVector2 v12 = v1 - v2;
LVector2 dir = new LVector2(-v12.y, v12.x);
int sign = LVector2.Dot(dir, pointTo - v1) > 0 ? 1 : -1;
dir *= sign;
return(dir);
}
/// <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);
}
internal override LVector2 GetFurtherPointInDirection(LVector2 direction)
{
LVector2 fp = mVertics[0];
LFloat fpdot = LVector2.Dot(fp, direction);
for (int i = mVertics.Length - 1; i >= 1; i--)
{
LFloat vdot = LVector2.Dot(mVertics[i], direction);
if (vdot > fpdot)
{
fp = mVertics[i];
fpdot = vdot;
}
}
return(fp);
}
/// <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();
}
private void TestLVector2()
{
LVector2 v1 = new LVector2(3.3d, 1.2d);
LVector2 v2 = new LVector2(1.2d, 3.3d);
AssertTrue(v1 != v2);
AssertFalse(v1 == v2);
AssertTrue(v1 + v2 == new LVector2(4.5d, 4.5d));
AssertTrue(v1 - v2 == new LVector2(2.1d, -2.1d));
AssertTrue(v1 * 2 == new LVector2(6.6d, 2.4d));
AssertTrue(v1 / 3 == new LVector2(1.1d, 0.4d));
AssertTrue(LVector2.zero.normalized == LVector2.zero);
AssertTrue(LVector2.Dot(v1, v2) == 7.92d);
Debug.Log(v1.sqrMagnitude);
Debug.Log(v1.magnitude);
Debug.Log(v1.normalized);
Debug.Log(LVector2.Cross(v1, v2));
Debug.Log("----------------------");
}
public static bool TestCircleOBB(LVector2 posA, LFloat rA, LVector2 posB, LFloat rB, LVector2 sizeB,
LVector2 up)
{
var diff = posA - posB;
var allRadius = rA + rB;
//circle 判定CollisionHelper
if (diff.sqrMagnitude > allRadius * allRadius)
{
return(false);
}
//空间转换
var absX = LMath.Abs(LVector2.Dot(diff, new LVector2(up.y, -up.x)));
var absY = LMath.Abs(LVector2.Dot(diff, up));
var size = sizeB;
var radius = rA;
var x = LMath.Max(absX - size.x, LFloat.zero);
var y = LMath.Max(absY - size.y, LFloat.zero);
return(x * x + y * y < radius * radius);
}
//http://www.kevlindev.com/geometry/2D/intersections/index.htm
//http://www.kevlindev.com/geometry/2D/intersections/index.htm
//https://bitlush.com/blog/circle-vs-polygon-collision-detection-in-c-sharp
public static bool TestCirclePolygon(LVector2 c, LFloat r, LVector2 *_points, int vertexCount)
{
var radiusSquared = r * r;
var circleCenter = c;
var nearestDistance = LFloat.MaxValue;
int nearestVertex = -1;
for (var i = 0; i < vertexCount; i++)
{
LVector2 axis = circleCenter - _points[i];
var distance = axis.sqrMagnitude - radiusSquared;
if (distance <= 0)
{
return(true);
}
if (distance < nearestDistance)
{
nearestVertex = i;
nearestDistance = distance;
}
}
LVector2 GetPoint(int index)
{
if (index < 0)
{
index += vertexCount;
}
else if (index >= vertexCount)
{
index -= vertexCount;
}
return(_points[index]);
}
var vertex = GetPoint(nearestVertex - 1);
for (var i = 0; i < 2; i++)
{
var nextVertex = GetPoint(nearestVertex + i);
var edge = nextVertex - vertex;
var edgeLengthSquared = edge.sqrMagnitude;
if (edgeLengthSquared != 0)
{
LVector2 axis = circleCenter - vertex;
var dot = LVector2.Dot(edge, axis);
if (dot >= 0 && dot <= edgeLengthSquared)
{
LVector2 projection = vertex + (dot / edgeLengthSquared) * edge;
axis = projection - circleCenter;
if (axis.sqrMagnitude <= radiusSquared)
{
return(true);
}
else
{
if (edge.x > 0)
{
if (axis.y > 0)
{
return(false);
}
}
else if (edge.x < 0)
{
if (axis.y < 0)
{
return(false);
}
}
else if (edge.y > 0)
{
if (axis.x < 0)
{
return(false);
}
}
else
{
if (axis.x > 0)
{
return(false);
}
}
}
}
}
vertex = nextVertex;
}
return(true);
}
public static bool TestOBBOBB(LVector2 posA, LFloat rA, LVector2 sizeA, LVector2 upA, LVector2 posB,
LFloat rB,
LVector2 sizeB,
LVector2 upB)
{
var diff = posA - posB;
var allRadius = rA + rB;
//circle 判定
if (diff.sqrMagnitude > allRadius * allRadius)
{
return(false);
}
var rightA = new LVector2(upA.y, -upA.x);
var rightB = new LVector2(upB.y, -upB.x);
{
//轴投影到 A.right
var BuProjAr = LMath.Abs(LVector2.Dot(upB, rightA));
var BrProjAr = LMath.Abs(LVector2.Dot(rightB, rightA));
var DiffProjAr = LMath.Abs(LVector2.Dot(diff, rightA));
var distX = BuProjAr * sizeB.y + BrProjAr * sizeB.x;
if (DiffProjAr > distX + sizeA.x)
{
return(false);
}
//轴投影到 A.up
var BuProjAu = LMath.Abs(LVector2.Dot(upB, upA));
var BrProjAu = LMath.Abs(LVector2.Dot(rightB, upA));
var DiffProjAu = LMath.Abs(LVector2.Dot(diff, upA));
var distY = BuProjAu * sizeB.y + BrProjAu * sizeB.x;
if (DiffProjAu > distY + sizeA.y)
{
return(false);
}
}
{
//轴投影到 B.right
var AuProjBr = LMath.Abs(LVector2.Dot(upA, rightB));
var ArProjBr = LMath.Abs(LVector2.Dot(rightA, rightB));
var DiffProjBr = LMath.Abs(LVector2.Dot(diff, rightB));
var distX = AuProjBr * sizeA.y + ArProjBr * sizeA.x;
if (DiffProjBr > distX + sizeB.x)
{
return(false);
}
//轴投影到 B.right
var AuProjBu = LMath.Abs(LVector2.Dot(upA, upB));
var ArProjBu = LMath.Abs(LVector2.Dot(rightA, upB));
var DiffProjBu = LMath.Abs(LVector2.Dot(diff, upB));
var distY = AuProjBu * sizeA.y + ArProjBu * sizeA.x;
if (DiffProjBu > distY + sizeB.x)
{
return(false);
}
}
return(true);
}
/// <summary>
/// 测试圆和多边形是否相交
/// </summary>
/// <param name="c"></param>
/// <param name="r"></param>
/// <param name="_points"></param>
/// <param name="vertextCount">多边形顶点数量</param>
/// <returns></returns>
public static bool TestCirclePolygon(LVector2 c, LFloat r, LVector2 *_points, int vertextCount)
{
//圆形的半径平方
var radiusSquared = r * r;
//圆的中心点
var circleCenter = c;
// 记录最近的距离
var nearestDistance = LFloat.MaxValue;
// 记录最近的顶点
int nearestVertex = -1;
for (int i = 0; i < vertextCount; i++)
{
LVector2 axix = circleCenter - _points[i];
// 多边形的任何顶点在圆的范围内 说明相交
var distance = axix.sqrMagnitude - radiusSquared;
if (distance <= 0)
{
return(true);
}
if (distance < nearestDistance)
{
nearestVertex = i;
nearestDistance = distance;
}
}
// 获取点
LVector2 GetPoint(int index)
{
if (index < 0)
{
index += vertextCount;
}
else if (index >= vertextCount)
{
index -= vertextCount;
}
return(_points[index]);
}
var vertex = GetPoint(nearestVertex - 1);
for (int i = 0; i < 2; i++)
{
LVector2 nextVertex = GetPoint(nearestVertex + 1);
LVector2 edge = nextVertex - vertex;
LFloat edgeLengthSquared = edge.sqrMagnitude;
if (edgeLengthSquared != 0)
{
LVector2 axis = circleCenter - vertex;
LFloat dot = LVector2.Dot(edge, axis);
if (dot >= 0 && dot <= edgeLengthSquared)
{
// (dot / edgeLengthSquare ) * edge 是求 向量的投影
// 两向量 a' b' 夹角β
// a·b = |a'||b'|cosβ
//
LVector2 projection = vertex + (dot / edgeLengthSquared) * edge;
axis = projection - circleCenter;
if (axis.sqrMagnitude <= radiusSquared)
{
return(true);
}
else
{
if (edge.x > 0)
{
if (axis.y > 0)
{
return(false);
}
else if (edge.x < 0)
{
if (axis.y < 0)
{
return(false);
}
else if (edge.y > 0)
{
if (axis.x < 0)
{
return(false);
}
else
if (axis.x > 0)
{
return(false);
}
}
}
}
}
}
}
vertex = nextVertex;
}
return(true);
}
public LFloat Dot(LVector2 v)
{
return(LVector2.Dot(this, v));
}
public static LFloat Angle(LVector2 lhs, LVector2 rhs)
{
lhs.Normalize();
rhs.Normalize();
return(LMath.Acos(lhs.Dot(rhs)) * LMath.RadToDeg);
}
public static LVector2 Project(LVector2 vec, LVector2 on)
{
return(vec.Dot(on) / on.sqrMagnitude * on);
}
