public Parabola2D(Point2D focus, Line2D directrix)
: base(ShapeType2D.ParametricCurve)
{
m_focus = focus;
m_directrix = directrix;
m_tOffset = m_directrix.NearestT(m_focus);
}
public Triangle2D(Point2D a, Point2D b, Point2D c)
: base(ShapeType2D.Triangle)
{
m_a = a;
m_b = b;
m_c = c;
m_eab = new Line2D(a, b);
m_ebc = new Line2D(b, c);
m_eca = new Line2D(c, a);
}
public Shape2D FindLineIntersection(Line2D line)
{
// line-parabola intersection will always have two points, unless we have the degenerate
// case where the line fails to reach the parabola or passes through the vertex and focus.
// we're looking for points equidistant from the directrix and focus which line contains.
// There should be one or two circles along the line (with radius from line to focus/directrix)
// which fit our criteria
// line: pt = p0 + v0 * t, xt = x0 + vx * t, yt = y0 + vy * t
// solve for where distance[linePoint, focus] == distance[linePoint, directrix]
//
// if distance[linePoint, focus] == distance[linePoint, directrix],
// then distance[linePoint, focus]^2 == distance[linePoint, directrix]^2,
// then distance[linePoint, focus]^2 - distance[linePoint, directrix]^2 == 0
//
// distance[linePoint, focus] = Sqrt[(xt - focusX)^2 + (yt - focusY)^2]
// = Sqrt[(x0 + vx * t - focusX)^2 + (y0 + vy * t - focusY)^2]
//
// distance[linePoint, focus]^2 = (x0 + vx * t - focusX)^2 + (y0 + vy * t - focusY)^2
// = (vx * t + (x0 - focusX))^2 + (vy * t + y0 - focusY)^2
// = t^2 * vx^2 + t * 2 * vx * (x0 - focusX) + (x0 - focusX)^2 +
// t^2 * vy^2 + t * 2 * vy * (y0 - focusY) + (y0 - focusY)^2
// = t^2 (vx^2 + vy^2) + 2t (vx(x0 - focusX) + vy(y0 - focusY)) + ((x0 - focusX)^2 + (y0 - focusY)^2)
//
// let dv = directrixPoint1 - directrixPoint2 = dp1 - dp2
// let dvPerp = dv.perp();
// let r = dp1 - pt
//
// distance[linePoint, directrix] = |Project r onto dvPerpHat|
// = |r dot dvPerpHat|
// = |r dot dvPerp| / |dvPerp|
// = (rx * dvpx + ry * dvpy) / sqrt(dvpx^2 + dvpy^2)
// = ((dp1x - ptx) * dvpx + (dp1y - pty) * dvpy) / Sqrt[dvpx * dvpx + dvpy * dvpy]
// = ((dp1x - (x0 + vx * t)) * dvpx + (dp1y - (y0 + vy * t)) * dvpy) / Sqrt[dvpx * dvpx + dvpy * dvpy]
// = (dp1x * dvpx + dp1y * dvpy - dvpx * t * vx - dvpy * t * vy - dvpx * x0 - dvpy * y0) / Sqrt[dvpx * dvpx + dvpy * dvpy]
// = (dp1x * dvpx + dp1y * dvpy - dvpx * x0 - dvpy * y0 - dvpx * t * vx - dvpy * t * vy) / Sqrt[dvpx * dvpx + dvpy * dvpy]
// = ((dp1x * dvpx + dp1y * dvpy - dvpx * x0 - dvpy * y0) - dvpx * t * vx - dvpy * t * vy) / Sqrt[dvpx * dvpx + dvpy * dvpy]
// = ((dp1x * dvpx + dp1y * dvpy - dvpx * x0 - dvpy * y0) + (-t)(dvpx * vx + dvpy * vy)) / Sqrt[dvpx * dvpx + dvpy * dvpy]
//
// distance[linePoint, directrix]^2 = the above squared.
// = dp1x^2 dvpx^2 + 2 dp1x dp1y dvpx dvpy + dp1y^2 dvpy^2 - 2 dp1x dvpx^2 t vx - 2 dp1y dvpx dvpy t vx + dvpx^2 t^2 vx^2 - 2 dp1x dvpx dvpy t vy - 2 dp1y dvpy^2 t vy + 2 dvpx dvpy t^2 vx vy + dvpy^2 t^2 vy^2 - 2 dp1x dvpx^2 x0 - 2 dp1y dvpx dvpy x0 + 2 dvpx^2 t vx x0 + 2 dvpx dvpy t vy x0 + dvpx^2 x0^2 - 2 dp1x dvpx dvpy y0 - 2 dp1y dvpy^2 y0 + 2 dvpx dvpy t vx y0 + 2 dvpy^2 t vy y0 + 2 dvpx dvpy x0 y0 + dvpy^2 y0^2
//
// back to distance[linePoint, focus]^2 - distance[linePoint, directrix]^2 == 0, we basically have a quadratic equation.
// a = (vx^2 - dvpx^2 vx^2 - 2 dvpx dvpy vx vy + vy^2 - dvpy^2 vy^2)
// b = (2 dp1x dvpx^2 vx + 2 dp1y dvpx dvpy vx + 2 dp1x dvpx dvpy vy + 2 dp1y dvpy^2 vy - 2 dvpx^2 vx x0 - 2 dvpx dvpy vy x0 - 2 dvpx dvpy vx y0 - 2 dvpy^2 vy y0 + 2 (vx (-focusX + x0) + vy (-focusY + y0)))
// c = -dp1x^2 dvpx^2 - 2 dp1x dp1y dvpx dvpy - dp1y^2 dvpy^2 + 2 dp1x dvpx^2 x0 + 2 dp1y dvpx dvpy x0 - dvpx^2 x0^2 + (-focusX + x0)^2 + 2 dp1x dvpx dvpy y0 + 2 dp1y dvpy^2 y0 - 2 dvpx dvpy x0 y0 - dvpy^2 y0^2 + (-focusY + y0)^2
// at^2 + bt + c == 0... t = (-b +- Sqrt[b^2 - 4ac)) / 2a
var p0 = line.Start;
var x0 = p0.X;
var y0 = p0.Y;
var v = line.Vector;
var vx = v.X;
var vy = v.Y;
var dp1 = m_directrix.Start;
var dp2 = m_directrix.PointAtT(1.0);
var dp1x = dp1.X;
var dp1y = dp1.Y;
var dv = dp1 - dp2;
var dvp = dv.Perp();
var dvpx = dvp.X;
var dvpy = dvp.Y;
var focusX = m_focus.X;
var focusY = m_focus.Y;
var a = vx * vx - dvpx * dvpx * vx * vx - 2.0 * dvpx * dvpy * vx * vy + vy * vy - dvpy * dvpy * vy * vy;
var b = 2.0 * dp1x * dvpx * dvpx * vx + 2.0 * dp1y * dvpx * dvpy * vx + 2.0 * dp1x * dvpx * dvpy * vy + 2.0 * dp1y * dvpy * dvpy * vy - 2.0 * dvpx * dvpx * vx * x0 - 2.0 * dvpx * dvpy * vy * x0 - 2.0 * dvpx * dvpy * vx * y0 - 2.0 * dvpy * dvpy * vy * y0 + 2.0 * (vx * (-focusX + x0) + vy * (-focusY + y0));
var c = -dp1x * dp1x * dvpx * dvpx - 2.0 * dp1x * dp1y * dvpx * dvpy - dp1y * dp1y * dvpy * dvpy + 2.0 * dp1x * dvpx * dvpx * x0 + 2.0 * dp1y * dvpx * dvpy * x0 - dvpx * dvpx * x0 * x0 + (-focusX + x0) * (-focusX + x0) + 2 * dp1x * dvpx * dvpy * y0 + 2.0 * dp1y * dvpy * dvpy * y0 - 2.0 * dvpx * dvpy * x0 * y0 - dvpy * dvpy * y0 * y0 + (-focusY + y0) * (-focusY + y0);
var discriminant = b * b - 4.0 * a * c;
var tCenter = -b / (2 * a);
if (discriminant < 0)
return null;
else if (discriminant.Within(0, kContainsEpsilon))
return PointAtT(tCenter);
else
{
var tOffset = Math.Sqrt(discriminant);
return new PointCollection2D(PointAtT(tCenter - tOffset), PointAtT(tCenter + tOffset));
}
}
public Point2D PointAtT(double t)
{
t = t + m_tOffset;
// Form perpendicular bisector of directrix at t
var offset = m_directrix.OffsetToT(t);
var perpBisector = new Line2D(m_directrix.Start + offset, m_directrix.Vector.Perp());
var offsetMagnitude = offset.Magnitude();
// Find point on perp bisector which is equidistant from directrix and site.
// directrix distance squared = (vx * t)^2 + (vy * t)^2
// the point is at p0 + v * t, x = x0 + vx * t, y = y0 + vy * t
// focus distance squared = (x - focusX)^2 + (y - focusY)^2
// = (x0 + vx * t - focusX)^2 + (y0 + vy * t - focusY)^2
// = (vx * t + (x0 - focusX))^2 + (vy * t + (y0 - focusY))^2
// let x0f = x0 - focusX, y0f = y0 - focusY
// = (vx * t + x0f)^2 + (vy * t + y0f)^2
// = vx^2 * t^2 + 2 * vx * t * x0f + x0f^2 + vy^2 * t^2 + 2 * vy * t * y0f + y0f^2
// = (vx^2 + vy^2) * t^2 + (2 * vx * x0f + 2 * vy * y0f) * t + (x0f^2 + y0f^2)
// Equate these two (with t being the only variable) to solve equation...
// todo: not fully implemented!
// Find two points on perp bisector at the same distance from point as offset...
// we want the point with the lowest t, as that is closest to the directrix.
// this is a simple line-circle intersection.
// circle: r^2 = (X-focusX)^2 + (Y-focusY)^2
// line: p@t = p + v*t, xt = x0 + vx * t, yt = y0 + vy * t
// r^2 = (x0 + vx * t - focusX)^2 + (y0 + vy * t - focusY)^2
// r^2 = (vx * t + x0 - focusX)^2 + (vy * t + y0 - focusY)^2
// r^2 = (vx * t)^2 - 2 * (vx * t) * (x0 - focusX) + (x0 - focusX)^2 +
// (vy * t)^2 - 2 * (vy * t) * (y0 - focusY) + (y0 - focusY)^2
// r^2 = t^2 * vx^2 - t * 2 * vx * (x0 - focusX) + (x0 - focusX)^2 +
// t^2 * vy^2 - t * 2 * vy * (y0 - focusY) + (y0 - focusY)^2
// r^2 = t^2 * (vx^2 + vy^2) - t * 2(vx(x0 - focusX) + vy(y0 - focusY)) + ((x0 - focusX)^2 + (y0 - focusY)^2)
// 0 = t^2 * (vx^2 + vy^2) - t * 2(vx(x0 - focusX) + vy(y0 - focusY)) + ((x0 - focusX)^2 + (y0 - focusY)^2 - r^2)
// a = vx^2 + vy^2
// b = -2 (vx(x0 - focusX) + vy(y0 - focusY))
// c = (x0 - focusX)^2 + (y0 - focusY)^2 - r^2
// t = (-b +- sqrt[b^2 - 4ac])/2a
var a = perpBisector.Start.X * perpBisector.Start.X + perpBisector.Start.Y * perpBisector.Start.Y;
var b = -2.0 * (perpBisector.Vector.X * (perpBisector.Start.X - m_focus.X) +
perpBisector.Vector.Y * (perpBisector.Start.Y - m_focus.Y));
var c = (perpBisector.Start.X - m_focus.X) * (perpBisector.Start.X - m_focus.X) +
(perpBisector.Start.Y - m_focus.Y) * (perpBisector.Start.Y - m_focus.Y) -
offsetMagnitude * offsetMagnitude;
var discriminant = b * b - 4.0 * a * c;
var amul2 = a * 2.0;
var tCenter = -b / amul2;
if (discriminant < 0)
throw new Exception("Math error - discriminant shouldn't ever be 0...");
else if (discriminant <= double.Epsilon)
return perpBisector.PointAtT(tCenter);
else // select the lower t; it's closer to the start of the perp bisector
{
var tOffset = Math.Sqrt(discriminant) / amul2; // always positive
return perpBisector.PointAtT(tCenter - tOffset);
}
}
public void ConnectWith(Cell other)
{
var segment = new Line2D(new Point2D(x, y), new Point2D(other.X, other.Y));
var connector = new CellConnector(this, other, segment, ConnectorState.Unlinked);
connectors.Add(connector);
other.connectors.Add(connector);
}
public void CutLine(Line2D line, Renderer renderer = null)
{
foreach (var connector in grid.Cells.SelectMany(x => x.Connectors).Distinct()) {
var intersection = connector.Segment.FindLineIntersection(line);
if (intersection == line) {
// wrong
connector.connectorState = ConnectorState.Banned;
} else if (intersection is Point2D) {
var t1 = connector.Segment.NearestT((Point2D)intersection);
var t2 = line.NearestT((Point2D)intersection);
if (t1 >= 0 && t1 <= 1 && t2 >= 0 && t2 <= 1) {
connector.connectorState = ConnectorState.Banned;
}
}
}
renderer?.RenderGrid(grid);
}
public CellConnector(Cell first, Cell second, Line2D segment, ConnectorState connectorState = ConnectorState.Unlinked)
{
this.first = first;
this.second = second;
this.segment = segment;
this.connectorState = connectorState;
}
public Voronoi(Point2D[] points, Line2D[] edges)
{
}
// - Intersections --------------------------------------------------------------------------
public Shape2D FindLineIntersection(Line2D line)
{
// :: handle the cases where lines are both horizontal or both vertical
if (m_isEssentiallyHorizontal && line.m_isEssentiallyHorizontal)
return Math.Abs(m_p.Y - line.m_p.Y) < kContainsEpsilon ? this : null;
if (m_isEssentiallyVertical && line.m_isEssentiallyVertical)
return Math.Abs(m_p.X - line.m_p.X) < kContainsEpsilon ? this : null;
// :: handle the case where we have a horizontal and a vertical line.
if (m_isEssentiallyHorizontal && line.m_isEssentiallyVertical)
return new Point2D(line.m_p.X, m_p.Y);
if (m_isEssentiallyVertical && line.m_isEssentiallyHorizontal)
return new Point2D(m_p.X, line.m_p.Y);
// :: handle the case where one of the lines is horizontal/vertical and the other isn't
Line2D a = this;
Line2D b = line;
if ((!m_isEssentiallyHorizontal && line.m_isEssentiallyHorizontal) ||
(!m_isEssentiallyVertical && line.m_isEssentiallyVertical))
{
a = line;
b = this;
}
// :: a can be horizontal/vertical, b cannot be either.
if (a.m_isEssentiallyVertical)
return b.PointAtX(a.m_p.X);
if (a.m_isEssentiallyHorizontal)
return b.PointAtY(a.m_p.Y);
// :: neither is vertical or horizontal, reparameterize line to start at m_p.x
var lineStart = line.PointAtX(m_p.X);
// let height = y difference, also find dHeight/dX
var height = lineStart.Y - m_p.Y;
var dhdx = (line.m_v.Y / line.m_v.X) - (m_v.Y / m_v.X);
var dxToIntersect = -(height / dhdx);
return m_p + (m_v / m_v.X) * dxToIntersect;
}
public Shape2D FindIntersection(Shape2D shape)
{
// ensure that 'this' is less or as primative as shape
if (Type > shape.Type)
return shape.FindIntersection(this);
// find intersection
if (this.Type == ShapeType2D.Point)
return shape.Contains((Point2D)this) ? this : null;
else if (this.Type == ShapeType2D.Line) {
if (shape.Type == ShapeType2D.Line)
return ((Line2D)this).FindLineIntersection((Line2D)shape);
else if (shape.Type == ShapeType2D.Triangle) {
var line = (Line2D)this;
var triangle = (Triangle2D)shape;
var abLine = new Line2D(triangle.A, triangle.B);
var abIntersection = line.FindIntersection(abLine);
var bcLine = new Line2D(triangle.B, triangle.C);
var bcIntersection = line.FindIntersection(bcLine);
var caLine = new Line2D(triangle.C, triangle.A);
var caIntersection = line.FindIntersection(caLine);
var abIntersectionLine = abIntersection as Line2D;
var bcIntersectionLine = bcIntersection as Line2D;
var caIntersectionLine = caIntersection as Line2D;
var intersectionLine = abIntersectionLine ?? bcIntersectionLine ?? caIntersectionLine;
if (intersectionLine != null) {
return intersectionLine;
} else {
var points = new List<Point2D>();
if (abIntersection != null) {
var abIntersectionPoint = (Point2D)abIntersection;
var t = abLine.NearestT(abIntersectionPoint);
if (0 <= t && t <= 1) {
points.Add(abIntersectionPoint);
}
Console.WriteLine(t);
}
if (bcIntersection != null) {
var bcIntersectionPoint = (Point2D)bcIntersection;
var t = bcLine.NearestT(bcIntersectionPoint);
if (0 <= t && t <= 1) {
points.Add(bcIntersectionPoint);
}
Console.WriteLine(t);
}
if (caIntersection != null) {
var caIntersectionPoint = (Point2D)caIntersection;
var t = caLine.NearestT(caIntersectionPoint);
if (0 <= t && t <= 1) {
points.Add(caIntersectionPoint);
}
Console.WriteLine(t);
}
if (points.Count == 0) {
return null;
} else if (points.Count == 1) {
return points.First();
} else {
Console.WriteLine(points.Count);
Trace.Assert(points.Count == 2);
var pointCollection = new PointCollection2D(points.ToArray());
return pointCollection;
}
}
}
}
throw new System.NotImplementedException("Intersection between " + shape.Type + " and " + Type + " not implemented.");
}
private void HandleRender(object sender, RenderEventArgs e)
{
var startPoint = new Point2D(15, 5);
var endPoint = new Point2D(55, 40);
this.gridlet.Cells[(int)(startPoint.X + startPoint.Y * gridlet.XLength)].Flags |= CellFlags.Debug;
this.gridlet.Cells[(int)(endPoint.X + endPoint.Y * gridlet.XLength)].Flags |= CellFlags.Debug;
if (!isGraphicsInitialized) {
InitializeGraphics(e.GraphicsDevice);
isGraphicsInitialized = true;
}
var graphicsDevice = e.GraphicsDevice;
var renderer = e.Renderer;
e.BasicEffect.World = gridlet.Orientation * Matrix.Translation(gridlet.X, gridlet.Y, gridlet.Z);
e.BasicEffect.Texture = debugTexture;
e.BasicEffect.TextureEnabled = true;
e.BasicEffect.DiffuseColor = Vector4.One;
var samplerStateDescription = SamplerStateDescription.Default();
samplerStateDescription.Filter = Filter.ComparisonMinMagMipPoint;
e.BasicEffect.Sampler = SamplerState.New(graphicsDevice, samplerStateDescription);
e.BasicEffect.CurrentTechnique.Passes[0].Apply();
graphicsDevice.SetRasterizerState(graphicsDevice.RasterizerStates.CullBack);
graphicsDevice.SetVertexBuffer(0, vertexBuffer, 0);
graphicsDevice.SetVertexInputLayout(vertexInputLayout);
graphicsDevice.SetIndexBuffer(indexBuffer, true);
graphicsDevice.DrawIndexed(PrimitiveType.TriangleList, indexBuffer.ElementCount);
var meshTransform = gridlet.Orientation * Matrix.Translation(gridlet.X, gridlet.Y, gridlet.Z);
foreach (var meshTriangle in gridlet.Mesh) {
var a = new Vector3((float)meshTriangle.Points[0].X, (float)meshTriangle.Points[0].Y, 0);
var b = new Vector3((float)meshTriangle.Points[1].X, (float)meshTriangle.Points[1].Y, 0);
var c = new Vector3((float)meshTriangle.Points[2].X, (float)meshTriangle.Points[2].Y, 0);
Vector3.Transform(ref a, ref meshTransform, out a);
Vector3.Transform(ref b, ref meshTransform, out b);
Vector3.Transform(ref c, ref meshTransform, out c);
// var n = meshTriangle. Vector3.Cross((a - b), b - c);
var color = true ? Color.Cyan : Color.Green;
renderer.DrawDebugLine(a, b, color);
;
renderer.DrawDebugLine(b, c, color);
renderer.DrawDebugLine(c, a, color);
foreach (var neighbor in meshTriangle.Neighbors.Where(n => n != null)) {
Console.WriteLine(neighbor.IsInterior);
}
foreach (var neighbor in meshTriangle.Neighbors.Where(n => n != null && n.IsInterior)) {
Console.WriteLine("!@#!@#@#");
var meshCentroid = meshTriangle.Centroid();
var neighborCentroid = neighbor.Centroid();
var centroidA = new Vector3(meshCentroid.Xf, meshCentroid.Yf, 0);
var centroidB = new Vector3(neighborCentroid.Xf, neighborCentroid.Yf, 0);
Vector3.Transform(ref centroidA, ref meshTransform, out centroidA);
Vector3.Transform(ref centroidB, ref meshTransform, out centroidB);
renderer.DrawDebugLine(
centroidA,
centroidB,
Color.Red
);
}
}
foreach (var neighbor in gridlet.Neighbors) {
renderer.DrawDebugLine(
gridlet.OrientedBoundingBox.Center,
neighbor.OrientedBoundingBox.Center,
Color.Magenta
);
}
var mesh = gridlet.Mesh;
var startTriangle = mesh.First(
x => new Triangle2D(
new Point2D(x.Points[0].Xf, x.Points[0].Yf),
new Point2D(x.Points[1].Xf, x.Points[1].Yf),
new Point2D(x.Points[2].Xf, x.Points[2].Yf)).Contains(startPoint - new Point2D(gridlet.XLength / 2.0, gridlet.YLength / 2.0))
);
var endTriangle = mesh.First(
x => new Triangle2D(
new Point2D(x.Points[0].Xf, x.Points[0].Yf),
new Point2D(x.Points[1].Xf, x.Points[1].Yf),
new Point2D(x.Points[2].Xf, x.Points[2].Yf)).Contains(endPoint - new Point2D(gridlet.XLength / 2.0, gridlet.YLength / 2.0))
);
ConcurrentDictionary<DelaunayTriangle, double> distancesByTriangle = new ConcurrentDictionary<DelaunayTriangle, double>();
distancesByTriangle.TryAdd(startTriangle, 0);
var lastCount = 0;
while (lastCount != distancesByTriangle.Count) {
lastCount = distancesByTriangle.Count;
foreach (var kvp in distancesByTriangle) {
var triangle = kvp.Key;
var distance = kvp.Value;
foreach (var neighbor in triangle.Neighbors.Where(n => n != null && n.IsInterior).Where(n => n.Points.Union(triangle.Points).Distinct().Count() == 4)) {
var triangleCentroid = triangle.Centroid();
if (triangle == startTriangle) {
triangleCentroid = new TriangulationPoint(startPoint.X - gridlet.XLength / 2.0, startPoint.Y - gridlet.YLength / 2.0);
} else if (triangle == endTriangle) {
triangleCentroid = new TriangulationPoint(endPoint.X - gridlet.XLength / 2.0, endPoint.Y - gridlet.YLength / 2.0);
}
var neighborCentorid = neighbor.Centroid();
var neighborDistance = distance + Math.Pow(triangleCentroid.X - neighborCentorid.X, 2) + Math.Pow(triangleCentroid.Y - neighborCentorid.Y, 2);
distancesByTriangle.AddOrUpdate(
neighbor,
add => neighborDistance,
(update, existing) => Math.Min(existing, neighborDistance)
);
}
}
}
foreach (var kvp in distancesByTriangle) {
var triangle = kvp.Key;
var distance = kvp.Value;
foreach (var neighbor in triangle.Neighbors.Where(n => n != null && n.IsInterior)) {
var triangleCentroid = triangle.Centroid();
var neighborCentorid = neighbor.Centroid();
var neighborDistance = distance + Math.Pow(triangleCentroid.X - neighborCentorid.X, 2) + Math.Pow(triangleCentroid.Y - neighborCentorid.Y, 2);
distancesByTriangle.AddOrUpdate(
neighbor,
add => neighborDistance,
(update, existing) => Math.Min(existing, neighborDistance)
);
}
}
var path = new List<DelaunayTriangle>();
{
var currentNode = endTriangle;
while (currentNode != startTriangle) {
path.Insert(0, currentNode);
currentNode = currentNode.Neighbors.Where(n => n != null && n.IsInterior).MinBy(distancesByTriangle.Get);
}
path.Insert(0, currentNode);
}
for (var i = 0; i < path.Count - 1; i++) {
var startCentroid = new Vector3(path[i].Centroid().Xf, path[i].Centroid().Yf, 0);
var endCentroid = new Vector3(path[i + 1].Centroid().Xf, path[i + 1].Centroid().Yf, 0);
Vector3.Transform(ref startCentroid, ref meshTransform, out startCentroid);
Vector3.Transform(ref endCentroid, ref meshTransform, out endCentroid);
e.Renderer.DrawDebugLine(startCentroid, endCentroid, Color.White);
}
// path = new[] { 5, 6, 13, 12, 11, 0 }.Select(mesh.Get).ToList();
{
var finalPath = new List<Point2D>();
var currentPoint = new Point2D(startPoint.X - Gridlet.XLength / 2.0, startPoint.Y - gridlet.YLength / 2.0);
var currentSharedPoints = path[0].Points.Intersect(path[1].Points).ToArray();
finalPath.Add(currentPoint);
try {
for (var i = 1; i < path.Count; i++) {
TriangulationPoint[] sharedPoints;
Point2D commonPoint, newPoint, oldOther;
if (i < path.Count - 1) {
sharedPoints = path[i].Points.Intersect(path[i + 1].Points).ToArray();
var commonPoints = sharedPoints.Intersect(currentSharedPoints).ToArray();
Trace.Assert(commonPoints.Length == 1);
var commonPoint_ = commonPoints[0];
commonPoint = new Point2D(commonPoint_.X, commonPoint_.Y);
var newPoint_ = sharedPoints.First(p => !p.Equals(commonPoint_));
newPoint = new Point2D(newPoint_.X, newPoint_.Y);
var oldOther_ = currentSharedPoints.First(p => !p.Equals(commonPoint_));
oldOther = new Point2D(oldOther_.X, oldOther_.Y);
} else {
sharedPoints = null;
var commonPoint_ = currentSharedPoints.First();
commonPoint = new Point2D(commonPoint_.X, commonPoint_.Y);
var oldOther_ = currentSharedPoints.Skip(1).First();
oldOther = new Point2D(oldOther_.X, oldOther_.Y);
newPoint = new Point2D(endPoint.X - Gridlet.XLength / 2.0, endPoint.Y - gridlet.YLength / 2.0);
var windingOldCurrentNew = GeometryUtilities.GetClockness(oldOther, currentPoint, newPoint);
var windingOldCurrentCommon = GeometryUtilities.GetClockness(oldOther, currentPoint, commonPoint);
if (windingOldCurrentNew == windingOldCurrentCommon) {
var temp = commonPoint;
commonPoint = oldOther;
oldOther = temp;
}
}
var oldOtherVector = new Vector2D(currentPoint, oldOther).ToUnitVector();
var commonVector = new Vector2D(currentPoint, commonPoint).ToUnitVector();
var newVector = new Vector2D(currentPoint, newPoint).ToUnitVector();
var oldAngle = Math.Acos(oldOtherVector.Dot(commonVector));
var newAngle = Math.Acos(newVector.Dot(commonVector));
var oldWinding = GeometryUtilities.GetClockness(commonPoint, currentPoint, oldOther);
var newWinding = GeometryUtilities.GetClockness(commonPoint, currentPoint, newPoint);
if (oldAngle > newAngle && oldWinding == newWinding) {
currentSharedPoints = sharedPoints;
} else {
finalPath.Add(oldOther);
currentPoint = oldOther;
currentSharedPoints = sharedPoints;
}
// if (i == 4) {
// Console.WriteLine(i + " SDFDFSD " + path.Count);
// var currentPointVect = new Vector3((float)currentPoint.X, (float)currentPoint.Y, 0);
// var commonPointVect = new Vector3((float)commonPoint.X, (float)commonPoint.Y, 0);
// var oldOtherPointVect = new Vector3((float)oldOther.X, (float)oldOther.Y, 0);
// var newPointVect = new Vector3((float)newPoint.X, (float)newPoint.Y, 0);
//
// Vector3.Transform(ref currentPointVect, ref meshTransform, out currentPointVect);
// Vector3.Transform(ref commonPointVect, ref meshTransform, out commonPointVect);
// Vector3.Transform(ref oldOtherPointVect, ref meshTransform, out oldOtherPointVect);
// Vector3.Transform(ref newPointVect, ref meshTransform, out newPointVect);
// e.Renderer.DrawDebugLine(currentPointVect, commonPointVect, Color.Green);
// e.Renderer.DrawDebugLine(currentPointVect, oldOtherPointVect, Color.Orange);
// e.Renderer.DrawDebugLine(currentPointVect, newPointVect, Color.Chocolate);
// renderer.DrawCube(Matrix.Translation((float)currentPointVect.X, (float)currentPointVect.Y, 1), new Vector4(0, 0, 1, 1), false);
// renderer.DrawCube(Matrix.Translation((float)commonPointVect.X, (float)commonPointVect.Y, 1), new Vector4(0, 1, 0, 1), false);
// renderer.DrawCube(Matrix.Translation((float)oldOtherPointVect.X, (float)oldOtherPointVect.Y, 1), new Vector4(1, 0.5f, 0, 1), false);
// renderer.DrawCube(Matrix.Translation((float)newPointVect.X, (float)newPointVect.Y, 1), new Vector4(0.5f, 0.25f, 0, 1), false);
//
// foreach (var p in finalPath) {
// renderer.DrawCube(Matrix.Translation((float)p.X, (float)p.Y, 1.5f), new Vector4(1, 1, 1, 1), false);
// }
//
// for (var j = 0; j < finalPath.Count - 1; j++) {
// Console.WriteLine("!@#@!###!@ " + finalPath.Count);
// var startCentroid = new Vector3((float)finalPath[j].X, (float)finalPath[j].Y, 0);
// var endCentroid = new Vector3((float)finalPath[j + 1].X, (float)finalPath[j + 1].Y, 0);
//
// Vector3.Transform(ref startCentroid, ref meshTransform, out startCentroid);
// Vector3.Transform(ref endCentroid, ref meshTransform, out endCentroid);
// e.Renderer.DrawDebugLine(startCentroid, endCentroid, Color.Magenta);
// }
// break;
// }
}
finalPath.Add(new Point2D(endPoint.X - Gridlet.XLength / 2.0, endPoint.Y - gridlet.YLength / 2.0));
var simplePathLine = new Line2D(
new Point2D(startPoint.X - Gridlet.XLength / 2.0, startPoint.Y - gridlet.YLength / 2.0),
new Point2D(endPoint.X - Gridlet.XLength / 2.0, endPoint.Y - gridlet.YLength / 2.0)
);
// for (var t = 0.0f; t < 1.0f; t+= 0.1f) {
// var p = simplePathLine.PointAtT(t);
// renderer.DrawCube(Matrix.Scaling(2) * Matrix.Translation((float)p.X, (float)p.Y, 2), new Vector4(1, 1, 1, 1), false);
// }
// var triangle = new Triangle2D(
// new Point2D(-32.5, 32.5),
// new Point2D(-10, 0),
// new Point2D(-32.5, -32.5));
// MessageBox.Show(triangle.FindIntersection(simplePathLine)?.ToString() ?? "null");
// var abLine = new Line2D(triangle.A, triangle.B);
// var abIntersection = (Point2D)abLine.FindLineIntersection(simplePathLine);
// var bcLine = new Line2D(triangle.B, triangle.C);
// var bcIntersection = (Point2D)bcLine.FindLineIntersection(simplePathLine);
// Console.WriteLine("AB INTERSECTION: " + abLine.NearestT(abIntersection) + " " + abLine.PointAtT(abLine.NearestT(abIntersection)) + " " + abIntersection);
// Console.WriteLine("BC INTERSECTION: " + bcLine.NearestT(bcIntersection));
// var f = new Form();
// var pb = new PictureBox();
// var b = new Bitmap(100, 100);
// using (var g = Graphics.FromImage(b)) {
// for (var t = 0.0; t < 1.0f; t += 0.05f) {
// var p = abLine.PointAtT(t);
// g.DrawRectangle(Pens.Red, (float)(p.X + 50 - 1), (float)(p.Y + 50 - 1), 2, 2);
// }
//
// g.DrawRectangle(Pens.Orange, (float)(abIntersection.X + 50 - 3), (float)(abIntersection.Y + 50 - 3), 6, 6);
//
//
// g.DrawRectangle(Pens.Red, (float)(triangle.A.X + 50 - 1), (float)(triangle.A.Y + 50 - 1), 2, 2);
// g.DrawRectangle(Pens.Green, (float)(triangle.B.X + 50 - 1), (float)(triangle.B.Y + 50 - 1), 2, 2);
// g.DrawRectangle(Pens.Blue, (float)(triangle.C.X + 50 - 1), (float)(triangle.C.Y + 50 - 1), 2, 2);
// g.DrawLine(Pens.Lime, -25 + 50, -25 + 50, -20 + 50, 25 + 50);
// }
// pb.Image = b;
// pb.SizeMode = PictureBoxSizeMode.AutoSize;
// f.Controls.Add(pb);
// f.ClientSize = b.Size;
// f.Show();
// Application.Run();
var tritris = mesh.Where(
x => new Triangle2D(
new Point2D(x.Points[0].Xf, x.Points[0].Yf),
new Point2D(x.Points[1].Xf, x.Points[1].Yf),
new Point2D(x.Points[2].Xf, x.Points[2].Yf)).FindIntersection(simplePathLine) != null
).ToArray();
if (tritris.Any()) {
var sadfaces = new ICL.HashSet<DelaunayTriangle>(tritris);
var s = new Stack<DelaunayTriangle>();
s.Push(tritris.First());
var lastSadfacesCount = 0;
while (s.Any() && lastSadfacesCount != sadfaces.Count) {
lastSadfacesCount = sadfaces.Count;
var tri = s.Pop();
sadfaces.Remove(tri);
foreach (var other in sadfaces) {
if (other.Points.Union(tri.Points).Distinct().Count() <= 4) {
s.Push(other);
}
}
}
if (sadfaces.None()) {
// MessageBox.Show("!@#!#!@#");
finalPath.Clear();
finalPath.Add(simplePathLine.Start);
finalPath.Add(simplePathLine.PointAtT(1));
}
}
Console.WriteLine("!@#!@#!@ " + tritris.SelectMany(x => x.Points).Distinct().Count() + " " + tritris.Length);
Console.WriteLine("!@#!@##@@!#@!@!@# " + tritris.Length);
foreach (var tri in tritris) {
var centroid = tri.Centroid();
var centroidVector = new Vector3(centroid.Xf, centroid.Yf, 0);
renderer.DrawCube(Matrix.Scaling(4) * Matrix.Translation(centroidVector), new Vector4(0, 1, 1, 1), false);
}
foreach (var p in finalPath) {
renderer.DrawCube(Matrix.Translation((float)p.X, (float)p.Y, 1.5f), new Vector4(1, 1, 1, 1), false);
}
for (var j = 0; j < finalPath.Count - 1; j++) {
Console.WriteLine("!@#@!###!@ " + finalPath.Count);
var startCentroid = new Vector3((float)finalPath[j].X, (float)finalPath[j].Y, 0);
var endCentroid = new Vector3((float)finalPath[j + 1].X, (float)finalPath[j + 1].Y, 0);
Vector3.Transform(ref startCentroid, ref meshTransform, out startCentroid);
Vector3.Transform(ref endCentroid, ref meshTransform, out endCentroid);
e.Renderer.DrawDebugLine(startCentroid, endCentroid, Color.Magenta);
}
} catch (Exception exsaqsd) {
Console.WriteLine(exsaqsd);
}
// for (var i = 0; i < finalPath.Count - 1; i++) {
// var startCentroid = new Vector3((float)finalPath[i].X - Gridlet.XLength / 2.0f, (float)finalPath[i].Y - Gridlet.YLength / 2.0f, 0);
// var endCentroid = new Vector3((float)finalPath[i + 1].X - Gridlet.XLength / 2.0f, (float)finalPath[i + 1].Y - Gridlet.YLength / 2.0f, 0);
// Vector3.Transform(ref startCentroid, ref meshTransform, out startCentroid);
// Vector3.Transform(ref endCentroid, ref meshTransform, out endCentroid);
// e.Renderer.DrawDebugLine(startCentroid, endCentroid, Color.Magenta);
// }
}
}
