public NodeAIPath(SegmentAIPath source, SegmentAIPath destination, ILoftPath path)
{
this.source = source;
this.destination = destination;
this.path = path;
this.agents = new LinkedAgentChain();
}
public static bool Intersects(ILoftPath a, float widthA, ILoftPath b, float widthB, IntersectionMode mode, out float offsetA, out float offsetB)
{
if (a is LinearPath && b is LinearPath)
{
return(intersectsLinears(a as LinearPath, widthA, b as LinearPath, widthB, mode, out offsetA, out offsetB));
}
if (a is BiArcLoftPath && b is BiArcLoftPath)
{
return(intersectsBiArcs(a as BiArcLoftPath, widthA, b as BiArcLoftPath, widthB, mode, out offsetA, out offsetB));
}
//TODO : arc - arc
//if (a is ArcLoftPath && b is ArcLoftPath)
// return intersectsArcArc(a as ArcLoftPath, widthA, b as ArcLoftPath, widthB, mode, out offsetA, out offsetB);
//TODO : arc - linear
//if (a is ArcLoftPath && b is LinearPath)
// return intersectsArcLinear(a as ArcLoftPath, widthA, b as LinearPath, widthB, mode, out offsetA, out offsetB);
//TODO : arc - linear
//if (a is LinearPath && b is ArcLoftPath)
// return intersectsArcLinear(b as ArcLoftPath, widthB, a as LinearPath, widthA, mode, out offsetB, out offsetA);
//if (intersectsDiscrete(a, widthA, b, widthB, mode, out offsetA, out offsetB))
// return true;
//Debug.LogWarningFormat("combination {0}-{1} not supported ", a.GetType().FullName, b.GetType().FullName);
offsetB = offsetA = float.NaN;
return(false);
}
public static float DistanceTo(this ILoftPath path, Vector3 point)
{
float distance;
Vector3 snap;
path.SnapTo(point, out snap, out distance);
return((point - snap).magnitude);
}
public static bool Sweep(ILoftPath pathA, float widthA, bool reverseA, ILoftPath pathB, float widthB, bool reverseB, out float offsetA, out float offsetB)
{
var lenA = pathA.Length;
var startA = reverseA ? lenA : 0;
var endA = reverseA ? 0 : lenA;
var lenB = pathB.Length;
var startB = reverseB ? lenB : 0;
var endB = reverseB ? 0 : lenB;
return(Sweep(pathA, startA, endA, widthA, pathB, startB, endB, widthB, out offsetA, out offsetB));
}
private static bool Sweep(ILoftPath pathA, float startA, float endA, float widthA, ILoftPath pathB, float startB, float endB, float widthB, out float offsetA, out float offsetB)
{
var distA = endA - startA;
var distB = endB - startB;
var lenA = Mathf.Abs(distA);
var lenB = Mathf.Abs(distB);
var max = Mathf.Max(lenA, lenB);
var min = Mathf.Min(lenA, lenB);
var scaleA = Mathf.Sign(distA);
var scaleB = Mathf.Sign(distB);
for (int x = 1; x < max + 1; x++)
{
if (x < lenB)
{
for (int i = 1; i <= Mathf.Min(lenA + 1, x + 1); i++)
{
if (test(i, x, pathA, startA, scaleA, widthA, pathB, startB, scaleB, widthB, out offsetA, out offsetB))
{
return(true);
}
}
}
if (x < lenA)
{
for (int j = 1; j < Mathf.Min(lenB + 1, x + 1); j++)
{
if (test(x, j, pathA, startA, scaleA, widthA, pathB, startB, scaleB, widthB, out offsetA, out offsetB))
{
return(true);
}
}
}
if (x < min)
{
if (test(x, x, pathA, startA, scaleA, widthA, pathB, startB, scaleB, widthB, out offsetA, out offsetB))
{
return(true);
}
}
}
offsetA = float.NaN;
offsetB = float.NaN;
return(false);
}
private void IsValid(ILoftPath value)
{
Assert.IsTrue(value.Length > 0, "length > 0");
Assert.IsFalse(float.IsNaN(value.Length), "length is NaN");
Assert.IsFalse(float.IsInfinity(value.Length), "length is Infinity");
if (value is ArcLoftPath)
{
IsValid(value as ArcLoftPath);
}
if (value is BiArcLoftPath)
{
IsValid(value as BiArcLoftPath);
}
}
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 void InvalidateLoftPath() => loftPath = null;
public BiArcLoftPath(Vector3 start, Vector3 tangentStart, Vector3 end, Vector3 tangentEnd)
{
#if DEBUG
if (float.IsNaN(start.x))
{
throw new InvalidOperationException("start.x is not a number");
}
if (float.IsNaN(start.y))
{
throw new InvalidOperationException("start.y is not a number");
}
if (float.IsNaN(start.z))
{
throw new InvalidOperationException("start.z is not a number");
}
if (float.IsNaN(end.x))
{
throw new InvalidOperationException("end.x is not a number");
}
if (float.IsNaN(end.y))
{
throw new InvalidOperationException("end.y is not a number");
}
if (float.IsNaN(end.z))
{
throw new InvalidOperationException("end.z is not a number");
}
if (float.IsNaN(tangentStart.x))
{
throw new InvalidOperationException("tangentStart.x is not a number");
}
if (float.IsNaN(tangentStart.y))
{
throw new InvalidOperationException("tangentStart.y is not a number");
}
if (float.IsNaN(tangentStart.z))
{
throw new InvalidOperationException("tangentStart.z is not a number");
}
if (float.IsNaN(tangentEnd.x))
{
throw new InvalidOperationException("tangentEnd.x is not a number");
}
if (float.IsNaN(tangentEnd.y))
{
throw new InvalidOperationException("tangentEnd.y is not a number");
}
if (float.IsNaN(tangentEnd.z))
{
throw new InvalidOperationException("tangentEnd.z is not a number");
}
var distSqr = (start - end).sqrMagnitude;
if (distSqr < 0.001)
{
throw new InvalidOperationException("Zero Length");
}
#endif
tangentStart.Normalize();
tangentEnd.Normalize();
var pointStart = start + tangentStart;
var pointEnd = end + tangentEnd;
var c1 = (start + end) / 2;
var c2 = (pointStart + pointEnd) / 2;
var n1 = end - start;
var n2 = pointEnd - pointStart;
n1.Normalize();
n2.Normalize();
//Debug.DrawLine(pointStart, pointEnd);
var d1 = Vector3.Dot(n1, c1);
var d2 = Vector3.Dot(n2, c2);
//DebugUtils.DrawPlane(c1, n1, .01f, Color.red);
//DebugUtils.DrawPlane(c2, n2, .01f, Color.red);
//Debug.DrawLine(Vector3.zero, n1 * d1);
//Debug.DrawLine(Vector3.zero, n2 * d2);
Ray r12;
if (intersect(n1, d1, n2, d2, out r12))
{
//Debug.DrawLine(r12.origin, r12.GetPoint(10), Color.blue);
var r = Vector3.Dot(start - r12.origin, r12.direction);
var center1 = r12.GetPoint(r);
//Debug.DrawLine(center1, start, Color.yellow);
//Debug.DrawLine(center1, end, Color.yellow);
#if DEBUG
if (float.IsNaN(center1.x))
{
throw new InvalidOperationException("center1.x is not a number");
}
if (float.IsNaN(center1.y))
{
throw new InvalidOperationException("center1.y is not a number");
}
if (float.IsNaN(center1.z))
{
throw new InvalidOperationException("center1.z is not a number");
}
#endif
var rH = (center1 - start).magnitude;
var rC = (center1 - c1).magnitude;
#if DEBUG
if (float.IsNaN(rH))
{
throw new InvalidOperationException("rH is not a number");
}
if (float.IsNaN(rC))
{
throw new InvalidOperationException("rC is not a number");
}
#endif
Vector3 pointH1, pointH2;
if (Mathf.Abs(rC) > 0.00001f)
{
var lCH = (c1 - center1) / rC * rH;
//DebugUtils.DrawCircle(center1, rH, r12.direction, Color.yellow);
#if DEBUG
if (float.IsNaN(lCH.x))
{
throw new InvalidOperationException("lCH.x is not a number");
}
if (float.IsNaN(lCH.y))
{
throw new InvalidOperationException("lCH.y is not a number");
}
if (float.IsNaN(lCH.z))
{
throw new InvalidOperationException("lCH.z is not a number");
}
#endif
pointH2 = center1 + lCH;
pointH1 = center1 - lCH;
}
else
{
pointH1 = pointH2 = center1 - tangentStart * (start - end).magnitude / 2;
}
//Debug.DrawLine(pointH1, pointH2, Color.green);
//arc1 = getArc(start, tangentStart, pointH1, true);
//arc2 = getArc(end, tangentEnd, pointH1, false);
var arc1start = getArc(start, tangentStart, pointH1, true);
var arc2start = getArc(start, tangentStart, pointH2, true);
var arc1end = getArc(end, tangentEnd, pointH1, false);
var arc2end = getArc(end, tangentEnd, pointH2, false);
if (arc1start.Length + arc1end.Length < arc2start.Length + arc2end.Length)
{
arc1 = arc1start;
arc2 = arc1end;
}
else
{
arc1 = arc2start;
arc2 = arc2end;
}
return;
}
// TODO: possible anomaly: both tangents are identical: perpendicular planes are parallel
var center = (start + end) / 2;
arc1 = getArc(start, tangentStart, center, true);
arc2 = getArc(end, tangentEnd, center, false);
}
private static bool intersectsDiscrete(ILoftPath a, float widthA, ILoftPath b, float widthB, IntersectionMode mode, out float offsetA, out float offsetB)
{
widthA /= 2;
widthB /= 2;
// TODO: optimize!
var lenA = a.Length;
var lenB = b.Length;
var countA = Mathf.CeilToInt(lenA) / 4;
var countB = Mathf.CeilToInt(lenB) / 4;
var dA = lenA / countA;
var dB = lenB / countB;
var lTA = a.GetTransform(0);
for (int iA = 1; iA <= countA; iA++)
{
var oA = iA * lenA / countA;
var tA = a.GetTransform(oA);
var aLP = t2d(lTA.GetColumn(3));
var aLT = t2d(lTA.GetColumn(0)) * widthA;
var aCP = t2d(tA.GetColumn(3));
var aCT = t2d(tA.GetColumn(0)) * widthA;
var aD = aCP - aLP;
lTA = tA;
var lTB = b.GetTransform(0);
for (int iB = 1; iB <= countB; iB++)
{
var oB = iB * lenB / countB;
var tB = b.GetTransform(oB);
var bLP = t2d(lTB.GetColumn(3));
var bLT = t2d(lTB.GetColumn(0)) * widthB;
var bCP = t2d(tB.GetColumn(3));
var bCT = t2d(tB.GetColumn(0)) * widthB;
var bD = bCP - bLP;
lTB = tB;
//Debug.DrawLine(t3d(bLP), t3d(bCP), Color.white);
//Debug.DrawLine(t3d(aLP), t3d(aCP), Color.white);
//Debug.DrawLine(t3d(bLP + bLT * signB), t3d(bCP + bCT * signB), Color.cyan);
//Debug.DrawLine(t3d(aLP + aLT * signA), t3d(aCP + aCT * signA), Color.cyan);
//Debug.DrawLine(t3d(bLP - bLT * signB), t3d(bCP - bCT * signB), Color.magenta);
//Debug.DrawLine(t3d(aLP - aLT * signA), t3d(aCP - aCT * signA), Color.magenta);
var aS1 = aLP + aLT;
var bS1 = bLP + bLT;
var aS2 = aLP - aLT;
var bS2 = bLP - bLT;
var aD1 = (aCP + aCT) - aS1;
var bD1 = (bCP + bCT) - bS1;
var aD2 = (aCP - aCT) - aS2;
var bD2 = (bCP - bCT) - bS2;
//Debug.DrawLine(t3d(aS1, iA), t3d(aS1 + aD1, iA), Color.cyan);
//Debug.DrawLine(t3d(bS1, iA), t3d(bS1 + bD1, iA), Color.cyan);
//Debug.DrawLine(t3d(aS2, iA), t3d(aS2 + aD2, iA), Color.magenta);
//Debug.DrawLine(t3d(bS2, iA), t3d(bS2 + bD2, iA), Color.magenta);
if (mode == IntersectionMode.Edge)
{
if (intersectsDiscreteSegment(0, a, b, out offsetA, out offsetB, dA, dB, oA, oB, aS1, aD1, bS1, bD1))
{
return(true);
}
if (intersectsDiscreteSegment(1, a, b, out offsetA, out offsetB, dA, dB, oA, oB, aS2, aD2, bS2, bD2))
{
return(true);
}
if (intersectsDiscreteSegment(2, a, b, out offsetA, out offsetB, dA, dB, oA, oB, aS1, aD1, bS2, bD2))
{
return(true);
}
if (intersectsDiscreteSegment(3, a, b, out offsetA, out offsetB, dA, dB, oA, oB, aS2, aD2, bS1, bD1))
{
return(true);
}
}
else
{
if (intersectsDiscreteSegment(0, a, b, out offsetA, out offsetB, dA, dB, oA, oB, aLP, bLP, aCP - aLP, bCP - bLP))
{
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);
}
public static Matrix4x4 GetTransform(this ILoftPath loft, float distance, Matrix4x4 baseTransform)
{
return(baseTransform * loft.GetTransform(distance));
}
public static Vector3 GetTransformedPoint(this ILoftPath loft, float distance, Vector3 input)
{
var m = loft.GetTransform(distance);
return(m.MultiplyPoint3x4(input));
}