Freesteel Blog » Line torus intersection code dump

Line torus intersection code dump

Thursday, February 19th, 2009 at 4:31 pm Written by:

Hm. This image didn’t come out quite as badly as I would have expected. Maybe I’ll do more like this.

In the non-axial projection function for a general cutter shape, it’s necessary to find the intersection between a line and a torus — for the case of a bull-nosed cutter against one of the corners of a triangle.

As you can see, there are four points of intersection of this polynomial surface, and no symmetries to simplify it down. Therefore, you can expect to solve a quartic function.

Luckily, I added the Quick and memorable solution [of a quartic] from first principles section in Wikipedia, so I could find it when I finally needed it.

I managed to get this far without it by using offset ellipse technology which gives enough global structure to contain a robust iterative solution (you need to find a point on either side of the curve to begin the algorithm and avoid stabbing in the dark).

Without loss of generality, the torus can be placed at the origin in the XY plane. Its flat radius is f, and corner radius is c (so, csq = c*c). Consider a line that intersects the torus. Without loss of generality, we can rotate the assembly so that the line lies in a single plane parallel to the YZ plane, so its coordinates (for variable x) are

(x + s, d, x * l)

where l is the slope to the XY plane, and (s, d) is the point it intersects the XY plane.

We need to solve the equation:

(r - f)^2 + (x * l)^2 = c^2
where r = sqrt((x + s)^2 + d^2)

So, here is my [PUBLIC DOMAIN] code for doing it, within the function ToroidLineIntersect

I’ve in-lined the quartic and cubic solutions so that some of their partial calculations can be optimized.

I’ll come back to this later and find more of the bugs when we run some tests on it. It’s important to get real world values here. I don’t know if the system is going to want to put in very large values of l, for example. If this is unstable, there might be enough negotiation with the higher order algorithms to ask them to round these cases to a vertical drop-down, which will make almost no difference to the operation of those algorithms, but make a great improvement in calculation efficiency.

It’s wise to steer clear of the weak spots in the mathematics.


// trivial functions
double Square(double X) { return X*X }
double Cbrt(double X) { return (X < 0.0 ? -pow(-X, 1./3) : pow(X, 1./3)); }
#define TOL_ZERO(X)  ASSERT(fabs(X) < 0.00001)  // macro to check things are adding up correctly

// subroutine performing one iteration and commiting the value
void ToroidLineIntersectSubiter(double& x, bool& bres, double lx, double f, 
                  double dsq, double ss, double lsqp1, double lcr)
{
    double rfr = lx * (lx * lsqp1 + 2 * ss) + lcr; 
    if (rfr < 0.0)
        return; 
    double rfrL = 2 * sqrt(dsq + Square(lx + ss)) * f; 

    // avoid doing in a loop, since one iteration is almost always enough
    double e1 = rfrL - rfr; 
    if (fabs(e1) > 1e-8)  // the iteration 
    {
        // double der = 2 * (lx * lsqp1 + ss) - 2 f * 0.5 * (dsq + Square(lx + ss))^-0.5 * 2 (lx + ss)
        double der = 2 * (lx * lsqp1 + ss) - 4 * Square(f) * (lx + ss) / rfrL; 
        double nlx = lx + e1 / der; 

        double nrfr = nlx * (nlx * lsqp1 + 2 * ss) + lcr; 
        double nrfrL = 2 * sqrt(dsq + Square(nlx + ss)) * f; 
        double e2 = nrfrL - nrfr;
        if (fabs(e2) < fabs(e1))
            lx = nlx; 
    }

    if (!bres || (lx > x))
    {
        x = lx; 
        bres = true; 
    }
}


