public static void Main(string[] args)
{
// var canvas = host.CreateAndAddCanvas(0);
// var smp = SectorMetadataPresets.Blank2D;
// var punchResult = PolygonOperations.Punch()
// .Include(smp.LocalIncludedContours)
//// .Exclude(smp.LocalExcludedContours)
// .Exclude(Polygon2.CreateRect(-10000, -10000, 20000, 20000))
// .Exclude(new []{(Polygon2.CreateRect(-8000, -8000, 16000, 16000), true)})
// .Execute();
//
// canvas.Transform = Matrix4x4.CreateScale(500 / 60000.0f) * Matrix4x4.CreateTranslation(500, 300, 0);
// canvas.DrawPolyNode(punchResult);
// return;
//var subjectPolygon = Polygon2.CreateCircle(0, 0, 100, 16);
var n = 128;
var random = new Random(0);
var subjectPolygon = new Polygon2(
Util.Generate(
n,
i => DoubleVector2.FromRadiusAngle(random.Next(10, 150), -i * Math.PI * 2 / n).LossyToIntVector2())
.ToList());
var clipPolygon = Polygon2.CreateRect(-80, -80, 160, 160);
RenderSomething(0, subjectPolygon, clipPolygon);
var offsetSubjectPolygon = new Polygon2(subjectPolygon.Points.Map(p => new IntVector2(0, 240) + p).ToList());
RenderSomething(1, offsetSubjectPolygon, clipPolygon);
}
public static void DrawVisibilityPolygon(this IDebugCanvas debugCanvas, VisibilityPolygon avss, double z = 0.0, FillStyle fillStyle = null, StrokeStyle angleBoundaryStrokeStyle = null, StrokeStyle visibleWallStrokeStyle = null)
{
fillStyle = fillStyle ?? DefaultFillStyle;
var oxy = avss.Origin;
foreach (var range in avss.Get().Where(range => range.Id != VisibilityPolygon.RANGE_ID_INFINITELY_FAR && range.Id != VisibilityPolygon.RANGE_ID_INFINITESIMALLY_NEAR))
{
var rstart = DoubleVector2.FromRadiusAngle(100, range.ThetaStart);
var rend = DoubleVector2.FromRadiusAngle(100, range.ThetaEnd);
var s = range.Segment;
var s1 = s.First.ToDoubleVector2();
var s2 = s.Second.ToDoubleVector2();
DoubleVector2 visibleStart, visibleEnd;
if (!GeometryOperations.TryFindLineLineIntersection(oxy, oxy + rstart, s1, s2, out visibleStart) ||
!GeometryOperations.TryFindLineLineIntersection(oxy, oxy + rend, s1, s2, out visibleEnd))
{
continue;
}
debugCanvas.FillTriangle(oxy, visibleStart, visibleEnd, fillStyle);
debugCanvas.DrawLine(
new DoubleVector3(oxy.X, oxy.Y, z),
new DoubleVector3(visibleStart.X, visibleStart.Y, z),
angleBoundaryStrokeStyle ?? DefaultAngleBoundaryStrokeStyle);
debugCanvas.DrawLine(
new DoubleVector3(oxy.X, oxy.Y, z),
new DoubleVector3(visibleEnd.X, visibleEnd.Y, z),
angleBoundaryStrokeStyle ?? DefaultAngleBoundaryStrokeStyle);
debugCanvas.DrawLine(
new DoubleVector3(visibleStart.X, visibleStart.Y, z),
new DoubleVector3(visibleEnd.X, visibleEnd.Y, z),
visibleWallStrokeStyle ?? DefaultVisibleWallStrokeStyle);
}
}
public static void Main(string[] args)
{
Environment.CurrentDirectory = @"V:\my-repositories\miyu\derp\RoboticsMotionPlan\Assets";
//MapPolygonizerForm.Run(@"C:\Users\Warty\occ.txt", "sieg_floor3.poly", "sieg_plan.plan");
//MapPolygonizerForm.Run("gates.png", "gates.poly", "gates.plan");
MapPolygonizerForm.Run("de_dust2.png", "de_dust2.poly", "de_dust2.plan");
// MapPolygonizerForm.Run("gates.png", "gates.poly");
var(landPolys, holePolys) = FileLoader.LoadMap("gates.poly");
var tsm = new TerrainStaticMetadata {
LocalBoundary = new Rectangle(0, 0, MapWidth, MapHeight),
LocalIncludedContours = landPolys.Map(p => new Polygon2(p, true)),
LocalExcludedContours = holePolys.Map(p => new Polygon2(p.Select(x => x).Reverse().ToList(), true)),
};
var start = FileLoader.LoadPoints("start.csv").First();
var goodWaypoints = FileLoader.LoadPoints("good_waypoints.csv");
var badWaypoints = FileLoader.LoadPoints("bad_waypoints.csv");
var holeMetadata = new SphereHoleStaticMetadata {
Radius = 13
};
var gf = new GameFactory();
gf.GameCreated += (s, game) => {
var debugger = GameDebugger.AttachToWithSoftwareRendering(game);
game.TerrainService.Clear();
var snd = game.TerrainService.CreateSectorNodeDescription(tsm);
game.TerrainService.AddSectorNodeDescription(snd);
foreach (var hsm in badWaypoints)
{
var hd = game.TerrainService.CreateHoleDescription(holeMetadata);
hd.WorldTransform = Matrix4x4.CreateTranslation(hsm.X, hsm.Y, 0);
game.TerrainService.AddTemporaryHoleDescription(hd);
}
debugger.RenderHook += (_, canvas) => {
var snapshot = game.TerrainService.SnapshotCompiler.CompileSnapshot();
// uneroded network
var unerodedOverlayNetwork = snapshot.OverlayNetworkManager.CompileTerrainOverlayNetwork(0);
Trace.Assert(unerodedOverlayNetwork.TerrainNodes.Count == 1);
var unerodedTerrainNode = unerodedOverlayNetwork.TerrainNodes.First();
// eroded network
var overlayNetwork = snapshot.OverlayNetworkManager.CompileTerrainOverlayNetwork(15); //1 unit = 0.02m
Trace.Assert(overlayNetwork.TerrainNodes.Count == 1);
var terrainNode = overlayNetwork.TerrainNodes.First();
// draw uneroded map
canvas.DrawPolyNode(unerodedTerrainNode.LocalGeometryView.PunchedLand, StrokeStyle.BlackHairLineSolid, StrokeStyle.RedHairLineSolid);
// draw eroded map
canvas.DrawPolyNode(terrainNode.LocalGeometryView.PunchedLand, new StrokeStyle(Color.Gray), new StrokeStyle(Color.DarkRed));
// draw waypoints
canvas.DrawPoints(goodWaypoints, new StrokeStyle(Color.Blue, 25));
canvas.DrawPoints(badWaypoints, new StrokeStyle(Color.Red, 25));
void DrawThetaedPoint(DoubleVector2 p, double theta, bool highlight)
{
canvas.DrawPoint(p, highlight ? StrokeStyle.OrangeThick35Solid : StrokeStyle.BlackThick25Solid);
canvas.DrawLine(
p,
p + DoubleVector2.FromRadiusAngle(50, theta),
new StrokeStyle(Color.Magenta, 3));
}
var emittedPoints = new List <(DoubleVector2, double, bool)>();
// find big waypoints
var bigWaypoints = new List <(DoubleVector2, bool)>();
for (var i = 0; i < goodWaypoints.Count; i++)
{
var from = i == 0 ? start : goodWaypoints[i - 1];
var to = goodWaypoints[i];
var ok = game.PathfinderCalculator.TryFindPath(terrainNode, from, terrainNode, to, out var roadmap);
Trace.Assert(ok);
var actions = roadmap.Plan.OfType <MotionRoadmapWalkAction>().ToArray();
var waypoints = new[] { actions[0].Source.ToDoubleVector2() }
.Concat(actions.Map(a => a.Destination.ToDoubleVector2())).ToArray();
bigWaypoints.AddRange(waypoints.Map((w, ind) => (w, ind == waypoints.Length - 1)));
}
var thetas = bigWaypoints.Zip(bigWaypoints.Skip(1), (a, b) => Math.Atan2(b.Item1.Y - a.Item1.Y, b.Item1.X - a.Item1.X))
.ToArray();
var chamferSpacing = 10;
var chamferSpacingThreshold = 100;
for (var i = 0; i < thetas.Length; i++)
{
var(src, srcIsRoi) = bigWaypoints[i];
var(dst, dstIsRoi) = bigWaypoints[i + 1];
var srcToDstTheta = thetas[i];
// if close or last goal, don't chamfer
if (i + 1 == thetas.Length)
{
emittedPoints.Add((dst, srcToDstTheta, dstIsRoi));
continue;
}
var dstToFollowingTheta = thetas[i + 1];
if (src.To(dst).Norm2D() < chamferSpacingThreshold)
{
emittedPoints.Add((dst, dstToFollowingTheta, dstIsRoi));
continue;
}
var chamfer1 = dst - DoubleVector2.FromRadiusAngle(chamferSpacing, dstToFollowingTheta) - DoubleVector2.FromRadiusAngle(chamferSpacing, srcToDstTheta);
var chamfer2 = dst + DoubleVector2.FromRadiusAngle(chamferSpacing, dstToFollowingTheta) + 2 * DoubleVector2.FromRadiusAngle(chamferSpacing, srcToDstTheta);
emittedPoints.Add((chamfer1, srcToDstTheta, false));
if (dstIsRoi)
{
emittedPoints.Add((dst, srcToDstTheta, true));
}
emittedPoints.Add((chamfer2, dstToFollowingTheta, false));
}
for (var i = 0; i < emittedPoints.Count - 1; i++)
{
canvas.DrawLine(emittedPoints[i].Item1, emittedPoints[i + 1].Item1, StrokeStyle.CyanThick3Solid);
}
void PrintPoint(DoubleVector2 p) => Console.Write("(" + p.X.ToString("F3") + ", " + (MapHeight - p.Y).ToString("F3") + ")");
Console.WriteLine("[");
for (var i = 0; i < emittedPoints.Count; i++)
{
var(p, theta, isRoi) = emittedPoints[i];
DrawThetaedPoint(p, theta, isRoi);
Console.Write("(");
PrintPoint(p);
Console.Write(", ");
Console.Write((-theta).ToString("F3"));
Console.Write(", ");
Console.Write(isRoi ? "True" : "False");
Console.Write(")");
if (i + 1 != emittedPoints.Count)
{
Console.Write(", ");
}
Console.WriteLine();
}
Console.WriteLine("]");
};
};
gf.Create().Run();
}
private static MultiValueDictionary <TerrainOverlayNetworkNode, IntLineSegment2> FindVisibleCrossoverSegmentsByNeighborAndClearLocalAt(
IDebugCanvas canvas,
TerrainOverlayNetworkNode terrainNode,
VisibilityPolygon visibilityPolygon,
IntVector2 visibilityPolygonOrigin,
HashSet <TerrainOverlayNetworkNode> visited = null)
{
var visibleCrossoverSegmentsByNeighbor = MultiValueDictionary <TerrainOverlayNetworkNode, IntLineSegment2> .Create(() => new HashSet <IntLineSegment2>());
foreach (var outboundEdgeGroup in terrainNode.OutboundEdgeGroups)
{
var otherTerrainNode = outboundEdgeGroup.Key;
if (visited?.Contains(otherTerrainNode) ?? false)
{
continue;
}
foreach (var outboundEdge in outboundEdgeGroup.Value)
{
var ranges = visibilityPolygon.Get();
(IntLineSegment2, bool) FlipMaybeSorta(IntLineSegment2 x) =>
GeometryOperations.Clockness(visibilityPolygonOrigin, x.First, x.Second) == Clockness.CounterClockwise
? (new IntLineSegment2(x.Second, x.First), true)
: (x, false);
var(localCrossoverSegment, lcsFlipped) = FlipMaybeSorta(outboundEdge.EdgeJob.EdgeDescription.SourceSegment);
var(remoteCrossoverSegment, rcsFlipped) = FlipMaybeSorta(outboundEdge.EdgeJob.EdgeDescription.DestinationSegment);
// todo: clamp visibleStartT, visibleEndT to account for agent radius eroding crossover segmetmentnt
var rangeIndexIntervals = visibilityPolygon.RangeStab(localCrossoverSegment);
var locallyClearedSegments = new List <IntLineSegment2>();
foreach (var(startIndexInclusive, endIndexInclusive) in rangeIndexIntervals)
{
for (var i = startIndexInclusive; i <= endIndexInclusive; i++)
{
if (ranges[i].Id == VisibilityPolygon.RANGE_ID_INFINITELY_FAR || ranges[i].Id == VisibilityPolygon.RANGE_ID_INFINITESIMALLY_NEAR)
{
continue;
}
var seg = ranges[i].Segment;
var rstart = DoubleVector2.FromRadiusAngle(100, ranges[i].ThetaStart) * 100;
var rend = DoubleVector2.FromRadiusAngle(100, ranges[i].ThetaEnd) * 100;
double visibleStartT, visibleEndT;
if (!GeometryOperations.TryFindNonoverlappingLineLineIntersectionT(localCrossoverSegment.First.ToDoubleVector2(), localCrossoverSegment.Second.ToDoubleVector2(), visibilityPolygonOrigin.ToDoubleVector2(), visibilityPolygonOrigin.ToDoubleVector2() + rstart, out visibleStartT) ||
!GeometryOperations.TryFindNonoverlappingLineLineIntersectionT(localCrossoverSegment.First.ToDoubleVector2(), localCrossoverSegment.Second.ToDoubleVector2(), visibilityPolygonOrigin.ToDoubleVector2(), visibilityPolygonOrigin.ToDoubleVector2() + rend, out visibleEndT))
{
// wtf?
Console.WriteLine("???");
continue;
}
// Todo: I don't actually understand why visibleEndT > 1 is a thing?
// t values are for parameterization of crossover line segment, so must be within [0, 1]
if ((visibleStartT < 0 && visibleEndT < 0) || (visibleStartT > 1 && visibleEndT > 1))
{
continue;
}
visibleStartT = Math.Min(1.0, Math.Max(0.0, visibleStartT));
visibleEndT = Math.Min(1.0, Math.Max(0.0, visibleEndT));
if (visibilityPolygon.SegmentComparer.Compare(localCrossoverSegment, seg) < 0)
{
var localVisibleStart = localCrossoverSegment.PointAt(visibleStartT).LossyToIntVector2();
var localVisibleEnd = localCrossoverSegment.PointAt(visibleEndT).LossyToIntVector2();
var remoteVisibleStart = remoteCrossoverSegment.PointAt(lcsFlipped == rcsFlipped ? visibleStartT : 1.0 - visibleStartT).LossyToIntVector2();
var remoteVisibleEnd = remoteCrossoverSegment.PointAt(lcsFlipped == rcsFlipped ? visibleEndT : 1.0 - visibleEndT).LossyToIntVector2();
if (localVisibleStart == localVisibleEnd)
{
continue;
}
if (remoteVisibleStart == remoteVisibleEnd)
{
continue;
}
var locallyClearedSegment = new IntLineSegment2(localVisibleStart, localVisibleEnd);
locallyClearedSegments.Add(locallyClearedSegment);
visibleCrossoverSegmentsByNeighbor.Add(otherTerrainNode, new IntLineSegment2(remoteVisibleStart, remoteVisibleEnd));
}
}
}
foreach (var locallyClearedSegment in locallyClearedSegments)
{
visibilityPolygon.ClearBefore(locallyClearedSegment);
}
}
}
return(visibleCrossoverSegmentsByNeighbor);
}
