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
|
#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;
}
extern float d;
extern float a1,a2,a3;
extern float total,denom,mu;
extern XYZ pa1,pa2,pa3,n;
float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc, XYZ n, XYZ *p)
{
//Calculate the parameters for the plane
d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
//Calculate the position on the line that intersects the plane
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;
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;
}
float findDistance(XYZ point1, XYZ point2){
return sqrt((point1.x-point2.x)*(point1.x-point2.x)+(point1.y-point2.y)*(point1.y-point2.y)+(point1.z-point2.z)*(point1.z-point2.z));
}
XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang){
XYZ newpoint;
if(xang){
xang*=6.283185;
xang/=360;
}
if(yang){
yang*=6.283185;
yang/=360;
}
if(zang){
zang*=6.283185;
zang/=360;
}
if(yang){
newpoint.z=thePoint.z*cos(yang)-thePoint.x*sin(yang);
newpoint.x=thePoint.z*sin(yang)+thePoint.x*cos(yang);
thePoint.z=newpoint.z;
thePoint.x=newpoint.x;
}
if(zang){
newpoint.x=thePoint.x*cos(zang)-thePoint.y*sin(zang);
newpoint.y=thePoint.y*cos(zang)+thePoint.x*sin(zang);
thePoint.x=newpoint.x;
thePoint.y=newpoint.y;
}
if(xang){
newpoint.y=thePoint.y*cos(xang)-thePoint.z*sin(xang);
newpoint.z=thePoint.y*sin(xang)+thePoint.z*cos(xang);
thePoint.z=newpoint.z;
thePoint.y=newpoint.y;
}
return thePoint;
}
|
