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
|
#include <cmath>
#include "Quaternions.h"
bool PointInTriangle(XYZ *p, XYZ normal, XYZ *p1, XYZ *p2, XYZ *p3)
{
bool bInter = false;
float pointv[3] { p->x, p->y, p->z };
float p1v[3] { p1->x, p1->y, p1->z };
float p2v[3] { p2->x, p2->y, p2->z };
float p3v[3] { p3->x, p3->y, p3->z };
float normalv[3] { normal.x, normal.y, normal.z };
int i = 0, j = 0;
#define ABS(X) (((X)<0.f)?-(X):(X) )
#define MAX(A, B) (((A)<(B))?(B):(A))
float max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2]));
#undef MAX
if (max == ABS(normalv[0])) {i = 1; j = 2;} // y, z
if (max == ABS(normalv[1])) {i = 0; j = 2;} // x, z
if (max == ABS(normalv[2])) {i = 0; j = 1;} // x, y
#undef ABS
float u0 = pointv[i] - p1v[i];
float v0 = pointv[j] - p1v[j];
float u1 = p2v[i] - p1v[i];
float v1 = p2v[j] - p1v[j];
float u2 = p3v[i] - p1v[i];
float v2 = p3v[j] - p1v[j];
if (u1 > -1.0e-05f && u1 < 1.0e-05f)// == 0.0f)
{
float b = u0 / u2;
if (0.0f <= b && b <= 1.0f)
{
float a = (v0 - b * v2) / v1;
if ((a >= 0.0f) && (( a + b ) <= 1.0f))
bInter = 1;
}
}
else
{
float b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1);
if (0.0f <= b && b <= 1.0f)
{
float a = (u0 - b * u2) / u1;
if ((a >= 0.0f) && (( a + b ) <= 1.0f ))
bInter = 1;
}
}
return bInter;
}
float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc, XYZ n, XYZ *p)
{
//Calculate the parameters for the plane
float d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
//Calculate the position on the line that intersects the plane
float denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
if (abs(denom) < 0.0000001) // Line and plane don't intersect
return 0;
float mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
p->x = p1.x + mu * (p2.x - p1.x);
p->y = p1.y + mu * (p2.y - p1.y);
p->z = p1.z + mu * (p2.z - p1.z);
if (mu < 0 || mu > 1) // Intersection not along line segment
return 0;
if(!PointInTriangle( p, n, &pa, &pb, &pc)){return 0;}
return 1;
}
|
