/*
* 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);
}
