public static Plane3 FromFourPoints(Vertex3 a, Vertex3 b, Vertex3 c, Vertex3 keep)
{
Plane3 p = Plane3.FromThreePoints(a, b, c);
if (p.Contains(keep))
{
throw new Exception("Fourth point lies in the plane of the first three");
}
else
{
return(p.FlipToInclude(keep));
}
}
public static IntersectionResult Intersect(Line3 line, Plane3 plane)
{
if (plane.Contains(line))
{
return(new IntersectionFailed(IntersectionFailedReason.Coincident));
}
else if (plane.IsParallelTo(line))
{
return(new IntersectionFailed(IntersectionFailedReason.Parallel));
}
else
{
Vector3 convergence = line.Direction.ComponentAlong(plane.Normal);
//Vector3 run = line.Direction.ComponentOrtho(plane.Normal);
Vector3 p1 = line.Origin - plane.Origin;
Vector3 p2 = (line.Origin + line.Direction) - plane.Origin;
return(new IntersectionVertex3
(
Interpolate(p1.ScaledLengthAlong(convergence), line.Origin, p2.ScaledLengthAlong(convergence), line.Origin + line.Direction, BigRational.Zero)
));
}
}
public static CH_Polyhedron Make(ImmutableList <CH_Polygon> facesAway, Vertex3 v)
{
ImmutableList <CH_Polygon> facesAway1 = facesAway;
foreach (CH_Polygon face in facesAway1)
{
if (!(face.Plane.Includes(v)))
{
throw new ArgumentException("Polygons must face away from new point");
}
}
DualIndexedSet <Tuple <Vertex3, Vertex3> > edgeMap = new DualIndexedSet <Tuple <Vertex3, Vertex3> >(x => new Tuple <Vertex3, Vertex3>(x.Item2, x.Item1));
ImmutableDictionary <int, int> edgeToFace = ImmutableDictionary <int, int> .Empty;
int iEnd = facesAway1.Count;
for (int i = 0; i < iEnd; ++i)
{
foreach (CH_Polygon.Edge edge in facesAway1[i].Edges)
{
var(edgeMap2, eIndex, isNew) = edgeMap.EnsureAdded(new Tuple <Vertex3, Vertex3>(edge.Start, edge.End));
edgeMap = edgeMap2;
if (edgeToFace.ContainsKey(eIndex))
{
throw new ArgumentException("Invalid polyhedron (edge traversed in same direction by more than one face)");
}
edgeToFace = edgeToFace.Add(eIndex, i);
}
}
ImmutableDictionary <Vertex3, int> startToEdge = ImmutableDictionary <Vertex3, int> .Empty;
int iEnd2 = edgeMap.Count;
for (int i = 0; i < iEnd2; ++i)
{
if (!(edgeToFace.ContainsKey(i)))
{
startToEdge = startToEdge.Add(edgeMap[i].Item1, i);
}
if (!(edgeToFace.ContainsKey(~i)))
{
startToEdge = startToEdge.Add(edgeMap[~i].Item1, ~i);
}
}
ImmutableList <int> edges = ImmutableList <int> .Empty;
edges = edges.Add(startToEdge.First().Value);
int sentinel = startToEdge.Count;
while (true)
{
Vertex3 end = edgeMap[edges[edges.Count - 1]].Item2;
if (!(startToEdge.ContainsKey(end)))
{
throw new ArgumentException("Unable to walk loose edges");
}
int nextEdge = startToEdge[end];
if (nextEdge == edges[0])
{
break;
}
edges = edges.Add(nextEdge);
--sentinel;
if (sentinel < 0)
{
throw new InvalidOperationException("Loose edges don't loop properly");
}
}
if (edges.Count != startToEdge.Count)
{
throw new InvalidOperationException("Not all loose edges were used");
}
sentinel = edges.Count;
Func <int, int, bool> inSamePlane = delegate(int edge1, int edge2)
{
Tuple <Vertex3, Vertex3> e1 = edgeMap[edge1];
Tuple <Vertex3, Vertex3> e2 = edgeMap[edge2];
Plane3 p = Plane3.FromThreePoints(e1.Item1, e1.Item2, v);
return(p.Contains(e2.Item1) && p.Contains(e2.Item2));
};
while (true)
{
if (!inSamePlane(edges[0], edges[edges.Count - 1]))
{
break;
}
int x = edges[0];
edges = edges.RemoveAt(0).Add(x);
--sentinel;
if (sentinel < 0)
{
throw new InvalidOperationException("Loose edges all lie in the same plane as the vertex (?!)");
}
}
ImmutableList <CH_Polygon> newPolys = ImmutableList <CH_Polygon> .Empty;
ImmutableList <Vertex3> corners = ImmutableList <Vertex3> .Empty;
int?lastEdge = null;
Action flush = delegate()
{
corners = corners.Add(edgeMap[lastEdge.Value].Item2).Add(v);
newPolys = newPolys.Add(new CH_Polygon(corners));
};
Action <int> addEdge = delegate(int edge)
{
if (lastEdge == null || inSamePlane(edge, lastEdge.Value))
{
corners = corners.Add(edgeMap[edge].Item1);
lastEdge = edge;
}
else
{
flush();
corners = ImmutableList <Vertex3> .Empty
.Add(edgeMap[edge].Item1);
lastEdge = edge;
}
};
foreach (int edge in edges)
{
addEdge(edge);
}
flush();
return(new CH_Polyhedron(newPolys.AddRange(facesAway1)));
}
