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
|
#include <neofx/collision.h>
#include <neofx/math.h>
static VERTEX TriangleCollisionPoint(TRIANGLE, VERTEX);
static VERTEX LineCollisionPoint(VERTEX, VERTEX, VERTEX);
int CollisionPointTriangle(VERTEX p, TRIANGLE t) {
VECTOR v1, v2;
TRIANGLE triangle = t;
if(VectorDot(VectorSub(p, triangle.vertices[0]), triangle.normal) > 0.0) return 0;
v1 = VectorCross(VectorSub(triangle.vertices[2], triangle.vertices[1]), VectorSub(p, triangle.vertices[1]));
v2 = VectorCross(VectorSub(triangle.vertices[2], triangle.vertices[1]), VectorSub(triangle.vertices[0], triangle.vertices[1]));
if(VectorDot(v1, v2) < 0.0) return 0;
v1 = VectorCross(VectorSub(triangle.vertices[2], triangle.vertices[0]), VectorSub(p, triangle.vertices[0]));
v2 = VectorCross(VectorSub(triangle.vertices[2], triangle.vertices[0]), VectorSub(triangle.vertices[1], triangle.vertices[0]));
if(VectorDot(v1, v2) < 0.0) return 0;
v1 = VectorCross(VectorSub(triangle.vertices[1], triangle.vertices[0]), VectorSub(p, triangle.vertices[0]));
v2 = VectorCross(VectorSub(triangle.vertices[1], triangle.vertices[0]), VectorSub(triangle.vertices[2], triangle.vertices[0]));
if(VectorDot(v1, v2) < 0.0) return 0;
return 1;
}
int CollisionRayTriangle(VERTEX p, VECTOR vec, TRIANGLE t, float* f) {
VECTOR pvec, tvec, qvec, edge1, edge2;
float det, u, v, invDet;
edge1 = VectorSub(t.vertices[1], t.vertices[0]);
edge2 = VectorSub(t.vertices[2], t.vertices[0]);
pvec = VectorCross(vec, edge2);
det = VectorDot(edge1, pvec);
if(det < 0.0001) return 0;
tvec = VectorSub(p, t.vertices[0]);
u = VectorDot(tvec, pvec);
if(u < 0.0 || u > det) return 0;
qvec = VectorCross(tvec, edge1);
v = VectorDot(vec, qvec);
if(v < 0.0 || u+v > det) return 0;
if(f) {
*f = VectorDot(edge2, qvec);
invDet = 1.0 / det;
*f *= invDet;
}
return 1;
}
static VERTEX LineCollisionPoint(VERTEX v1, VERTEX v2, VERTEX p) {
VECTOR c;
VECTOR V;
float d;
float t;
c = VectorSub(p, v1);
V = VectorSub(v2, v1);
d = VectorLengthSq(V);
t = VectorDot(V, c);
if(t < 0) return v1;
if(t > d) return v2;
return VectorAdd(v1, VectorMul(V, t/d));
}
static VERTEX TriangleCollisionPoint(TRIANGLE t, VERTEX p) {
VERTEX v1, v2, v3;
v1 = LineCollisionPoint(t.vertices[0], t.vertices[1], p);
v2 = LineCollisionPoint(t.vertices[1], t.vertices[2], p);
v3 = LineCollisionPoint(t.vertices[2], t.vertices[0], p);
if(VectorLengthSq(VectorSub(v1, p)) < VectorLengthSq(VectorSub(v2, p)) &&
VectorLengthSq(VectorSub(v1, p)) < VectorLengthSq(VectorSub(v3, p))) return v1;
if(VectorLengthSq(VectorSub(v2, p)) < VectorLengthSq(VectorSub(v3, p))) return v2;
return v3;
}
int CollisionSphereTriangle(VERTEX p, float r, TRIANGLE t) {
float f;
VERTEX v;
/*if(CollisionRayTriangle(VectorAdd(p, VectorMul(VectorNeg(t.normal), r)), m, t, &f)) {*/
if(CollisionRayTriangle(p, VectorNeg(t.normal), t, &f)) {
if(f < r) return 1;
return 0;
}
v = TriangleCollisionPoint(t, p);
if(VectorLengthSq(VectorSub(p, v)) < r*r) return 1;
return 0;
}
int CollisionMovingSphereTriangle(VERTEX p, float r, VECTOR m, float l, TRIANGLE t) {
float f;
VERTEX v;
if(CollisionRayTriangle(VectorAdd(p, VectorMul(VectorNeg(t.normal), r)), m, t, &f)) {
if(f < l) return 1;
return 0;
}
v = TriangleCollisionPoint(t, p);
v = LineCollisionPoint(v, VectorSub(v, VectorMul(m, l)), p);
if(VectorLengthSq(VectorSub(p, v)) < r*r) return 1;
return 0;
}
|
