internal static TerrainStaticMetadata Twitch(this TerrainStaticMetadata tsm)
{
var offsetX = -(tsm.LocalBoundary.X + tsm.LocalBoundary.Width / 2);
var offsetY = -(tsm.LocalBoundary.Y + tsm.LocalBoundary.Height / 2);
var scaleX = 2 * DesiredSectorExtents / (double)tsm.LocalBoundary.Width;
var scaleY = 2 * DesiredSectorExtents / (double)tsm.LocalBoundary.Height;
void TransformPolygonInPlace(Polygon2 poly)
{
for (var i = 0; i < poly.Points.Count; i++)
{
var p = poly.Points[i];
var x = (int)((p.X + offsetX) * scaleX);
var y = (int)((p.Y + offsetY) * scaleY);
poly.Points[i] = new IntVector2(x, y);
}
}
tsm.LocalBoundary.Offset(offsetX, offsetY);
tsm.LocalBoundary = new Rectangle(
(int)(tsm.LocalBoundary.X * scaleX), (int)(tsm.LocalBoundary.Y * scaleY),
(int)(tsm.LocalBoundary.Width * scaleX), (int)(tsm.LocalBoundary.Height * scaleY));
foreach (var contour in tsm.LocalIncludedContours)
{
TransformPolygonInPlace(contour);
}
foreach (var contour in tsm.LocalExcludedContours)
{
TransformPolygonInPlace(contour);
}
return(tsm);
}
public static void Main(string[] args)
{
var sectorMetadataPresets = SectorMetadataPresets.HashCircle2;
var terrainStaticMetadata = new TerrainStaticMetadata {
LocalBoundary = sectorMetadataPresets.LocalBoundary,
LocalIncludedContours = sectorMetadataPresets.LocalIncludedContours,
LocalExcludedContours = sectorMetadataPresets.LocalExcludedContours
};
var(localGeometryView, landPolyNode, crossoverPointManager) = BenchmarkAddCrossoverPoints(terrainStaticMetadata);
var canvas = host.CreateAndAddCanvas(0);
canvas.Transform = Matrix4x4.CreateScale(1000 / 60000.0f) * Matrix4x4.CreateTranslation(500, 500, 0);
canvas.DrawPolyNode((PolyTree)landPolyNode.Parent);
canvas.DrawVisibilityGraph(landPolyNode.ComputeVisibilityGraph());
canvas.DrawLineList(landPolyNode.FindContourAndChildHoleBarriers(), StrokeStyle.BlackHairLineSolid);
canvas.DrawPoints(crossoverPointManager.CrossoverPoints, StrokeStyle.RedThick5Solid);
// var a = landPolyNode.FindAggregateContourCrossoverWaypoints()[6];
// var b = landPolyNode.FindAggregateContourCrossoverWaypoints()[13];
// var q = new IntLineSegment2(a, b);
// canvas.DrawPoint(a, StrokeStyle.RedThick5Solid);
// canvas.DrawPoint(b, StrokeStyle.RedThick5Solid);
// var bvh = landPolyNode.FindContourAndChildHoleBarriersBvh();
// canvas.DrawBvh(bvh);
// foreach (var (i, val) in bvh.BoundingBoxes.Enumerate()) {
// if (val.Intersects(q)) Console.WriteLine(i + " " + val);
// }
// var intersects = bvh.Intersects(q);
// canvas.DrawLine(a, b, intersects ? StrokeStyle.RedHairLineSolid : StrokeStyle.LimeHairLineSolid);
PolyNodeCrossoverPointManager.DumpPerformanceCounters();
while (true)
{
const int ntrials = 10;
var sw = new Stopwatch();
sw.Start();
for (var i = 0; i < ntrials; i++)
{
BenchmarkAddCrossoverPoints(terrainStaticMetadata);
}
Console.WriteLine($"{ntrials} trials in {sw.ElapsedMilliseconds} ms");
}
}
private LocalGeometryView BuildLgv(TerrainStaticMetadata mapStaticMetadata, double holeDilationRadius, params IHoleStaticMetadata[] holeMetadatas)
{
var store = new SectorGraphDescriptionStore();
var terrainService = new TerrainService(store, new TerrainSnapshotCompiler(store));
var sector = terrainService.CreateSectorNodeDescription(mapStaticMetadata);
sector.WorldTransform = Matrix4x4.Multiply(Matrix4x4.CreateScale(1), Matrix4x4.CreateTranslation(-500, -500, 0));
terrainService.AddSectorNodeDescription(sector);
var left2 = new IntLineSegment2(new IntVector2(0, 600), new IntVector2(0, 800));
terrainService.AddSectorEdgeDescription(PortalSectorEdgeDescription.Build(sector, sector, left2, left2));
foreach (var holeMetadata in holeMetadatas)
{
var terrainHole = terrainService.CreateHoleDescription(holeMetadata);
terrainService.AddTemporaryHoleDescription(terrainHole);
}
var terrainOverlayNetwork = terrainService.CompileSnapshot().OverlayNetworkManager.CompileTerrainOverlayNetwork(holeDilationRadius);
return(terrainOverlayNetwork.TerrainNodes.First().LocalGeometryView);
}
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 (LocalGeometryView, PolyNode, PolyNodeCrossoverPointManager) BenchmarkAddCrossoverPoints(TerrainStaticMetadata terrainStaticMetadata)
{
/*
* var left1 = new IntLineSegment2(new IntVector2(-30000, -18000), new IntVector2(-30000, -6000));
* var left2 = new IntLineSegment2(new IntVector2(-30000, 6000), new IntVector2(-30000, 18000));
* var right1 = new IntLineSegment2(new IntVector2(30000, -18000), new IntVector2(30000, -6000));
* var right2 = new IntLineSegment2(new IntVector2(30000, 6000), new IntVector2(30000, 18000));*/
var crossoverSegments = new[] {
