private static double TwoTrianglesParallelCheck(double[] aNormal, double[] bNormal) { // they must be parralel but in the opposite direction. // This function can be fuzzified in order to give a certainty to the connection return(OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.ParralelLinesL, OverlappingFuzzification.ParralelLinesU, Math.Abs(bNormal.dotProduct(aNormal) + 1))); }
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 PosSphereNegSphereOverlappingCheck(Sphere primitiveA, Sphere primitiveB) { //postive(A)-negative(B) // if their centers are the same or really close // if their radius is equal or close var centerDif = primitiveA.Center.subtract(primitiveB.Center); var p0 = OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.PointPointL, OverlappingFuzzification.PointPointU, Math.Abs(centerDif[0])); var p1 = OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.PointPointL, OverlappingFuzzification.PointPointU, Math.Abs(centerDif[1])); var p2 = OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.PointPointL, OverlappingFuzzification.PointPointU, Math.Abs(centerDif[2])); if (p0 > 0 && p1 > 0 && p2 < 0) { if (Math.Abs(primitiveA.Radius - primitiveB.Radius) < 0.001) { return(true); } } return(false); }
internal static bool ProximityFastener(TessellatedSolid solid1, TessellatedSolid solid2) { var OverlapAABBPartitions = new List <PartitionAABB[]>(); PartitionOverlapFinder(PartitioningSolid.PartitionsAABB[solid1], PartitioningSolid.PartitionsAABB[solid2], OverlapAABBPartitions); var memoFace = new HashSet <HashSet <PolygonalFace> >(HashSet <PolygonalFace> .CreateSetComparer()); var counter1 = 0; var counter2 = 0; var counter3 = 0; var counter4 = 0; // first one is for solid1, second one is for solid2 var finalProb = 0.0; foreach (var overlapPrtn in OverlapAABBPartitions) { foreach (var a in overlapPrtn[0].SolidTriangles) { if (a.Vertices.Count < 3) { continue; } foreach (var b in overlapPrtn[1].SolidTriangles) { if (b.Vertices.Count < 3) { continue; } var newSet = new HashSet <PolygonalFace> { a, b }; if (memoFace.Contains(newSet)) { continue; } memoFace.Add(newSet); counter1++; var localProb = 0.0; var parallel = Math.Abs(a.Normal.dotProduct(b.Normal) + 1); var probPara = OverlappingFuzzification.FuzzyProbabilityCalculator(0.0055, 0.01, parallel); if (probPara == 0) { continue; // 0.0055 } // if they are on the wrong side of each other if (a.Vertices.All(av => (av.Position.subtract(b.Vertices[0].Position)).dotProduct(b.Normal) < 0.0) || b.Vertices.All(bv => (bv.Position.subtract(a.Vertices[0].Position)).dotProduct(a.Normal) < 0.0)) { continue; } counter2++; var aAverageEdgeLength = a.Edges.Sum(e => e.Length) / 3.0; var bAverageEdgeLength = b.Edges.Sum(e => e.Length) / 3.0; var q = a.Center; var p = b.Center; var pq = q.subtract(p); var qp = p.subtract(q); var devisionFactor = Math.Min(aAverageEdgeLength, bAverageEdgeLength); var samePlane1 = Math.Abs(pq.dotProduct(a.Normal)) / devisionFactor; // I need to devide it by a better factor var samePlane2 = Math.Abs(qp.dotProduct(b.Normal)) / devisionFactor; var probPlane1 = OverlappingFuzzification.FuzzyProbabilityCalculator(0.1, 0.6, samePlane1); //0.4, 0.5 var probPlane2 = OverlappingFuzzification.FuzzyProbabilityCalculator(0.1, 0.6, samePlane2); //0.4, 0.5 if (probPlane1 == 0 && probPlane2 == 0) { continue; //0.11 //0.005 } counter3++; if (!TriangleOverlapping(a, b)) { continue; } return(true); } } } if (counter3 > 0) { Fastener.PotentialCollisionOfFastenerAndSolid.Add(solid2.Name); } else if (counter2 > 0) { Fastener.PotentialCollisionOfFastenerAndSolidStep2.Add(solid2.Name); } else if (counter1 > 0) { Fastener.PotentialCollisionOfFastenerAndSolidStep3.Add(solid2.Name); } return(false); }
internal static bool ProximityBoosted(TessellatedSolid solid1, TessellatedSolid solid2, List <int> localDirInd, out double certainty) { var OverlapAABBPartitions = new List <PartitionAABB[]>(); PartitionOverlapFinder(PartitioningSolid.PartitionsAABB[solid1], PartitioningSolid.PartitionsAABB[solid2], OverlapAABBPartitions); var memoFace = new HashSet <HashSet <PolygonalFace> >(HashSet <PolygonalFace> .CreateSetComparer()); var memoNormal = new HashSet <double[]>(); var counter1 = 0; var counter2 = 0; var counter3 = 0; var counter4 = 0; // first one is for solid1, second one is for solid2 var triTriOver = new HashSet <PolygonalFace[]>(); var finalProb = 0.0; foreach (var overlapPrtn in OverlapAABBPartitions) { foreach (var a in overlapPrtn[0].SolidTriangles) { if (a.Vertices.Count < 3) { continue; } foreach (var b in overlapPrtn[1].SolidTriangles) { if (b.Vertices.Count < 3) { continue; } var newSet = new HashSet <PolygonalFace> { a, b }; if (memoFace.Contains(newSet)) { continue; } memoFace.Add(newSet); counter4++; var localProb = 0.0; var parallel = Math.Abs(a.Normal.dotProduct(b.Normal) + 1); var probPara = OverlappingFuzzification.FuzzyProbabilityCalculator(0.0055, 0.006, parallel); if (probPara == 0) { continue; // 0.0055 } var aAverageEdgeLength = a.Edges.Sum(e => e.Length) / 3.0; var bAverageEdgeLength = b.Edges.Sum(e => e.Length) / 3.0; // if they are on the wrong side of each other if (a.Vertices.All(av => (av.Position.subtract(b.Vertices[0].Position)).dotProduct(b.Normal) / aAverageEdgeLength < -1e-1) || b.Vertices.All(bv => (bv.Position.subtract(a.Vertices[0].Position)).dotProduct(a.Normal) / bAverageEdgeLength < -1e-1)) { continue; } counter2++; var q = a.Center; var p = b.Center; var pq = q.subtract(p); var qp = p.subtract(q); var devisionFactor = Math.Min(aAverageEdgeLength, bAverageEdgeLength); var samePlane1 = Math.Abs(pq.dotProduct(a.Normal)) / devisionFactor; // I need to devide it by a better factor var samePlane2 = Math.Abs(qp.dotProduct(b.Normal)) / devisionFactor; var probPlane1 = OverlappingFuzzification.FuzzyProbabilityCalculator(0.1, 0.4, samePlane1); //0.4, 0.5 var probPlane2 = OverlappingFuzzification.FuzzyProbabilityCalculator(0.1, 0.4, samePlane2); //0.4, 0.5 if (probPlane1 == 0 || probPlane2 == 0) { continue; //0.11 //0.005 } counter3++; if (!TriangleOverlapping(a, b)) { continue; } triTriOver.Add(new[] { a, b }); counter1++; localProb = Math.Min(probPara, Math.Max(probPlane1, probPlane2)); if (localProb > finalProb) { finalProb = localProb; } var newNorm = b.Normal.multiply(-1); if (memoNormal.Count < localDirInd.Count * 2.0) { if (memoNormal.Any(n => Math.Abs(a.Normal.dotProduct(n) - 1) < 0.000001 || Math.Abs(newNorm.dotProduct(n) - 1) < 0.000001)) { continue; } } var delete1 = localDirInd.Where(dir => a.Normal.dotProduct(DisassemblyDirections.Directions[dir]) < -0.06).ToList(); List <int> delete2; if (Math.Abs(a.Normal.dotProduct(newNorm) - 1) < 0.000001) { delete2 = delete1; } else { delete2 = localDirInd.Where(dir => newNorm.dotProduct(DisassemblyDirections.Directions[dir]) < -0.06).ToList(); } if (delete1.Count() < delete2.Count()) { foreach (var i1 in delete1) { localDirInd.Remove(i1); } memoNormal.Add(a.Normal); } else { foreach (var i1 in delete2) { localDirInd.Remove(i1); } memoNormal.Add(newNorm); } //break; } } } certainty = finalProb; if (counter1 == 0) { return(false); } var primOver = new List <PrimitiveSurface[]>(); foreach (var tto in triTriOver) { var prim1 = DisassemblyDirectionsWithFastener.SolidPrimitive[solid1].Where(sp => sp.Faces.Contains(tto[0])).ToList(); var prim2 = DisassemblyDirectionsWithFastener.SolidPrimitive[solid2].Where(sp => sp.Faces.Contains(tto[1])).ToList(); if (!prim1.Any() || !prim2.Any()) { continue; } if (primOver.Any(p => p[0] == prim1[0] && p[1] == prim2[0])) { continue; } primOver.Add(new[] { prim1[0], prim2[0] }); } FilteroutBadPrimitiveProximities(primOver); OverlappingSurfaces.Add(new OverlappedSurfaces { Solid1 = solid1, Solid2 = solid2, Overlappings = primOver }); return(true); }
private static bool NegCylinderPosCylinderOverlappingCheck(Cylinder cylinder1, Cylinder cylinder2, int reference) { // this is actually positive cylinder with negative cylinder. primitiveA is negative cylinder and // primitiveB is positive cylinder. Like a normal // check the centerlines. Is the vector of the center lines the same? // now check the radius. // Update: I need to consider one more case: half cylinders : it's already considered var partialCylinder1 = false; var localProb = 0.0; var p0 = OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.ParralelLinesL, OverlappingFuzzification.ParralelLinesU, Math.Abs(cylinder1.Axis.dotProduct(cylinder2.Axis)) - 1); if (p0 > 0) { // now centerlines are either parallel or the same. Now check and see if they are exactly the same // Take the anchor of B, using the axis of B, write the equation of the line. Check and see if // the anchor of A is on the line equation. var t = new List <double>(); for (var i = 0; i < 3; i++) { var axis = cylinder2.Axis[i]; if (Math.Abs(axis) < OverlappingFuzzification.EqualToZeroL) // if a, b or c is zero { if (Math.Abs(cylinder1.Anchor[i] - cylinder2.Anchor[i]) > OverlappingFuzzification.EqualToZero2L) { return(false); } } else { t.Add((cylinder1.Anchor[i] - cylinder2.Anchor[i]) / axis); } } for (var i = 0; i < t.Count - 1; i++) { for (var j = i + 1; j < t.Count; j++) { if (Math.Abs(t[i] - t[j]) > OverlappingFuzzification.PointOnLineL) { return(false); } } } // Now check the radius var p1 = OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.RadiusDifsL, OverlappingFuzzification.RadiusDifsU, Math.Abs(cylinder1.Radius - cylinder2.Radius)); if (p1 > 0) { localProb = Math.Min(p0, p1); foreach (var f1 in cylinder1.Faces) { foreach (var f2 in cylinder2.Faces.Where(f2 => TwoTriangleOverlapCheck(f1, f2))) { if (localProb == 1) { break; } } if (localProb == 1) { break; } } } } if (localProb == 0 || double.IsNaN(localProb)) { return(false); } if (localProb > MaxProb) { MaxProb = localProb; } // is cylinder1 (negative) half? var sum = new [] { 0.0, 0.0, 0.0 }; sum = cylinder1.Faces.Aggregate(sum, (current, face) => face.Normal.add(current)); if (Math.Sqrt((Math.Pow(sum[0], 2.0)) + (Math.Pow(sum[1], 2.0)) + (Math.Pow(sum[2], 2.0))) > 6) { partialCylinder1 = true; } // only keep the directions along the axis of the cylinder. Keep the ones with the angle close to zero. if (!partialCylinder1) { for (var i = 0; i < DirInd.Count; i++) { var dir = Geometric_Reasoning.StartProcess.Directions[DirInd[i]]; if (1 - Math.Abs(cylinder1.Axis.normalize().dotProduct(dir)) < OverlappingFuzzification.CheckWithGlobDirsParall) { continue; } DirInd.Remove(DirInd[i]); i--; } } else { if (reference == 1) // cylinder1(negative) is reference { for (var i = 0; i < DirInd.Count; i++) { var dir = Geometric_Reasoning.StartProcess.Directions[DirInd[i]]; if ((1 - Math.Abs(cylinder1.Axis.normalize().dotProduct(dir)) > OverlappingFuzzification.CheckWithGlobDirsParall) && cylinder1.Faces.All( f => (Math.Abs(1 - dir.dotProduct(f.Normal))) > OverlappingFuzzification.ParralelLines2L)) { DirInd.Remove(DirInd[i]); i--; } } } else // cylinder2(positive) is reference { for (var i = 0; i < DirInd.Count; i++) { var dir = Geometric_Reasoning.StartProcess.Directions[DirInd[i]]; if ((1 - Math.Abs(cylinder1.Axis.normalize().dotProduct(dir)) > OverlappingFuzzification.CheckWithGlobDirsParall) && cylinder1.Faces.All( f => (Math.Abs(1 + dir.dotProduct(f.Normal))) > OverlappingFuzzification.ParralelLines2L)) { DirInd.Remove(DirInd[i]); i--; } } } } return(true); }
private static bool NegConePosConeOverlappingCheck(Cone cone1, Cone cone2, int re) { var localProb = 0.0; // cone1 is negative cone and cone2 is positive cone. var p0 = OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.ParralelLinesL, OverlappingFuzzification.ParralelLinesU, Math.Abs(cone1.Axis.normalize().dotProduct(cone2.Axis.normalize())) - 1); if (p0 > 0) { foreach (var f1 in cone1.Faces) { foreach (var f2 in cone2.Faces) { var pP = TwoTrianglesParallelCheck(f1.Normal, f2.Normal); var sP = TwoTrianglesSamePlaneCheck(f1, f2); if (pP == 0 || sP == 0 || !TwoTriangleOverlapCheck(f1, f2)) { continue; } var p = Math.Min(Math.Min(pP, sP), p0); if (p > localProb) { localProb = p; } if (localProb == 1) { break; } } if (localProb == 1) { break; } } } if (localProb == 0 || double.IsNaN(localProb)) { return(false); } if (localProb > MaxProb) { MaxProb = localProb; } // work with cone1: negative if (re == 20) { if (cone1.Axis.normalize().dotProduct(cone1.Faces[0].Normal) > 0) { for (var i = 0; i < DirInd.Count; i++) { var dir = Geometric_Reasoning.StartProcess.Directions[DirInd[i]]; if (1 + cone1.Axis.normalize().dotProduct(dir) < Math.Abs(cone1.Aperture)) { continue; } DirInd.Remove(DirInd[i]); i--; } } else { for (var i = 0; i < DirInd.Count; i++) { var dir = Geometric_Reasoning.StartProcess.Directions[DirInd[i]]; if (1 - cone1.Axis.normalize().dotProduct(dir) < Math.Abs(cone1.Aperture)) { continue; } DirInd.Remove(DirInd[i]); i--; } } return(true); } if (cone1.Axis.normalize().dotProduct(cone1.Faces[0].Normal) > 0) { for (var i = 0; i < DirInd.Count; i++) { var dir = Geometric_Reasoning.StartProcess.Directions[DirInd[i]]; if (1 - cone1.Axis.normalize().dotProduct(dir) < Math.Abs(cone1.Aperture)) { continue; } DirInd.Remove(DirInd[i]); i--; } } else { for (var i = 0; i < DirInd.Count; i++) { var dir = Geometric_Reasoning.StartProcess.Directions[DirInd[i]]; if (1 + cone1.Axis.normalize().dotProduct(dir) < Math.Abs(cone1.Aperture)) { continue; } DirInd.Remove(DirInd[i]); i--; } } return(true); }
private static bool NegConePosSphereOverlappingCheck(Cone cone, Sphere sphere, int re) { var t1 = (sphere.Center[0] - cone.Apex[0]) / (cone.Axis[0]); var t2 = (sphere.Center[1] - cone.Apex[1]) / (cone.Axis[1]); var t3 = (sphere.Center[2] - cone.Apex[2]) / (cone.Axis[2]); var p0 = OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.PointOnLineL, OverlappingFuzzification.PointOnLineU, Math.Abs(t1 - t2)); var p1 = OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.PointOnLineL, OverlappingFuzzification.PointOnLineU, Math.Abs(t1 - t3)); var p2 = OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.PointOnLineL, OverlappingFuzzification.PointOnLineU, Math.Abs(t3 - t2)); // min of p0, p1 and p2 var maxOverlappingProb = Math.Min(Math.Min(p0, p1), p2); if (maxOverlappingProb > 0) /*if (Math.Abs(t1 - t2) < ConstantsPrimitiveOverlap.PointOnLine && * Math.Abs(t1 - t3) < ConstantsPrimitiveOverlap.PointOnLine && * Math.Abs(t3 - t2) < ConstantsPrimitiveOverlap.PointOnLine)*/ { var localProb = 0.0; foreach (var f1 in cone.Faces) { foreach (var f2 in sphere.Faces) { var pP = TwoTrianglesParallelCheck(f1.Normal, f2.Normal); var sP = TwoTrianglesSamePlaneCheck(f1, f2); if (pP == 0 || sP == 0 || !TwoTriangleOverlapCheck(f1, f2)) { continue; } var p = Math.Min(pP, sP); if (p > localProb) { localProb = p; } if (localProb == 1) { break; } } if (localProb == 1) { break; } } maxOverlappingProb = Math.Min(maxOverlappingProb, localProb); } if (maxOverlappingProb == 0 || double.IsNaN(maxOverlappingProb)) { return(false); } if (maxOverlappingProb > MaxProb) { MaxProb = maxOverlappingProb; } // For a sphere inside of a cone: // if sphere is reference: // if the angle between axis and the normal of one of the faces is less than 90 (dot is pos) // removal direction has he same direction as the neg axis // if dot is neg // removal direction has he same direction as the axis // if cone is reference: // if the angle between axis and the normal of one of the faces is less than 90 (dot is pos) // removal direction has he same direction as the axis // if dot is neg // removal direction has he same direction as the neg axis if (re == 3) { if (cone.Axis.normalize().dotProduct(cone.Faces[0].Normal) > 0) { for (var i = 0; i < DirInd.Count; i++) { var dir = Geometric_Reasoning.StartProcess.Directions[DirInd[i]]; if (1 + cone.Axis.normalize().dotProduct(dir) < OverlappingFuzzification.CheckWithGlobDirsParall) { continue; } DirInd.Remove(DirInd[i]); i--; } } else { for (var i = 0; i < DirInd.Count; i++) { var dir = Geometric_Reasoning.StartProcess.Directions[DirInd[i]]; if (1 - cone.Axis.normalize().dotProduct(dir) < OverlappingFuzzification.CheckWithGlobDirsParall) { continue; } DirInd.Remove(DirInd[i]); i--; } } return(true); } if (cone.Axis.normalize().dotProduct(cone.Faces[0].Normal) > 0) { for (var i = 0; i < DirInd.Count; i++) { var dir = Geometric_Reasoning.StartProcess.Directions[DirInd[i]]; if (1 - cone.Axis.normalize().dotProduct(dir) < OverlappingFuzzification.CheckWithGlobDirsParall) { continue; } DirInd.Remove(DirInd[i]); i--; } } else { for (var i = 0; i < DirInd.Count; i++) { var dir = Geometric_Reasoning.StartProcess.Directions[DirInd[i]]; if (1 + cone.Axis.normalize().dotProduct(dir) < OverlappingFuzzification.CheckWithGlobDirsParall) { continue; } DirInd.Remove(DirInd[i]); i--; } } return(true); }
private static bool NegCylinderPosSphereOverlappingCheck(Cylinder cylinder, Sphere sphere) { // if the center of the sphere is on the cylinder centerline. // or again: two faces parralel, on the same plane and overlap var t1 = (sphere.Center[0] - cylinder.Anchor[0]) / (cylinder.Axis[0]); var t2 = (sphere.Center[1] - cylinder.Anchor[1]) / (cylinder.Axis[1]); var t3 = (sphere.Center[2] - cylinder.Anchor[2]) / (cylinder.Axis[2]); var p0 = OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.PointOnLineL, OverlappingFuzzification.PointOnLineU, Math.Abs(t1 - t2)); var p1 = OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.PointOnLineL, OverlappingFuzzification.PointOnLineU, Math.Abs(t1 - t3)); var p2 = OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.PointOnLineL, OverlappingFuzzification.PointOnLineU, Math.Abs(t3 - t2)); var pRadius = OverlappingFuzzification.FuzzyProbabilityCalculator(OverlappingFuzzification.RadiusDifsL, OverlappingFuzzification.RadiusDifsU, Math.Abs(cylinder.Radius - cylinder.Radius)); var maxOverlappingProb = Math.Min(Math.Min(Math.Min(p0, p1), p2), pRadius); if (maxOverlappingProb > 0) { var localProb = 0.0; foreach (var f1 in cylinder.Faces) { foreach (var f2 in sphere.Faces) { var pP = TwoTrianglesParallelCheck(f1.Normal, f2.Normal); var sP = TwoTrianglesSamePlaneCheck(f1, f2); if (pP == 0 || sP == 0 || !TwoTriangleOverlapCheck(f1, f2)) { continue; } var p = Math.Min(pP, sP); if (p > localProb) { localProb = p; } if (localProb == 1) { break; } } if (localProb == 1) { break; } } maxOverlappingProb = Math.Min(maxOverlappingProb, localProb); } if (maxOverlappingProb == 0 || double.IsNaN(maxOverlappingProb)) { return(false); } if (maxOverlappingProb > MaxProb) { MaxProb = maxOverlappingProb; } // the axis of the cylinder is the removal direction for (var i = 0; i < DirInd.Count; i++) { var dir = Geometric_Reasoning.StartProcess.Directions[DirInd[i]]; if (1 - Math.Abs(cylinder.Axis.normalize().dotProduct(dir)) < OverlappingFuzzification.CheckWithGlobDirsParall) { continue; } DirInd.Remove(DirInd[i]); i--; } return(true); }