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