989 lines
30 KiB
C
989 lines
30 KiB
C
/* plane */
|
|
vec4 plane4(vec3 p, vec3 n) {
|
|
return vec34(n, -dot3(n,p));
|
|
}
|
|
|
|
// ---
|
|
|
|
static void transform_(vec3 *r, vec3 v, const float *r33, vec3 t3) {
|
|
for (int i = 0; i < 3; ++i) {
|
|
i[&r->x] = i[&v.x] * r33[i*3+0];
|
|
i[&r->x] += i[&v.x] * r33[i*3+1];
|
|
i[&r->x] += i[&v.x] * r33[i*3+2];
|
|
i[&r->x] += i[&t3.x];
|
|
}
|
|
}
|
|
static void transformS(vec3 *v, const float *r33, vec3 t3) {
|
|
vec3 tmp = *v;
|
|
transform_(v, tmp, r33, t3);
|
|
}
|
|
static void transformT(vec3 *r, vec3 v, const float *r33, vec3 t3) {
|
|
for (int i = 0; i < 3; ++i) {
|
|
float p = i[&v.x] - i[&t3.x];
|
|
i[&r->x] = p * r33[0*3+i];
|
|
i[&r->x] += p * r33[1*3+i];
|
|
i[&r->x] += p * r33[2*3+i];
|
|
}
|
|
}
|
|
static void transformST(vec3 *v, const float *r33, vec3 t3) {
|
|
vec3 tmp = *v;
|
|
transformT(v, tmp, r33, t3);
|
|
}
|
|
|
|
/* ============================================================================
|
|
*
|
|
* COLLISION
|
|
*
|
|
* =========================================================================== */
|
|
|
|
static __thread hit hits[16] = {0};
|
|
static __thread int hit_index = -1;
|
|
#define hit_next() &hits[ (++hit_index) & 15 ]
|
|
|
|
static float line_closest_line_(float *t1, float *t2, vec3 *c1, vec3 *c2, line l, line m) {
|
|
vec3 r, d1, d2;
|
|
d1 = sub3(l.b, l.a); /* direction vector segment s1 */
|
|
d2 = sub3(m.b, m.a); /* direction vector segment s2 */
|
|
r = sub3(l.a, m.a);
|
|
|
|
float i = dot3(d1, d1);
|
|
float e = dot3(d2, d2);
|
|
float f = dot3(d2, r);
|
|
|
|
if (i <= C_EPSILON && e <= C_EPSILON) {
|
|
/* both segments degenerate into points */
|
|
vec3 d12;
|
|
*t1 = *t2 = 0.0f;
|
|
*c1 = l.a;
|
|
*c2 = m.a;
|
|
d12 = sub3(*c1, *c2);
|
|
return dot3(d12,d12);
|
|
}
|
|
if (i > C_EPSILON) {
|
|
float c = dot3(d1,r);
|
|
if (e > C_EPSILON) {
|
|
/* non-degenerate case */
|
|
float b = dot3(d1,d2);
|
|
float denom = i*e - b*b;
|
|
|
|
/* compute closest point on L1/L2 if not parallel else pick any t2 */
|
|
if (denom != 0.0f)
|
|
*t1 = clampf(0.0f, (b*f - c*e) / denom, 1.0f);
|
|
else *t1 = 0.0f;
|
|
|
|
/* cmpute point on L2 closest to S1(s) */
|
|
*t2 = (b*(*t1) + f) / e;
|
|
if (*t2 < 0.0f) {
|
|
*t2 = 0.0f;
|
|
*t1 = clampf(0.0f, -c/i, 1.0f);
|
|
} else if (*t2 > 1.0f) {
|
|
*t2 = 1.0f;
|
|
*t1 = clampf(0.0f, (b-c)/i, 1.0f);
|
|
}
|
|
} else {
|
|
/* second segment degenerates into a point */
|
|
*t1 = clampf(0.0f, -c/i, 1.0f);
|
|
*t2 = 0.0f;
|
|
}
|
|
} else {
|
|
/* first segment degenerates into a point */
|
|
*t2 = clampf(0.0f, f/e, 1.0f);
|
|
*t1 = 0.0f;
|
|
}
|
|
/* calculate closest points */
|
|
vec3 n, d12;
|
|
n = scale3(d1, *t1);
|
|
*c1 = add3(l.a, n);
|
|
n = scale3(d2, *t2);
|
|
*c2 = add3(m.a, n);
|
|
|
|
/* calculate squared distance */
|
|
d12 = sub3(*c1, *c2);
|
|
return dot3(d12,d12);
|
|
}
|
|
|
|
vec3 line_closest_point(line l, vec3 p) {
|
|
vec3 ab = sub3(l.b,l.a), pa = sub3(p,l.a);
|
|
float t = dot3(pa,ab) / dot3(ab,ab);
|
|
return add3(l.a, scale3(ab, t < 0 ? 0 : t > 1 ? 1 : t));
|
|
}
|
|
float line_distance2_point(line l, vec3 p) {
|
|
vec3 ab = sub3(l.a,l.b), ap = sub3(l.a,p), bp = sub3(l.b,p);
|
|
/* handle cases p proj outside ab */
|
|
float e = dot3(ap,ab); if (e <= 0) return dot3(ap,ap);
|
|
float f = dot3(ab,ab); if (e >= f) return dot3(bp,bp);
|
|
return dot3(ap,ap) - (e*e)/f;
|
|
}
|
|
float ray_test_plane(ray r, vec4 plane) {
|
|
/* Ray: P = origin + rd * t
|
|
* Plane: plane_normal * P + d = 0
|
|
*
|
|
* Substitute:
|
|
* normal * (origin + rd*t) + d = 0
|
|
*
|
|
* Solve for t:
|
|
* plane_normal * origin + plane_normal * rd*t + d = 0
|
|
* -(plane_normal*rd*t) = plane_normal * origin + d
|
|
*
|
|
* plane_normal * origin + d
|
|
* t = -1 * -------------------------
|
|
* plane_normal * rd
|
|
*
|
|
* Result:
|
|
* Behind: t < 0
|
|
* Infront: t >= 0
|
|
* Parallel: t = 0
|
|
* Intersection point: ro + rd * t
|
|
*/
|
|
vec3 p = ptr3(&plane.x);
|
|
float n = -(dot3(p,r.p) + plane.w);
|
|
if (fabs(n) < 0.0001f) return 0.0f;
|
|
return n/(dot3(p,r.d));
|
|
}
|
|
float ray_test_triangle(ray r, triangle tr) {
|
|
float t = 0;
|
|
vec3 di0, di1, di2;
|
|
vec3 d21, d02, in;
|
|
vec3 n, d10, d20;
|
|
vec3 in0, in1, in2;
|
|
|
|
/* calculate triangle normal */
|
|
d10 = sub3(tr.p1,tr.p0);
|
|
d20 = sub3(tr.p2,tr.p0);
|
|
d21 = sub3(tr.p2,tr.p1);
|
|
d02 = sub3(tr.p0,tr.p2);
|
|
n = cross3(d10,d20);
|
|
|
|
/* check for plane intersection */
|
|
vec4 p = plane4(tr.p0, n);
|
|
t = ray_test_plane(r, p);
|
|
if (t <= 0.0f) return t;
|
|
|
|
/* intersection point */
|
|
in = scale3(r.d,t);
|
|
in = add3(in,r.p);
|
|
|
|
/* check if point inside triangle in plane */
|
|
di0 = sub3(in, tr.p0);
|
|
di1 = sub3(in, tr.p1);
|
|
di2 = sub3(in, tr.p2);
|
|
|
|
in0 = cross3(d10, di0);
|
|
in1 = cross3(d21, di1);
|
|
in2 = cross3(d02, di2);
|
|
|
|
if (dot3(in0,n) < 0.0f)
|
|
return -1;
|
|
if (dot3(in1,n) < 0.0f)
|
|
return -1;
|
|
if (dot3(in2,n) < 0.0f)
|
|
return -1;
|
|
return t;
|
|
}
|
|
int ray_test_sphere(float *t0, float *t1, ray r, sphere s) {
|
|
vec3 a;
|
|
float tc,td,d2,r2;
|
|
a = sub3(s.c,r.p);
|
|
tc = dot3(r.d,a);
|
|
if (tc < 0) return 0;
|
|
|
|
r2 = s.r*s.r;
|
|
d2 = dot3(a,a) - tc*tc;
|
|
if (d2 > r2) return 0;
|
|
td = sqrtf(r2 - d2);
|
|
|
|
*t0 = tc - td;
|
|
*t1 = tc + td;
|
|
return 1;
|
|
}
|
|
int ray_test_aabb(float *t0, float *t1, ray r, aabb a) {
|
|
float t0x = (a.min.x - r.p.x) / r.d.x;
|
|
float t0y = (a.min.y - r.p.y) / r.d.y;
|
|
float t0z = (a.min.z - r.p.z) / r.d.z;
|
|
float t1x = (a.max.x - r.p.x) / r.d.x;
|
|
float t1y = (a.max.y - r.p.y) / r.d.y;
|
|
float t1z = (a.max.z - r.p.z) / r.d.z;
|
|
|
|
float tminx = minf(t0x, t1x);
|
|
float tminy = minf(t0y, t1y);
|
|
float tminz = minf(t0z, t1z);
|
|
float tmaxx = maxf(t0x, t1x);
|
|
float tmaxy = maxf(t0y, t1y);
|
|
float tmaxz = maxf(t0z, t1z);
|
|
if (tminx > tmaxy || tminy > tmaxx)
|
|
return 0;
|
|
|
|
*t0 = maxf(tminx, tminy);
|
|
*t1 = minf(tmaxy, tmaxx);
|
|
if (*t0 > tmaxz || tminz> *t1)
|
|
return 0;
|
|
|
|
*t0 = maxf(*t0, tminz);
|
|
*t1 = minf(*t1, tmaxz);
|
|
return 1;
|
|
}
|
|
vec3 sphere_closest_point(sphere s, vec3 p) {
|
|
vec3 d = norm3(sub3(p, s.c));
|
|
return add3(s.c, scale3(d,s.r));
|
|
}
|
|
int sphere_test_sphere(sphere a, sphere b) {
|
|
vec3 d = sub3(b.c, a.c);
|
|
float r = a.r + b.r;
|
|
if (dot3(d,d) > r*r)
|
|
return 0;
|
|
return 1;
|
|
}
|
|
hit *sphere_hit_sphere(sphere a, sphere b) {
|
|
vec3 d = sub3(b.c, a.c);
|
|
float r = a.r + b.r;
|
|
float d2 = dot3(d,d);
|
|
if (d2 > r*r) return 0;
|
|
|
|
hit *m = hit_next();
|
|
float l = sqrtf(d2);
|
|
float linv = 1.0f / ((l != 0) ? l: 1.0f);
|
|
m->normal = scale3(d, linv);
|
|
m->depth = r - l;
|
|
d = scale3(m->normal, b.r);
|
|
m->contact_point = sub3(b.c, d);
|
|
return m;
|
|
}
|
|
int sphere_test_aabb(sphere s, aabb a) {
|
|
return aabb_test_sphere(a, s);
|
|
}
|
|
hit *sphere_hit_aabb(sphere s, aabb a) {
|
|
/* find closest aabb point to sphere center point */
|
|
vec3 ap = aabb_closest_point(a, s.c);
|
|
vec3 d = sub3(s.c, ap);
|
|
float d2 = dot3(d, d);
|
|
if (d2 > s.r*s.r) return 0;
|
|
|
|
hit *m = hit_next();
|
|
/* calculate distance vector between sphere and aabb center points */
|
|
vec3 ac = add3(a.min, scale3(sub3(a.max, a.min), 0.5f));
|
|
d = sub3(ac, s.c);
|
|
|
|
/* normalize distance vector */
|
|
float l2 = dot3(d,d);
|
|
float l = l2 != 0.0f ? sqrtf(l2): 1.0f;
|
|
float linv = 1.0f/l;
|
|
d = scale3(d, linv);
|
|
|
|
m->normal = d;
|
|
m->contact_point = scale3(m->normal, s.r);
|
|
m->contact_point = add3(s.c, m->contact_point);
|
|
|
|
/* calculate penetration depth */
|
|
vec3 sp = sphere_closest_point(s, ap);
|
|
d = sub3(sp, ap);
|
|
m->depth = sqrtf(dot3(d,d)) - l;
|
|
return m;
|
|
}
|
|
int sphere_test_capsule(sphere s, capsule c) {
|
|
return capsule_test_sphere(c, s);
|
|
}
|
|
hit *sphere_hit_capsule(sphere s, capsule c) {
|
|
#if 0
|
|
// original code
|
|
/* find closest capsule point to sphere center point */
|
|
hit *m = hit_next();
|
|
vec3 cp = capsule_closest_point(c, s.c);
|
|
m->normal = sub3(cp, s.c);
|
|
float d2 = dot3(m->normal, m->normal);
|
|
if (d2 > s.r*s.r) return 0;
|
|
|
|
/* normalize hit normal vector */
|
|
m->normal = norm3(m->normal);
|
|
|
|
/* calculate penetration depth */
|
|
m->depth = d2 - s.r*s.r;
|
|
m->depth = m->depth != 0.0f ? sqrtf(m->depth): 0.0f;
|
|
m->contact_point = add3(s.c, scale3(m->normal, s.r));
|
|
return m;
|
|
#else
|
|
// aproximation of I would expect this function to return instead
|
|
vec3 l = sub3(c.a,c.b); float len = len3(l);
|
|
vec3 d = norm3(l);
|
|
ray r = ray(add3(c.a,scale3(d,-2*len)), d);
|
|
s.r += c.r;
|
|
hit *h = ray_hit_sphere(r, s);
|
|
if(!h) return 0;
|
|
s.r -= c.r;
|
|
h->contact_point = add3(s.c,scale3(norm3(sub3(h->contact_point,s.c)),s.r));
|
|
return h;
|
|
#endif
|
|
}
|
|
void aabb_rebalance_transform(aabb *b, aabb a, mat33 m, vec3 t) {
|
|
for (int i = 0; i < 3; ++i) {
|
|
i[&b->min.x] = i[&b->max.x] = i[&t.x];
|
|
for (int j = 0; j < 3; ++j) {
|
|
float e = m[i*3+j] * j[&a.min.x];
|
|
float f = m[i*3+j] * j[&a.max.x];
|
|
if (e < f) {
|
|
i[&b->min.x] += e;
|
|
i[&b->max.x] += f;
|
|
} else {
|
|
i[&b->min.x] += f;
|
|
i[&b->max.x] += e;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
vec3 aabb_closest_point(aabb a, vec3 p) {
|
|
vec3 res;
|
|
for (int i = 0; i < 3; ++i) {
|
|
float v = i[&p.x];
|
|
if (v < i[&a.min.x]) v = i[&a.min.x];
|
|
if (v > i[&a.max.x]) v = i[&a.max.x];
|
|
i[&res.x] = v;
|
|
}
|
|
return res;
|
|
}
|
|
float aabb_distance2_point(aabb a, vec3 p) {
|
|
float r = 0;
|
|
for (int i = 0; i < 3; ++i) {
|
|
float v = i[&p.x];
|
|
if (v < i[&a.min.x]) r += (i[&a.min.x]-v) * (i[&a.min.x]-v);
|
|
if (v > i[&a.max.x]) r += (v-i[&a.max.x]) * (v-i[&a.max.x]);
|
|
} return r;
|
|
}
|
|
int aabb_contains_point(aabb a, vec3 p) {
|
|
if (p.x < a.min.x || p.x > a.max.x) return 0;
|
|
if (p.y < a.min.y || p.y > a.max.y) return 0;
|
|
if (p.z < a.min.z || p.z > a.max.z) return 0;
|
|
return 1;
|
|
}
|
|
int aabb_test_aabb(aabb a, aabb b) {
|
|
if (a.max.x < b.min.x || a.min.x > b.max.x) return 0;
|
|
if (a.max.y < b.min.y || a.min.y > b.max.y) return 0;
|
|
if (a.max.z < b.min.z || a.min.z > b.max.z) return 0;
|
|
return 1;
|
|
}
|
|
hit *aabb_hit_aabb(aabb a, aabb b) {
|
|
if (!aabb_test_aabb(a, b))
|
|
return 0;
|
|
|
|
hit *m = hit_next();
|
|
/* calculate distance vector between both aabb center points */
|
|
vec3 ac, bc, d;
|
|
ac = sub3(a.max, a.min);
|
|
bc = sub3(b.max, b.min);
|
|
|
|
ac = scale3(ac, 0.5f);
|
|
bc = scale3(bc, 0.5f);
|
|
|
|
ac = add3(a.min, ac);
|
|
bc = add3(b.min, bc);
|
|
d = sub3(bc, ac);
|
|
|
|
/* normalize distance vector */
|
|
float l2 = dot3(d,d);
|
|
float l = l2 != 0.0f ? sqrtf(l2): 1.0f;
|
|
float linv = 1.0f/l;
|
|
d = scale3(d, linv);
|
|
|
|
/* calculate contact point */
|
|
m->normal = d;
|
|
m->contact_point = aabb_closest_point(a, bc);
|
|
d = sub3(m->contact_point, ac);
|
|
|
|
/* calculate penetration depth */
|
|
float r2 = dot3(d,d);
|
|
float r = sqrtf(r2);
|
|
m->depth = r - l;
|
|
return m;
|
|
}
|
|
int aabb_test_sphere(aabb a, sphere s) {
|
|
/* compute squared distance between sphere center and aabb */
|
|
float d2 = aabb_distance2_point(a, s.c);
|
|
/* intersection if distance is smaller/equal sphere radius*/
|
|
return d2 <= s.r*s.r;
|
|
}
|
|
hit *aabb_hit_sphere(aabb a, sphere s) {
|
|
/* find closest aabb point to sphere center point */
|
|
hit *m = hit_next();
|
|
m->contact_point = aabb_closest_point(a, s.c);
|
|
vec3 d = sub3(s.c, m->contact_point);
|
|
float d2 = dot3(d, d);
|
|
if (d2 > s.r*s.r) return 0;
|
|
|
|
/* calculate distance vector between aabb and sphere center points */
|
|
vec3 ac = add3(a.min, scale3(sub3(a.max, a.min), 0.5f));
|
|
d = sub3(s.c, ac);
|
|
|
|
/* normalize distance vector */
|
|
float l2 = dot3(d,d);
|
|
float l = l2 != 0.0f ? sqrtf(l2): 1.0f;
|
|
float linv = 1.0f/l;
|
|
d = scale3(d, linv);
|
|
|
|
/* calculate penetration depth */
|
|
m->normal = d;
|
|
d = sub3(m->contact_point, ac);
|
|
m->depth = sqrtf(dot3(d,d));
|
|
return m;
|
|
}
|
|
int aabb_test_capsule(aabb a, capsule c) {
|
|
return capsule_test_aabb(c, a);
|
|
}
|
|
hit *aabb_hit_capsule(aabb a, capsule c) {
|
|
/* calculate aabb center point */
|
|
vec3 ac = add3(a.min, scale3(sub3(a.max, a.min), 0.5f));
|
|
|
|
/* calculate closest point from aabb to point on capsule and check if inside aabb */
|
|
vec3 cp = capsule_closest_point(c, ac);
|
|
if (!aabb_contains_point(a, cp))
|
|
return 0;
|
|
|
|
hit *m = hit_next();
|
|
/* vector and distance between both capsule closests point and aabb center*/
|
|
vec3 d; float d2;
|
|
d = sub3(cp, ac);
|
|
d2 = dot3(d,d);
|
|
|
|
/* calculate penetration depth from closest aabb point to capsule */
|
|
vec3 ap = aabb_closest_point(a, cp);
|
|
vec3 dt = sub3(ap, cp);
|
|
m->depth = sqrtf(dot3(dt,dt));
|
|
|
|
/* calculate normal */
|
|
float l = sqrtf(d2);
|
|
float linv = 1.0f / ((l != 0.0f) ? l: 1.0f);
|
|
m->normal = scale3(d, linv);
|
|
m->contact_point = ap;
|
|
return m;
|
|
}
|
|
float capsule_distance2_point(capsule c, vec3 p) {
|
|
float d2 = line_distance2_point(line(c.a,c.b), p);
|
|
return d2 - (c.r*c.r);
|
|
}
|
|
vec3 capsule_closest_point(capsule c, vec3 p) {
|
|
/* calculate closest point to internal capsule segment */
|
|
vec3 pp = line_closest_point(line(c.a,c.b), p);
|
|
|
|
/* extend point out by radius in normal direction */
|
|
vec3 d = norm3(sub3(p,pp));
|
|
return add3(pp, scale3(d, c.r));
|
|
}
|
|
int capsule_test_capsule(capsule a, capsule b) {
|
|
float t1, t2;
|
|
vec3 c1, c2;
|
|
float d2 = line_closest_line_(&t1, &t2, &c1, &c2, line(a.a,a.b), line(b.a,b.b));
|
|
float r = a.r + b.r;
|
|
return d2 <= r*r;
|
|
}
|
|
hit *capsule_hit_capsule(capsule a, capsule b) {
|
|
float t1, t2;
|
|
vec3 c1, c2;
|
|
float d2 = line_closest_line_(&t1, &t2, &c1, &c2, line(a.a,a.b), line(b.a,b.b));
|
|
float r = a.r + b.r;
|
|
if (d2 > r*r) return 0;
|
|
|
|
hit *m = hit_next();
|
|
/* calculate normal from both closest points for each segement */
|
|
vec3 cp, d;
|
|
m->normal = sub3(c2, c1);
|
|
m->normal = norm3(m->normal);
|
|
|
|
/* calculate contact point from closest point and depth */
|
|
m->contact_point = capsule_closest_point(a, c2);
|
|
cp = capsule_closest_point(b, c1);
|
|
d = sub3(c1, cp);
|
|
m->depth = sqrtf(dot3(d,d));
|
|
return m;
|
|
}
|
|
int capsule_test_sphere(capsule c, sphere s) {
|
|
/* squared distance bwetween sphere center and capsule line segment */
|
|
float d2 = line_distance2_point(line(c.a,c.b), s.c);
|
|
float r = s.r + c.r;
|
|
return d2 <= r * r;
|
|
}
|
|
hit *capsule_hit_sphere(capsule c, sphere s) {
|
|
/* find closest capsule point to sphere center point */
|
|
hit *m = hit_next();
|
|
m->contact_point = capsule_closest_point(c, s.c);
|
|
m->normal = sub3(s.c, m->contact_point);
|
|
float d2 = dot3(m->normal, m->normal);
|
|
if (d2 > s.r*s.r) return 0;
|
|
|
|
/* normalize hit normal vector */
|
|
float l = d2 != 0.0f ? sqrtf(d2): 1;
|
|
float linv = 1.0f/l;
|
|
m->normal = scale3(m->normal, linv);
|
|
|
|
/* calculate penetration depth */
|
|
m->depth = d2 - s.r*s.r;
|
|
m->depth = m->depth != 0.0f ? sqrtf(m->depth): 0.0f;
|
|
return m;
|
|
}
|
|
int capsule_test_aabb(capsule c, aabb a) {
|
|
/* calculate aabb center point */
|
|
vec3 ac = scale3(sub3(a.max, a.min), 0.5f);
|
|
|
|
/* calculate closest point from aabb to point on capsule and check if inside aabb */
|
|
vec3 p = capsule_closest_point(c, ac);
|
|
return aabb_contains_point(a, p);
|
|
}
|
|
hit *capsule_hit_aabb(capsule c, aabb a) {
|
|
/* calculate aabb center point */
|
|
vec3 ac = add3(a.min, scale3(sub3(a.max, a.min), 0.5f));
|
|
|
|
/* calculate closest point from aabb to point on capsule and check if inside aabb */
|
|
vec3 cp = capsule_closest_point(c, ac);
|
|
if (!aabb_contains_point(a, cp))
|
|
return 0;
|
|
|
|
hit *m = hit_next();
|
|
/* vector and distance between both capsule closests point and aabb center*/
|
|
vec3 d; float d2;
|
|
d = sub3(ac, cp);
|
|
d2 = dot3(d,d);
|
|
|
|
/* calculate penetration depth from closest aabb point to capsule */
|
|
vec3 ap = aabb_closest_point(a, cp);
|
|
vec3 dt = sub3(ap, cp);
|
|
m->depth = sqrtf(dot3(dt,dt));
|
|
|
|
/* calculate normal */
|
|
float l = sqrtf(d2);
|
|
float linv = 1.0f / ((l != 0.0f) ? l: 1.0f);
|
|
m->normal = scale3(d, linv);
|
|
m->contact_point = cp;
|
|
return m;
|
|
}
|
|
|
|
/* ============================================================================
|
|
*
|
|
* COLLISION VOLUME
|
|
*
|
|
* =========================================================================== */
|
|
|
|
hit *ray_hit_plane(ray r, plane p) {
|
|
vec4 pf = plane4(p.p, p.n);
|
|
float t = ray_test_plane(r, pf);
|
|
if (t <= 0.0f) return 0;
|
|
hit *o = hit_next();
|
|
o->p = add3(r.p, scale3(r.d, t));
|
|
o->t0 = o->t1 = t;
|
|
o->n = scale3(p.n, -1.0f);
|
|
return o;
|
|
}
|
|
hit *ray_hit_triangle(ray r, triangle tr) {
|
|
float t = ray_test_triangle(r, tr);
|
|
if (t <= 0) return 0;
|
|
|
|
hit *o = hit_next();
|
|
o->t0 = o->t1 = t;
|
|
o->p = add3(r.p, scale3(r.d, t));
|
|
o->n = norm3(cross3(sub3(tr.p1,tr.p0),sub3(tr.p2,tr.p0)));
|
|
return o;
|
|
}
|
|
hit *ray_hit_sphere(ray r, sphere s) {
|
|
hit *o = hit_next();
|
|
if (!ray_test_sphere(&o->t0, &o->t1, r, s))
|
|
return 0;
|
|
o->p = add3(r.p, scale3(r.d, minf(o->t0,o->t1)));
|
|
o->n = norm3(sub3(o->p, s.c));
|
|
return o;
|
|
}
|
|
hit *ray_hit_aabb(ray r, aabb a) {
|
|
hit *o = hit_next();
|
|
|
|
vec3 pnt, ext, c;
|
|
float d, min;
|
|
if (!ray_test_aabb(&o->t0, &o->t1, r, a))
|
|
return 0;
|
|
|
|
o->p = add3(r.p, scale3(r.d, minf(o->t0,o->t1)));
|
|
ext = sub3(a.max, a.min);
|
|
c = add3(a.min, scale3(ext,0.5f));
|
|
pnt = sub3(o->p, c);
|
|
|
|
min = fabs(ext.x - fabs(pnt.x));
|
|
o->n = scale3(vec3(1,0,0), signf(pnt.x));
|
|
d = fabs(ext.y - fabs(pnt.y));
|
|
if (d < min) {
|
|
min = d;
|
|
o->n = scale3(vec3(0,1,0), signf(pnt.y));
|
|
}
|
|
d = fabs(ext.z - fabs(pnt.z));
|
|
if (d < min)
|
|
o->n = scale3(vec3(0,0,1), signf(pnt.z));
|
|
return o;
|
|
}
|
|
|
|
frustum frustum_build(mat44 pv) {
|
|
frustum f;
|
|
f.l = vec4(pv[ 3]+pv[ 0], pv[ 7]+pv[ 4], pv[11]+pv[ 8], pv[15]+pv[12]);
|
|
f.r = vec4(pv[ 3]-pv[ 0], pv[ 7]-pv[ 4], pv[11]-pv[ 8], pv[15]-pv[12]);
|
|
f.t = vec4(pv[ 3]-pv[ 1], pv[ 7]-pv[ 5], pv[11]-pv[ 9], pv[15]-pv[13]);
|
|
f.b = vec4(pv[ 3]+pv[ 1], pv[ 7]+pv[ 5], pv[11]+pv[ 9], pv[15]+pv[13]);
|
|
f.n = vec4(pv[ 3]+pv[ 2], pv[ 7]+pv[ 6], pv[11]+pv[10], pv[15]+pv[14]);
|
|
f.f = vec4(pv[ 3]-pv[ 2], pv[ 7]-pv[ 6], pv[11]-pv[10], pv[15]-pv[14]);
|
|
for (int i = 0; i < 6; i++) f.pl[i] = scale4(f.pl[i], 1 / len3(f.pl[i].xyz));
|
|
return f;
|
|
}
|
|
int frustum_test_sphere(frustum f, sphere s) {
|
|
for(int i = 0; i < 6; i++) {
|
|
if((dot3(f.pl[i].xyz, s.c) + f.pl[i].w + s.r) < 0) return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
int frustum_test_aabb(frustum f, aabb a) {
|
|
for(int i = 0; i < 6; i++) {
|
|
vec3 v = vec3(f.pl[i].x > 0 ? a.max.x : a.min.x, f.pl[i].y > 0 ? a.max.y : a.min.y, f.pl[i].z > 0 ? a.max.z : a.min.z);
|
|
if((dot3(f.pl[i].xyz, v) + f.pl[i].w) < 0) return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
void collide_demo() { // debug draw collisions
|
|
|
|
// animation
|
|
static float dx = 0, dy = 0;
|
|
float delta = (0.25f / 60.f);
|
|
dx = dx + delta * 2.0f;
|
|
dy = dy + delta * 0.8f;
|
|
|
|
#if 0
|
|
// 3D
|
|
glEnable(GL_DEPTH_TEST);
|
|
|
|
// grid
|
|
ddraw_grid(0);
|
|
#endif
|
|
|
|
{
|
|
// Triangle-Ray Intersection*/
|
|
vec3 ro, rd;
|
|
int suc;
|
|
|
|
triangle tri = { vec3(-9,1,28), vec3(-10,0,28), vec3(-11,1,28) };
|
|
|
|
// ray
|
|
ro = vec3(-10,-1,20);
|
|
rd = vec3(-10+0.4f*sin(dx), 2.0f*cos(dy), 29.81023f);
|
|
rd = sub3(rd, ro);
|
|
rd = norm3(rd);
|
|
|
|
ray r = ray(ro, rd);
|
|
hit *hit = ray_hit_triangle(r, tri);
|
|
if (hit) {
|
|
// point of intersection
|
|
ddraw_color(RED);
|
|
ddraw_box(hit->p, vec3(0.10f, 0.10f, 0.10f));
|
|
|
|
// intersection normal
|
|
ddraw_color(BLUE);
|
|
vec3 v = add3(hit->p, hit->n);
|
|
ddraw_arrow(hit->p, v);
|
|
}
|
|
|
|
// line
|
|
ddraw_color(RED);
|
|
rd = scale3(rd,10);
|
|
rd = add3(ro,rd);
|
|
ddraw_line(ro, rd);
|
|
|
|
// triangle
|
|
if (hit) ddraw_color(RED);
|
|
else ddraw_color(WHITE);
|
|
ddraw_triangle(tri.p0,tri.p1,tri.p2);
|
|
}
|
|
{
|
|
// Plane-Ray Intersection*/
|
|
vec3 ro, rd;
|
|
mat33 rot;
|
|
|
|
// ray
|
|
static float d = 0;
|
|
d += delta * 2.0f;
|
|
ro = vec3(0,-1,20);
|
|
rd = vec3(0.1f, 0.5f, 9.81023f);
|
|
rd = sub3(rd, ro);
|
|
rd = norm3(rd);
|
|
|
|
// rotation
|
|
rotation33(rot, deg(d), 0,1,0);
|
|
rd = mulv33(rot, rd);
|
|
|
|
// intersection
|
|
ray r = ray(ro, rd);
|
|
plane pl = plane(vec3(0,0,28), vec3(0,0,1));
|
|
hit *hit = ray_hit_plane(r, pl);
|
|
if (hit) {
|
|
// point of intersection
|
|
ddraw_color(RED);
|
|
ddraw_box(hit->p, vec3(0.10f, 0.10f, 0.10f));
|
|
|
|
// intersection normal
|
|
ddraw_color(BLUE);
|
|
vec3 v = add3(hit->p, hit->n);
|
|
ddraw_arrow(hit->p, v);
|
|
ddraw_color(RED);
|
|
}
|
|
// line
|
|
ddraw_color(RED);
|
|
rd = scale3(rd,9);
|
|
rd = add3(ro,rd);
|
|
ddraw_line(ro, rd);
|
|
|
|
// plane
|
|
if (hit) ddraw_color(RED);
|
|
else ddraw_color(WHITE);
|
|
ddraw_plane(vec3(0,0,28), vec3(0,0,1), 3.0f);
|
|
}
|
|
{
|
|
// Sphere-Ray Intersection*/
|
|
vec3 ro, rd;
|
|
sphere s;
|
|
|
|
// ray
|
|
ro = vec3(0,-1,0);
|
|
rd = vec3(0.4f*sin(dx), 2.0f*cos(dy), 9.81023f);
|
|
rd = sub3(rd, ro);
|
|
rd = norm3(rd);
|
|
|
|
ray r = ray(ro, rd);
|
|
s = sphere(vec3(0,0,8), 1);
|
|
hit *hit = ray_hit_sphere(r, s);
|
|
if(hit) {
|
|
// points of intersection
|
|
vec3 in = add3(ro,scale3(rd,hit->t0));
|
|
|
|
ddraw_color(GREEN);
|
|
ddraw_box(in, vec3(0.05f, 0.05f, 0.05f));
|
|
|
|
in = add3(ro,scale3(rd,hit->t1));
|
|
|
|
ddraw_color(YELLOW);
|
|
ddraw_box(in, vec3(0.05f, 0.05f, 0.05f));
|
|
|
|
// intersection normal
|
|
ddraw_color(BLUE);
|
|
vec3 v = add3(hit->p, hit->n);
|
|
ddraw_arrow(hit->p, v);
|
|
ddraw_color(RED);
|
|
}
|
|
// line
|
|
ddraw_color(RED);
|
|
rd = scale3(rd,10);
|
|
rd = add3(ro,rd);
|
|
ddraw_line(ro, rd);
|
|
|
|
// sphere
|
|
if (hit) ddraw_color(RED);
|
|
else ddraw_color(WHITE);
|
|
ddraw_sphere(vec3(0,0,8), 1);
|
|
}
|
|
{ // ray-aabb
|
|
aabb bounds = aabb(vec3(10-0.5f,-0.5f,7.5f), vec3(10.5f,0.5f,8.5f));
|
|
|
|
vec3 ro = vec3(10,-1,0);
|
|
vec3 rd = vec3(10+0.4f*sin(dx), 2.0f*cos(dy), 9.81023f);
|
|
rd = norm3(sub3(rd, ro));
|
|
ray r = ray(ro, rd);
|
|
|
|
hit *hit = ray_hit_aabb(r, bounds);
|
|
if(hit) {
|
|
// points of intersection
|
|
vec3 in;
|
|
in = scale3(rd,hit->t0);
|
|
in = add3(ro,in);
|
|
|
|
ddraw_color(RED);
|
|
ddraw_box(in, vec3(0.05f, 0.05f, 0.05f));
|
|
|
|
in = scale3(rd,hit->t1);
|
|
in = add3(ro,in);
|
|
|
|
ddraw_color(RED);
|
|
ddraw_box(in, vec3(0.05f, 0.05f, 0.05f));
|
|
|
|
// intersection normal
|
|
ddraw_color(BLUE);
|
|
vec3 v = add3(hit->p, hit->n);
|
|
ddraw_arrow(hit->p, v);
|
|
ddraw_color(RED);
|
|
} else ddraw_color(WHITE);
|
|
ddraw_box(vec3(10,0,8), vec3(1,1,1));
|
|
|
|
// line
|
|
ddraw_color(RED);
|
|
rd = scale3(rd,10);
|
|
rd = add3(ro,rd);
|
|
ddraw_line(ro, rd);
|
|
}
|
|
{
|
|
// Sphere-Sphere intersection*/
|
|
sphere a = sphere(vec3(-10,0,8), 1);
|
|
sphere b = sphere(vec3(-10+0.6f*sin(dx), 3.0f*cos(dy),8), 1);
|
|
hit *m = sphere_hit_sphere(a, b);
|
|
if (m) {
|
|
vec3 v;
|
|
ddraw_color(BLUE);
|
|
ddraw_box(m->contact_point, vec3(0.05f, 0.05f, 0.05f));
|
|
v = add3(m->contact_point, m->normal);
|
|
ddraw_arrow(m->contact_point, v);
|
|
ddraw_color(RED);
|
|
} else ddraw_color(WHITE);
|
|
|
|
ddraw_sphere(a.c, 1);
|
|
ddraw_sphere(b.c, 1);
|
|
}
|
|
{
|
|
// AABB-AABB intersection*/
|
|
const float x = 10+0.6f*sin(dx);
|
|
const float y = 3.0f*cos(dy);
|
|
const float z = 20.0f;
|
|
|
|
aabb a = aabb(vec3(10-0.5f,-0.5f,20-0.5f), vec3(10+0.5f,0.5f,20.5f));
|
|
aabb b = aabb(vec3(x-0.5f,y-0.5f,z-0.5f), vec3(x+0.5f,y+0.5f,z+0.5f));
|
|
hit *m = aabb_hit_aabb(a, b);
|
|
if(m) {
|
|
vec3 v;
|
|
ddraw_color(BLUE);
|
|
ddraw_box(m->contact_point, vec3(0.05f, 0.05f, 0.05f));
|
|
v = add3(m->contact_point, m->normal);
|
|
ddraw_arrow(m->contact_point, v);
|
|
ddraw_color(RED);
|
|
} else ddraw_color(WHITE);
|
|
|
|
ddraw_box(vec3(10,0,20), vec3(1,1,1));
|
|
ddraw_box(vec3(x,y,z), vec3(1,1,1));
|
|
}
|
|
{
|
|
// Capsule-Capsule intersection*/
|
|
const float x = 20+0.4f*sin(dx);
|
|
const float y = 3.0f*cos(dy);
|
|
const float z = 28.5f;
|
|
|
|
capsule a = capsule(vec3(20.0f,-1.0f,28.0f), vec3(20.0f,1.0f,28.0f), 0.2f);
|
|
capsule b = capsule(vec3(x,y-1.0f,z), vec3(x,y+1.0f,z-1.0f), 0.2f);
|
|
hit *m = capsule_hit_capsule(a, b);
|
|
if( m ) {
|
|
vec3 v;
|
|
ddraw_color(BLUE);
|
|
ddraw_box(m->contact_point, vec3(0.05f, 0.05f, 0.05f));
|
|
v = add3(m->contact_point, m->normal);
|
|
ddraw_arrow(m->contact_point, v);
|
|
ddraw_color(RED);
|
|
} else ddraw_color(WHITE);
|
|
ddraw_capsule(vec3(x,y-1.0f,z), vec3(x,y+1.0f,z-1.0f), 0.2f);
|
|
ddraw_capsule(vec3(20.0f,-1.0f,28.0f), vec3(20.0f,1.0f,28.0f), 0.2f);
|
|
}
|
|
{
|
|
// AABB-Sphere intersection*/
|
|
aabb a = aabb(vec3(20-0.5f,-0.5f,7.5f), vec3(20.5f,0.5f,8.5f));
|
|
sphere s = sphere(vec3(20+0.6f*sin(dx), 3.0f*cos(dy),8), 1);
|
|
hit *m = aabb_hit_sphere(a, s);
|
|
if(m) {
|
|
vec3 v;
|
|
ddraw_color(BLUE);
|
|
ddraw_box(m->contact_point, vec3(0.05f, 0.05f, 0.05f));
|
|
v = add3(m->contact_point, m->normal);
|
|
ddraw_arrow(m->contact_point, v);
|
|
ddraw_color(RED);
|
|
} else ddraw_color(WHITE);
|
|
|
|
ddraw_box(vec3(20,0,8), vec3(1,1,1));
|
|
ddraw_sphere(s.c, 1);
|
|
}
|
|
{
|
|
// Sphere-AABB intersection*/
|
|
const float x = 10+0.6f*sin(dx);
|
|
const float y = 3.0f*cos(dy);
|
|
const float z = -8.0f;
|
|
|
|
sphere s = sphere(vec3(10,0,-8), 1);
|
|
aabb a = aabb(vec3(x-0.5f,y-0.5f,z-0.5f), vec3(x+0.5f,y+0.5f,z+0.5f));
|
|
hit *m = sphere_hit_aabb(s, a);
|
|
if(m) {
|
|
vec3 v;
|
|
ddraw_color(BLUE);
|
|
ddraw_box(m->contact_point, vec3(0.05f, 0.05f, 0.05f));
|
|
v = add3(m->contact_point, m->normal);
|
|
ddraw_arrow(m->contact_point, v);
|
|
ddraw_color(RED);
|
|
} else ddraw_color(WHITE);
|
|
|
|
ddraw_box(vec3(x,y,z), vec3(1,1,1));
|
|
ddraw_sphere(s.c, 1);
|
|
}
|
|
{
|
|
// Capsule-Sphere intersection*/
|
|
capsule c = capsule(vec3(-20.5f,-1.0f,7.5f), vec3(-20+0.5f,1.0f,8.5f), 0.2f);
|
|
sphere b = sphere(vec3(-20+0.6f*sin(dx), 3.0f*cos(dy),8), 1);
|
|
hit *m = capsule_hit_sphere(c, b);
|
|
if(m) {
|
|
vec3 v;
|
|
ddraw_color(BLUE);
|
|
ddraw_box(m->contact_point, vec3(0.05f, 0.05f, 0.05f));
|
|
v = add3(m->contact_point, m->normal);
|
|
ddraw_arrow(m->contact_point, v);
|
|
ddraw_color(RED);
|
|
} else ddraw_color(WHITE);
|
|
ddraw_sphere(b.c, 1);
|
|
ddraw_capsule(vec3(-20.5f,-1.0f,7.5f), vec3(-20+0.5f,1.0f,8.5f), 0.2f);
|
|
}
|
|
{
|
|
// Sphere-Capsule intersection*/
|
|
const float x = 20+0.4f*sin(dx);
|
|
const float y = 3.0f*cos(dy);
|
|
const float z = -8;
|
|
|
|
sphere s = sphere(vec3(20,0,-8), 1);
|
|
capsule c = capsule(vec3(x,y-1.0f,z), vec3(x,y+1.0f,z-1.0f), 0.2f);
|
|
hit *m = sphere_hit_capsule(s, c);
|
|
if(m) {
|
|
vec3 v;
|
|
ddraw_color(BLUE);
|
|
ddraw_box(m->contact_point, vec3(0.05f, 0.05f, 0.05f));
|
|
v = add3(m->contact_point, m->normal);
|
|
ddraw_arrow(m->contact_point, v);
|
|
ddraw_color(RED);
|
|
} else ddraw_color(WHITE);
|
|
|
|
ddraw_capsule(vec3(x,y-1.0f,z), vec3(x,y+1.0f,z-1.0f), 0.2f);
|
|
ddraw_sphere(s.c, 1);
|
|
}
|
|
{
|
|
// Capsule-AABB intersection*/
|
|
const float x = -20+0.6f*sin(dx);
|
|
const float y = 3.0f*cos(dy);
|
|
const float z = 28.0f;
|
|
|
|
capsule c = capsule(vec3(-20.5f,-1.0f,27.5f), vec3(-20+0.5f,1.0f,28.5f), 0.2f);
|
|
aabb b = aabb(vec3(x-0.5f,y-0.5f,z-0.5f), vec3(x+0.5f,y+0.5f,z+0.5f));
|
|
hit *m = capsule_hit_aabb(c, b);
|
|
if(m) {
|
|
vec3 v;
|
|
ddraw_color(BLUE);
|
|
ddraw_box(m->contact_point, vec3(0.05f, 0.05f, 0.05f));
|
|
v = add3(m->contact_point, m->normal);
|
|
ddraw_arrow(m->contact_point, v);
|
|
ddraw_color(RED);
|
|
} else ddraw_color(WHITE);
|
|
ddraw_box(vec3(x,y,z), vec3(1,1,1));
|
|
ddraw_capsule(vec3(-20.5f,-1.0f,27.5f), vec3(-20+0.5f,1.0f,28.5f), 0.2f);
|
|
}
|
|
{
|
|
// AABB-Capsule intersection*/
|
|
const float x = 0.4f*sin(dx);
|
|
const float y = 3.0f*cos(dy);
|
|
const float z = -8;
|
|
|
|
aabb a = aabb(vec3(-0.5f,-0.5f,-8.5f), vec3(0.5f,0.5f,-7.5f));
|
|
capsule c = capsule(vec3(x,y-1.0f,z), vec3(x,y+1.0f,z-1.0f), 0.2f);
|
|
hit *m = aabb_hit_capsule(a, c);
|
|
if(m) {
|
|
ddraw_color(RED);
|
|
ddraw_box(m->contact_point, vec3(0.05f, 0.05f, 0.05f));
|
|
ddraw_arrow(m->contact_point, add3(m->contact_point, m->normal));
|
|
} else ddraw_color(WHITE);
|
|
|
|
ddraw_capsule(vec3(x,y-1.0f,z), vec3(x,y+1.0f,z-1.0f), 0.2f);
|
|
ddraw_box(vec3(0,0,-8.0f), vec3(1,1,1));
|
|
}
|
|
}
|