TriangleIntersection/Intersection/IdentifyIntersectionTriangles.cs at main · z1kurat/TriangleIntersection · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
using TriangleIntersection.Geometry;

namespace TriangleIntersection.Intersection
{
  internal static class IdentifyIntersectionTriangles
  {
    public static bool IsIntersection(Triangle triangleA, Triangle triangleB)
    {
      if (IdentifyIntersectionVertexes.IsIntersection(triangleA, triangleB))
        return true;

      if (RaysTriangleIntersection(triangleA, triangleB))
        return true;

      if (RaysTriangleIntersection(triangleB, triangleA))
        return true;

      return false;
    }

    private static bool RaysTriangleIntersection(Triangle triangleA, Triangle triangleB)
    {
      var length = -1.0;

      var rayAB = triangleA.B - triangleA.A;
      var rayBC = triangleA.C - triangleA.B;
      var rayCA = triangleA.A - triangleA.C;

      var normalRayAB = rayAB.Normalize();
      var normalRayBC = rayBC.Normalize();
      var normalRayCA = rayCA.Normalize();

      if (RayTriangleIntersection(normalRayAB, triangleB, out length) && IsBelongsTriangle(length, rayAB))
        return true;

      if (RayTriangleIntersection(normalRayBC, triangleB, out length) && IsBelongsTriangle(length, rayBC))
        return true;

      if (RayTriangleIntersection(normalRayCA, triangleB, out length) && IsBelongsTriangle(length, rayCA))
        return true;

      return false;
    }

    private static bool RayTriangleIntersection(Vector ray, Triangle triangle, out double rayT)
    {
      rayT = -1;

      var edgeAB = triangle.B - triangle.A;
      var edgeAC = triangle.C - triangle.A;

      var rayDirection = Point.RadiusVector(ray.Direction).Free;

      var subDeterminat = VectorFree.VectorProduct(rayDirection, edgeAC);
      var determinat = edgeAB * subDeterminat;

      if (Сomparison.IsEquals(determinat, 0))
        return false;

      var originRayToTrangleA = ray.Origin - triangle.A;
      var baryU = (originRayToTrangleA * subDeterminat) / determinat;

      if (baryU < 0 || Сomparison.IsMore(baryU, 1))
        return false;

      var subToCalculateBaryV = VectorFree.VectorProduct(originRayToTrangleA, edgeAB);
      var baryV = (rayDirection * subToCalculateBaryV) / determinat;

      if (baryV < 0 || Сomparison.IsMore(baryU + baryV, 1))
        return false;

      rayT = (edgeAC * subToCalculateBaryV) / determinat;

      return true;
    }

    private static bool IsBelongsTriangle(double length, Vector edge)
    {
      return length >= 0 && Сomparison.IsLessOrEquals(length, edge.Length);
    }

  }
}