void Update()
{
var l0P = L0.position.xz();
var l1P = L1.position.xz();
var dir = l1P - l0P;
DebugUtil.DrawLine(l0P + 100 * dir, l0P - 100 * dir);
DebugUtil.DrawLine(S0.position.xz(), S1.position.xz());
R.position = IntersectSegSeg(l0P, l1P, S0.position.xz(), S1.position.xz()).ToXxY();
R.position += new Vector3(0, .5f, 0);
}
void Update()
{
var a = A.position.xz();
var b = B.position.xz();
DebugUtil.DrawLine(a, b);
var c = C.position.xz();
Math.ProjectSeg(a, b, c, out var r);
DebugUtil.DrawPoint((float2)r);
DebugUtil.DrawPoint(a);
DebugUtil.DrawPoint(b);
DebugUtil.DrawPoint(c);
}
void Update()
{
var a = A.position.xz();
var b = B.position.xz();
DebugUtil.DrawCircle(a, R);
DebugUtil.DrawCircle(b, R);
var r = TransformRect((a + b) / 2, new float2(2 * R, math.length(b - a)), Math.Angle(b - a));
DebugUtil.Draw(r);
DebugUtil.DrawLine(C.position.xz(), D.position.xz());
E.gameObject.SetActive(Intersect(r, C.position.xz(), D.position.xz()));
// DebugUtil.DrawCircle(r.A, .5f);
// DebugUtil.DrawCircle(r.B, .5f, Color.yellow);
// DebugUtil.DrawCircle(r.C, .5f, Color.red);
}
void Update()
{
var p1 = ((float3)A.position).xz;
var p2 = ((float3)B.position).xz;
var p3 = ((float3)C.position).xz;
DebugUtil.DrawLine(p1.ToXxY(), p2.ToXxY());
DebugUtil.DrawCircle(p3, R, StartRot, Rot);
var hits = new NativeList <float2>(Allocator.Persistent);
IntersectSegCircleSeg(p1, p2, p3, R, NormalizeAngle(StartRot), NormalizeAngle(StartRot + Rot), hits);
foreach (var hit in hits)
{
DebugUtil.DrawPoint(hit, Color.magenta);
}
hits.Dispose();
}
void Update()
{
var p0 = C0.position.xz();
var p1 = C1.position.xz();
var c = C.position.xz();
var p = P.position.xz();
DebugUtil.DrawCircle(p0, R, Color.green);
DebugUtil.DrawCircle(p1, R, Color.red);
DebugUtil.DrawCircle(c, R);
Math.GetOuterTangentRight(p0, p1, R, out var ol0, out var ol1);
DebugUtil.DrawLine(ol0, ol1, Color.black);
DebugUtil.DrawPoint(ol0, Color.green);
DebugUtil.DrawPoint(ol1, Color.red);
Math.GetOuterTangentLeft(p0, p1, R, out var or0, out var or1);
DebugUtil.DrawLine(or0, or1, Color.white);
DebugUtil.DrawPoint(or0, Color.green);
DebugUtil.DrawPoint(or1, Color.red);
Math.GetInnerTangentRight(p0, p1, R, out var il0, out var il1);
DebugUtil.DrawLine(il0, il1, Color.black);
DebugUtil.DrawPoint(il0, Color.green);
DebugUtil.DrawPoint(il1, Color.red);
Math.GetInnerTangentLeft(p0, p1, R, out var ir0, out var ir1);
DebugUtil.DrawLine(ir0, ir1, Color.white);
DebugUtil.DrawPoint(ir0, Color.green);
DebugUtil.DrawPoint(ir1, Color.red);
var left = Math.GetTangentLeft(p, c, R);
DebugUtil.DrawLine(p, left, Color.white);
var right = Math.GetTangentRight(p, c, R);
DebugUtil.DrawLine(p, right, Color.black);
}
void Update()
{
if (!_f.IsCreated)
{
Debug.Log("creating");
_f = new Funnel(64, Allocator.Persistent);
}
var portals = Portals.Take(Amount == 0 ? Portals.Length : math.min(Amount, Portals.Length)).ToArray();
foreach (var portal in portals)
{
DebugUtil.DrawCircle(portal.position, Radius);
}
for (int i = 2; i < portals.Length; i++)
{
DebugUtil.DrawLine(portals[i - 2].position, portals[i - 1].position, Color.green);
DebugUtil.DrawLine(portals[i - 2].position, portals[i].position);
DebugUtil.DrawLine(portals[i - 1].position, portals[i].position);
// if (i == 3 || i == 4)
// DebugUtil.DrawCircle(portals[i - 2].position.TakeXZ(), portals[i - 1].position.TakeXZ(), portals[i].position.TakeXZ(), i == 3 ? Color.black : Color.blue);
}
// DebugUtil.DrawLine(V0.position, Portals[0].position);
// DebugUtil.DrawLine(V0.position, Portals[1].position);
var l1 = portals.Select(t => t.position.xz()).ToList();
var start = Begin.position.xz();
var l = new System.Collections.Generic.List <Gate>();
var b = true;
for (int j = 0; j < l1.Count - 1; j++)
{
l.Add(b ? new Gate {
Left = l1[j], Right = l1[j + 1]
} : new Gate {
Left = l1[j + 1], Right = l1[j]
});
b = !b;
}
var end = End.position.xz();
var ll = new List <Gate>(64, Allocator.Persistent);
foreach (var portal in l)
{
ll.Add(portal);
}
var result = new Deque <Funnel.Node>(10, Allocator.Persistent);
_f.GetPath(ll, start, end, Radius, result);
_waypoints = result.Count;
var ra = new Funnel.Node[result.Count];
for (int i = 0; i < result.Count; i++)
{
ra[i] = result.FromFront(i);
}
_unique = ra.Distinct().Count();
_path = new StringBuilder().Concat(ra.SelectMany(r => new[] { r.From, r.To }));
for (int i = 0; i < ra.Length; ++i)
{
DebugUtil.DrawLine(ra[i].From.ToXxY(), ra[i].To.ToXxY(), Color.red);
}
ll.Dispose();
result.Dispose();
}
void Update()
{
var a = A.position.xz();
var b = B.position.xz();
var c = C.position.xz();
var s = S.position.xz();
var g = G.position.xz();
DebugUtil.DrawCircle(a, b, c, Color.black);
DebugUtil.DrawLine(A.position.xz(), B.position.xz(), Color.black);
DebugUtil.DrawLine(B.position.xz(), C.position.xz(), Color.black);
DebugUtil.DrawLine(C.position.xz(), A.position.xz(), Color.black);
DebugUtil.DrawLine(S.position.xz(), G.position.xz());
DebugUtil.DrawCircle(S.position.xz(), R);
DebugUtil.DrawCircle(G.position.xz(), R);
foreach (var edge in GetEdges())
{
DebugUtil.DrawLine(edge.Item1, edge.Item2, Color.red);
}
FindPath();
void FindPath()
{
var sg = g - s;
var perp = Math.PerpCcw(sg);
var pd = math.normalize(perp) * 2 * R;
var sp = s + perp;
var gp = g + perp;
double2 bl = default;
double2 tl = default;
double2 br = default;
double2 tr = default;
var topFound = false;
var bottomFound = false;
CheckVertex(a);
CheckVertex(b);
CheckVertex(c);
if (!bottomFound)
{
bl = IntersectTri(false, s, sp);
br = IntersectTri(false, g, gp);
}
if (!topFound)
{
tl = IntersectTri(true, s, sp);
tr = IntersectTri(true, g, gp);
}
void CheckVertex(double2 v)
{
if (GeometricPredicates.Orient2DFast(s, sp, v) <= 0 && GeometricPredicates.Orient2DFast(g, gp, v) >= 0)
{
if (GeometricPredicates.Orient2DFast(s, g, v) > 0)
{
tl = Math.ProjectLine(s, sp, v + pd);
tr = Math.ProjectLine(g, gp, v + pd);
topFound = true;
}
else
{
bl = Math.ProjectLine(s, sp, v - pd);
br = Math.ProjectLine(g, gp, v - pd);
bottomFound = true;
}
}
}
double2 IntersectTri(bool up, double2 l0, double2 l1)
{
if (IntersectLineSeg(l0, l1, a, b, out var r))
{
var orient = GeometricPredicates.Orient2DFast(s, g, r);
if ((up ? orient > 0 : orient < 0) || orient == 0 && (up ? math.dot(sg, b - a) < 0 : math.dot(sg, b - a) > 0))
{
return(up ? r + pd : r - pd);
}
}
if (IntersectLineSeg(l0, l1, b, c, out r))
{
var orient = GeometricPredicates.Orient2DFast(s, g, r);
if ((up ? orient > 0 : orient < 0) || orient == 0 && (up ? math.dot(sg, c - b) < 0 : math.dot(sg, c - b) > 0))
{
return(up ? r + pd : r - pd);
}
}
if (IntersectLineSeg(l0, l1, c, a, out r))
{
var orient = GeometricPredicates.Orient2DFast(s, g, r);
if ((up ? orient > 0 : orient < 0) || orient == 0 && (up ? math.dot(sg, a - c) < 0 : math.dot(sg, a - c) > 0))
{
return(up ? r + pd : r - pd);
}
}
throw new BreakDebuggerException();
}
// DebugUtil.DrawLine(bl, br);
// DebugUtil.DrawLine(br, tr);
// DebugUtil.DrawLine(tr, tl);
// DebugUtil.DrawLine(tl, bl);
}
}
void Update()
{
var a = ((float3)A.position).xz;
var b = ((float3)B.position).xz;
var c = ((float3)C.position).xz;
var s1 = ((float3)S1.position).xz;
var s2 = ((float3)S2.position).xz;
var v = ((float3)V.position).xz;
DebugUtil.DrawLine(s1, s2, Color.red);
Math.CircleFromPoints(a, b, c, out var p, out var r);
DebugUtil.DrawCircle((float2)p, (float)r);
var ba = a - b;
var bc = c - b;
var validBacProjection = Math.ProjectSeg(a, c, b, out var bac);
if (validBacProjection)
{
var bbac = bac - b;
var bbaca = Math.Angle(bbac);
var lba = math.length(ba);
var lbc = math.length(bc);
float2 shortestEdge;
float clearance;
if (lba <= lbc)
{
shortestEdge = ba;
clearance = lba;
}
else
{
shortestEdge = bc;
clearance = lbc;
}
var angle = Math.Angle(bbac, shortestEdge);
var startRot = bbaca + angle;
var rot = bbaca - angle - startRot;
DebugUtil.DrawCircle(b, clearance, startRot, rot);
DebugUtil.DrawCircle(b + (c - 2 * bac + a), clearance, startRot, rot);
// todo check all edges crossing traversal zones to find s1, s2
var validBsProjection = Math.ProjectSeg(s1, s2, b, out var bi);
clearance = math.min(clearance, math.length(bi - b));
if (validBsProjection)
{
var s12 = s2 - s1;
// The paper says R is delimited by a line parallel to s passing by b.
// When cl(a, b, c) == dist(b, a) and dist(b, a) < dist(b, s) this allows
// for vertices in R violating definition 3.4: dist(v, s) < cl(a, b, c).
// R will be delimited by a line parallel to s with distance cl(a, b, c)
// Technially r1 should be at the intersection point with bc, but as bc
// is enclosed in the Delaunay circle of triangle abc this is irrelevant.
// R is triangle r1, r2, c
var r1 = bi + math.normalize(b - bi) * clearance;
DebugUtil.DrawLine(r1, r1 + s12);
var ac = c - a;
var aco = Math.PerpCw(ac);
DebugUtil.DrawLine(c, c + aco);
var s12o = Math.PerpCw(s12);
DebugUtil.DrawLine(c, c + s12o);
var r2 = Math.Angle(aco, s12o) < 0
? Math.IntersectLineLine(r1, r1 + s12, c, c + aco)
: Math.IntersectLineLine(r1, r1 + s12, c, c + s12o);
DebugUtil.DrawPoint(r1, Color.magenta);
DebugUtil.DrawPoint(r2, Color.magenta);
if (Math.TriContains(r1, r2, c, v))
{
// todo check all edges crossing R to find v, e and u
// todo check v is not shared by _two_ collinear constraints
var e = c;
var u = b;
var vi = Math.ProjectLine(s1, s2, v);
var lvs = math.length(vi - v);
var lve = math.length(e - v);
if (lvs < lve)
{
Assert.IsTrue(lvs <= clearance); // , $"lvs: {lvs} <= clearance: {clearance}");
Math.CircleFromPoints(u, v, e, out var centre, out var radius);
var t = Math.IntersectLineCircle(s1, s2, centre, radius, out var x1, out var x2);
Assert.IsTrue(t == 2);
var pRef = (x1 + x2) / 2;
DebugUtil.DrawPoint(pRef, Color.magenta);
}
}
}
}
DebugUtil.DrawLine(a, b, Color.black);
DebugUtil.DrawLine(b, c, Color.black);
DebugUtil.DrawLine(c, a, Color.black);
}
public static void DrawDebugBounds(Bounds bound, Matrix4x4 mtx, Color col)
{
if (isDebug)
{
Vector3 src = Vector3.zero;
Vector3 dst = Vector3.zero;
//bottom
src.x = bound.min.x;
src.y = bound.min.y;
src.z = bound.min.z;
dst.x = bound.max.x;
dst.y = bound.min.y;
dst.z = bound.min.z;
src = mtx.MultiplyPoint(src);
dst = mtx.MultiplyPoint(dst);
DebugUtil.DrawLine(src, dst, col);
src.x = bound.min.x;
src.y = bound.min.y;
src.z = bound.min.z;
dst.x = bound.min.x;
dst.y = bound.min.y;
dst.z = bound.max.z;
src = mtx.MultiplyPoint(src);
dst = mtx.MultiplyPoint(dst);
DebugUtil.DrawLine(src, dst, col);
src.x = bound.max.x;
src.y = bound.min.y;
src.z = bound.max.z;
dst.x = bound.max.x;
dst.y = bound.min.y;
dst.z = bound.min.z;
src = mtx.MultiplyPoint(src);
dst = mtx.MultiplyPoint(dst);
DebugUtil.DrawLine(src, dst, col);
src.x = bound.max.x;
src.y = bound.min.y;
src.z = bound.max.z;
dst.x = bound.min.x;
dst.y = bound.min.y;
dst.z = bound.max.z;
src = mtx.MultiplyPoint(src);
dst = mtx.MultiplyPoint(dst);
DebugUtil.DrawLine(src, dst, col);
//top
src.x = bound.max.x;
src.y = bound.max.y;
src.z = bound.max.z;
dst.x = bound.max.x;
dst.y = bound.max.y;
dst.z = bound.min.z;
src = mtx.MultiplyPoint(src);
dst = mtx.MultiplyPoint(dst);
DebugUtil.DrawLine(src, dst, col);
src.x = bound.max.x;
src.y = bound.max.y;
src.z = bound.max.z;
dst.x = bound.min.x;
dst.y = bound.max.y;
dst.z = bound.max.z;
src = mtx.MultiplyPoint(src);
dst = mtx.MultiplyPoint(dst);
DebugUtil.DrawLine(src, dst, col);
src.x = bound.min.x;
src.y = bound.max.y;
src.z = bound.min.z;
dst.x = bound.max.x;
dst.y = bound.max.y;
dst.z = bound.min.z;
src = mtx.MultiplyPoint(src);
dst = mtx.MultiplyPoint(dst);
DebugUtil.DrawLine(src, dst, col);
src.x = bound.min.x;
src.y = bound.max.y;
src.z = bound.min.z;
dst.x = bound.min.x;
dst.y = bound.max.y;
dst.z = bound.max.z;
src = mtx.MultiplyPoint(src);
dst = mtx.MultiplyPoint(dst);
DebugUtil.DrawLine(src, dst, col);
//side
src.x = bound.min.x;
src.y = bound.min.y;
src.z = bound.min.z;
dst.x = bound.min.x;
dst.y = bound.max.y;
dst.z = bound.min.z;
src = mtx.MultiplyPoint(src);
dst = mtx.MultiplyPoint(dst);
DebugUtil.DrawLine(src, dst, col);
src.x = bound.max.x;
src.y = bound.min.y;
src.z = bound.min.z;
dst.x = bound.max.x;
dst.y = bound.max.y;
dst.z = bound.min.z;
src = mtx.MultiplyPoint(src);
dst = mtx.MultiplyPoint(dst);
DebugUtil.DrawLine(src, dst, col);
src.x = bound.max.x;
src.y = bound.max.y;
src.z = bound.max.z;
dst.x = bound.max.x;
dst.y = bound.min.y;
dst.z = bound.max.z;
src = mtx.MultiplyPoint(src);
dst = mtx.MultiplyPoint(dst);
DebugUtil.DrawLine(src, dst, col);
src.x = bound.min.x;
src.y = bound.min.y;
src.z = bound.max.z;
dst.x = bound.min.x;
dst.y = bound.max.y;
dst.z = bound.max.z;
src = mtx.MultiplyPoint(src);
dst = mtx.MultiplyPoint(dst);
DebugUtil.DrawLine(src, dst, col);
}
}
