internal static IEnumerable <Ray> AddingMoreRays(Edge[] edges, double[] dir)
{
//For the case that two objects are nonadacently blocking each other but the rays shot from the corner
//vertices cannot detect them, we will add more vertices here to solve this issue.
var sections = edges.Select(edge => new[] { new Vertex(edge.From.Position), new Vertex(edge.To.Position) }).ToList();
var newRays = new List <Ray>();
var counter = 0;
while (counter < 1)
{
counter++;
var section2 = new List <Vertex[]>();
foreach (var sec in sections)
{
var aP = sec[0];
var bP = sec[1];
var midPoint =
new Vertex(new[]
{
(aP.Position[0] + bP.Position[0]) / 2.0, (aP.Position[1] + bP.Position[1]) / 2.0,
(aP.Position[2] + bP.Position[2]) / 2.0
});
newRays.Add(new Ray(midPoint, dir));
section2.Add(new[] { aP, midPoint });
section2.Add(new[] { midPoint, bP });
}
sections = new List <Vertex[]>(section2);
}
return(newRays);
}
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);
}
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));
}
public static bool RayIntersectsWithFace(Ray ray, PolygonalFace face)
{
if (Math.Abs(ray.Direction.dotProduct(face.Normal)) < Assembly_Planner.Constants.NearlyParallelFace)
{
return(false);
}
var inPlaneVerts = new Vertex[3];
var negativeDirCounter = 3;
for (var i = 0; i < 3; i++)
{
var vectFromRayToFacePt = face.Vertices[i].Position.subtract(ray.Position); // Interesting
var dxtoPlane = ray.Direction.dotProduct(vectFromRayToFacePt);
if (dxtoPlane < 0)
{
negativeDirCounter--;
}
if (negativeDirCounter == 0)
{
return(false);
}
inPlaneVerts[i] = new Vertex(face.Vertices[i].Position.add(StarMath.multiply(-dxtoPlane, ray.Direction)));
}
if (inPlaneVerts.Min(v => v.Position[0]) > ray.Position[0])
{
return(false);
}
if (inPlaneVerts.Max(v => v.Position[0]) < ray.Position[0])
{
return(false);
}
if (inPlaneVerts.Min(v => v.Position[1]) > ray.Position[1])
{
return(false);
}
if (inPlaneVerts.Max(v => v.Position[1]) < ray.Position[1])
{
return(false);
}
if (inPlaneVerts.Min(v => v.Position[2]) > ray.Position[2])
{
return(false);
}
if (inPlaneVerts.Max(v => v.Position[2]) < ray.Position[2])
{
return(false);
}
if (inPlaneVerts.GetLength(0) > 3)
{
return(true);
}
var crossProductsToCorners = new List <double[]>();
for (int i = 0; i < 3; i++)
{
var j = (i == 2) ? 0 : i + 1;
var crossProductsFrom_i_To_j =
inPlaneVerts[i].Position.subtract(ray.Position)
.normalizeInPlace(3)
.crossProduct(inPlaneVerts[j].Position.subtract(ray.Position).normalizeInPlace(3));
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 bool newApproachBoundingBox(double[] v, double[] bB, double[] mB)
{
var mV1 = new Vertex(new[] { mB[0], mB[2], mB[4] });
var mV2 = new Vertex(new[] { mB[0], mB[3], mB[4] });
var mV3 = new Vertex(new[] { mB[1], mB[3], mB[4] });
var mV4 = new Vertex(new[] { mB[1], mB[2], mB[4] });
var mV5 = new Vertex(new[] { mB[0], mB[2], mB[5] });
var mV6 = new Vertex(new[] { mB[0], mB[3], mB[5] });
var mV7 = new Vertex(new[] { mB[1], mB[3], mB[5] });
var mV8 = new Vertex(new[] { mB[1], mB[2], mB[5] });
var movingVer = new List <Vertex> {
mV1, mV2, mV3, mV4, mV5, mV6, mV7, mV8
};
var mE1 = new Edge(mV1, mV2, true);
var mE2 = new Edge(mV1, mV4, true);
var mE3 = new Edge(mV1, mV5, true);
var mE4 = new Edge(mV2, mV3, true);
var mE5 = new Edge(mV2, mV6, true);
var mE6 = new Edge(mV7, mV6, true);
var mE7 = new Edge(mV7, mV3, true);
var mE8 = new Edge(mV7, mV8, true);
var mE9 = new Edge(mV5, mV6, true);
var mE10 = new Edge(mV5, mV8, true);
var mE11 = new Edge(mV4, mV3, true);
var mE12 = new Edge(mV4, mV8, true);
var movingEdg = new List <Edge> {
mE1, mE2, mE3, mE4, mE5, mE6, mE7, mE8, mE9, mE10, mE11, mE12
};
var bV1 = new Vertex(new[] { bB[0], bB[2], bB[4] });
var bV2 = new Vertex(new[] { bB[0], bB[3], bB[4] });
var bV3 = new Vertex(new[] { bB[1], bB[3], bB[4] });
var bV4 = new Vertex(new[] { bB[1], bB[2], bB[4] });
var bV5 = new Vertex(new[] { bB[0], bB[2], bB[5] });
var bV6 = new Vertex(new[] { bB[0], bB[3], bB[5] });
var bV7 = new Vertex(new[] { bB[1], bB[3], bB[5] });
var bV8 = new Vertex(new[] { bB[1], bB[2], bB[5] });
var blockingVer = new List <Vertex> {
bV1, bV2, bV3, bV4, bV5, bV6, bV7, bV8
};
var bE1 = new Edge(bV1, bV2, true);
var bE2 = new Edge(bV1, bV4, true);
var bE3 = new Edge(bV1, bV5, true);
var bE4 = new Edge(bV2, bV3, true);
var bE5 = new Edge(bV2, bV6, true);
var bE6 = new Edge(bV7, bV6, true);
var bE7 = new Edge(bV7, bV3, true);
var bE8 = new Edge(bV7, bV8, true);
var bE9 = new Edge(bV5, bV6, true);
var bE10 = new Edge(bV5, bV8, true);
var bE11 = new Edge(bV4, bV3, true);
var bE12 = new Edge(bV4, bV8, true);
var blockingEdg = new List <Edge> {
bE1, bE2, bE3, bE4, bE5, bE6, bE7, bE8, bE9, bE10, bE11, bE12
};
var movingProj = _3Dto2D.Get2DProjectionPoints(movingVer, v);
var blockingProj = _3Dto2D.Get2DProjectionPoints(blockingVer, v);
var moving2D = new _3Dto2D {
Points = movingProj, Edges = _3Dto2D.Get2DEdges2(movingEdg, movingVer, movingProj)
};
var blocking2D = new _3Dto2D {
Points = blockingProj, Edges = _3Dto2D.Get2DEdges2(blockingEdg, blockingVer, movingProj)
};
return(moving2D.Edges.Any(movEdge => blocking2D.Edges.Any(refEdge => NonadjacentBlockingDeterminationPro.DoIntersect(movEdge, refEdge))));
}
/// <summary>
/// Initializes a new instance of the <see cref="Point" /> class.
/// </summary>
/// <param name="vertex">The vertex.</param>
/// <param name="x">The x.</param>
/// <param name="y">The y.</param>
public Point(Vertex vertex, double x, double y)
: this(vertex, x, y, 0.0)
{
}
/// <summary>
/// Initializes a new instance of the <see cref="Point" /> class.
/// </summary>
/// <param name="v">The v.</param>
public Point(Vertex v)
: this(v, v.Position[0], v.Position[1], 0.0)
{
}
