private static Point3f[] getPointsWherePlaneCrossesEdges(Plain3D plain) { var list = new Point3f[EDGE_COUNT]; var i = 0; foreach (var c1 in EDGE_POSSIBLE_COORDINATES) { foreach (var c2 in EDGE_POSSIBLE_COORDINATES) { // will occur 4 times var x = (c1 * (plain.P3.X * plain.P2.Z - plain.P2.X * plain.P3.Z) + c1 * (plain.P2.X * plain.P3.Y - plain.P3.X * plain.P2.Y)) / (plain.P3.Y * plain.P2.Z - plain.P2.Y * plain.P3.Z); var y = (c1 * (plain.P2.Y * plain.P3.Z - plain.P3.Y * plain.P2.Z) + c2 * (plain.P2.X * plain.P3.Y - plain.P3.X * plain.P2.Y)) / (plain.P2.X * plain.P3.Z - plain.P3.X * plain.P2.Z); var z = (c1 * (plain.P2.Y * plain.P3.Z - plain.P3.Y * plain.P2.Z) + c2 * (plain.P2.Z * plain.P3.X - plain.P2.X * plain.P3.Z)) / (plain.P3.X * plain.P2.Y - plain.P2.X * plain.P3.Y); if (Math.Abs(x) <= 1) { list[i++] = new Point3f(x, c1, c2); } if (Math.Abs(y) <= 1) { list[i++] = new Point3f(c1, y, c2); } if (Math.Abs(z) <= 1) { list[i++] = new Point3f(c1, c2, z); } } } var result = new Point3f[i]; Array.Copy(list, result, i); return(result); }
public List <List <Point3f> > getPath(Point3f from, Point3f to) { // 1 edge case if (isOnOneEdge(from, to)) { var lists = new List <List <Point3f> >(); var list = new List <Point3f>(new Point3f[] { from, to }); lists.Add(list); return(lists); } else if (new Line3D(from, to).passesZero()) { // TODO the case has my dick on it return(null); } else // multi edge { var plain = new Plain3D(from, to); var points = getPointsWherePlaneCrossesEdges(plain); return(calculateBestPaths(from, points, to)); } }