|
@@ -4,6 +4,7 @@
|
4
|
4
|
* \brief Vector and Matrix math
|
5
|
5
|
*
|
6
|
6
|
* \author Mongoose
|
|
7
|
+ * \author xythobuz
|
7
|
8
|
*/
|
8
|
9
|
|
9
|
10
|
#include <stdlib.h>
|
|
@@ -22,20 +23,11 @@ bool equalEpsilon(vec_t a, vec_t b) {
|
22
|
23
|
return false;
|
23
|
24
|
}
|
24
|
25
|
|
25
|
|
-
|
26
|
|
-inline vec_t square(vec_t a)
|
27
|
|
-{
|
28
|
|
- return a * a;
|
29
|
|
-}
|
30
|
|
-
|
31
|
|
-
|
32
|
|
-int helIntersectionLineAndPolygon(vec3_t intersect,
|
33
|
|
- vec3_t p1, vec3_t p2,
|
34
|
|
- vec3_t *polygon)
|
35
|
|
-{
|
|
26
|
+int intersectionLinePolygon(vec3_t intersect,
|
|
27
|
+ vec3_t p1, vec3_t p2, vec3_t *polygon) {
|
36
|
28
|
assert(polygon != NULL);
|
37
|
29
|
|
38
|
|
- // vec3_t normal, a, b;
|
|
30
|
+ // vec3_t normal, a, b;
|
39
|
31
|
Vector3d a, b, normal, pA, pB;
|
40
|
32
|
vec_t d, denominator, mu;
|
41
|
33
|
|
|
@@ -50,41 +42,29 @@ int helIntersectionLineAndPolygon(vec3_t intersect,
|
50
|
42
|
normal.normalize();
|
51
|
43
|
|
52
|
44
|
// find D
|
53
|
|
- //d = (normal[0] * polygon[0][0] -
|
54
|
|
- // normal[1] * polygon[0][1] -
|
55
|
|
- // normal[2] * polygon[0][2]);
|
56
|
45
|
d = (normal.mVec[0] * polygon[0][0] -
|
57
|
|
- normal.mVec[1] * polygon[0][1] -
|
58
|
|
- normal.mVec[2] * polygon[0][2]);
|
|
46
|
+ normal.mVec[1] * polygon[0][1] -
|
|
47
|
+ normal.mVec[2] * polygon[0][2]);
|
59
|
48
|
|
60
|
49
|
// line segment parallel to plane?
|
61
|
50
|
a = pB - pA;
|
62
|
51
|
|
63
|
|
- //denominator = (normal[0] * a[0] +
|
64
|
|
- // normal[1] * a[1] +
|
65
|
|
- // normal[2] * a[2]);
|
66
|
52
|
denominator = Vector3d::dot(normal, a);
|
67
|
53
|
|
68
|
54
|
if (denominator > 0.0)
|
69
|
55
|
return 0;
|
70
|
56
|
|
71
|
57
|
// Line segment contains intercept point?
|
72
|
|
- //mu = - ((d + normal[0] * p1[0] + normal[1] * p1[1] + normal[2] * p1[2]) /
|
73
|
|
- // denominator);
|
74
|
58
|
mu = -((d + Vector3d::dot(normal, pA)) / denominator);
|
75
|
59
|
|
76
|
60
|
if (mu < 0.0 || mu > 1.0)
|
77
|
61
|
return 0;
|
78
|
62
|
|
79
|
|
- //intersect[0] = p1[0] + mu * a[0];
|
80
|
|
- //intersect[1] = p1[1] + mu * a[1];
|
81
|
|
- //intersect[2] = p1[2] + mu * a[2];
|
82
|
63
|
b = pA + (a * mu);
|
83
|
64
|
intersect[0] = b.mVec[0];
|
84
|
65
|
intersect[1] = b.mVec[1];
|
85
|
66
|
intersect[2] = b.mVec[2];
|
86
|
67
|
|
87
|
|
-
|
88
|
68
|
// See if the intercept is bound by polygon by winding number
|
89
|
69
|
// assume convex polygons here for sure
|
90
|
70
|
double theta = Vector3d::dot(b - Vector3d(polygon[0]), normal); // b = intersect
|
|
@@ -95,43 +75,19 @@ int helIntersectionLineAndPolygon(vec3_t intersect,
|
95
|
75
|
return 1;
|
96
|
76
|
}
|
97
|
77
|
|
98
|
|
-
|
99
|
|
-vec_t helDist3v(vec3_t a, vec3_t b)
|
100
|
|
-{
|
101
|
|
- return (sqrtf( ((b[0] - a[0]) * (b[0] - a[0])) +
|
102
|
|
- ((b[1] - a[1]) * (b[1] - a[1])) +
|
103
|
|
- ((b[2] - a[2]) * (b[2] - a[2]))));
|
|
78
|
+vec_t distance(vec3_t a, vec3_t b) {
|
|
79
|
+ return sqrtf(((b[0] - a[0]) * (b[0] - a[0])) +
|
|
80
|
+ ((b[1] - a[1]) * (b[1] - a[1])) +
|
|
81
|
+ ((b[2] - a[2]) * (b[2] - a[2])));
|
104
|
82
|
}
|
105
|
83
|
|
106
|
|
-
|
107
|
|
-void helMidpoint3v(vec3_t a, vec3_t b, vec3_t mid)
|
108
|
|
-{
|
|
84
|
+void midpoint(vec3_t a, vec3_t b, vec3_t mid) {
|
109
|
85
|
mid[0] = (a[0] + b[0]) / 2.0f;
|
110
|
86
|
mid[1] = (a[1] + b[1]) / 2.0f;
|
111
|
87
|
mid[2] = (a[2] + b[2]) / 2.0f;
|
112
|
88
|
}
|
113
|
89
|
|
114
|
|
-
|
115
|
|
-vec_t helNorm4v(vec4_t v)
|
116
|
|
-{
|
117
|
|
- return (sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3]));
|
118
|
|
-}
|
119
|
|
-
|
120
|
|
-
|
121
|
|
-vec_t helNorm3v(vec3_t v)
|
122
|
|
-{
|
123
|
|
- return (sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
|
124
|
|
-}
|
125
|
|
-
|
126
|
|
-
|
127
|
|
-vec_t helNorm2v(vec2_t v)
|
128
|
|
-{
|
129
|
|
- return (sqrtf(v[0]*v[0] + v[1]*v[1]));
|
130
|
|
-}
|
131
|
|
-
|
132
|
|
-
|
133
|
|
-vec_t helRandomNum(vec_t from, vec_t to)
|
134
|
|
-{
|
|
90
|
+vec_t randomNum(vec_t from, vec_t to) {
|
135
|
91
|
return from + ((to - from) * rand() / (RAND_MAX + 1.0f));
|
136
|
92
|
}
|
137
|
93
|
|