public static bool RayIntersectsForObb(Ray ray, PolygonalFace face)
{
if (Math.Abs(ray.Direction.dotProduct(face.Normal)) < 0.06)
{
return(false);
}
if (OnTheWrongSide(ray, face))
{
return(false);
}
var point = face.Vertices[0].Position;
var w = new double[] { ray.Position[0] - point[0], ray.Position[1] - point[1], ray.Position[2] - point[2] };
var s1 = (face.Normal.dotProduct(w)) / (face.Normal.dotProduct(ray.Direction));
//var v = new double[] { w[0] + s1 * ray.Direction[0] + point[0], w[1] + s1 * ray.Direction[1] + point[1], w[2] + s1 * ray.Direction[2] + point[2] };
var v = new double[] { ray.Position[0] - s1 * ray.Direction[0], ray.Position[1] - s1 * ray.Direction[1], ray.Position[2] - s1 * ray.Direction[2] };
foreach (var corner in face.Vertices)
{
var otherCorners = face.Vertices.Where(ver => ver != corner).ToList();
var v1 = otherCorners[0].Position.subtract(corner.Position);
var v2 = otherCorners[1].Position.subtract(corner.Position);
var v0 = v.subtract(corner.Position);
if (v1.crossProduct(v0).dotProduct(v2.crossProduct(v0)) > -0.15) // -0.09
{
return(false);
}
}
return(true);
}
internal static Vertex[][] SortedEdgesOfTriangle(PolygonalFace triangle)
{
// the first one is the shortest edge, the last one is the longest
var sorted = new Vertex[3][];
var dic = new Dictionary <double, Vertex[]>
{
{
DistanceBetweenTwoVertices(triangle.Vertices[0].Position, triangle.Vertices[1].Position),
new[] { triangle.Vertices[0], triangle.Vertices[1] }
}
};
var dis1 = DistanceBetweenTwoVertices(triangle.Vertices[0].Position, triangle.Vertices[2].Position);
var dis2 = DistanceBetweenTwoVertices(triangle.Vertices[1].Position, triangle.Vertices[2].Position);
if (dic.ContainsKey(dis1))
{
dis1 += 1e-6;
}
dic.Add(dis1, new[] { triangle.Vertices[0], triangle.Vertices[2] });
if (dic.ContainsKey(dis2))
{
dis2 += 1e-6;
}
dic.Add(dis2, new[] { triangle.Vertices[1], triangle.Vertices[2] });
var sortedKey = dic.Keys.ToList();
sortedKey.Sort();
sorted[0] = dic[sortedKey[0]];
sorted[1] = dic[sortedKey[1]];
sorted[2] = dic[sortedKey[2]];
return(sorted);
}
/// <summary>
/// Initializes a new instance of the <see cref="Node" /> class.
/// </summary>
/// <param name="triangle">The triangle.</param>
internal Node(PolygonalFace triangle)
{
ReferenceFaces = new List <PolygonalFace> {
triangle
};
ReferenceEdges = triangle.Edges.ToList();
ReferenceVertices = new List <Vertex>(); //Create a null list, to build up later.
Vector = triangle.Normal; //Set unit normal as location on sphere
X = triangle.Normal[0];
Y = triangle.Normal[1];
Z = triangle.Normal[2];
Arcs = new List <Arc>();
//bound azimuthal angle (theta) to 0 <= θ <= 360
Theta = 0.0;
if (triangle.Normal[0] < 0) //If both negative or just x, add 180 (Q2 and Q3)
{
Theta = Math.Atan(triangle.Normal[1] / triangle.Normal[0]) + Math.PI;
}
else if (triangle.Normal[1] < 0) //If only y is negative, add 360 (Q4)
{
Theta = Math.Atan(triangle.Normal[1] / triangle.Normal[0]) + Constants.TwoPi;
}
else //Everything is positive (Q1).
{
Theta = Math.Atan(triangle.Normal[1] / triangle.Normal[0]);
}
//Calculate polar angle. Note that Acos is bounded 0 <= φ <= 180.
//Aslo, note that r = 1, so this calculation is simpler that usual.
Phi = Math.Acos(triangle.Normal[2]);
}
internal static bool TriangleOverlapping(PolygonalFace tri1, PolygonalFace tri2)
{
var edges1 = new HashSet <Vertex[]>();
var edges2 = new HashSet <Vertex[]>();
// project tri2 on tri1 plane
edges1.Add(new[] { tri1.Vertices[0], tri1.Vertices[1] });
edges1.Add(new[] { tri1.Vertices[0], tri1.Vertices[2] });
edges1.Add(new[] { tri1.Vertices[1], tri1.Vertices[2] });
var newTri2Verts = new Vertex[3];
for (int i = 0; i < tri2.Vertices.Count; i++)
{
var ver = tri2.Vertices[i];
var w = ver.Position.subtract(tri1.Vertices[0].Position);
var s1 = (tri1.Normal.dotProduct(w)) / (tri1.Normal.dotProduct(tri2.Normal));
newTri2Verts[i] = new Vertex(new[]
{
ver.Position[0] - s1 * tri2.Normal[0], ver.Position[1] - s1 * tri2.Normal[1],
ver.Position[2] - s1 * tri2.Normal[2]
});
}
edges2.Add(new[] { newTri2Verts[0], newTri2Verts[1] });
edges2.Add(new[] { newTri2Verts[0], newTri2Verts[2] });
edges2.Add(new[] { newTri2Verts[1], newTri2Verts[2] });
return(edges1.Any(movEdge => edges2.Any(refEdge => DoIntersect(movEdge, refEdge))));
}
internal static Edge[] SortedEdgesOfTriangle2(PolygonalFace triangle)
{
// the first one is the shortest edge, the last one is the longest
var sorted = new Edge[3];
var dic = new Dictionary <double, Edge>();
double dis0 = triangle.Edges[0].Length;
double dis1 = triangle.Edges[1].Length;
double dis2 = triangle.Edges[2].Length;
dic.Add(dis0, triangle.Edges[0]);
if (dic.ContainsKey(dis1))
{
dis1 += 1e-6;
}
dic.Add(dis1, triangle.Edges[1]);
if (dic.ContainsKey(dis2))
{
dis2 += 1e-6;
}
dic.Add(dis2, triangle.Edges[2]);
var sortedKey = dic.Keys.ToList();
sortedKey.Sort();
sorted[0] = dic[sortedKey[0]];
sorted[1] = dic[sortedKey[1]];
sorted[2] = dic[sortedKey[2]];
return(sorted);
}
internal FastenerBoundingBoxPartition(TessellatedSolid solid, PolygonalFace faceFromLongestSide, int numberOfPartitions)
{
this.NumberOfPartitions = numberOfPartitions;
this.Solid = solid;
this.Partitions = CreatePartitions(faceFromLongestSide, this.NumberOfPartitions);
SolidTrianglesOfPartitions(this.Partitions, solid, faceFromLongestSide);
}
public static bool RayIntersectsWithFace(Ray ray, PolygonalFace face, out double[] hittingPoint, out bool outer)
{
//if (ray.Direction.dotProduct(face.Normal) > -0.06) return false;
var w = ray.Position.subtract(face.Vertices[0].Position);
var s1 = (face.Normal.dotProduct(w)) / (face.Normal.dotProduct(ray.Direction));
//var v = new double[] { w[0] + s1 * ray.Direction[0] + point[0], w[1] + s1 * ray.Direction[1] + point[1], w[2] + s1 * ray.Direction[2] + point[2] };
//var v = new double[] { ray.Position[0] - s1 * ray.Direction[0], ray.Position[1] - s1 * ray.Direction[1], ray.Position[2] - s1 * ray.Direction[2] };
var pointOnTrianglesPlane = new[] { ray.Position[0] - s1 * ray.Direction[0], ray.Position[1] - s1 * ray.Direction[1], ray.Position[2] - s1 * ray.Direction[2] };
hittingPoint = pointOnTrianglesPlane;
outer = true;
if (pointOnTrianglesPlane.subtract(ray.Position).dotProduct(ray.Direction) < 0.001)
{
return(false); // on the opposite side
}
if (ray.Direction.dotProduct(face.Normal) > -0.06)
{
return(false);
}
var v0 = face.Vertices[0].Position.subtract(pointOnTrianglesPlane);
var v1 = face.Vertices[1].Position.subtract(pointOnTrianglesPlane);
var v2 = face.Vertices[2].Position.subtract(pointOnTrianglesPlane);
var crossv0v1 = v0.crossProduct(v1).normalize();
var crossv1v2 = v1.crossProduct(v2).normalize();
var dot = crossv0v1.dotProduct(crossv1v2);
if (dot < 0 || double.IsNaN(dot))
{
return(false);
}
var crossv2v0 = v2.crossProduct(v0).normalize();
dot = crossv1v2.dotProduct(crossv2v0);
return(dot >= 0);
}
private double findFaceArea(PolygonalFace face)
{
var v1 = face.Vertices[1].Position.subtract(face.Vertices[0].Position);
var v2 = face.Vertices[2].Position.subtract(face.Vertices[1].Position);
return(0.5 * v1.crossProduct(v2).norm2());
}
/// <summary>
/// Initializes a new instance of the <see cref="Edge" /> class.
/// </summary>
/// <param name="fromVertex">From vertex.</param>
/// <param name="toVertex">To vertex.</param>
/// <param name="ownedFace">The face.</param>
/// <param name="otherFace">The other face.</param>
/// <param name="doublyLinkedFaces"></param>
/// <param name="doublyLinkedVertices"></param>
public Edge(Vertex fromVertex, Vertex toVertex, PolygonalFace ownedFace, PolygonalFace otherFace,
Boolean doublyLinkedFaces = false, Boolean doublyLinkedVertices = false)
{
From = fromVertex;
To = toVertex;
_ownedFace = ownedFace;
_otherFace = otherFace;
if (doublyLinkedVertices)
{
fromVertex.Edges.Add(this);
toVertex.Edges.Add(this);
}
if (doublyLinkedFaces)
{
if (_ownedFace != null) _ownedFace.Edges.Add(this);
if (_otherFace != null) _otherFace.Edges.Add(this);
}
Vector = new[]
{
(To.Position[0] - From.Position[0]),
(To.Position[1] - From.Position[1]),
(To.Position[2] - From.Position[2])
};
Length =
Math.Sqrt(Vector[0] * Vector[0] + Vector[1] * Vector[1] + Vector[2] * Vector[2]);
DefineInternalEdgeAngle();
}
private static bool TwoTriangleOverlapCheck(PolygonalFace fA, PolygonalFace fB)
{
// this function is not really fuzziabled
foreach (var edge in fA.Edges)
{
var edgeVector = edge.Vector;
var third = fA.Vertices.Where(a => a != edge.From && a != edge.To).ToList()[0].Position;
var checkVec = new[]
{ third[0] - edge.From.Position[0], third[1] - edge.From.Position[1], third[2] - edge.From.Position[2] };
double[] cross1 = edgeVector.crossProduct(checkVec);
var c = 0;
foreach (var vertexB in fB.Vertices)
{
var newVec = vertexB.Position.subtract(edge.From.Position);
var cross2 = edgeVector.crossProduct(newVec);
if ((Math.Sign(cross1[0]) != Math.Sign(cross2[0]) ||
(Math.Sign(cross1[0]) == 0 && Math.Sign(cross2[0]) == 0)) &&
(Math.Sign(cross1[1]) != Math.Sign(cross2[1]) ||
(Math.Sign(cross1[1]) == 0 && Math.Sign(cross2[1]) == 0)) &&
(Math.Sign(cross1[2]) != Math.Sign(cross2[2]) ||
(Math.Sign(cross1[2]) == 0 && Math.Sign(cross2[2]) == 0)))
{
c++;
}
}
if (c == 3)
{
return(false);
}
}
return(true);
}
private static void WriteFacet(BinaryWriter writer, PolygonalFace face, bool defineColors, Color defaultColor)
{
writer.Write(BitConverter.GetBytes((float)face.Normal[0]));
writer.Write(BitConverter.GetBytes((float)face.Normal[1]));
writer.Write(BitConverter.GetBytes((float)face.Normal[2]));
writer.Write(BitConverter.GetBytes((float)face.Vertices[0].X));
writer.Write(BitConverter.GetBytes((float)face.Vertices[0].Y));
writer.Write(BitConverter.GetBytes((float)face.Vertices[0].Z));
writer.Write(BitConverter.GetBytes((float)face.Vertices[1].X));
writer.Write(BitConverter.GetBytes((float)face.Vertices[1].Y));
writer.Write(BitConverter.GetBytes((float)face.Vertices[1].Z));
writer.Write(BitConverter.GetBytes((float)face.Vertices[2].X));
writer.Write(BitConverter.GetBytes((float)face.Vertices[2].Y));
writer.Write(BitConverter.GetBytes((float)face.Vertices[2].Z));
var colorBytes = (ushort)0;
if (defineColors)
{
colorBytes += 32768;
var red = (ushort)(face.Color.R / 8);
colorBytes += red;
var green = (ushort)(face.Color.G / 8);
colorBytes += (ushort)(green * 32);
var blue = (ushort)(face.Color.B / 8);
colorBytes += (ushort)(blue * 1024);
}
writer.Write(BitConverter.GetBytes(colorBytes));
}
internal static double DistanceBetweenVertexAndPlane(double[] ver, PolygonalFace plane)
{
// create a vector from a vertex on plane to the ver
var vector = ver.subtract(plane.Vertices[0].Position);
// distance is the dot product of the normal and the vector:
return(vector.dotProduct(plane.Normal));
}
private static double TwoTrianglesSamePlaneCheck(PolygonalFace rndFaceA, PolygonalFace rndFaceB)
{
var q = rndFaceA.Center;
var p = rndFaceB.Center;
var pq = new[] { q[0] - p[0], q[1] - p[1], q[2] - p[2] };
return(OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.PlaneDistL,
OverlappingFuzzification.PlaneDistU, Math.Abs(pq.dotProduct(rndFaceA.Normal))));
}
private static bool PosSpherePosSphereOverlappingCheck(Sphere sphere1, Sphere sphere2)
{
//postive(A)-Positive(B)
// Seems to be really time consuming
var localProb = 0.0;
PolygonalFace closestFace = new PolygonalFace();
var overlap = false;
foreach (var fA in sphere1.Faces)
{
foreach (var fB in sphere2.Faces)
{
var pP = TwoTrianglesParallelCheck(fA.Normal, fB.Normal);
var sP = TwoTrianglesSamePlaneCheck(fA, fB);
if (pP == 0 || sP == 0 || !TwoTriangleOverlapCheck(fA, fB))
{
continue;
}
var p = Math.Min(pP, sP);
if (p > localProb)
{
localProb = p;
closestFace = fB;
}
if (localProb == 1)
{
break;
}
}
if (localProb == 1)
{
break;
}
}
if (localProb == 0 || double.IsNaN(localProb))
{
return(false);
}
if (localProb > MaxProb)
{
MaxProb = localProb;
}
for (var i = 0; i < DirInd.Count; i++)
{
var dir = Geometric_Reasoning.StartProcess.Directions[DirInd[i]];
if (closestFace.Normal.dotProduct(dir) > OverlappingFuzzification.CheckWithGlobDirs)
{
DirInd.Remove(DirInd[i]);
i--;
}
}
return(true);
}
public static double[] RayIntersectionPointWithFace(Ray ray, PolygonalFace face)
{
// this is exclusively for fastener detection -> Partitioning bounding box and cylinder
var w = ray.Position.subtract(face.Vertices[0].Position);
var s1 = (face.Normal.dotProduct(w)) / (face.Normal.dotProduct(ray.Direction));
return(new[]
{
ray.Position[0] - s1 * ray.Direction[0],
ray.Position[1] - s1 * ray.Direction[1],
ray.Position[2] - s1 * ray.Direction[2]
});
}
internal static double[] SortedEdgeLengthOfTriangle(PolygonalFace triangle)
{
// shortest, medium, longest. this function returns the lenght of medium
var lengths = new List <double>
{
DistanceBetweenTwoVertices(triangle.Vertices[0].Position, triangle.Vertices[1].Position),
DistanceBetweenTwoVertices(triangle.Vertices[0].Position, triangle.Vertices[2].Position),
DistanceBetweenTwoVertices(triangle.Vertices[1].Position, triangle.Vertices[2].Position)
};
lengths.Sort();
return(lengths.ToArray());
}
private static bool PosCylinderPosCylinderOverlappingCheck(Cylinder cylinder1, Cylinder cylinder2)
{
var localProb = 0.0;
PolygonalFace closestFace = new PolygonalFace();
foreach (var fA in cylinder1.Faces)
{
foreach (var fB in cylinder2.Faces)
{
var pP = TwoTrianglesParallelCheck(fA.Normal, fB.Normal);
var sP = TwoTrianglesSamePlaneCheck(fA, fB);
if (pP == 0 || sP == 0 || !TwoTriangleOverlapCheck(fA, fB))
{
continue;
}
var p = Math.Min(pP, sP);
if (p > localProb)
{
localProb = p;
closestFace = fB;
}
if (localProb == 1)
{
break;
}
}
if (localProb == 1)
{
break;
}
}
if (localProb == 0 || double.IsNaN(localProb))
{
return(false);
}
if (localProb > MaxProb)
{
MaxProb = localProb;
}
for (var i = 0; i < DirInd.Count; i++)
{
var dir = DisassemblyDirections.Directions[DirInd[i]];
if (closestFace.Normal.dotProduct(dir) > OverlappingFuzzification.CheckWithGlobDirs)
{
DirInd.Remove(DirInd[i]);
i--;
}
}
return(true);
}
public static List <Point[]> Get2DEdgesFromFace(PolygonalFace face, Point[] Points2D)
{
var edges3D = face.Edges;
var vertices = face.Vertices.ToList();
var edges2D = new List <Point[]>();
foreach (var edge in edges3D)
{
var edge2D = new Point[2];
edge2D[0] = Points2D[vertices.IndexOf(edge.From)];
edge2D[1] = Points2D[vertices.IndexOf(edge.To)];
edges2D.Add(edge2D);
}
return(edges2D);
}
/// <summary>
/// Creates the silhouette of the solid in the direction provided.
/// </summary>
/// <param name="tessellatedSolid">The tessellated solid.</param>
/// <param name="direction">The direction.</param>
/// <returns>Polygon.</returns>
public static Polygon CreateSilhouette(this TessellatedSolid tessellatedSolid, Vector3 direction)
{
direction = direction.Normalize();
var unvisitedFaces = tessellatedSolid.Faces.ToHashSet();
if (tessellatedSolid.Faces[0].Edges == null || tessellatedSolid.Faces[0].Edges.Count == 0)
{
tessellatedSolid.CompleteInitiation();
}
var linearTolerance = tessellatedSolid.SameTolerance;
var areaTolerance = linearTolerance * linearTolerance / Constants.BaseTolerance;
var dotTolerance = (areaTolerance > 0.002) ? 0.002 : areaTolerance;
// areaTolerance is used for the dot-product angle because dot is in units of length-squared
// but in rare cases the number may be quite large. so we set the max to 0.002 or 89.9 degrees - nearly orthogonal
var transform = direction.TransformToXYPlane(out _);
var polygons = new List <Polygon>();
while (true)
{
PolygonalFace startingFace = null;
var dot = 0.0;
foreach (var face in unvisitedFaces)
{
// get a face that does not have a dot product orthogonal to the direction
// notice that IsNegligible is used with the dotTolerance specified above
dot = face.Normal.Dot(direction);
if (!dot.IsNegligible(dotTolerance)) // && !face.Area.IsNegligible(areaTolerance))
{
startingFace = face;
break;
}
}
if (startingFace == null)
{
break; // the only way to exit the loop is here in the midst of the loop, hence
}
// the use of the while (true) above
unvisitedFaces.Remove(startingFace); //remove this from unvisitedFaces
var outerEdges = GetOuterEdgesOfContiguousPatch(unvisitedFaces, direction, Math.Sign(dot), startingFace);
// first we get the list of outerEdges of the patch of faces ("GetOuterEdgesOfContiguousPatch") in the same sense as the
// startingFace with the direction (that's the easy part), then we arrange those outerEdges into polygons in the
// function in "ArrangeOuterEdgesIntoPolygon".
polygons.AddRange(ArrangeOuterEdgesIntoPolygon(outerEdges, dot > 0, transform, linearTolerance, areaTolerance));
}
//Presenter.ShowAndHang(polygons);
return(polygons.UnionPolygons(PolygonCollection.PolygonWithHoles, linearTolerance).LargestPolygon());
}
private List <FastenerAndNutPartition> CreatePartitions(PolygonalFace faceFromLongestSide, int numberOfPartitions)
{
var sortedEdges = GeometryFunctions.SortedEdgesOfTriangle(faceFromLongestSide);
var cornerVer = sortedEdges[0].First(sortedEdges[1].Contains);
var otherVerShertestEdge = sortedEdges[0].First(a => a != cornerVer);
var partitionGeneratorDirection = sortedEdges[1].First(a => a != cornerVer).Position.subtract(cornerVer.Position);
var stepVector = partitionGeneratorDirection.divide((double)numberOfPartitions);
var partis = new List <FastenerAndNutPartition>();
for (var i = 0; i < NumberOfPartitions; i++)
{
var prt = new FastenerAndNutPartition
{
Edge1 =
new[]
{
new Vertex(cornerVer.Position.add(stepVector.multiply(i))),
new Vertex(otherVerShertestEdge.Position.add(stepVector.multiply(i)))
},
Edge2 =
new[]
{
new Vertex(cornerVer.Position.add(stepVector.multiply(i + 1))),
new Vertex(otherVerShertestEdge.Position.add(stepVector.multiply(i + 1)))
}
};
if (i == 0)
{
prt.Edge1 = new[]
{
new Vertex(cornerVer.Position.add(stepVector.multiply(-0.1))),
new Vertex(otherVerShertestEdge.Position.add(stepVector.multiply(-0.1)))
};
}
if (i == NumberOfPartitions - 1)
{
prt.Edge2 = new[]
{
new Vertex(cornerVer.Position.add(stepVector.multiply(i + 1.1))),
new Vertex(otherVerShertestEdge.Position.add(stepVector.multiply(i + 1.1)))
};
}
partis.Add(prt);
}
return(partis);
}
private static double[] FaceCenterFinder(PolygonalFace side)
{
var longestEdge = new Vertex[2];
var maxLength = double.NegativeInfinity;
for (var i = 0; i < side.Vertices.Count - 1; i++)
{
for (var j = i + 1; j < side.Vertices.Count; j++)
{
var dis = GeometryFunctions.DistanceBetweenTwoVertices(side.Vertices[i].Position,
side.Vertices[j].Position);
if (dis > maxLength)
{
maxLength = dis;
longestEdge = new[] { side.Vertices[i], side.Vertices[j] };
}
}
}
return((longestEdge[0].Position.add(longestEdge[1].Position)).divide(2.0));
}
private void ComputeStabilityMetric(Vertex Point, double[] insertionAxis)
{
// Find fixed face for a given subassembly
var maxDot = 0.001;
var FixedFaces = new List <PolygonalFace>();
var FixedFace_Vertices = new List <Vertex>();
var FixedFace_Normal = new double[3] {
insertionAxis[0],
insertionAxis[1],
insertionAxis[2]
};
for (var i = 0; i < convexHullFaces.Count(); i++)
{
PolygonalFace F = convexHullFaces[i];
var Dot = F.Normal[0] * insertionAxis[0] +
F.Normal[1] * insertionAxis[1] +
F.Normal[2] * insertionAxis[2];
if (Dot > 0 & Dot < maxDot)
{
FixedFaces.Add(F);
FixedFace_Vertices.AddRange(F.Vertices.ToList());
}
}
// Find stability metrics
if (FixedFaces.Count() > 0)
{
double h = 0;
var CoM = PolygonCoM(FixedFace_Vertices); // This is an approximation of center of mass
var projected_point = LinePlaneIntersection(FixedFace_Normal, CoM, Point, ref h);
var boundaryEdges = findBoundaryEdges(FixedFaces, FixedFace_Vertices);
var d = pointPolygonMinDistance(projected_point, boundaryEdges);
var Area = pointPolygonAtt(projected_point, FixedFaces, ref isStable);
SPF = (2 / Math.PI) * Math.Abs(Math.Atan2(h, Math.Abs(d)));
}
else
{
SPF = 1;
}
}
private static double[] ShortestEdgeMidPointOfTriangle(PolygonalFace triangle)
{
var shortestEdge = new Vertex[2];
var shortestDist = double.PositiveInfinity;
for (var i = 0; i < triangle.Vertices.Count - 1; i++)
{
for (var j = i + 1; j < triangle.Vertices.Count; j++)
{
var dis = GeometryFunctions.DistanceBetweenTwoVertices(triangle.Vertices[i].Position,
triangle.Vertices[j].Position);
if (dis >= shortestDist)
{
continue;
}
shortestDist = dis;
shortestEdge = new[] { triangle.Vertices[i], triangle.Vertices[j] };
}
}
return((shortestEdge[0].Position.add(shortestEdge[1].Position)).divide(2.0));
}
private static Vertex[] CornerEdgeFinder(PolygonalFace polygonalFace)
{
// We want to find the second long edge:
var dist0 = GeometryFunctions.DistanceBetweenTwoVertices(polygonalFace.Vertices[0].Position,
polygonalFace.Vertices[1].Position);
var dist1 = GeometryFunctions.DistanceBetweenTwoVertices(polygonalFace.Vertices[0].Position,
polygonalFace.Vertices[2].Position);
var dist2 = GeometryFunctions.DistanceBetweenTwoVertices(polygonalFace.Vertices[1].Position,
polygonalFace.Vertices[2].Position);
if ((dist0 > dist1 && dist0 < dist2) || (dist0 > dist2 && dist0 < dist1))
{
return new[] { polygonalFace.Vertices[0], polygonalFace.Vertices[1] }
}
;
if ((dist1 > dist0 && dist1 < dist2) || (dist1 > dist2 && dist1 < dist0))
{
return new[] { polygonalFace.Vertices[0], polygonalFace.Vertices[2] }
}
;
return(new[] { polygonalFace.Vertices[1], polygonalFace.Vertices[2] });
}
public static bool RayIntersectsWithFace2(Ray ray, PolygonalFace face)
{
if (ray.Direction.dotProduct(face.Normal) > -0.06)
{
return(false);
}
var raysPointOnFacePlane = MiscFunctions.PointOnPlaneFromRay(face.Normal,
face.Vertices[0].Position.dotProduct(face.Normal), ray.Position, ray.Direction);
if (raysPointOnFacePlane == null)
{
return(false);
}
var crossProductsToCorners = new List <double[]>();
for (int i = 0; i < 3; i++)
{
var j = (i == 2) ? 0 : i + 1;
var crossProductsFrom_i_To_j =
face.Vertices[i].Position.subtract(raysPointOnFacePlane).normalize()
.crossProduct(face.Vertices[j].Position.subtract(ray.Position).normalize());
if (crossProductsFrom_i_To_j.norm2(true) < Assembly_Planner.Constants.NearlyOnLine)
{
return(false);
}
crossProductsToCorners.Add(crossProductsFrom_i_To_j);
}
for (int i = 0; i < 3; i++)
{
var j = (i == 2) ? 0 : i + 1;
if (crossProductsToCorners[i].dotProduct(crossProductsToCorners[j], 3) <= 0)
{
return(false); // 0.15
}
}
return(true);
}
private static Dictionary <PolygonalFace, double> ThreeSidesOfTheObb(TessellatedSolid solid)
{
// This is based on my own OBB function:
// it returns a dictionary with size of three (3 sides).
// Key: triangle, value: length of the potential cylinder
var cornerVer = BoundingGeometry.OrientedBoundingBoxDic[solid].CornerVertices;
var face1 = new PolygonalFace(new[] { new Vertex(cornerVer[0].Position), new Vertex(cornerVer[1].Position),
new Vertex(cornerVer[3].Position) }, ((cornerVer[3].Position.subtract(cornerVer[0].Position)).crossProduct(
cornerVer[1].Position.subtract(cornerVer[0].Position))).normalize());
var length1 = GeometryFunctions.DistanceBetweenTwoVertices(cornerVer[0].Position, cornerVer[4].Position);
var face2 = new PolygonalFace(new[] { new Vertex(cornerVer[1].Position), new Vertex(cornerVer[0].Position),
new Vertex(cornerVer[4].Position) }, ((cornerVer[1].Position.subtract(cornerVer[0].Position)).crossProduct(
cornerVer[4].Position.subtract(cornerVer[0].Position))).normalize());
var length2 = GeometryFunctions.DistanceBetweenTwoVertices(cornerVer[0].Position, cornerVer[3].Position);
var face3 = new PolygonalFace(new[] { new Vertex(cornerVer[0].Position), new Vertex(cornerVer[3].Position),
new Vertex(cornerVer[7].Position) }, ((cornerVer[0].Position.subtract(cornerVer[3].Position)).crossProduct(
cornerVer[7].Position.subtract(cornerVer[3].Position))).normalize());
var length3 = GeometryFunctions.DistanceBetweenTwoVertices(cornerVer[0].Position, cornerVer[1].Position);
return(new Dictionary <PolygonalFace, double> {
{ face1, length1 }, { face2, length2 }, { face3, length3 }
});
}
public static bool RayIntersectsWithFace(Ray ray, PolygonalFace face)
{
if (ray.Direction.dotProduct(face.Normal) > -0.06)
{
return(false);
}
var w = ray.Position.subtract(face.Vertices[0].Position);
var s1 = (face.Normal.dotProduct(w)) / (face.Normal.dotProduct(ray.Direction));
//var v = new double[] { w[0] + s1 * ray.Direction[0] + point[0], w[1] + s1 * ray.Direction[1] + point[1], w[2] + s1 * ray.Direction[2] + point[2] };
//var v = new double[] { ray.Position[0] - s1 * ray.Direction[0], ray.Position[1] - s1 * ray.Direction[1], ray.Position[2] - s1 * ray.Direction[2] };
var pointOnTrianglesPlane = new[]
{
ray.Position[0] - s1 * ray.Direction[0], ray.Position[1] - s1 * ray.Direction[1],
ray.Position[2] - s1 * ray.Direction[2]
};
if (pointOnTrianglesPlane.subtract(ray.Position).dotProduct(ray.Direction) < 0.001)
{
return(false); // on the opposite side
}
var v0 = face.Vertices[0].Position.subtract(pointOnTrianglesPlane);
var v1 = face.Vertices[1].Position.subtract(pointOnTrianglesPlane);
var v2 = face.Vertices[2].Position.subtract(pointOnTrianglesPlane);
var crossv0v1 = v0.crossProduct(v1);
var crossv1v2 = v1.crossProduct(v2);
var dot = crossv0v1.dotProduct(crossv1v2);
if (dot < 2.1)
{
return(false);
}
var crossv2v0 = v2.crossProduct(v0);
dot = crossv1v2.dotProduct(crossv2v0);
return(dot >= 2.1);
}
public override void UpdateWith(PolygonalFace face)
{
base.UpdateWith(face);
}
/// <summary>
/// Updates the with.
/// </summary>
/// <param name="face">The face.</param>
public override void UpdateWith(PolygonalFace face)
{
var numFaces = Faces.Count;
double[] inBetweenPoint;
var distance = GeometryFunctions.SkewedLineIntersection(face.Center, face.Normal, Anchor, Axis, out inBetweenPoint);
var fractionToMove = 1 / numFaces;
var moveVector = Anchor.crossProduct(face.Normal);
if (moveVector.dotProduct(face.Center.subtract(inBetweenPoint)) < 0)
moveVector = moveVector.multiply(-1);
moveVector.normalizeInPlace();
/**** set new Anchor (by averaging in with last n values) ****/
Anchor = Anchor.add(new[] { moveVector[0] * fractionToMove * distance, moveVector[1] * fractionToMove * distance, moveVector[2] * fractionToMove * distance });
/* to adjust the Axis, we will average the cross products of the new face with all the old faces */
var totalAxis = new double[3];
for (var i = 0; i < numFaces; i++)
{
var newAxis = face.Normal.crossProduct(Faces[i].Normal);
if (newAxis.dotProduct(Axis) < 0)
newAxis.multiply(-1);
totalAxis = totalAxis.add(newAxis);
}
var numPrevCrossProducts = numFaces * (numFaces - 1) / 2;
totalAxis = totalAxis.add(Axis.multiply(numPrevCrossProducts));
/**** set new Axis (by averaging in with last n values) ****/
Axis = totalAxis.divide((numFaces + numPrevCrossProducts)).normalize();
foreach (var v in face.Vertices)
if (!Vertices.Contains(v))
Vertices.Add(v);
var totalOfRadii = Vertices.Sum(v => GeometryFunctions.DistancePointToLine(v.Position, Anchor, Axis));
/**** set new Radius (by averaging in with last n values) ****/
Radius = totalOfRadii / Vertices.Count;
base.UpdateWith(face);
}
public virtual void UpdateWith(PolygonalFace face)
{
Area += face.Area;
foreach (var v in face.Vertices)
if (!Vertices.Contains(v))
Vertices.Add(v);
foreach (var e in face.Edges)
{
if (InnerEdges.Contains(e)) continue; //throw new Exception("edge of new face is already an inner edge of surface, WTF?!");
if (OuterEdges.Contains(e))
{
OuterEdges.Remove(e);
InnerEdges.Add(e);
}
else OuterEdges.Add(e);
}
Faces.Add(face);
}
internal static void SolidTrianglesOfPartitions(List <FastenerAndNutPartition> partitions, TessellatedSolid solid, PolygonalFace faceFromLongestSide)
{
foreach (var vertex in solid.Vertices)
{
var ray = new Ray(new Vertex(vertex.Position), faceFromLongestSide.Normal);
var intersectionPoint = GeometryFunctions.RayIntersectionPointWithFace(ray, faceFromLongestSide);
var chosenPrtn = PartitionOfThePoint(partitions, intersectionPoint);
chosenPrtn.VerticesOfSolidInPartition.Add(vertex);
}
foreach (var face in solid.Faces)
{
var inds = new List <int>();
foreach (var fV in face.Vertices)
{
var prt = partitions.First(p => p.VerticesOfSolidInPartition.Contains(fV));
inds.Add(partitions.IndexOf(prt));
}
for (var i = inds.Min(); i <= inds.Max(); i++)
{
partitions[i].FacesOfSolidInPartition.Add(face);
}
}
}
/// <summary>
/// Determines whether [is new member of] [the specified face].
/// </summary>
/// <param name="face">The face.</param>
/// <returns><c>true</c> if [is new member of] [the specified face]; otherwise, <c>false</c>.</returns>
public override bool IsNewMemberOf(PolygonalFace face)
{
if (Faces.Contains(face)) return false;
if (!StarMath.IsPracticallySame(face.Normal.dotProduct(Normal), 1.0))
return false;
foreach (var v in face.Vertices)
if (Math.Abs(v.Position.dotProduct(Normal) - DistanceToOrigin) > Constants.ErrorForFaceInSurface * Math.Abs(DistanceToOrigin))
return false;
return true;
}
/// <summary>
/// Initializes a new instance of the <see cref="GaussSphereArc" /> class.
/// </summary>
/// <param name="edge">The edge.</param>
/// <param name="toFace">To face.</param>
internal GaussSphereArc(Edge edge, PolygonalFace toFace)
{
Edge = edge;
ToFace = toFace;
}
/// <summary>
/// Updates the with.
/// </summary>
/// <param name="face">The face.</param>
/// <exception cref="System.NotImplementedException"></exception>
public override void UpdateWith(PolygonalFace face)
{
throw new NotImplementedException();
}
internal void AddFaces(IList<PolygonalFace> facesToAdd)
{
var numToAdd = facesToAdd.Count();
var newFaces = new PolygonalFace[NumberOfFaces + numToAdd];
for (int i = 0; i < NumberOfFaces; i++)
newFaces[i] = Faces[i];
for (int i = 0; i < numToAdd; i++)
newFaces[NumberOfFaces + i] = facesToAdd[i];
Faces = newFaces;
NumberOfFaces += numToAdd;
}
private static bool OnTheWrongSide(Ray ray, PolygonalFace face)
{
return(ray.Direction.dotProduct(ray.Position.subtract(face.Vertices[0].Position)) > 0);
}
internal static Boolean FindNegativeAndPositiveFaces(Flat plane, Vertex onPlaneVertex, double[] vertexDistancesToPlane, out PolygonalFace negativeFace, out PolygonalFace positiveFace)
{
negativeFace = null;
positiveFace = null;
foreach (var face in onPlaneVertex.Faces)
{
var otherEdge = face.OtherEdge(onPlaneVertex);
var toDistance = vertexDistancesToPlane[otherEdge.To.IndexInList];
var fromDistance = vertexDistancesToPlane[otherEdge.From.IndexInList];
if ((toDistance > 0 && fromDistance < 0) || (toDistance < 0 && fromDistance > 0))
if ((toDistance > 0) == (face == otherEdge.OwnedFace))
positiveFace = face;
else negativeFace = face;
}
return (negativeFace != null && positiveFace != null);
}
internal ThroughVertexContactElement(Vertex onPlaneVertex, PolygonalFace negativeFace, PolygonalFace positiveFace)
{
StartVertex = onPlaneVertex;
SplitFacePositive = positiveFace;
SplitFaceNegative = negativeFace;
}
public abstract Boolean IsNewMemberOf(PolygonalFace face);
/// <summary>
/// Divides up contact.
/// </summary>
/// <param name="ts">The ts.</param>
/// <param name="contactData">The contact data.</param>
/// <param name="plane">The plane.</param>
/// <exception cref="System.Exception">face is supposed to be split at plane but lives only on one side</exception>
private static void DivideUpContact(TessellatedSolid ts, ContactData contactData, Flat plane)
{
var edgesToAdd = new List<Edge>();
var facesToAdd = new List<PolygonalFace>();
var verticesToAdd = new List<Vertex>();
var edgesToDelete = new List<Edge>();
var facesToDelete = new List<PolygonalFace>();
var edgesToModify = new List<Edge>();
foreach (var loop in contactData.AllLoops)
{
for (int i = 0; i < loop.Count; i++)
{
var ce = loop[i];
if (ce is CoincidentEdgeContactElement)
// If the contact element is at a coincident edge, then there is nothing to do in this stage. When contact element was
// created, it properly defined SplitFacePositive and SplitFaceNegative.
continue;
edgesToAdd.Add(ce.ContactEdge); // the contact edge is a new edge for the solid
edgesToModify.Add(ce.ContactEdge); // the contact edge will need to be linked to vertices and faces further down.
var faceToSplit = ce.SplitFacePositive; //faceToSplit will be removed, but before we do that, we use
facesToDelete.Add(faceToSplit); // use it to build the new 2 to 3 triangles
PolygonalFace positiveFace, negativeFace;
if (ce is ThroughVertexContactElement)
{
var vertPlaneDistances = //signed distances of faceToSplit's vertices from the plane
faceToSplit.Vertices.Select(
v => v.Position.dotProduct(plane.Normal) - plane.DistanceToOrigin).ToArray();
var maxIndex = vertPlaneDistances.FindIndex(vertPlaneDistances.Max());
var maxVert = faceToSplit.Vertices[maxIndex];
var minIndex = vertPlaneDistances.FindIndex(vertPlaneDistances.Min());
var minVert = faceToSplit.Vertices[minIndex];
positiveFace = new PolygonalFace(new[] { ce.ContactEdge.From, ce.ContactEdge.To, maxVert });
facesToAdd.Add(positiveFace);
negativeFace = new PolygonalFace(new[] { ce.ContactEdge.To, ce.ContactEdge.From, minVert });
facesToAdd.Add(negativeFace);
} //#+1 add v to f (both of these are done in the preceding PolygonalFace
//#+2 add f to v constructors as well as the one for thirdFace below)
else // then ce is a ThroughFaceContactElement
{
var tfce = (ThroughFaceContactElement)ce; // ce is renamed and recast as tfce
edgesToDelete.Add(tfce.SplitEdge);
verticesToAdd.Add(tfce.StartVertex);
Vertex positiveVertex, negativeVertex;
if (tfce.SplitEdge.To.Position.dotProduct(plane.Normal) > plane.DistanceToOrigin)
{
positiveVertex = tfce.SplitEdge.To;
negativeVertex = tfce.SplitEdge.From;
}
else
{
positiveVertex = tfce.SplitEdge.From;
negativeVertex = tfce.SplitEdge.To;
}
positiveFace =
new PolygonalFace(new[] { ce.ContactEdge.From, ce.ContactEdge.To, positiveVertex });
facesToAdd.Add(positiveFace);
negativeFace =
new PolygonalFace(new[] { ce.ContactEdge.To, ce.ContactEdge.From, negativeVertex });
facesToAdd.Add(negativeFace);
var positiveEdge = new Edge(positiveVertex, ce.ContactEdge.From, positiveFace, null, true,
true);
edgesToAdd.Add(positiveEdge);
edgesToModify.Add(positiveEdge);
var negativeEdge = new Edge(ce.ContactEdge.From, negativeVertex, negativeFace, null, true,
true);
edgesToAdd.Add(negativeEdge);
edgesToModify.Add(negativeEdge);
var otherVertex = faceToSplit.Vertices.First(v => v != positiveVertex && v != negativeVertex);
PolygonalFace thirdFace;
if (otherVertex.Position.dotProduct(plane.Normal) > plane.DistanceToOrigin)
{
thirdFace = new PolygonalFace(new[] { ce.ContactEdge.To, otherVertex, positiveVertex });
facesToAdd.Add(thirdFace);
edgesToAdd.Add(new Edge(ce.ContactEdge.To, positiveVertex, positiveFace, thirdFace, true, true));
}
else
{
thirdFace = new PolygonalFace(new[] { ce.ContactEdge.To, negativeVertex, otherVertex });
facesToAdd.Add(thirdFace);
edgesToAdd.Add(new Edge(negativeVertex, ce.ContactEdge.To, negativeFace, thirdFace, true, true));
}
// for the new edges in a through face this line accomplishes: +3 add f to e; +4 add e to f; +5 add v to e;
// +6 add e to v
}
loop[i] = new CoincidentEdgeContactElement
{
ContactEdge = ce.ContactEdge,
EndVertex = ce.ContactEdge.To,
StartVertex = ce.ContactEdge.From,
SplitFaceNegative = negativeFace,
SplitFacePositive = positiveFace
};
}
}
// -1 remove v from f - no need to do this as no v's are removed
foreach (var face in facesToDelete)
{
foreach (var vertex in face.Vertices)
vertex.Faces.Remove(face); //-2 remove f from v
foreach (var edge in face.Edges)
{
if (edgesToDelete.Contains(edge)) continue;
edgesToModify.Add(edge);
if (edge.OwnedFace == face) edge.OwnedFace = null; //-3 remove f from e
else edge.OtherFace = null;
}
}
//-4 remove e from f - no need to do as the only edges deleted are the ones between deleted faces
ts.RemoveFaces(facesToDelete);
// -5 remove v from e - not needed as no vertices are deleted (like -1 above)
foreach (var edge in edgesToDelete)
{
edge.From.Edges.Remove(edge); //-6 remove e from v
edge.To.Edges.Remove(edge);
}
ts.RemoveEdges(edgesToDelete);
// now to add new faces to modified edges
ts.AddVertices(verticesToAdd);
ts.AddFaces(facesToAdd);
foreach (var edge in edgesToModify)
{
var facesToAttach = facesToAdd.Where(f => f.Vertices.Contains(edge.To) && f.Vertices.Contains(edge.From)
&& !f.Edges.Contains(edge));
if (facesToAttach.Count() > 2) throw new Exception();
foreach (var face in facesToAttach)
{
face.Edges.Add(edge); //+4 add e to f
var fromIndex = face.Vertices.IndexOf(edge.From);
if ((fromIndex == face.Vertices.Count - 1 && face.Vertices[0] == edge.To)
|| (fromIndex < face.Vertices.Count - 1 && face.Vertices[fromIndex + 1] == edge.To))
edge.OwnedFace = face; //+3 add f to e
else edge.OtherFace = face;
}
}
ts.AddEdges(edgesToAdd);
}
/// <summary>
/// Makes the faces.
/// </summary>
/// <param name="normals">The normals.</param>
private void MakeFaces(IList<double[]> normals)
{
NumberOfFaces = normals.Count;
Faces = new PolygonalFace[NumberOfFaces];
for (var i = 0; i < NumberOfFaces; i++)
{
/* the normal vector read in the from the file should already be a unit vector,
* but just to be certain, and to increase the precision since most numbers in an
* STL or similar file are only 10 characters or so (and that often includes E-001) */
var normal = normals[i].normalize();
if (normal.Any(double.IsNaN)) normal = new double[3];
Faces[i] = new PolygonalFace(normal);
}
}
internal void AddFace(PolygonalFace newFace)
{
var newFaces = new PolygonalFace[NumberOfFaces + 1];
for (int i = 0; i < NumberOfFaces; i++)
newFaces[i] = Faces[i];
newFaces[NumberOfFaces] = newFace;
Faces = newFaces;
NumberOfFaces++;
}
private Edge[] MakeEdges(PolygonalFace[] localFaces, Vertex[] localVertices)
{
NumberOfEdges = 3 * NumberOfFaces / 2;
var alreadyDefinedEdges = new Dictionary<int, Edge>();
for (var i = 0; i < NumberOfFaces; i++)
{
var face = localFaces[i];
var lastIndex = face.Vertices.Count - 1;
for (var j = 0; j <= lastIndex; j++)
{
var fromIndex = face.Vertices[j].IndexInList;
var toIndex = (j == lastIndex)
? face.Vertices[0].IndexInList
: face.Vertices[j + 1].IndexInList;
if (fromIndex == toIndex) throw new Exception("edge to same vertices.");
var checksum = (fromIndex < toIndex)
? fromIndex + (NumberOfVertices * toIndex)
: toIndex + (NumberOfVertices * fromIndex);
if (alreadyDefinedEdges.ContainsKey(checksum))
{
var edge = alreadyDefinedEdges[checksum];
edge.OtherFace = face;
face.Edges.Add(edge);
}
else
{
var edge = new Edge(localVertices[fromIndex], localVertices[toIndex], face, null, true, true);
alreadyDefinedEdges.Add(checksum, edge);
}
}
}
var badEdges = new List<Edge>();
foreach (var edge in alreadyDefinedEdges.Values)
if (edge.OwnedFace == null || edge.OtherFace == null)
{
badEdges.Add(edge);
edge.OwnedFace = edge.OtherFace = edge.OwnedFace ?? edge.OtherFace;
Debug.WriteLine("Edge found with only face (face normal = " +
edge.OwnedFace.Normal.MakePrintString()
+ ", between vertices " + edge.From.Position.MakePrintString() + " & " +
edge.To.Position.MakePrintString());
}
var localEdges = alreadyDefinedEdges.Values.ToArray();
NumberOfEdges = localEdges.GetLength(0);
return localEdges;
}
/// <summary>
/// Determines whether [is new member of] [the specified face].
/// </summary>
/// <param name="face">The face.</param>
/// <returns><c>true</c> if [is new member of] [the specified face]; otherwise, <c>false</c>.</returns>
/// <exception cref="System.NotImplementedException"></exception>
public override bool IsNewMemberOf(PolygonalFace face)
{
throw new NotImplementedException();
}
/// <summary>
/// Creates the convex hull.
/// </summary>
private void CreateConvexHull()
{
var convexHull = ConvexHull.Create<Vertex, DefaultConvexFace<Vertex>>(Vertices);
ConvexHullVertices = convexHull.Points.ToArray();
var numCvxFaces = convexHull.Faces.Count();
ConvexHullFaces = new PolygonalFace[numCvxFaces];
ConvexHullEdges = new Edge[3 * numCvxFaces / 2];
var faceIndex = 0;
var edgeIndex = 0;
foreach (var cvxFace in convexHull.Faces)
{
var newFace = new PolygonalFace(cvxFace.Normal) { Vertices = cvxFace.Vertices.ToList() };
//foreach (var v in newFace.Vertices)
// v.Faces.Add(newFace);
ConvexHullFaces[faceIndex++] = newFace;
}
faceIndex = 0;
foreach (var cvxFace in convexHull.Faces)
{
var newFace = ConvexHullFaces[faceIndex++];
for (var j = 0; j < cvxFace.Adjacency.GetLength(0); j++)
{
var adjacentOldFace = cvxFace.Adjacency[j];
if (newFace.Edges.Count <= j || newFace.Edges[j] == null)
{
var adjFaceIndex = convexHull.Faces.FindIndex(adjacentOldFace);
var adjFace = ConvexHullFaces[adjFaceIndex];
var sharedVerts = newFace.Vertices.Intersect(adjacentOldFace.Vertices).ToList();
var newEdge = new Edge(sharedVerts[0], sharedVerts[1], newFace, adjFace, true);
while (newFace.Edges.Count <= j) newFace.Edges.Add(null);
newFace.Edges[j] = newEdge;
var k = adjacentOldFace.Adjacency.FindIndex(cvxFace);
while (adjFace.Edges.Count <= k) adjFace.Edges.Add(null);
adjFace.Edges[k] = newEdge;
ConvexHullEdges[edgeIndex++] = newEdge;
}
}
}
}
/// <summary>
/// Copies this instance.
/// </summary>
/// <returns>TessellatedSolid.</returns>
public TessellatedSolid Copy()
{
var copyOfFaces = new PolygonalFace[NumberOfFaces];
for (var i = 0; i < NumberOfFaces; i++)
copyOfFaces[i] = Faces[i].Copy();
var copyOfVertices = new Vertex[NumberOfVertices];
for (var i = 0; i < NumberOfVertices; i++)
copyOfVertices[i] = Vertices[i].Copy();
for (var fIndex = 0; fIndex < NumberOfFaces; fIndex++)
{
var thisFace = copyOfFaces[fIndex];
var oldFace = Faces[fIndex];
var vertexIndices = new List<int>();
foreach (var oldVertex in oldFace.Vertices)
{
var vIndex = oldVertex.IndexInList;
vertexIndices.Add(vIndex);
var thisVertex = copyOfVertices[vIndex];
thisFace.Vertices.Add(thisVertex);
thisVertex.Faces.Add(thisFace);
}
}
Edge[] copyOfEdges = MakeEdges(copyOfFaces, copyOfVertices);
var copy = new TessellatedSolid
{
SurfaceArea = SurfaceArea,
Center = (double[])Center.Clone(),
Faces = copyOfFaces,
Vertices = copyOfVertices,
Edges = copyOfEdges,
Name = Name,
NumberOfFaces = NumberOfFaces,
NumberOfVertices = NumberOfVertices,
Volume = Volume,
XMax = XMax,
XMin = XMin,
YMax = YMax,
YMin = YMin,
ZMax = ZMax,
ZMin = ZMin
};
copy.CreateConvexHull();
return copy;
}
internal void RemoveFaces(List<int> removeIndices)
{
var offset = 0;
var numToRemove = removeIndices.Count;
removeIndices.Sort();
NumberOfFaces -= numToRemove;
var newFaces = new PolygonalFace[NumberOfFaces];
for (int i = 0; i < NumberOfFaces; i++)
{
while (offset < numToRemove && (i + offset) == removeIndices[offset])
offset++;
newFaces[i] = Faces[i + offset];
}
Faces = newFaces;
}
/// <summary>
/// Updates the with.
/// </summary>
/// <param name="face">The face.</param>
public override void UpdateWith(PolygonalFace face)
{
Normal = Normal.multiply(Faces.Count).add(face.Normal).divide(Faces.Count + 1);
Normal.normalizeInPlace();
var newVerts = new List<Vertex>();
var newDistanceToPlane = 0.0;
foreach (var v in face.Vertices)
if (!Vertices.Contains(v))
{
newVerts.Add(v);
newDistanceToPlane += v.Position.dotProduct(Normal);
}
DistanceToOrigin = (Vertices.Count * DistanceToOrigin + newDistanceToPlane) / (Vertices.Count + newVerts.Count);
base.UpdateWith(face);
}
internal FaceWithScores(PolygonalFace face)
{
Face = face;
}
/// <summary>
/// Determines whether [is new member of] [the specified face].
/// </summary>
/// <param name="face">The face.</param>
/// <returns><c>true</c> if [is new member of] [the specified face]; otherwise, <c>false</c>.</returns>
public override bool IsNewMemberOf(PolygonalFace face)
{
if (Faces.Contains(face)) return false;
if (Math.Abs(face.Normal.dotProduct(Axis)) > Constants.ErrorForFaceInSurface)
return false;
foreach (var v in face.Vertices)
if (Math.Abs(GeometryFunctions.DistancePointToLine(v.Position, Anchor, Axis) - Radius) > Constants.ErrorForFaceInSurface * Radius)
return false;
return true;
}
internal void RemoveFace(int removeFaceIndex)
{
NumberOfFaces--;
var newFaces = new PolygonalFace[NumberOfFaces];
for (int i = 0; i < removeFaceIndex; i++)
newFaces[i] = Faces[i];
for (int i = removeFaceIndex; i < NumberOfFaces; i++)
newFaces[i] = Faces[i + 1];
Faces = newFaces;
}
/// <summary>
/// Defines the edge angle.
/// </summary>
/// <param name="edges">The edges.</param>
private void DefineInternalEdgeAngle()
{
/* this is a tricky function. What we need to do is take the dot-product of the normals.
* which will give the cos(theta). Calling inverse cosine will result in a value from 0 to
* pi, but is the edge convex or concave? It is convex if the crossproduct of the normals is
* in the same direction as the edge vector (dot product is positive). But we need to know
* which face-normal goes first in the cross product calculation as this will change the
* resulting direction. The one to go first is the one that "owns" the edge. What I mean by
* own is that the from-to of the edge makes sense in the counter-clockwise prediction of
* the face normal. For one face the from-to will be incorrect (normal facing inwards) -
* in some geometry approaches this is solved by the concept of half-edges. Here we will
* just re-order the two faces referenced in the edge so that the first is the one that
* owns the edge...the face for which the direction makes sense, and the second face will
* need to reverse the edge vector to make it work out in a proper counter-clockwise loop
* for that face. */
if (_ownedFace == _otherFace || _ownedFace == null || _otherFace == null)
{
InternalAngle = double.NaN;
Curvature = CurvatureType.Undefined;
return;
}
var ownedFaceToIndex = _ownedFace.Vertices.IndexOf(To);
var ownedFaceNextIndex = (ownedFaceToIndex + 1 == _ownedFace.Vertices.Count) ? 0 : ownedFaceToIndex + 1;
var nextOwnedFaceVertex = _ownedFace.Vertices[ownedFaceNextIndex];
var nextEdgeVector = nextOwnedFaceVertex.Position.subtract(To.Position);
if (Vector.crossProduct(nextEdgeVector).dotProduct(_ownedFace.Normal) < 0)
{
/* then switch owned face and opposite face since the predicted normal
* is in the wrong direction. When OwnedFace and OppositeFace were defined
* it was arbitrary anyway - so this is another by-product of this method -
* correct the owned and opposite faces. */
var temp = _ownedFace;
_ownedFace = _otherFace;
_otherFace = temp;
}
var dot = _ownedFace.Normal.dotProduct(_otherFace.Normal, 3);
if (dot > 1.0 || StarMath.IsPracticallySame(dot, 1.0))
{
InternalAngle = Math.PI;
Curvature = CurvatureType.SaddleOrFlat;
}
else
{
var cross = _ownedFace.Normal.crossProduct(_otherFace.Normal);
if (cross.dotProduct(Vector) < 0)
{
InternalAngle = Math.PI + Math.Acos(dot);
Curvature = CurvatureType.Concave;
}
else
{
InternalAngle = Math.Acos(dot);
Curvature = CurvatureType.Convex;
}
}
//Debug.WriteLine("angle = " + (InternalAngle * (180 / Math.PI)).ToString() + "; " + SurfaceIs.ToString());
}
internal void RemoveFace(PolygonalFace removeFace)
{
RemoveFace(Faces.FindIndex(removeFace));
}
/// <summary>
/// Gets the in plane flat.
/// </summary>
/// <param name="startFace">The start face.</param>
/// <returns>Flat.</returns>
private static Flat GetInPlaneFlat(PolygonalFace startFace)
{
var flat = new Flat(new List<PolygonalFace>(new[] { startFace }));
var visitedFaces = new HashSet<PolygonalFace>(startFace.AdjacentFaces);
visitedFaces.Add(startFace);
var stack = new Stack<PolygonalFace>(visitedFaces);
while (stack.Any())
{
var face = stack.Pop();
if (!flat.IsNewMemberOf(face)) continue;
flat.UpdateWith(face);
foreach (var adjacentFace in face.AdjacentFaces)
if (!visitedFaces.Contains(adjacentFace))
{
visitedFaces.Add(adjacentFace);
stack.Push(adjacentFace);
}
}
return flat;
}
public override bool IsNewMemberOf(PolygonalFace face)
{
return false;
// todo
throw new NotImplementedException();
}
