void SetGraphZoom()
{
if (CheckBounds() == false)
{
return;
}
var graph = GetComponent <GraphChart>();
double x1, y1;
double x2, y2;
if (graph.PointToClient(mStart, out x1, out y1))
{
if (graph.PointToClient(mPointer.ScreenPosition, out x2, out y2))
{
mIsZoomed = true;
graph.DataSource.AutomaticHorizontalView = false;
graph.DataSource.AutomaticVerticallView = false;
DoubleVector2 min = new DoubleVector2(Math.Min(x1, x2), Math.Min(y1, y2));
DoubleVector2 max = new DoubleVector2(Math.Max(x1, x2), Math.Max(y1, y2));
graph.HorizontalScrolling = min.x;
graph.VerticalScrolling = min.y;
graph.DataSource.HorizontalViewSize = max.x - min.x;
graph.DataSource.VerticalViewSize = max.y - min.y;
graph.DataSource.HorizontalViewOrigin = 0;
graph.DataSource.VerticalViewOrigin = 0;
}
}
}
public void Deconstruct(out PolyNode SourcePolyNode, out PolyNode DestinationPolyNode, out DoubleVector2 SourceCrossoverPoint, out DoubleVector2 DestinationCrossoverPoint)
{
SourcePolyNode = this.SourcePolyNode;
DestinationPolyNode = this.DestinationPolyNode;
SourceCrossoverPoint = this.SourceCrossoverPoint;
DestinationCrossoverPoint = this.DestinationCrossoverPoint;
}
public CrossoverJob(PolyNode sourcePolyNode, PolyNode destinationPolyNode, DoubleVector2 sourceCrossoverPoint, DoubleVector2 destinationCrossoverPoint)
{
SourcePolyNode = sourcePolyNode;
DestinationPolyNode = destinationPolyNode;
SourceCrossoverPoint = sourceCrossoverPoint;
DestinationCrossoverPoint = destinationCrossoverPoint;
}
public void Update(GraphChartBase graphChart)
{
if (graphChart == null || points == null || points.Count == 0)
{
return;
}
if (index >= points.Count)
{
return;
}
float leapTime = totalTime / (float)points.Count;
if (next == -1f)
{
next = leapTime * 2;
}
next += Time.deltaTime;
if (next >= leapTime)
{
int totalLeaps = (int)(next / (leapTime));
for (int i = 0; i < totalLeaps && index < points.Count; i++)
{
DoubleVector2 point = points[index];
graphChart.DataSource.AddPointToCategoryRealtime(category, point.x, point.y, leapTime);
++index;
}
next -= leapTime * totalLeaps;
}
}
public void Vector3Cast()
{
var a = new DoubleVector2(1, 2);
var b = new Vector3(1, 2, 0);
Assert.IsTrue((Vector3)a == b);
}
public void Substract()
{
var a = new DoubleVector2(1, 1);
var b = new DoubleVector2(2, 2);
var c = a - b;
Assert.AreEqual(c.x, -1);
Assert.AreEqual(c.y, -1);
}
public void Multiply()
{
var a = new DoubleVector2(2, 2);
var b = new DoubleVector2(3, 3);
var c = a * b;
Assert.AreEqual(c.x, 6);
Assert.AreEqual(c.y, 6);
}
public void Division()
{
var a = new DoubleVector2(8, 8);
var b = new DoubleVector2(2, 2);
var c = a / b;
Assert.AreEqual(c.x, 4);
Assert.AreEqual(c.y, 4);
}
public void OperatorEqual()
{
var a = new DoubleVector2(1, 2);
var b = new DoubleVector2(1, 2);
var c = new DoubleVector2(2, 3);
Assert.IsTrue(a == b);
Assert.IsFalse(a == c);
}
public void OperatorDifferent()
{
var a = new DoubleVector2(1, 2);
var b = new DoubleVector2(1, 2);
var c = new DoubleVector2(2, 3);
Assert.IsFalse(a != b);
Assert.IsTrue(a != c);
}
public void Equals()
{
var a = new DoubleVector2(1, 2);
var b = new DoubleVector2(1, 2);
var c = new DoubleVector2(2, 3);
Assert.IsTrue(a.Equals(b));
Assert.IsFalse(a.Equals(c));
}
public void Add()
{
var a = new DoubleVector2(1, 1);
var b = new DoubleVector2(2, 2);
var c = a + b;
Assert.AreEqual(c.x, 3);
Assert.AreEqual(c.y, 3);
}
private static void RenderRandomVisualizationFrames()
{
for (var i = 0; i < 100; i++)
{
var(p, segments) = RandomInput();
p = new DoubleVector2(bounds.Width / 2, bounds.Height / 2);
segments = segments.Where(s => GeometryOperations.Clockness(p.X, p.Y, s.X1, s.Y1, s.X2, s.Y2) == Clockness.Clockwise).ToArray();
RenderVisualizationFrame(p, segments);
}
}
public new void GetHashCode()
{
var a = new DoubleVector2(1, 2);
var b = new DoubleVector2(1, 2);
var c = new DoubleVector2(1, 2);
var hashSet = new HashSet <DoubleVector2> ();
hashSet.Add(a);
hashSet.Add(b);
hashSet.Add(c);
Assert.AreEqual(hashSet.Count, 1);
}
public static List <(DoubleVector2, double, bool)> LoadPlan(string path)
{
var text = File.ReadAllText(path);
var lines = text.Trim().Split(new[] { '\r', '\n' }, StringSplitOptions.RemoveEmptyEntries);
return(lines.Map(line => line.Split(',').Map(tok => tok.Trim('(', ' ', ')', ',')))
.Where(tokens => tokens[0] != "[" && tokens[0] != "]" && !tokens[0].StartsWith("//"))
.ToArray()
.Map(tokens => {
var p = new DoubleVector2(double.Parse(tokens[0]), MapHeight - double.Parse(tokens[1]) - 15);
var theta = -double.Parse(tokens[2]);
var isRoi = bool.Parse(tokens[3].ToLower());
return (p, theta, isRoi);
}).ToList());
}
public void ApplyLimits()
{
DoubleVector2 v2l = LimitedPositionMovements();
/* float dist = Vector2.Distance(pos1.position.XY() - v2l.pos1, center);
* if (dist < Vector2.Distance(pos1.position.XY(), center))
* {
* pos1.position -= v2l.pos1.XYZ(0);
* pos2.position -= v2l.pos2.XYZ(0);
* }
* else
* {
* pos1.position += v2l.pos1.XYZ(0);
* pos2.position += v2l.pos2.XYZ(0);
* }
* lastFrameLength = getChainLength();*/
}
public void AppendRealtimeWithDownSampling(double x, double y, double slideTime = 0f)
{
if (graph == null)
{
return;
}
bool show = false;
if (mData.Count == 0)
{
show = true;
}
else
{
double viewX = mData[mData.Count - 1].x;
double pageStartThreshold = currentPagePosition - mCurrentPageSizeFactor;
double pageEndThreshold = currentPagePosition + mCurrentPageSizeFactor - graph.DataSource.HorizontalViewSize;
if (viewX >= pageStartThreshold && viewX <= pageEndThreshold)
{
show = true;
}
}
var v = new DoubleVector2(x, y);
mData.Add(v);
if (show)
{
if (currentDownSampleCount >= RealtimeDownSampleCount)
{
RestartDownSampleCount();
graph.DataSource.AddPointToCategoryRealtime(Category, x, y, slideTime);
}
DownSampleSum += v.ToDoubleVector3();
currentDownSampleCount++;
var avarage = DownSampleSum * 1.0 / (double)currentDownSampleCount;
if (currentDownSampleCount != 1)
{
graph.DataSource.UpdateLastPointInCategoryRealtime(Category, avarage.x, avarage.y, slideTime);
}
}
else
{
RestartDownSampleCount();
}
}
public void Update(GraphChartBase graphChart)
{
if (graphChart == null || points == null || points.Count == 0)
{
return;
}
if (index >= points.Count)
{
return;
}
next -= Time.deltaTime;
if (next <= 0)
{
next = totalTime / (float)points.Count;
DoubleVector2 point = points[index];
graphChart.DataSource.AddPointToCategoryRealtime(category, point.x, point.y, next);
++index;
}
}
// public static bool TryFindSector(this TerrainSnapshot terrainSnapshot, IntVector3 queryWorld, out SectorSnapshot result) {
// return TryFindSector(terrainSnapshot, queryWorld.ToDoubleVector3(), out result);
// }
// public static bool TryFindSector(this TerrainSnapshot terrainSnapshot, DoubleVector3 queryWorld, out SectorSnapshot result) {
// return terrainSnapshot.SectorSnapshots.TryFindFirst(sectorSnapshot => {
// var queryLocal = sectorSnapshot.WorldToLocal(queryWorld);
// var localBoundary = sectorSnapshot.StaticMetadata.LocalBoundary;
// return localBoundary.X <= queryLocal.X && queryLocal.X <= localBoundary.Right &&
// localBoundary.Y <= queryLocal.Y && queryLocal.Y <= localBoundary.Bottom;
// }, out result);
// }
// public static bool IsInHole(this SectorSnapshotGeometryContext sectorSnapshotGeometryContext, IntVector3 query) {
// var punchedLandPolytree = sectorSnapshotGeometryContext.PunchedLand;
// punchedLandPolytree.AssertIsContourlessRootHolePunchResult();
//
// PolyNode pickedNode;
// bool isHole;
// punchedLandPolytree.PickDeepestPolynode(query.XY, out pickedNode, out isHole);
//
// return isHole;
// }
/// <summary>
/// Note: Containment definition varies by hole vs terrain: Containment for holes
/// does not include the hole edge, containment for terrain includes the terrain edge.
/// This is important, else e.g. knockback + terrain push placing an entity on an edge
/// would potentially infinite loop.
/// </summary>
public static bool FindNearestLandPointAndIsInHole(this LocalGeometryView localGeometryView, DoubleVector2 query, out DoubleVector2 nearestLandPoint)
{
var punchedLandPolytree = localGeometryView.PunchedLand;
punchedLandPolytree.AssertIsContourlessRootHolePunchResult();
PolyNode pickedNode;
bool isHole;
punchedLandPolytree.PickDeepestPolynode(query.LossyToIntVector2(), out pickedNode, out isHole);
// If query point not in a hole, nearest land point is query point
if (!isHole)
{
nearestLandPoint = query;
return(false);
}
// Else, two cases to consider: nearest point is on an island inside this hole, alternatively
// and (only if the hole has a contour), nearest point is on the hole contour.
nearestLandPoint = DoubleVector2.Zero;
double bestDistance = double.PositiveInfinity;
if (pickedNode.Contour.Any())
{
// the hole has a contour; that is, it's a hole inside of a landmass
var result = GeometryOperations.FindNearestPointOnContour(pickedNode.Contour, query);
bestDistance = result.Distance;
nearestLandPoint = result.NearestPoint;
}
foreach (var childLandNode in pickedNode.Childs)
{
var result = GeometryOperations.FindNearestPointOnContour(childLandNode.Contour, query);
if (result.Distance < bestDistance)
{
bestDistance = result.Distance;
nearestLandPoint = result.NearestPoint;
}
}
return(true);
}
public bool TryIntersect(ref IntLineSegment2 segment, out DoubleVector2 p)
{
if (!Bounds.ContainsOrIntersects(ref segment))
{
p = default(DoubleVector2);
return(false);
}
if (First != null)
{
return(First.TryIntersect(ref segment, out p) || Second.TryIntersect(ref segment, out p));
}
for (var i = SegmentsStartIndexInclusive; i < SegmentsEndIndexExclusive; i++)
{
if (GeometryOperations.TryFindSegmentSegmentIntersection(ref Segments[i], ref segment, out p))
{
return(true);
}
}
p = default(DoubleVector2);
return(false);
}
private static void RenderVisualizationFrame(DoubleVector2 p, IntLineSegment2[] segments, int eventLimit = -1)
{
var canvas = host.CreateAndAddCanvas(frameCounter++);
var vp = VisibilityPolygon.Create(p, segments, eventLimit);
vp = new VisibilityPolygon(p);
foreach (var seg in segments)
{
vp.Insert(seg);
}
// vp.Insert(new IntLineSegment2(p.LossyToIntVector2() + new IntVector2(50, -20), p.LossyToIntVector2() + new IntVector2(50, 20)));
canvas.BatchDraw(() => {
canvas.DrawVisibilityPolygon(vp, 0, new FillStyle(Color.FromArgb(120, Color.Cyan)));
foreach (var s in segments)
{
canvas.DrawLine(s.First, s.Second, StrokeStyle.BlackHairLineSolid);
}
canvas.DrawPoint(p, StrokeStyle.RedThick5Solid);
});
}
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 bool TryIntersect(IntLineSegment2 segment, out DoubleVector2 p)
{
return(TryIntersect(ref segment, out p));
}
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();
}
public static DoubleVector3 LocalToWorld(this SectorNodeDescription snd, DoubleVector2 local)
{
return(Vector3.Transform(new Vector3((float)local.X, (float)local.Y, 0), snd.WorldTransform).ToOpenMobaVector());
}
private static DoubleVector3 ToDV3(DoubleVector2 p) => new DoubleVector3(p);
public static void DrawLine(this IDebugCanvas canvas, DoubleVector2 p1, DoubleVector2 p2, StrokeStyle strokeStyle)
{
canvas.DrawLine(ToDV3(p1), ToDV3(p2), strokeStyle);
}
public virtual void ConfigureTransformForAnnotationObject(MapView sender, IAnnotation annotation, DoubleVector2 offset, Transform transform)
{
// The map view needs to know where the annotation lives on the texture.
// We used a plane for the texture, which has dimensions of 10x10.
// The offset in the call tells us where the annotation sits on the texture
// surface, from extents 0,0 to 1,1. We need to convert these values to
// a location on the plane itself.
Vector3 pos =
(float)offset.X * MapX - (MapX / 2) +
(IsAnnotationSelected(annotation) ? SelectedAnnotationElevation : DefaultAnnotationElevation) +
(float)offset.Y * MapY - (MapY / 2);
//new
//Vector3((float)offset.X * 10.0f - 5f, dy, (float)offset.Y * 10.0f - 5f);
transform.localPosition = pos;
}
public bool NN(TriangulationIsland island, DoubleVector2 destination, out (int, int[]) res)
public static void FillTriangle(this IDebugCanvas canvas, DoubleVector2 p1, DoubleVector2 p2, DoubleVector2 p3, FillStyle fillStyle)
{
canvas.FillTriangle(ToDV3(p1), ToDV3(p2), ToDV3(p3), fillStyle);
}