//////////////////////////////////////////////////////////////////////
// Main function
//////////////////////////////////////////////////////////////////////
bool ToroidLineIntersect(double& x, double f, double csq, double dsq, 
                                 double ss, double l)
{
    // solve point p = P3(x + s, d, x * l) intersecting the torus (flat=f, corner=c) for max x
    // or: (P2(x+s,d).Len() - f)^2 + (x*l)^2 = csq

    // dsq = d*d
    // r*r = (x+s)^2 + dsq = x^2 + 2xs + s^2 + dsq
    // csq = (x*l)^2 + (r-f)^2 = x^2 * lsq + r^2 - 2rf + f^2 
    //                         = x^2(lsq + 1) + 2xs + s^2 + dsq - 2rf + f^2 
    // 2rf = x^2(lsq + 1) + 2xs + (s^2 + dsq - csq + f^2)
    double lsqp1 = Square(l) + 1.0; 
    double br = 2 * ss / lsqp1; 
    double lcr = Square(ss) + dsq - csq + Square(f); 
    double cr = lcr / lsqp1; 
    double rr = 2 * f / lsqp1; 

    // r rr = x^2 + x br + cr
    // (x^2 + 2xs + s^2 + dsq) * rr^2 = x^4 + x^3 br 2 + x^2 (br^2 + cr 2) + x (br cr 2) + cr^2
    double rrsq = Square(rr); 
    double b = br / 2; 
    double qb2 = Square(br) + 2 * cr - rrsq; 
    double qb1 = 2 * cr * br - 2 * ss * rrsq; 
    double qb0 = Square(cr) - Square(ss) * rrsq - dsq * rrsq; 

    // 0 = x^4 + x^3 4 b + x^2 qb2 + x qb1 + qb0
    // convert to a depressed quartic
    // x = u - b
    // 0 = u^4 - u^3 4 b + u^2 6 b^2  - u 4 b^3   + b^4 + 
    //           u^3 4 b - u^2 12 b^2 + u 12 b^3  - 4 b^4 +
    //                     u^2 qb2    - u 2 qb2 b + b^2 qb2
    //                                  u qb1     - b qb1
    //                                            + qb0

    double bsq = Square(b); 
    double qc = -6 * bsq + qb2; 
    double qd = 8 * b * bsq - 2 * qb2 * b + qb1; 
    double qe = -3 * Square(bsq) + bsq * qb2 - b * qb1 + qb0; 
    // 0 = u^4 + u^2 qc + u qd + qe = (u^2 + p u + q) (u^2 + r u + s) -- factored out
    // multiplying out and reducing the unknowns gives:
    //      0 = p + r,        qc = q + s + p r,    qd = p s + q r,      qe = q s
    //      qc + p^2 = s + q, qd / p = s - q,      qe = s q
    //      (qc + p^2)^2 - (qd / p)^2 = 4 qe
    // P = p^2 gives:
    // 0 = P^3 + P^2 2 qc + P (qc^2 - 4 qe) - qd^2
    double cb = 2 * qc; 
    double cc = Square(qc) - 4 * qe; 
    double cd = -Square(qd); 
    // 0 = P^3 + P^2 cb + P cc + cd

    // now solve the cubic
    double cbsq = Square(cb); 
    double ccq = (3 * cc - cbsq) / 9; 
    double ccr = (9 * cb * cc - 27 * cd - 2 * cb * cbsq) / 54; 
    double ccqcb = ccq * Square(ccq); 
    double disc = ccqcb + Square(ccr); 
    double psq; 
    if (disc >= 0.0)
    {
        double discrt = sqrt(disc); 
        psq = Cbrt(ccr + discrt) + Cbrt(ccr - discrt) - cb / 3; 
    }
    else
    {
        double crho = sqrt(-ccqcb); 
        double cthet = acos(ccr / crho); 
        psq = 2 * Cbrt(crho) * cos(cthet / 3) - cb / 3; 
    }

    TOL_ZERO(Square(psq) * (psq + cb) + psq * cc + cd); 
    if (psq < 0.0)
        return false; 

    // 0 = u^4 + u^2 qc + u qd + qe = (u^2 + pq u + qq) (u^2 + rq u + sq) -- factored out
    double pq = sqrt(psq); 
    double rq = -pq; 
    double sq = (qc + psq + qd / pq) / 2; 
    double qq = (qc + psq - qd / pq) / 2;  

    bool bres = false; 

    // we're solving, which has been squared.  The 
    // 2rf = x^2(lsq + 1) + 2xs + (s^2 + dsq - csq + f^2)

    // solve (u^2 + rq u + sq) = 0
    double qqur = Square(rq) - 4 * sq; 
    if (qqur >= 0.0)
    {
        double qqurs = sqrt(qqur); 
        double u1 = (-rq - qqurs) / 2; 
        TOL_ZERO(u1*u1*u1*u1 + u1*u1 * qc + u1 * qd + qe); 
        double u2 = (-rq + qqurs) / 2; 
        TOL_ZERO(u2*u2*u2*u2 + u2*u2 * qc + u2 * qd + qe); 

        ToroidLineIntersectSubiter(x, bres, u1 - b, f, dsq, ss, lsqp1, lcr); 
        ToroidLineIntersectSubiter(x, bres, u2 - b, f, dsq, ss, lsqp1, lcr); 
    }

    // solve (u^2 + p u + q) = 0
    double qqup = Square(pq) - 4 * qq; 
    if (qqup >= 0.0)
    {
        double qqups = sqrt(qqup); 
        double u1 = (-pq - qqups) / 2; 
        TOL_ZERO(u1*u1*u1*u1 + u1*u1 * qc + u1 * qd + qe); 
        double u2 = (-pq + qqups) / 2; 
        TOL_ZERO(u2*u2*u2*u2 + u2*u2 * qc + u2 * qd + qe); 

        ToroidLineIntersectSubiter(x, bres, u1 - b, f, dsq, ss, lsqp1, lcr); 
        ToroidLineIntersectSubiter(x, bres, u2 - b, f, dsq, ss, lsqp1, lcr); 
    }
    return bres; // actual value is in the x
}

1 Comment

  • 1. Topics about Music »&hellip replies at 19th February 2009, 5:06 pm :

    […] #mefi link log put an intriguing blog post on Line torus intersection code dumpHere’s a quick excerpt Hm. This image didn’t come out quite as badly as I would have expected. Maybe I’ll do more like this. In the non-axial projection function for a general cutter shape, it’s necessary to find the intersection between a line and a torus — for the case of a bull-nosed cutter against one of the corners of a triangle. As you can see, there are four points of intersection of this polynomial surface, and no symmetries to simplify it down. Therefore, you can expect to solve a quartic function. Lucki […]

Leave a comment

XHTML: You can use these tags: <a href="" title=""> <abbr title=""> <acronym title=""> <blockquote cite=""> <code> <em> <strong>