forked from gradientspace/geometry3Sharp
-
Notifications
You must be signed in to change notification settings - Fork 0
/
IntersectionUtil.cs
218 lines (183 loc) · 8.45 KB
/
IntersectionUtil.cs
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
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace g3
{
public enum IntersectionResult
{
NotComputed,
Intersects,
NoIntersection,
InvalidQuery
}
public enum IntersectionType
{
Empty, Point, Segment, Line, Polygon, Plane, Unknown
}
/// <summary>
/// returned by linear-primitive intersection functions
/// </summary>
public struct LinearIntersection
{
public bool intersects;
public int numIntersections; // 0, 1, or 2
public Interval1d parameter; // t-values along ray
}
public static class IntersectionUtil
{
// same code as IntrRay3Triangle3, but can be called w/o constructing additional data structures
public static bool Intersects(Vector3d rayOrigin, Vector3d rayDirection, Vector3d V0, Vector3d V1, Vector3d V2)
{
// Compute the offset origin, edges, and normal.
Vector3d diff = rayOrigin - V0;
Vector3d edge1 = V1 - V0;
Vector3d edge2 = V2 - V0;
Vector3d normal = edge1.Cross(edge2);
// Solve Q + t*D = b1*E1 + b2*E2 (Q = kDiff, D = ray direction,
// E1 = kEdge1, E2 = kEdge2, N = Cross(E1,E2)) by
// |Dot(D,N)|*b1 = sign(Dot(D,N))*Dot(D,Cross(Q,E2))
// |Dot(D,N)|*b2 = sign(Dot(D,N))*Dot(D,Cross(E1,Q))
// |Dot(D,N)|*t = -sign(Dot(D,N))*Dot(Q,N)
double DdN = rayDirection.Dot(normal);
double sign;
if (DdN > MathUtil.ZeroTolerance) {
sign = 1;
} else if (DdN < -MathUtil.ZeroTolerance) {
sign = -1;
DdN = -DdN;
} else {
// Ray and triangle are parallel, call it a "no intersection"
// even if the ray does intersect.
return false;
}
double DdQxE2 = sign * rayDirection.Dot(diff.Cross(edge2));
if (DdQxE2 >= 0) {
double DdE1xQ = sign * rayDirection.Dot(edge1.Cross(diff));
if (DdE1xQ >= 0) {
if (DdQxE2 + DdE1xQ <= DdN) {
// Line intersects triangle, check if ray does.
double QdN = -sign * diff.Dot(normal);
if (QdN >= 0) {
// Ray intersects triangle.
return true;
}
// else: t < 0, no intersection
}
// else: b1+b2 > 1, no intersection
}
// else: b2 < 0, no intersection
}
// else: b1 < 0, no intersection
return false;
}
/// <summary>
/// Test if line intersects sphere
/// </summary>
public static bool LineSphereTest(ref Vector3d lineOrigin, ref Vector3d lineDirection, ref Vector3d sphereCenter, double sphereRadius)
{
// [RMS] adapted from GeometricTools GTEngine
// https://www.geometrictools.com/GTEngine/Include/Mathematics/GteIntrLine3Sphere3.h
// The sphere is (X-C)^T*(X-C)-1 = 0 and the line is X = P+t*D.
// Substitute the line equation into the sphere equation to obtain a
// quadratic equation Q(t) = t^2 + 2*a1*t + a0 = 0, where a1 = D^T*(P-C),
// and a0 = (P-C)^T*(P-C)-1.
Vector3d diff = lineOrigin - sphereCenter;
double a0 = diff.LengthSquared - sphereRadius * sphereRadius;
double a1 = lineDirection.Dot(diff);
// Intersection occurs when Q(t) has real roots.
double discr = a1 * a1 - a0;
return (discr >= 0);
}
/// <summary>
/// Intersect ray with sphere and return intersection info (# hits, ray parameters)
/// </summary>
public static bool LineSphere(ref Vector3d lineOrigin, ref Vector3d lineDirection, ref Vector3d sphereCenter, double sphereRadius, ref LinearIntersection result)
{
// [RMS] adapted from GeometricTools GTEngine
// https://www.geometrictools.com/GTEngine/Include/Mathematics/GteIntrLine3Sphere3.h
// The sphere is (X-C)^T*(X-C)-1 = 0 and the line is X = P+t*D.
// Substitute the line equation into the sphere equation to obtain a
// quadratic equation Q(t) = t^2 + 2*a1*t + a0 = 0, where a1 = D^T*(P-C),
// and a0 = (P-C)^T*(P-C)-1.
Vector3d diff = lineOrigin - sphereCenter;
double a0 = diff.LengthSquared - sphereRadius * sphereRadius;
double a1 = lineDirection.Dot(diff);
// Intersection occurs when Q(t) has real roots.
double discr = a1 * a1 - a0;
if (discr > 0) {
result.intersects = true;
result.numIntersections = 2;
double root = Math.Sqrt(discr);
result.parameter.a = -a1 - root;
result.parameter.b = -a1 + root;
} else if (discr < 0) {
result.intersects = false;
result.numIntersections = 0;
} else {
result.intersects = true;
result.numIntersections = 1;
result.parameter.a = -a1;
result.parameter.b = -a1;
}
return result.intersects;
}
public static LinearIntersection LineSphere(ref Vector3d lineOrigin, ref Vector3d lineDirection, ref Vector3d sphereCenter, double sphereRadius)
{
LinearIntersection result = new LinearIntersection();
LineSphere(ref lineOrigin, ref lineDirection, ref sphereCenter, sphereRadius, ref result);
return result;
}
/// <summary>
/// Test if ray intersects sphere
/// </summary>
public static bool RaySphereTest(ref Vector3d rayOrigin, ref Vector3d rayDirection, ref Vector3d sphereCenter, double sphereRadius)
{
// [RMS] adapted from GeometricTools GTEngine
// https://www.geometrictools.com/GTEngine/Include/Mathematics/GteIntrRay3Sphere3.h
// The sphere is (X-C)^T*(X-C)-1 = 0 and the line is X = P+t*D.
// Substitute the line equation into the sphere equation to obtain a
// quadratic equation Q(t) = t^2 + 2*a1*t + a0 = 0, where a1 = D^T*(P-C),
// and a0 = (P-C)^T*(P-C)-1.
Vector3d diff = rayOrigin - sphereCenter;
double a0 = diff.LengthSquared - sphereRadius * sphereRadius;
if (a0 <= 0)
return true; // P is inside the sphere.
// else: P is outside the sphere
double a1 = rayDirection.Dot(diff);
if (a1 >= 0)
return false;
// Intersection occurs when Q(t) has double roots.
double discr = a1 * a1 - a0;
return (discr >= 0);
}
/// <summary>
/// Intersect ray with sphere and return intersection info (# hits, ray parameters)
/// </summary>
public static bool RaySphere(ref Vector3d rayOrigin, ref Vector3d rayDirection, ref Vector3d sphereCenter, double sphereRadius, ref LinearIntersection result)
{
// [RMS] adapted from GeometricTools GTEngine
// https://www.geometrictools.com/GTEngine/Include/Mathematics/GteIntrRay3Sphere3.h
LineSphere(ref rayOrigin, ref rayDirection, ref sphereCenter, sphereRadius, ref result);
if ( result.intersects ) {
// The line containing the ray intersects the sphere; the t-interval
// is [t0,t1]. The ray intersects the sphere as long as [t0,t1]
// overlaps the ray t-interval [0,+infinity).
if ( result.parameter.b < 0 ) {
result.intersects = false;
result.numIntersections = 0;
} else if ( result.parameter.a < 0 ) {
result.numIntersections--;
result.parameter.a = result.parameter.b;
}
}
return result.intersects;
}
public static LinearIntersection RaySphere(ref Vector3d rayOrigin, ref Vector3d rayDirection, ref Vector3d sphereCenter, double sphereRadius)
{
LinearIntersection result = new LinearIntersection();
LineSphere(ref rayOrigin, ref rayDirection, ref sphereCenter, sphereRadius, ref result);
return result;
}
}
}