Maybe we can test this to see how it performs vs what we're using now.

-BE

#include <Eigen/Dense>
#include <cmath>
#include <limits>

using namespace Eigen;

bool intersectCylinderWithCaps(
    const Vector3d& rayOrigin,
    const Vector3d& rayDir,     // Assumed normalized
    const Vector3d& cylBase,
    const Vector3d& cylAxis,    // Assumed normalized
    double radius,
    double height,
    double& tIn,
    double& tOut)
{
    Vector3d RC = rayOrigin - cylBase;
    Vector3d n = rayDir.cross(cylAxis);
    double ln = n.norm();

    bool hit = false;
    tIn = std::numeric_limits<double>::infinity();
    tOut = -std::numeric_limits<double>::infinity();

    // Side intersection
    if (ln > 1e-8) {
        n.normalize();
        double d = std::abs(RC.dot(n));
        if (d <= radius) {
            Vector3d O = RC.cross(cylAxis);
            double t = -O.dot(n) / ln;

            O = n.cross(cylAxis);
            O.normalize();
            double s = std::sqrt(radius * radius - d * d) / rayDir.dot(O);

            double t1 = t - s;
            double t2 = t + s;

            // Check if points are within cylinder height
            Vector3d p1 = rayOrigin + t1 * rayDir;
            Vector3d p2 = rayOrigin + t2 * rayDir;

            double h1 = (p1 - cylBase).dot(cylAxis);
            double h2 = (p2 - cylBase).dot(cylAxis);

            if (h1 >= 0 && h1 <= height) {
                tIn = std::min(tIn, t1);
                hit = true;
            }
            if (h2 >= 0 && h2 <= height) {
                tOut = std::max(tOut, t2);
                hit = true;
            }
        }
    }

    // Cap intersection
    for (int i = 0; i < 2; ++i) {
        double capZ = (i == 0) ? 0.0 : height;
        Vector3d capCenter = cylBase + cylAxis * capZ;
        double denom = rayDir.dot(cylAxis);

        if (std::abs(denom) > 1e-8) {
            double tCap = (capCenter - rayOrigin).dot(cylAxis) / denom;
            if (tCap >= 0) {
                Vector3d pCap = rayOrigin + tCap * rayDir;
                Vector3d v = pCap - capCenter;
                v -= cylAxis * v.dot(cylAxis); // Project onto cap plane
                if (v.norm() <= radius) {
                    tIn = std::min(tIn, tCap);
                    tOut = std::max(tOut, tCap);
                    hit = true;
                }
            }
        }
    }

    return hit;
}








#include <Eigen/Dense>
#include <cmath>
#include <limits>

using namespace Eigen;

bool intersectCone(
    const Vector3d& rayOrigin,
    const Vector3d& rayDir,     // Assumed normalized
    const Vector3d& coneBase,
    const Vector3d& coneAxis,   // Assumed normalized
    double baseRadius,
    double height,
    double& tIn,
    double& tOut)
{
    Vector3d apex = coneBase + coneAxis * height;
    Vector3d RC = rayOrigin - apex;

    double k = baseRadius / height;
    double k2 = k * k;

    // Project ray and cone axis
    double DdotA = rayDir.dot(coneAxis);
    double PdotA = RC.dot(coneAxis);

    Vector3d Dperp = rayDir - DdotA * coneAxis;
    Vector3d Pperp = RC - PdotA * coneAxis;

    double a = Dperp.dot(Dperp) - k2 * DdotA * DdotA;
    double b = 2 * (Dperp.dot(Pperp) - k2 * DdotA * PdotA);
    double c = Pperp.dot(Pperp) - k2 * PdotA * PdotA;

    double discriminant = b * b - 4 * a * c;
    bool hit = false;
    tIn = std::numeric_limits<double>::infinity();
    tOut = -std::numeric_limits<double>::infinity();

    if (std::abs(a) > 1e-8 && discriminant >= 0.0) {
        double sqrtDisc = std::sqrt(discriminant);
        double t1 = (-b - sqrtDisc) / (2 * a);
        double t2 = (-b + sqrtDisc) / (2 * a);

        for (double t : {t1, t2}) {
            if (t >= 0.0) {
                Vector3d p = rayOrigin + t * rayDir;
                double h = (p - coneBase).dot(coneAxis);
                if (h >= 0.0 && h <= height) {
                    tIn = std::min(tIn, t);
                    tOut = std::max(tOut, t);
                    hit = true;
                }
            }
        }
    }

    // Cap intersection (base only)
    double denom = rayDir.dot(coneAxis);
    if (std::abs(denom) > 1e-8) {
        double tCap = (coneBase - rayOrigin).dot(coneAxis) / denom;
        if (tCap >= 0.0) {
            Vector3d pCap = rayOrigin + tCap * rayDir;
            Vector3d v = pCap - coneBase;
            v -= coneAxis * v.dot(coneAxis); // Project onto base plane
            if (v.norm() <= baseRadius) {
                tIn = std::min(tIn, tCap);
                tOut = std::max(tOut, tCap);
                hit = true;
            }
        }
    }

    return hit;
}

Post a reply to this message