private bool IntersectsSharedEdge(TopoVertex[] shared, TopoVertex a, TopoVertex b, TopoTriangle tri) { // Test if coplanar. If not no intersection is possible if (Math.Abs(DistanceToPlane(b.pos)) > epsilonZero) { return(false); } int idx2, idx1; DominantAxis(out idx1, out idx2); if (InPlanePointInside(idx1, idx2, b.pos)) { return(true); } if (tri.InPlanePointInside(idx1, idx2, a.pos)) { return(true); } return(false); }
public bool Intersects(TopoTriangle tri) { // First detect shared edges for more reliable and faster tests TopoVertex[] shared = new TopoVertex[3]; TopoVertex[] uniqueA = new TopoVertex[3]; TopoVertex[] uniqueB = new TopoVertex[3]; int nShared = 0, nUniqueA = 0, nUniqueB = 0; for (int i = 0; i < 3; i++) { bool isDouble = false; for (int j = 0; j < 3; j++) { if (vertices[i] == tri.vertices[j]) { shared[nShared++] = vertices[i]; isDouble = true; break; } } if (!isDouble) { uniqueA[nUniqueA++] = vertices[i]; } } if (nShared > 0) { for (int i = 0; i < 3; i++) { bool isDouble = false; for (int j = 0; j < nShared; j++) { if (tri.vertices[i] == shared[j]) { isDouble = true; break; } } if (!isDouble) { uniqueB[nUniqueB++] = tri.vertices[i]; } } if (nShared == 1) { return(IntersectsSharedVertex(shared[0], uniqueA, uniqueB, tri)); } if (nShared == 2) { return(IntersectsSharedEdge(shared, uniqueA[0], uniqueB[0], tri)); } return(true); } // Nice to read but unoptimized intersection computation RHMatrix3 A = new RHMatrix3(); RHVector3 p1 = vertices[1].pos.Subtract(vertices[0].pos); RHVector3 p2 = vertices[2].pos.Subtract(vertices[0].pos); A.SetXColumn(p1); A.SetYColumn(p2); RHVector3 P = new RHVector3(vertices[0].pos); RHVector3 q1 = tri.vertices[1].pos.Subtract(tri.vertices[0].pos); RHVector3 q2 = tri.vertices[2].pos.Subtract(tri.vertices[0].pos); RHVector3 r1 = tri.vertices[0].pos.Subtract(P); // r2 == r1! RHVector3 r3 = tri.vertices[2].pos.Subtract(P); A.SetZColumn(q1); double detAq1 = A.Determinant; A.SetZColumn(q2); double detAq2 = A.Determinant; //A.SetZColumn(q3); double detAq3 = detAq1 - detAq2; // A.Determinant; A.SetZColumn(r1); double detAr1 = A.Determinant; //A.SetZColumn(r3); double detAr3 = detAr1 + detAq2; // A.Determinant; int intersect = 0; if (detAq1 == 0 && detAq2 == 0 && detAq3 == 0) // same plane case { if (detAr1 != 0) { return(false); // other parallel plance } // Select plane for computation x-y or x-z based on normal int idx1, idx2; DominantAxis(out idx1, out idx2); if (InPlaneIntersectLine(idx1, idx2, vertices[0].pos, vertices[1].pos, tri.vertices[0].pos, tri.vertices[1].pos)) { return(true); } if (InPlaneIntersectLine(idx1, idx2, vertices[0].pos, vertices[1].pos, tri.vertices[1].pos, tri.vertices[2].pos)) { return(true); } if (InPlaneIntersectLine(idx1, idx2, vertices[0].pos, vertices[1].pos, tri.vertices[2].pos, tri.vertices[0].pos)) { return(true); } if (InPlaneIntersectLine(idx1, idx2, vertices[1].pos, vertices[2].pos, tri.vertices[0].pos, tri.vertices[1].pos)) { return(true); } if (InPlaneIntersectLine(idx1, idx2, vertices[1].pos, vertices[2].pos, tri.vertices[1].pos, tri.vertices[2].pos)) { return(true); } if (InPlaneIntersectLine(idx1, idx2, vertices[1].pos, vertices[2].pos, tri.vertices[2].pos, tri.vertices[0].pos)) { return(true); } if (InPlaneIntersectLine(idx1, idx2, vertices[2].pos, vertices[0].pos, tri.vertices[0].pos, tri.vertices[1].pos)) { return(true); } if (InPlaneIntersectLine(idx1, idx2, vertices[2].pos, vertices[0].pos, tri.vertices[1].pos, tri.vertices[2].pos)) { return(true); } if (InPlaneIntersectLine(idx1, idx2, vertices[2].pos, vertices[0].pos, tri.vertices[2].pos, tri.vertices[0].pos)) { return(true); } // Test if point inside. 1 test per triangle is enough if (InPlanePointInside(idx1, idx2, tri.vertices[0].pos)) { return(true); } if (InPlanePointInside(idx1, idx2, tri.vertices[1].pos)) { return(true); } if (InPlanePointInside(idx1, idx2, tri.vertices[2].pos)) { return(true); } if (tri.InPlanePointInside(idx1, idx2, vertices[0].pos)) { return(true); } if (tri.InPlanePointInside(idx1, idx2, vertices[1].pos)) { return(true); } if (tri.InPlanePointInside(idx1, idx2, vertices[2].pos)) { return(true); } return(false); } double beta1 = -1, beta2 = -1, beta3 = -1; if (detAq1 != 0) { beta1 = -detAr1 / detAq1; if (beta1 >= epsilonZeroMinus && beta1 <= epsilonOnePlus) { intersect = 1; } } if (detAq2 != 0) { beta2 = -detAr1 / detAq2; if (beta2 >= epsilonZeroMinus && beta2 <= epsilonOnePlus) { intersect |= 2; } } if (detAq3 != 0) { beta3 = -detAr3 / detAq3; if (beta3 >= epsilonZeroMinus && beta3 <= epsilonOnePlus) { intersect |= 4; } } if (intersect == 7) { // Special case intersection in one point caused 3 valid betas if (Math.Abs(beta1) < epsilonZero) { intersect = 6; } else if (Math.Abs(beta3) < epsilonZero) { intersect = 3; } else { intersect = 5; } } //if (intersect == 0) return false; // Lies on wrong side RHVector3 T = null, t = null; if ((intersect & 1) == 1) { T = new RHVector3(q1); T.Scale(beta1); T.AddInternal(tri.vertices[0].pos); } if ((intersect & 2) == 2) { if (T == null) { T = new RHVector3(q2); T.Scale(beta2); T.AddInternal(tri.vertices[0].pos); } else { q2.Scale(beta2); q2.AddInternal(tri.vertices[0].pos); t = q2.Subtract(T); } } if ((intersect & 4) == 4 && T != null && (t == null || t.Length < epsilonZero)) { RHVector3 q3 = tri.vertices[1].pos.Subtract(tri.vertices[2].pos); q3.Scale(beta3); q3.AddInternal(tri.vertices[2].pos); t = q3.Subtract(T); } if (t == null) { return(false); } if (t.Length < epsilonZero) { // Only one point touches the plane int idx1, idx2; DominantAxis(out idx1, out idx2); return(InPlanePointInside(idx1, idx2, T)); } // Compute intersection points with this triangle double d1 = p1.x * t.y - p1.y * t.x; double d2 = p1.x * t.z - p1.z * t.x; double delta1 = -1, delta2 = -1, delta3 = -1, gamma1 = -1, gamma2 = -1, gamma3 = -1; if (Math.Abs(d1) > epsilonZero || Math.Abs(d2) > epsilonZero) { if (Math.Abs(d1) > Math.Abs(d2)) { delta1 = -(t.x * T.y - t.y * T.x + P.x * t.y - P.y * t.x) / d1; gamma1 = -(p1.x * T.y - p1.y * T.x - p1.x * P.y + p1.y * P.x) / d1; } else { delta1 = -(t.x * T.z - t.z * T.x + P.x * t.z - P.z * t.x) / d2; gamma1 = -(p1.x * T.z - p1.z * T.x - p1.x * P.z + p1.z * P.x) / d2; } } d1 = p2.x * t.y - p2.y * t.x; d2 = p2.x * t.z - p2.z * t.x; if (Math.Abs(d1) > epsilonZero || Math.Abs(d2) > epsilonZero) { if (Math.Abs(d1) > Math.Abs(d2)) { delta2 = -(t.x * T.y - t.y * T.x + P.x * t.y - P.y * t.x) / d1; gamma2 = -(p2.x * T.y - p2.y * T.x - p2.x * P.y + p2.y * P.x) / d1; } else { delta2 = -(t.x * T.z - t.z * T.x + P.x * t.z - P.z * t.x) / d2; gamma2 = -(p2.x * T.z - p2.z * T.x - p2.x * P.z + p2.z * P.x) / d2; } } P.AddInternal(p1); p2.SubtractInternal(p1); // p2 is now p3! d1 = p2.x * t.y - p2.y * t.x; d2 = p2.x * t.z - p2.z * t.x; if (Math.Abs(d1) > epsilonZero || Math.Abs(d2) > epsilonZero) { if (Math.Abs(d1) > Math.Abs(d2)) { delta3 = -(t.x * T.y - t.y * T.x + P.x * t.y - P.y * t.x) / d1; gamma3 = -(p2.x * T.y - p2.y * T.x - p2.x * P.y + p2.y * P.x) / d1; } else { delta3 = -(t.x * T.z - t.z * T.x + P.x * t.z - P.z * t.x) / d2; gamma3 = -(p2.x * T.z - p2.z * T.x - p2.x * P.z + p2.z * P.x) / d2; } } // Check for line intersection inside the line. Hits at the vertices to not count! if (delta1 >= epsilonZero && delta1 <= epsilonOneMinus && gamma1 >= epsilonZero && gamma1 <= epsilonOneMinus) { return(true); } if (delta2 >= epsilonZero && delta2 <= epsilonOneMinus && gamma2 >= epsilonZero && gamma2 <= epsilonOneMinus) { return(true); } if (delta3 >= epsilonZero && delta3 <= epsilonOneMinus && gamma3 >= epsilonZero && gamma3 <= epsilonOneMinus) { return(true); } // Test if intersection is inside triangle intersect = 0; if (delta1 >= epsilonZeroMinus && delta1 <= epsilonOnePlus) { intersect |= 1; } if (delta2 >= epsilonZeroMinus && delta2 <= epsilonOnePlus) { intersect |= 2; } if (delta3 >= epsilonZeroMinus && delta3 <= epsilonOnePlus) { intersect |= 4; } /* if (gamma1 == 0) gamma1 = -1; * if (gamma2 == 0) gamma2 = -1; * if (gamma3 == 0) gamma3 = -1;*/ if (gamma1 == 0) { intersect &= ~1; } if (gamma2 == 0) { intersect &= ~2; } if (gamma3 == 0) { intersect &= ~4; } /* if ((intersect & 3) == 3) return gamma1 * gamma2 < 0; * if ((intersect & 5) == 5) return gamma1 * gamma3 < 0; * if ((intersect & 6) == 6) return gamma3 * gamma2 < 0;*/ if ((intersect & 3) == 3) { if (gamma1 * gamma2 < 0) { return(true); } else { return(false); } } if ((intersect & 5) == 5) { if (gamma1 * gamma3 < 0) { return(true); } else { return(false); } } if ((intersect & 6) == 6) { if (gamma3 * gamma2 < 0) { return(true); } else { return(false); } } // if (intersect!=0) happens only with numeric problems // return true; return(false); // No intersection found :-) }