112 lines
3.3 KiB
C
112 lines
3.3 KiB
C
#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;
|
|
|
|
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) / det;
|
|
|
|
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;
|
|
}
|