public void Add(TopoVertex vertex,int level) { // if (level == 20) // Console.WriteLine("Level 20"); count++; if (left == null && leaf == null) { leaf = new TopoVertexStorageLeaf(); } if (IsLeaf) { if (leaf.vertices.Count < maxVerticesPerNode || level == 20) { leaf.Add(vertex); return; } // Split into 2 container left = new TopoVertexStorage(); right = new TopoVertexStorage(); splitDimension = leaf.LargestDimension(); splitPosition = 0.5 * (leaf.box.minPoint[splitDimension] + leaf.box.maxPoint[splitDimension]); foreach (TopoVertex moveVertex in leaf.vertices) { if (moveVertex.pos[splitDimension] < splitPosition) left.Add(moveVertex,level+1); else right.Add(moveVertex,level+1); } leaf = null; } if (vertex.pos[splitDimension] < splitPosition) left.Add(vertex,level+1); else right.Add(vertex,level+1); }
public TopoTriangle(TopoModel model, TopoVertex v1, TopoVertex v2, TopoVertex v3, double nx, double ny, double nz) { vertices[0] = v1; vertices[1] = v2; vertices[2] = v3; RHVector3 normalTest = new RHVector3(nx, ny, nz); //normal.NormalizeSafe(); edges[0] = model.getOrCreateEdgeBetween(v1, v2); edges[1] = model.getOrCreateEdgeBetween(v2, v3); edges[2] = model.getOrCreateEdgeBetween(v3, v1); edges[0].connectFace(this); edges[1].connectFace(this); edges[2].connectFace(this); v1.connectFace(this); v2.connectFace(this); v3.connectFace(this); boundingBox.Add(v1.pos); boundingBox.Add(v2.pos); boundingBox.Add(v3.pos); RecomputeNormal(); if (normalTest.ScalarProduct(normal) < 0) { FlipDirection(); } /* double d1 = edges[0].EdgeLength; * double d2 = edges[1].EdgeLength; * double d3 = edges[2].EdgeLength; * if (d1 < epsilonZero || d2 < epsilonZero || d3 < epsilonZero) * Console.WriteLine("Df:" + this);*/ }
public TopoTriangle(TopoModel model, TopoVertex v1, TopoVertex v2, TopoVertex v3) { vertices[0] = v1; vertices[1] = v2; vertices[2] = v3; RecomputeNormal(); edges[0] = model.getOrCreateEdgeBetween(v1, v2); edges[1] = model.getOrCreateEdgeBetween(v2, v3); edges[2] = model.getOrCreateEdgeBetween(v3, v1); edges[0].connectFace(this); edges[1].connectFace(this); edges[2].connectFace(this); v1.connectFace(this); v2.connectFace(this); v3.connectFace(this); boundingBox.Add(v1.pos); boundingBox.Add(v2.pos); boundingBox.Add(v3.pos); /*double d1 = edges[0].EdgeLength; * double d2 = edges[1].EdgeLength; * double d3 = edges[2].EdgeLength; * if (d1 < epsilonZero || d2 < epsilonZero || d3 < epsilonZero) * Console.WriteLine("Df:" + this);*/ }
private bool IntersectsSharedVertex(TopoVertex shared, TopoVertex[] a, TopoVertex[] b, TopoTriangle tri) { double d1 = DistanceToPlane(b[0].pos); double d2 = DistanceToPlane(b[1].pos); // Compute intersection point with plane int idx1, idx2; DominantAxis(out idx1, out idx2); if (Math.Abs(d1) < 1e-8 && Math.Abs(d2) < 1e-8) // In plane { if (InPlanePointInside(idx1, idx2, b[0].pos)) { return(true); } if (InPlanePointInside(idx1, idx2, b[1].pos)) { return(true); } if (InPlaneIntersectLineSmall(idx1, idx2, a[0].pos, a[1].pos, shared.pos, b[0].pos)) { return(true); } if (InPlaneIntersectLineSmall(idx1, idx2, a[0].pos, a[1].pos, shared.pos, b[1].pos)) { return(true); } return(false); } if (d1 * d2 > 0) { return(false); // Both points on same side, no intersection possible } double factor = Math.Abs(d1 / (d1 - d2)); RHVector3 p1 = b[0].pos; // new RHVector3(normal); RHVector3 p2 = b[1].pos; // new RHVector3(normal); /*p1.Scale(-d1); * p2.Scale(-d2); * p1.AddInternal(b[0].pos); * p2.AddInternal(b[1].pos);*/ RHVector3 inter = new RHVector3((1 - factor) * p1.x + factor * p2.x, (1 - factor) * p1.y + factor * p2.y, (1 - factor) * p1.z + factor * p2.z); if (inter.Subtract(shared.pos).Length < epsilonZero) { if (Math.Abs(d1) < epsilonZero || Math.Abs(d2) < epsilonZero) { return(false); // Connection ends at shared vertex - does not count as intersection } } if (InPlanePointInside(idx1, idx2, inter)) { return(true); } if (InPlaneIntersectLineSmall(idx1, idx2, a[0].pos, a[1].pos, shared.pos, inter)) { return(true); } return(false); }
public TopoEdge EdgeWithVertices(TopoVertex v1, TopoVertex v2) { foreach (TopoEdge e in edges) { if ((e.v1 == v1 && e.v2 == v2) || (e.v2 == v1 && e.v1 == v2)) { return(e); } } return(null); }
public void Remove(TopoVertex vertex) { if (leaf == null && left == null) { return; } if (RemoveTraverse(vertex)) { count--; } }
public void FlipDirection() { normal.Scale(-1); TopoVertex v = vertices[0]; vertices[0] = vertices[1]; vertices[1] = v; TopoEdge e = edges[1]; edges[1] = edges[2]; edges[2] = e; }
public void Remove(TopoVertex vertex) { int hash = vertextHash(vertex.pos); if (!list.ContainsKey(hash)) return; foreach (TopoVertex v in list[hash]) { if (v == vertex) { count--; list[hash].Remove(vertex); return; } } }
public void Add(TopoVertex vertex) { int hash = vertextHash(vertex.pos); count++; if (list.ContainsKey(hash)) { list[hash].AddLast(vertex); } else { LinkedList<TopoVertex> vl = new LinkedList<TopoVertex>(); vl.AddFirst(vertex); list[hash] = vl; } }
public int VertexIndexFor(TopoVertex test) { if (test == vertices[0]) { return(0); } if (test == vertices[1]) { return(1); } if (test == vertices[2]) { return(2); } return(-1); }
public void Add(TopoVertex vertex) { int hash = vertextHash(vertex.pos); count++; if (list.ContainsKey(hash)) { list[hash].AddLast(vertex); } else { LinkedList <TopoVertex> vl = new LinkedList <TopoVertex>(); vl.AddFirst(vertex); list[hash] = vl; } }
public void Remove(TopoVertex vertex) { int hash = vertextHash(vertex.pos); if (!list.ContainsKey(hash)) { return; } foreach (TopoVertex v in list[hash]) { if (v == vertex) { count--; list[hash].Remove(vertex); return; } } }
void TestInplaneOutside() { TopoModel model = new TopoModel(); TopoVertex v1 = model.addVertex(new RHVector3(0, 0, 0)); TopoVertex v2 = model.addVertex(new RHVector3(10, 0, 0)); TopoVertex v3 = model.addVertex(new RHVector3(10, 10, 0)); TopoVertex v4 = model.addVertex(new RHVector3(11, 1, 0)); TopoVertex v5 = model.addVertex(new RHVector3(17, 1, 0)); TopoVertex v6 = model.addVertex(new RHVector3(11, 7, 0)); TopoTriangle t1 = model.AddTriangle(new TopoTriangle(model, v1, v2, v3)); TopoTriangle t2 = model.AddTriangle(new TopoTriangle(model, v4, v5, v6)); if (t1.Intersects(t2)) { faildTests++; Console.WriteLine("Failed test:TestInplaneOutside"); } }
void TestInplane3D_1() { TopoModel model = new TopoModel(); TopoVertex v1 = model.addVertex(new RHVector3(3.67848944664001, -2.6547646522522, 1.38814495312454E-14)); TopoVertex v2 = model.addVertex(new RHVector3(1.62981510162354, -1.05116808414459, 1.83297828141877E-14)); TopoVertex v3 = model.addVertex(new RHVector3(2.29873323440552, -0.79055267572403, 2.11497486191092E-14)); TopoVertex v4 = model.addVertex(new RHVector3(1.63205575942993, -1.05116808414459, 2.78849697113037)); TopoVertex v5 = model.addVertex(new RHVector3(0.916237592697144, -1.1297744512558, 1.83297828141877E-14)); TopoVertex v6 = model.addVertex(new RHVector3(1.38571500778198, -1.07829427719116, 2.67316389083862)); TopoTriangle t1 = model.AddTriangle(new TopoTriangle(model, v1, v2, v3)); TopoTriangle t2 = model.AddTriangle(new TopoTriangle(model, v4, v5, v6)); if (t1.Intersects(t2)) { faildTests++; Console.WriteLine("Failed test:TestInplane3D_1"); } }
void TestSharedPointIntersect() { TopoModel model = new TopoModel(); TopoVertex v1 = model.addVertex(new RHVector3(0, 0, 0)); TopoVertex v2 = model.addVertex(new RHVector3(10, 0, 0)); TopoVertex v3 = model.addVertex(new RHVector3(5, 5, 0)); TopoVertex v4 = model.addVertex(new RHVector3(0, 0, -5)); TopoVertex v5 = model.addVertex(new RHVector3(10, 0, -5)); TopoVertex v6 = model.addVertex(new RHVector3(5, 0, 5)); TopoTriangle t1 = model.AddTriangle(new TopoTriangle(model, v1, v2, v3)); TopoTriangle t2 = model.AddTriangle(new TopoTriangle(model, v1, v5, v6)); if (!t1.Intersects(t2)) { faildTests++; Console.WriteLine("Failed test:TestSharedPointIntersect"); } }
void TestInplaneInsideSameEdgeIntersects() { TopoModel model = new TopoModel(); TopoVertex v1 = model.addVertex(new RHVector3(0, 0, 0)); TopoVertex v2 = model.addVertex(new RHVector3(10, 0, 0)); TopoVertex v3 = model.addVertex(new RHVector3(10, 10, 0)); TopoVertex v4 = model.addVertex(new RHVector3(1, 1, 0)); TopoVertex v5 = model.addVertex(new RHVector3(7, 1, 0)); TopoVertex v6 = model.addVertex(new RHVector3(5, 3, 0)); TopoTriangle t1 = model.AddTriangle(new TopoTriangle(model, v1, v2, v3)); TopoTriangle t2 = model.AddTriangle(new TopoTriangle(model, v1, v2, v6)); if (!t1.Intersects(t2)) { faildTests++; Console.WriteLine("Failed test:TestInplaneInsideSameEdgeIntersects"); } }
public void Add(TopoVertex vertex, int level) { // if (level == 20) // Console.WriteLine("Level 20"); count++; if (left == null && leaf == null) { leaf = new TopoVertexStorageLeaf(); } if (IsLeaf) { if (leaf.vertices.Count < maxVerticesPerNode || level == 20) { leaf.Add(vertex); return; } // Split into 2 container left = new TopoVertexStorage(); right = new TopoVertexStorage(); splitDimension = leaf.LargestDimension(); splitPosition = 0.5 * (leaf.box.minPoint[splitDimension] + leaf.box.maxPoint[splitDimension]); foreach (TopoVertex moveVertex in leaf.vertices) { if (moveVertex.pos[splitDimension] < splitPosition) { left.Add(moveVertex, level + 1); } else { right.Add(moveVertex, level + 1); } } leaf = null; } if (vertex.pos[splitDimension] < splitPosition) { left.Add(vertex, level + 1); } else { right.Add(vertex, level + 1); } }
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 SmoothAspectRatio(TopoModel model, double maxRatio) { double maxLen, minLen; int maxIdx, minIdx; LongestShortestEdgeLength(out maxIdx, out maxLen, out minIdx, out minLen); if (minLen == 0) { return(false); } if (maxLen > 1 && maxLen / minLen > maxRatio) { RHVector3 center = edges[maxIdx].v1.pos.Add(edges[maxIdx].v2.pos); center.Scale(0.5); TopoVertex newVertex = new TopoVertex(0, center); model.addVertex(newVertex); edges[maxIdx].InsertVertex(model, newVertex); return(true); } return(false); }
public TopoTriangle BuildTriangle(TopoModel model) { TopoVertex sharedPoint = null; TopoVertex p1 = null, p2 = null; if (edgeA.v1 == edgeB.v1) { sharedPoint = edgeA.v1; p1 = edgeA.v2; p2 = edgeB.v2; } else if (edgeA.v1 == edgeB.v2) { sharedPoint = edgeA.v1; p1 = edgeA.v2; p2 = edgeB.v1; } else if (edgeA.v2 == edgeB.v1) { sharedPoint = edgeA.v1; p1 = edgeA.v1; p2 = edgeB.v2; } else if (edgeA.v2 == edgeB.v2) { sharedPoint = edgeA.v2; p1 = edgeA.v1; p2 = edgeB.v1; } TopoTriangle faceA = edgeA.faces.First.Value; TopoTriangle newTriangle = new TopoTriangle(model, sharedPoint, p1, p2, 0, 0, 1); if (newTriangle.SameNormalOrientation(faceA) == false) { newTriangle.FlipDirection(); } newTriangle.RecomputeNormal(); model.AddTriangle(newTriangle); return(newTriangle); }
private bool RemoveTraverse(TopoVertex vertex) { if (IsLeaf) { if (leaf.vertices.Remove(vertex)) { return(true); } else { return(false); // should not happen } } if (vertex.pos[splitDimension] < splitPosition) { return(left.RemoveTraverse(vertex)); } else { return(right.RemoveTraverse(vertex)); } }
public TopoEdge CommonThirdEdge() { TopoVertex v1 = edgeA.v1; if (edgeB.v1 == v1 || edgeB.v2 == v1) { v1 = edgeA.v2; } TopoVertex v2 = edgeB.v1; if (edgeA.v1 == v2 || edgeA.v2 == v2) { v2 = edgeB.v2; } foreach (TopoTriangle t in v1.connectedFacesList) { TopoEdge e = t.EdgeWithVertices(v1, v2); if (e != null) { return(e); } } return(null); }
public void Add(TopoVertex vertex) { Add(vertex, 0); }
/// <summary> /// Splits an edge and changes the connected triangles to maintain /// a topological correct system. /// </summary> /// <param name="model"></param> /// <param name="vertex"></param> public void InsertVertex(TopoModel model, TopoVertex vertex) { LinkedList <TopoTriangle> delList = new LinkedList <TopoTriangle>(); LinkedList <TopoTriangle> testFaces = new LinkedList <TopoTriangle>(); foreach (TopoTriangle oldTriangle in faces) { testFaces.AddLast(oldTriangle); } foreach (TopoTriangle oldTriangle in testFaces) { delList.AddLast(oldTriangle); for (int i = 0; i < 3; i++) { if (oldTriangle.vertices[i] != v1 && oldTriangle.vertices[i] != v2) { TopoTriangle newTriangle = new TopoTriangle(model, v1, vertex, oldTriangle.vertices[i]); if (newTriangle.IsDegenerated()) { newTriangle.Unlink(model); } else { // Test orientation for (int e = 0; e < 3; e++) { TopoTriangle neigbour = newTriangle.edges[i].GetFaceExcept(newTriangle); if (neigbour != null) { if (!newTriangle.SameNormalOrientation(neigbour)) { newTriangle.FlipDirection(); } break; } } model.AddTriangle(newTriangle); } newTriangle = new TopoTriangle(model, vertex, v2, oldTriangle.vertices[i]); if (newTriangle.IsDegenerated()) { newTriangle.Unlink(model); } else { // Test orientation for (int e = 0; e < 3; e++) { TopoTriangle neigbour = newTriangle.edges[i].GetFaceExcept(newTriangle); if (neigbour != null) { if (!newTriangle.SameNormalOrientation(neigbour)) { newTriangle.FlipDirection(); } break; } } model.AddTriangle(newTriangle); } } } } foreach (TopoTriangle tri in delList) { tri.Unlink(model); model.removeTriangle(tri); } }
public bool isBuildOf(TopoVertex _v1, TopoVertex _v2) { return((v1 == _v1 && v2 == _v2) || (v1 == _v2 && v2 == _v1)); }
public TopoVertex addVertex(RHVector3 pos) { /*if (Math.Abs(pos.x + 3.94600009918213) < 0.001 && Math.Abs(pos.y + 2.16400003433228) < 0.001 && Math.Abs(pos.z - 7.9980001449585) < 0.001) { Console.WriteLine(pos); }*/ TopoVertex newVertex = findVertexOrNull(pos); if (newVertex == null) { newVertex = new TopoVertex(vertices.Count + 1, pos); addVertex(newVertex); } return newVertex; }
public TopoEdge(TopoVertex _v1, TopoVertex _v2) { v1 = _v1; v2 = _v2; }
public void Merge(TopoModel model, Matrix4 trans) { int nOld = vertices.Count; int i = 0; List<TopoVertex> vcopy = new List<TopoVertex>(model.vertices.Count); foreach (TopoVertex v in model.vertices) { v.id = i++; TopoVertex newVert = new TopoVertex(v.id, v.pos, trans); addVertex(newVert); vcopy.Add(newVert); } foreach (TopoTriangle t in model.triangles) { TopoTriangle triangle = new TopoTriangle(this, vcopy[t.vertices[0].id], vcopy[t.vertices[1].id], vcopy[t.vertices[2].id]); triangle.RecomputeNormal(); triangles.Add(triangle); } RemoveUnusedDatastructures(); intersectionsUpToDate = false; }
public void addVertex(TopoVertex v) { vertices.Add(v); boundingBox.Add(v.pos); }
public void ChangeCoordinates(TopoVertex vertex, RHVector3 newPos) { Remove(vertex); vertex.pos = new RHVector3(newPos); Add(vertex); }
public void Add(TopoVertex vertex) { vertices.Add(vertex); box.Add(vertex.pos); }
public void Remove(TopoVertex vertex) { if (leaf == null && left == null) return; if (RemoveTraverse(vertex)) count--; }
public double distance(TopoVertex vertex) { return pos.Distance(vertex.pos); }
public bool ContainsVertex(TopoVertex v) { return(v1 == v || v2 == v); }
public TopoModel Copy() { TopoModel newModel = new TopoModel(); int nOld = vertices.Count; int i = 0; List<TopoVertex> vcopy = new List<TopoVertex>(vertices.Count); foreach (TopoVertex v in vertices) { v.id = i++; TopoVertex newVert = new TopoVertex(v.id, v.pos); newModel.addVertex(newVert); vcopy.Add(newVert); } foreach (TopoTriangle t in triangles) { TopoTriangle triangle = new TopoTriangle(newModel, vcopy[t.vertices[0].id], vcopy[t.vertices[1].id], vcopy[t.vertices[2].id], t.normal.x, t.normal.y, t.normal.z); newModel.triangles.Add(triangle); } UpdateVertexNumbers(); newModel.UpdateVertexNumbers(); newModel.badEdges = 0; newModel.badTriangles = badTriangles; newModel.shells = shells; newModel.updatedNormals = updatedNormals; newModel.loopEdges = loopEdges; newModel.manyShardEdges = 0; newModel.manifold = manifold; newModel.normalsOriented = normalsOriented; return newModel; }
private bool RemoveTraverse(TopoVertex vertex) { if (IsLeaf) { if (leaf.vertices.Remove(vertex)) return true; else return false; // should not happen } if (vertex.pos[splitDimension] < splitPosition) return left.RemoveTraverse(vertex); else return right.RemoveTraverse(vertex); }
public TopoEdge getOrCreateEdgeBetween(TopoVertex v1, TopoVertex v2) { foreach (TopoTriangle t in v1.connectedFacesList) { for (int i = 0; i < 3; i++) { if ((v1 == t.vertices[i] && v2 == t.vertices[(i + 1) % 3]) || (v2 == t.vertices[i] && v1 == t.vertices[(i + 1) % 3])) return t.edges[i]; } } /*foreach (TopoEdge edge in edges) { if (edge.isBuildOf(v1, v2)) return edge; }*/ TopoEdge newEdge = new TopoEdge(v1, v2); edges.AddLast(newEdge); return newEdge; }
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 :-) }
public double distance(TopoVertex vertex) { return(pos.Distance(vertex.pos)); }