public void TestAngle(float angleA, float angleB)
{
Vector2 point = Vector2.zero;
var vA = new Vector2(Mathf.Cos(angleA), Mathf.Sin(angleA));
var deltaAngle = (Math.Max(angleA, angleB) - Math.Min(angleA, angleB)) % (Mathf.PI * 2);
var vB = new Vector2(Mathf.Cos(angleB), Mathf.Sin(angleB));
var halfAngle = deltaAngle / 2;
var result = VectorMath2D.IntersectsLineLine(point, vA, 2, point, vB, 2, out var aL, out var bL);
var d = Mathf.Abs(1 / Mathf.Tan(halfAngle));
if (almostEqual(deltaAngle, Mathf.PI))
{
Assert.IsFalse(result);
}
else if (almostEqual(deltaAngle, 0) || almostEqual(deltaAngle, Mathf.PI * 2))
{
Assert.IsTrue(float.IsInfinity(aL));
Assert.IsTrue(float.IsInfinity(bL));
Assert.IsFalse(result);
}
else
{
Assert.AreEqual(d, aL, 0.0001f);
Assert.AreEqual(d, bL, 0.0001f);
Assert.IsTrue(result);
}
}
public void TestIntersectLineLineInequalWidth()
{
var a = Vector2.zero;
var b = Vector2.right;
var c = Vector2.up;
var w1 = 2;
var w2 = 4;
VectorMath2D.IntersectsLineLine(a, b, w1, a, c, w2, out var aL, out var bL);
}
public void TestIntersectLineLineDoubleEnd()
{
var a = Vector2.one;
var b = Vector2.right;
var c = Vector2.up;
VectorMath2D.IntersectsLineLine(a + b * 2, -b, a + c * 2, -c, out var aL, out var bL);
Assert.AreEqual(2, aL);
Assert.AreEqual(2, bL);
}
public void TestIntersectLineLineCommonPointEnd()
{
var a = Vector2.one;
var b = Vector2.right;
var c = Vector2.up;
VectorMath2D.IntersectsLineLine(a + b, -b, a + c, -c, out var aL, out var bL);
Assert.AreEqual(1, aL);
Assert.AreEqual(1, bL);
}
public void TestIntersectLineLineDoubleEndWithWith(float tB, float tC, float eAL, float eBL)
{
var a = Vector2.one;
var b = Vector2.right * tB;
var c = Vector2.up * tC;
var w = 2;
VectorMath2D.IntersectsLineLine(a + b * 2, -b, w, a + c * 2, -c, w, out var aL, out var bL);
Assert.AreEqual(eAL, aL);
Assert.AreEqual(eBL, bL);
}
private static bool intersectsRays(Vector2 aP, Vector2 aD, Vector2 aT, float widthA, float lenA, Vector2 bP, Vector2 bD, Vector2 bT, float widthB, float lenB, out float offsetA, out float offsetB)
{
aP += aT * widthA;
bP += bT * widthB;
VectorMath2D.IntersectsLineLine(aP, aD, bP, bD, out offsetA, out offsetB);
//Debug.DrawLine(t3d(aP), t3d(aP + aD * offsetA, 0), Color.magenta);
//Debug.DrawLine(t3d(bP), t3d(bP + bD * offsetB, 0), Color.cyan);
if (offsetA >= 0 && offsetA <= lenA && offsetB >= 0 && offsetB <= lenB)
{
//Debug.DrawLine(t3d(aP + aD * offsetA, 0), t3d(aP + aD * offsetA, 10), Color.green);
return(true);
}
else
{
//Debug.DrawLine(t3d(aP + aD * offsetA, 0), t3d(aP + aD * offsetA, 10), Color.red);
return(false);
}
}
private static bool getOffsets(Node node, SegmentNodeConnection con1, SegmentNodeConnection con2, out float offsetA, out float offsetB)
{
var wA = con1.Segment.GetWidth();
var wB = con2.Segment.GetWidth();
var o = node.Position.GetXZ();
var dA = con1.Tangent.GetXZ();
var dB = con2.Tangent.GetXZ();
var dot = Vector3.Dot(dA, dB);
if (dot < -0.75)
{
offsetA = float.PositiveInfinity;
offsetB = float.PositiveInfinity;
return(false);
}
return(VectorMath2D.IntersectsLineLine(o, dA, wA, o, dB, wB, out offsetA, out offsetB));
}
private static bool intersectsDiscreteSegment(int slot, ILoftPath a, ILoftPath b, out float offsetA, out float offsetB, float dA, float dB, float oA, float oB, Vector2 aS, Vector2 aD, Vector2 bS, Vector2 bD)
{
//int t = Mathf.FloorToInt(Time.time % 4);
//if (t == slot)
//{
// Debug.DrawLine(t3d(aS), t3d(aS + aD), colors[t]);
// Debug.DrawLine(t3d(bS), t3d(bS + bD), Color.cyan);
//}
VectorMath2D.IntersectsLineLine(aS, aD, bS, bD, out offsetA, out offsetB);
if (offsetA >= 0 && offsetA <= 1 && offsetB >= 0 && offsetB <= 1)
{
offsetA *= dA;
offsetB *= dB;
offsetA += oA - dA;
offsetB += oB - dB;
//Debug.DrawLine(b.GetTransformedPoint(offsetB, Vector3.zero), a.GetTransformedPoint(offsetA, Vector3.zero), Color.blue);
return(true);
}
offsetB = offsetA = float.NaN;
return(false);
}
private static bool intersectEllipseCenter(ArcLoftPath a, out float offsetA, out float offsetB, Vector3 center, float radiusX, float radiusY, float bDirLen, Vector3 bStart, Vector3 bDir)
{
float nearDistance;
float farDistance;
if (VectorMath2D.IntersectsLineEllipse(t2d(bStart), t2d(bDir), Vector3.zero, radiusX, radiusY, out nearDistance, out farDistance))
{
float bestOffsetA = float.PositiveInfinity, bestOffsetB = float.PositiveInfinity;
if (checkRange(a, bDir, center, bDirLen, bStart, nearDistance, ref bestOffsetA, ref bestOffsetB))
{
offsetA = bestOffsetA;
offsetB = bestOffsetB;
return(true);
}
if (checkRange(a, bDir, center, bDirLen, bStart, farDistance, ref bestOffsetA, ref bestOffsetB))
{
offsetA = bestOffsetA;
offsetB = bestOffsetB;
return(true);
}
}
offsetA = offsetB = float.NaN;
return(false);
}
private static bool intersectsEllipseEdge(ArcLoftPath a, Vector3 bStart, Vector3 bDir, Vector3 bTan, Vector3 center, float bDirLen, float widthB, float radiusX, float radiusY, out float offsetA, out float offsetB)
{
var startLeft = bStart - bTan;
var startRight = bStart + bTan;
float outerRadiusX = radiusX + widthB;
float outerRadiusY = radiusY + widthB;
float innerRadiusX = radiusX - widthB;
float innerRadiusY = radiusY - widthB;
float nearDistanceInnerLeft, farDistanceInnerLeft;
float nearDistanceInnerRight, farDistanceInnerRight;
float nearDistanceOuterLeft, farDistanceOuterLeft;
float nearDistanceOuterRight, farDistanceOuterRight;
bool intersectsInnerLeft = VectorMath2D.IntersectsLineEllipse(t2d(startLeft), t2d(bDir), Vector3.zero, innerRadiusX, innerRadiusY, out nearDistanceInnerLeft, out farDistanceInnerLeft);
bool intersectsInnerRight = VectorMath2D.IntersectsLineEllipse(t2d(startRight), t2d(bDir), Vector3.zero, innerRadiusX, innerRadiusY, out nearDistanceInnerRight, out farDistanceInnerRight);
bool intersectsOuterLeft = VectorMath2D.IntersectsLineEllipse(t2d(startLeft), t2d(bDir), Vector3.zero, outerRadiusX, outerRadiusY, out nearDistanceOuterLeft, out farDistanceOuterLeft);
bool intersectsOuterRight = VectorMath2D.IntersectsLineEllipse(t2d(startRight), t2d(bDir), Vector3.zero, outerRadiusX, outerRadiusY, out nearDistanceOuterRight, out farDistanceOuterRight);
//if (intersectsOuterLeft)
// Debug.DrawLine(startLeft + bDir * nearDistanceOuterLeft, startLeft + bDir * farDistanceOuterLeft, Color.green);
//if (intersectsOuterRight)
// Debug.DrawLine(startRight + bDir * nearDistanceOuterRight, startRight + bDir * farDistanceOuterRight, Color.green);
//if (intersectsInnerLeft)
// Debug.DrawLine(startLeft + bDir * nearDistanceInnerLeft, startLeft + bDir * farDistanceInnerLeft, Color.yellow);
//if (intersectsInnerRight)
// Debug.DrawLine(startRight + bDir * nearDistanceInnerRight, startRight + bDir * farDistanceInnerRight, Color.yellow);
float bestOffsetA = float.PositiveInfinity, bestOffsetB = float.PositiveInfinity;
bool any = false;
if (intersectsOuterLeft)
{
any |= checkRange(a, bDir, center, bDirLen, startLeft, nearDistanceOuterLeft, ref bestOffsetA, ref bestOffsetB);
//any |= checkRange(a, bDir, center, bDirLen, startLeft, farDistanceOuterLeft, ref bestOffsetA, ref bestOffsetB);
}
if (intersectsOuterRight)
{
any |= checkRange(a, bDir, center, bDirLen, startRight, nearDistanceOuterRight, ref bestOffsetA, ref bestOffsetB);
//any |= checkRange(a, bDir, center, bDirLen, startRight, farDistanceOuterRight, ref bestOffsetA, ref bestOffsetB);
}
if (intersectsInnerLeft)
{
//any |= checkRange(a, bDir, center, bDirLen, startLeft, nearDistanceInnerLeft, ref bestOffsetA, ref bestOffsetB);
any |= checkRange(a, bDir, center, bDirLen, startLeft, farDistanceInnerLeft, ref bestOffsetA, ref bestOffsetB);
}
if (intersectsInnerRight)
{
//any |= checkRange(a, bDir, center, bDirLen, startRight, nearDistanceInnerRight, ref bestOffsetA, ref bestOffsetB);
any |= checkRange(a, bDir, center, bDirLen, startRight, farDistanceInnerRight, ref bestOffsetA, ref bestOffsetB);
}
if (any)
{
offsetA = bestOffsetA;
offsetB = bestOffsetB;
return(true);
}
offsetA = offsetB = float.NaN;
return(false);
}
private static bool test(int a, int b, ILoftPath pathA, float startA, float scaleA, float widthA, ILoftPath pathB, float startB, float scaleB, float widthB, out float offsetA, out float offsetB)
{
var sA = startA + (a - 1) * scaleA;
var eA = startA + (a) * scaleA;
var sB = startB + (b - 1) * scaleB;
var eB = startB + (b) * scaleB;
var sAM = pathA.GetTransform(sA);
var eAM = pathA.GetTransform(eA);
var sBM = pathB.GetTransform(sB);
var eBM = pathB.GetTransform(eB);
var sAP = getPoint(sAM, 3);
var eAP = getPoint(eAM, 3);
var sAT = getPoint(sAM, 0);
var eAT = getPoint(eAM, 0);
var sBP = getPoint(sBM, 3);
var eBP = getPoint(eBM, 3);
var sBT = getPoint(sBM, 0);
var eBT = getPoint(eBM, 0);
var cA = (sAP + eAP) / 2;
var cB = (sBP + eBP) / 2;
var tA = cA - cB;
var tB = -tA;
var mA = Mathf.Sign(Vector2.Dot(tA, sAT)) * widthA;
var mB = Mathf.Sign(Vector2.Dot(tB, sBT)) * widthB;
eAP -= eAT * mA;
sAP -= sAT * mA;
eBP -= eBT * mB;
sBP -= sBT * mB;
var dA = eAP - sAP;
var dB = eBP - sBP;
VectorMath2D.IntersectsLineLine(sAP, dA, sBP, dB, out offsetA, out offsetB);
//Debug.DrawLine(t3d(sAP), t3d(eAP), Color.magenta);
//Debug.DrawLine(t3d(sBP), t3d(eBP), Color.cyan);
//Debug.DrawLine(t3d(cA + sAT), t3d(cA - sAT), Color.red);
//Debug.DrawLine(t3d(cB + sBT), t3d(cB - sBT), Color.blue);
//Debug.DrawLine(t3d(cB), t3d(cB), Color.yellow);
if (offsetA >= 0 && offsetB >= 0 && offsetA <= 1 && offsetB <= 1)
{
offsetA = sA + offsetA * scaleA;
offsetB = sB + offsetB * scaleB;
//var pA = pathA.GetTransformedPoint(offsetA, Vector3.zero);
//var pB = pathB.GetTransformedPoint(offsetB, Vector3.zero);
//Debug.DrawLine(pB, t3d(sAP), Color.red);
//Debug.DrawLine(pA, t3d(sBP), Color.blue);
return(true);
}
offsetA = offsetB = float.NaN;
return(false);
}
