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); }