/* * Check if two edges collide with eachother. */ public static bool EdgesCollide(Edge edge1, Edge edge2, NodeGraph nodeGraph1, NodeGraph nodeGraph2) { // Set up Edge1 Node edge1Node1 = nodeGraph1.GetNode(edge1.nodeIndex1); Node edge1Node2 = nodeGraph1.GetNode(edge1.nodeIndex2); Vect3 edge1Forward = (edge1Node2.coordinate - edge1Node1.coordinate).GetNormalized(); Vect3 edge1Right = Vect3.Cross(edge1Forward, Vect3.Up).GetNormalized(); Vect3 edge1Up = Vect3.Cross(edge1Right, edge1Forward).GetNormalized(); AxisBBox bBox1 = GetEdgeBoundingBox(edge1, edge1Node1.coordinate, edge1Node2.coordinate, edge1Right, edge1Up); // Set up Edge2 Node edge2Node1 = nodeGraph2.GetNode(edge2.nodeIndex1); Node edge2Node2 = nodeGraph2.GetNode(edge2.nodeIndex2); Vect3 edge2Forward = (edge2Node2.coordinate - edge2Node1.coordinate).GetNormalized(); Vect3 edge2Right = Vect3.Cross(edge2Forward, Vect3.Up).GetNormalized(); Vect3 edge2Up = Vect3.Cross(edge2Right, edge2Forward).GetNormalized(); AxisBBox bBox2 = GetEdgeBoundingBox(edge2, edge2Node1.coordinate, edge2Node2.coordinate, edge2Right, edge2Up); // Check BoundingBoxes if (bBox1.Overlaps(bBox2)) { if (IsParallel2D((edge1Node1.coordinate.x, edge1Node1.coordinate.z), (edge1Node2.coordinate.x, edge1Node2.coordinate.z), (edge2Node1.coordinate.x, edge2Node1.coordinate.z), (edge2Node2.coordinate.x, edge2Node2.coordinate.z))) { return(ParallelEdgesCollide(edge1, edge2, nodeGraph1, nodeGraph2)); } else { Vect3 edge1RightWall1 = edge1Node1.coordinate + ((edge1.width / 2) * edge1Right); Vect3 edge1RightWall2 = edge1Node2.coordinate + ((edge1.width / 2) * edge1Right); Vect3 edge2RightWall1 = edge2Node1.coordinate + ((edge2.width / 2) * edge2Right); Vect3 edge2RightWall2 = edge2Node2.coordinate + ((edge2.width / 2) * edge2Right); Vect3 edge1LeftWall1 = edge1Node1.coordinate - ((edge1.width / 2) * edge1Right); Vect3 edge1LeftWall2 = edge1Node2.coordinate - ((edge1.width / 2) * edge1Right); Vect3 edge2LeftWall1 = edge2Node1.coordinate - ((edge2.width / 2) * edge2Right); Vect3 edge2LeftWall2 = edge2Node2.coordinate - ((edge2.width / 2) * edge2Right); // Check all Possible wall collisions. return(WallsIntersect(edge1, edge2, edge1RightWall1, edge1RightWall2, edge2RightWall1, edge2RightWall2) || WallsIntersect(edge1, edge2, edge1RightWall1, edge1RightWall2, edge2LeftWall1, edge2LeftWall2) || WallsIntersect(edge1, edge2, edge1LeftWall1, edge1LeftWall2, edge2RightWall1, edge2RightWall2) || WallsIntersect(edge1, edge2, edge1LeftWall1, edge1LeftWall2, edge2LeftWall1, edge2LeftWall2) || WallsIntersect(edge1, edge2, edge1RightWall1, edge1LeftWall1, edge2RightWall1, edge2RightWall2) || WallsIntersect(edge1, edge2, edge1RightWall1, edge1LeftWall1, edge2LeftWall1, edge2LeftWall2) || WallsIntersect(edge1, edge2, edge1RightWall2, edge1LeftWall2, edge2RightWall1, edge2RightWall2) || WallsIntersect(edge1, edge2, edge1RightWall2, edge1LeftWall2, edge2LeftWall1, edge2LeftWall2) || WallsIntersect(edge1, edge2, edge1RightWall1, edge1RightWall2, edge2RightWall1, edge2LeftWall1) || WallsIntersect(edge1, edge2, edge1RightWall1, edge1RightWall2, edge2RightWall2, edge2LeftWall2) || WallsIntersect(edge1, edge2, edge1LeftWall1, edge1LeftWall2, edge2RightWall1, edge2LeftWall1) || WallsIntersect(edge1, edge2, edge1LeftWall1, edge1LeftWall2, edge2RightWall2, edge2LeftWall2) || WallsIntersect(edge1, edge2, edge1RightWall1, edge1LeftWall1, edge2RightWall1, edge2LeftWall1) || WallsIntersect(edge1, edge2, edge1RightWall1, edge1LeftWall1, edge2RightWall2, edge2LeftWall2) || WallsIntersect(edge1, edge2, edge1RightWall2, edge1LeftWall2, edge2RightWall1, edge2LeftWall1) || WallsIntersect(edge1, edge2, edge1RightWall2, edge1LeftWall2, edge2RightWall2, edge2LeftWall2)); } }
/* * Add a face to face list if the face is correct. */ public int AddFace(Face face) { int [] vertices = face.vertexIndices; Vertex vertex1 = GetVertex(vertices[0]); Vertex vertex2 = GetVertex(vertices[1]); Vertex vertex3 = GetVertex(vertices[2]); Vect3 normal = Vect3.Cross(vertex1.coordinate - vertex2.coordinate, vertex1.coordinate - vertex3.coordinate).GetNormalized(); if (normal != face.normal.GetNormalized()) { throw new InvalidFaceException("Normal does not match vertices of face."); } _faces.Add(face); vertex1.AddFaceIndex(_faces.Count - 1); vertex2.AddFaceIndex(_faces.Count - 1); vertex3.AddFaceIndex(_faces.Count - 1); return(_faces.Count - 1); }
/* * Calculates Line/Face intersections using the Möller-Trumbore intersection algorithm. * * https://en.wikipedia.org/wiki/Möller–Trumbore_intersection_algorithm */ public static bool rayIntersectsTriangle(Vect3 rayOrigin, Vect3 rayVector, Vect3 vertex0, Vect3 vertex1, Vect3 vertex2) { double EPSILON = 1e-6; Vect3 edge1 = vertex1 - vertex0; Vect3 edge2 = vertex2 - vertex0; double a, f, u, v; Vect3 h = Vect3.Cross(rayVector, edge2); a = Vect3.Dot(edge1, h); if (a > -EPSILON && a < EPSILON) { return(false); // This ray is parallel to this triangle. } f = 1.0 / a; Vect3 s = rayOrigin - vertex0; u = f * Vect3.Dot(s, h); if (u < 0.0 || u > 1.0) { return(false); } Vect3 q = Vect3.Cross(s, edge1); v = f * Vect3.Dot(rayVector, q); if (v < 0.0 || u + v > 1.0) { return(false); } double t = f * Vect3.Dot(edge2, q); return(t > EPSILON); }