Triangle - Capsule Intersection

Some time ago I needed a function to find the intersection point between a capsule or SSV (Swept Sphere Volume) and a triangle. This took me surprisingly long to find information about and the ones I did find all seemed pretty complicated.

I ended up making my own algorithm. I have no idea if it's any good or not, I'm not that smart, but it seems to work well.

How it works

• Make an infinite plane out of the triangle surface
• Move this plane towards the origin of the ray capsule by it's radius
• Check if we intersect this plane
• If an intersection point is found check if the point is outside the triangle (as projected onto the moved plane)
• If outside:
• Find the closest edge to the capsule
• Perform a ray / capsule intersect test
• If it intersected calculate the capsule hit normal
• If inside:
• Get the normal

The functions ray_capsule_intersect, point_line_distance and capsule_normal are all taken from iquilezles.org.

I hope this helps anyone searching for the same as I did.

The code

static inline float point_line_distance(v3f p, v3f a, v3f b) {

        v3f pa = v3f_sub(p, a);
        v3f ba = v3f_sub(b, a);

        float h = v3f_dot(pa, ba) / v3f_dot(ba, ba);
              h = fminf(fmaxf(h, 0.0f), 1.0f);

        return v3f_len(v3f_sub(pa, v3f_scale(ba, h)));
}


static inline bool ray_capsule_intersect(v3f ro, v3f rd, v3f pa, v3f pb, float ra, float* distance) {

        v3f ba = v3f_sub(pb, pa);
        v3f oa = v3f_sub(ro, pa);

        float baba = v3f_dot(ba, ba);
        float bard = v3f_dot(ba, rd);
        float baoa = v3f_dot(ba, oa);
        float rdoa = v3f_dot(rd, oa);
        float oaoa = v3f_dot(oa, oa);

        float a = baba      - bard*bard;
        float b = baba*rdoa - baoa*bard;
        float c = baba*oaoa - baoa*baoa - ra*ra*baba;
        float h = b*b - a*c;

        if (h >= 0.0f) {
                float t = (-b-sqrt(h))/a;
                float y = baoa + t*bard;

                // body
                if (y > 0.0f && y < baba) {
                        *distance = t;
                        return true;
                }

                // caps
                v3f oc = (y <= 0.0) ? oa : v3f_sub(ro, pb);

                b = v3f_dot(rd, oc);
                c = v3f_dot(oc, oc) - ra*ra;
                h = b*b - c;

                if (h > 0.0f) {
                        *distance = -b - sqrtf(h);
                        return true;
                }
        }
        return false;
}


static inline v3f capsule_normal(v3f p, v3f a, v3f b, float r) {

        v3f ba = v3f_sub(b, a);
        v3f pa = v3f_sub(p, a);

        float h = v3f_dot(pa, ba) / v3f_dot(ba, ba);
              h = fminf(fmaxf(h, 0.0f), 1.0f);

        return v3f_divs(v3f_sub(pa, v3f_scale(ba, h)), r);
}


bool ssv_triangle_intersect(v3f origin, v3f velocity, float radius, v3f v0, v3f v1, v3f v2, float *distance, v3f *hit_point, v3f *hit_normal) {

        // plane normal
        v3f e0   = v3f_sub(v1, v0);
        v3f e1   = v3f_sub(v2, v0);
        v3f e0e1 = v3f_cross(e0, e1);
        v3f pn   = v3f_normalize(e0e1);
        // ----

        float cap_len    = v3f_len(velocity);
        v3f   cap_normal = v3f_normalize(velocity);

        float denom = v3f_dot(pn, cap_normal);
        if (fabsf(denom) < 0.00001f) return false;

        // grow plane thickness by radius towards origin
        float r  = (denom < 0.0f) ? radius : -radius;
        v3f   po = v3f_add(v0, v3f_scale(pn, r));
        // ----

        // find intersection point
        float u = v3f_dot(pn, v3f_sub(po, origin)) / denom;
        if (u > cap_len) return false;
        v3f p = v3f_add(origin, v3f_scale(cap_normal, u));
        // ----

        // is projected point outside triangle
        bool outside = true;

        v3f   w = v3f_sub(p, v0);
        float y = v3f_dot(v3f_cross(e0, w), e0e1) / v3f_norm2(e0e1); // γ=[(u×w)⋅n]/n²
        float b = v3f_dot(v3f_cross(w, e1), e0e1) / v3f_norm2(e0e1); // β=[(w×v)⋅n]/n²
        float a = 1.0f - y - b;

        if ((0.0f <= a) && (a <= 1.0f) &&
            (0.0f <= b) && (b <= 1.0f) &&
            (0.0f <= y) && (y <= 1.0f)) {
                outside = false;
        }
        // ----

        if (outside) {
                // find closest edge
                float d1 = point_line_distance(p, v0, v1);
                float d2 = point_line_distance(p, v1, v2);
                float d3 = point_line_distance(p, v2, v0);

                v3f va = v0;
                v3f vb = v1;

                float dt = d1;

                if (d2 < dt) { dt = d2;  va = v1;  vb = v2; }
                if (d3 < dt) {           va = v2;  vb = v0; }
                // ----

                if (!ray_capsule_intersect(origin, cap_normal, va, vb, radius, &u)) return false;
                if (u > cap_len) return false;

                p  = v3f_add(origin, v3f_scale(cap_normal, u));
                pn = capsule_normal(p, va, vb, radius);
        }

        if (u < 0.0f) return false;

        *distance   = u;
        *hit_point  = p;
        *hit_normal = pn;

        return true;
}