Exemplo n.º 1
0
Real DistLine3Circle3<Real>::GetSquared ()
{
    Vector3<Real> diff = mLine->Origin - mCircle->Center;
    Real diffSqrLen = diff.SquaredLength();
    Real MdM = mLine->Direction.SquaredLength();
    Real DdM = diff.Dot(mLine->Direction);
    Real NdM = mCircle->Normal.Dot(mLine->Direction);
    Real DdN = diff.Dot(mCircle->Normal);

    Real a0 = DdM;
    Real a1 = MdM;
    Real b0 = DdM - NdM*DdN;
    Real b1 = MdM - NdM*NdM;
    Real c0 = diffSqrLen - DdN*DdN;
    Real c1 = b0;
    Real c2 = b1;
    Real rsqr = mCircle->Radius*mCircle->Radius;

    Real a0sqr = a0*a0;
    Real a1sqr = a1*a1;
    Real twoA0A1 = ((Real)2)*a0*a1;
    Real b0sqr = b0*b0;
    Real b1Sqr = b1*b1;
    Real twoB0B1 = ((Real)2)*b0*b1;
    Real twoC1 = ((Real)2)*c1;

    // The minimum point B+t*M occurs when t is a root of the quartic
    // equation whose coefficients are defined below.
    Polynomial1<Real> poly(4);
    poly[0] = a0sqr*c0 - b0sqr*rsqr;
    poly[1] = twoA0A1*c0 + a0sqr*twoC1 - twoB0B1*rsqr;
    poly[2] = a1sqr*c0 + twoA0A1*twoC1 + a0sqr*c2 - b1Sqr*rsqr;
    poly[3] = a1sqr*twoC1 + twoA0A1*c2;
    poly[4] = a1sqr*c2;

    PolynomialRoots<Real> polyroots(Math<Real>::ZERO_TOLERANCE);
    polyroots.FindB(poly, 6);
    int count = polyroots.GetCount();
    const Real* roots = polyroots.GetRoots();

    Real minSqrDist = Math<Real>::MAX_REAL;
    for (int i = 0; i < count; ++i)
    {
        // Compute distance from P(t) to circle.
        Vector3<Real> P = mLine->Origin + roots[i]*mLine->Direction;
        DistPoint3Circle3<Real> query(P, *mCircle);
        Real sqrDist = query.GetSquared();
        if (sqrDist < minSqrDist)
        {
            minSqrDist = sqrDist;
            mClosestPoint0 = query.GetClosestPoint0();
            mClosestPoint1 = query.GetClosestPoint1();
        }
    }

    return minSqrDist;
}
Exemplo n.º 2
0
static PyObject* py_polyroots(PyObject *obj, PyObject *args, PyObject *kwds)
{
    PyArrayObject *coeffs = NULL;
    PyArrayObject *result = NULL;
    Py_ssize_t dims;
    int error;
    static char *kwlist[] = {"coeffs", NULL};

    if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&", kwlist,
        PyConverter_ComplexArrayCopy, &coeffs))
        return NULL;

    if (PyArray_NDIM(coeffs) != 1) {
        PyErr_Format(PyExc_ValueError, "invalid coefficients");
        goto _fail;
    }

    dims = PyArray_DIM(coeffs, 0) - 1;
    result = (PyArrayObject *)PyArray_SimpleNew(1, &dims, NPY_COMPLEX128);
    if (result == NULL) {
        PyErr_Format(PyExc_MemoryError, "failed to allocate roots array");
        goto _fail;
    }

    error = polyroots((int)PyArray_DIM(coeffs, 0), PyArray_DATA(coeffs),
                      PyArray_DATA(result));
    if (error != 0) {
        PyErr_Format(PyExc_ValueError,
            "polyroots() failed with error code %i", error);
        goto _fail;
    }

    Py_DECREF(coeffs);
    return PyArray_Return(result);

  _fail:
    Py_XDECREF(coeffs);
    Py_XDECREF(result);
    return NULL;
}
Exemplo n.º 3
0
// From WildMagic, (c) Geometric Tools LLC
// See Eberly, Distance between Line and Circle
void CriticalPointsLineCircle(Circle const &c,
                              osg::Vec3d const &line_p, // line point
                              osg::Vec3d const &line_d, // line dirn
                              osg::Vec3d &min,
                              osg::Vec3d &max)
{
    osg::Vec3d diff = line_p - c.center;
    double diffSqrLen = diff.length2();
    double MdM = line_d.length2();
    double DdM = diff*line_d;
    double NdM = c.normal*line_d;
    double DdN = diff*c.normal;

    double a0 = DdM;
    double a1 = MdM;
    double b0 = DdM - NdM*DdN;
    double b1 = MdM - NdM*NdM;
    double c0 = diffSqrLen - DdN*DdN;
    double c1 = b0;
    double c2 = b1;
    double rsqr = c.radius*c.radius;

    double a0sqr = a0*a0;
    double a1sqr = a1*a1;
    double twoA0A1 = 2.0*a0*a1;
    double b0sqr = b0*b0;
    double b1Sqr = b1*b1;
    double twoB0B1 = 2.0*b0*b1;
    double twoC1 = 2.0*c1;

    // The minimum point B+t*M occurs when t is a root of the quartic
    // equation whose coefficients are defined below.

    // I think index specifies polynomial degree
    // ie. poly[3] = coefficient for x^3

    Polynomial1<Real> poly(4);
    poly[0] = a0sqr*c0 - b0sqr*rsqr;
    poly[1] = twoA0A1*c0 + a0sqr*twoC1 - twoB0B1*rsqr;
    poly[2] = a1sqr*c0 + twoA0A1*twoC1 + a0sqr*c2 - b1Sqr*rsqr;
    poly[3] = a1sqr*twoC1 + twoA0A1*c2;
    poly[4] = a1sqr*c2;

    PolynomialRoots<Real> polyroots(Math<Real>::ZERO_TOLERANCE);
    polyroots.FindB(poly, 6);
    int count = polyroots.GetCount();
    const Real* roots = polyroots.GetRoots();

    Real minSqrDist = Math<Real>::MAX_REAL;
    for (int i = 0; i < count; ++i)
    {
        // Compute distance from P(t) to circle.
        Vector3<Real> P = mLine->Origin + roots[i]*mLine->Direction;
        DistPoint3Circle3<Real> query(P, *mCircle);
        Real sqrDist = query.GetSquared();
        if (sqrDist < minSqrDist)
        {
            minSqrDist = sqrDist;
            mClosestPoint0 = query.GetClosestPoint0();
            mClosestPoint1 = query.GetClosestPoint1();
        }
    }

    return minSqrDist;
}
Exemplo n.º 4
0
/*
Fit frequency-domain data with photobleaching.
*/
int fitexpsin(
    char *data,      /* data array of doubles */
    int data_stride, /* number of bytes to move from one data value to next */
    int numdata,     /* number of double values in data array */
    double *poly,    /* precalculated normalized Chebyshev polynomial Tj(t) */
    double *coef,    /* buffer for dnj of shape (numexps+1, numcoef+1) */
    int numcoef,     /* number of coefficients */
    double deltat,   /* duration between data points */
    int startcoef,   /* start coefficient. usually equals numexps-1 */
    double *buff,    /* working buffer of shape (numexps, numdata) */
    double *result,  /* buffer to receive fitted parameters
                        offset, tau, frq, amp[numexps] */
    char *fitt,      /* buffer to receive fitted data in double [numpoints] */
    int fitt_stride) /* number bytes to move from one fitted value to next */
{
    PyThreadState *_save = NULL;
    Py_complex xroots[5];
    Py_complex xcoefs[6];
    double matrix[3*3];
    double *pbuff;
    double *ppoly;
    double *pcoef;
    double *pmat;
    double *pvec;
    double *prow;
    double *pcol;
    double *pdn0;
    double *pdn1;
    double *off = result;
    double *rat = result + 1;
    double *amp = result + 2;
    double sum, temp, ratn, frqn;
    double cosw, cos2w, t0, t1, t2, t3, t4, t5, t6;
    int i, j, k, t, n, N, row, col, error;
    int stride = numcoef + 1;

    /* discrete Chebyshev coefficients dj */
    ppoly = poly;
    pcoef = coef;
    if (data_stride == sizeof(double)) {
        double *pdata;
        for (j = 0; j < numcoef; j++) {
            pdata = (double *)data;
            sum = 0.0;
            for (t = 0; t < numdata; t++) {
                sum += (*pdata++) * (*ppoly++);
            }
            *pcoef++ = sum;
        }
    } else {
        char *pdata;
        for (j = 0; j < numcoef; j++) {
            pdata = data;
            sum = 0.0;
            for (t = 0; t < numdata; t++) {
                sum += (*((double *) pdata)) * (*ppoly++);
                pdata += data_stride;
            }
            *pcoef++ = sum;
        }
    }

    _save = PyEval_SaveThread();

    /* integral coefficients dnj */
    N = numdata - 1;
    pdn0 = coef;
    pdn1 = coef + stride;
    for (n = 0; n < 3; n++) {
        pdn0[numcoef] = 0.0;
        pdn1[0] = 0.0;
        for (j = 1; j < numcoef; j++) {
            pdn1[j] = ((N + j + 2) * pdn0[j + 1] / (2*j + 3) - pdn0[j] -
                       (N - j + 1) * pdn0[j - 1] / (2*j - 1)) / 2.0;
        }
        pdn0 += stride;
        pdn1 += stride;
    }

    /* cubic polynomial coefficients a + bx + cx^2 + dx^3 */
    frqn = TWOPI / numdata;
    cosw = cos(frqn);
    cos2w = cos(frqn * 2.0);
    t0 = 1.0 / (1.0 + 2.0*cosw);
    t1 = t0 * t0 * t0;
    t2 = 9.0 * t0 - 3.0;
    t3 = (16.0*cosw - 4.0*cosw*cosw + 8.0*cos2w - 8.0*cosw*cos2w - 12.0) * t1;
    t4 = 2.0 - 6.0 * t0;
    t5 = (14.0 - 14.0*cosw - 8.0*cos2w + 4.0*cosw*cosw + 4.0*cosw*cos2w) * t1;
    t6 = (4.0*cosw + 2.0*cos2w - 6.0) * t1;
    pbuff = buff;
    for (j = startcoef; j < (numcoef - 3); j++) {
        *pbuff++ = coef[j] + coef[j+stride*2] * t2 + coef[j+stride*3] * t3;
        *pbuff++ = coef[j+stride] + coef[j+stride*2] * t4 +
                   coef[j+stride*3] * t5;
        *pbuff++ = coef[j+stride*2] * t0 + coef[j+stride*3] * t6;
        *pbuff++ = coef[j+stride*3] * t1;
    }

    /* regression coefficients */
    /* quintic polynomial (a + bx + cx^2 + dx^3)*(b + 2cx + 3dx^2) */
    memset(xcoefs, 0, 6 * sizeof(Py_complex));
    pbuff = buff;
    for (j = startcoef; j < (numcoef - 3); j++) {
        xcoefs[0].real += pbuff[0]*pbuff[1];
        xcoefs[1].real += pbuff[0]*pbuff[2]*2.0 + pbuff[1]*pbuff[1];
        xcoefs[2].real += pbuff[1]*pbuff[2]*3.0 + pbuff[0]*pbuff[3]*3.0;
        xcoefs[3].real += pbuff[2]*pbuff[2]*2.0 + pbuff[1]*pbuff[3]*4.0;
        xcoefs[4].real += pbuff[2]*pbuff[3]*5.0;
        xcoefs[5].real += pbuff[3]*pbuff[3]*3.0;
        pbuff += 4;
    }

    /* roots of quintic polynomial */
    error = polyroots(6, xcoefs, xroots);
    if (error != 0) {
        PyEval_RestoreThread(_save);
        return error;
    }

    /* decay rate and frequency of harmonics */
    /* find smallest chi-square */
    t0 = DBL_MAX;
    for (i = 0; i < 5; i++) {
        pbuff = buff;
        t1 = xroots[i].real;
        t2 = t1*t1;
        t3 = t1*t1*t1;
        sum = 0.0;
        for (j = 0; j < (numcoef - startcoef - 3); j++) {
            temp = pbuff[0] + t1*pbuff[1] + t2*pbuff[2] + t3*pbuff[3];
            sum += temp*temp;
            pbuff += 4;
        }
        if (sum < t0) {
            t0 = sum;
            k = i;
        }
    }

    *rat = ratn = log((3.0 - xroots[k].real) / (2.0*cosw + 1.0));

    /* fitting amplitudes */
    /* Chebyshev transform signal for each exponential component */
    pcoef = coef + stride;

    /* decay component */
    buff[0] = 1.0;
    for (t = 1; t < numdata; t++)
        buff[t] = exp(ratn*(double)t);
    ppoly = poly;
    for (j = 0; j < numcoef; j++) {
        sum = 0.0;
        for (t = 0; t < numdata; t++)
            sum += buff[t] * (*ppoly++);
        *pcoef++ = sum;
    }
    pcoef++;

    /* sine component */
    pbuff = buff + numdata;
    pbuff[0] = 0.0;
    for (t = 1; t < numdata; t++)
        pbuff[t] = -buff[t] * sin(frqn*(double)t);
    ppoly = poly;
    for (j = 0; j < numcoef; j++) {
        sum = 0.0;
        for (t = 0; t < numdata; t++)
            sum += pbuff[t] * (*ppoly++);
        *pcoef++ = sum;
    }
    pcoef++;

    /* cosine component */
    pbuff += numdata;
    pbuff[0] = 1.0;
    for (t = 1; t < numdata; t++)
        pbuff[t] = buff[t] * cos(frqn*(double)t);
    ppoly = poly;
    for (j = 0; j < numcoef; j++) {
        sum = 0.0;
        for (t = 0; t < numdata; t++)
            sum += pbuff[t] * (*ppoly++);
        *pcoef++ = sum;
    }
    pcoef++;

    *rat = -deltat / *rat;

    /* regression matrix for fitting amplitudes */
    pmat = matrix;
    pcol = coef;
    for (col = 0; col < 3; col++) {
        pcol += stride;
        prow = coef;
        for (row = 0; row < 3; row++) {
            prow += stride;
            sum = 0.0;
            for (j = 1; j < numcoef; j++)
                sum += prow[j] * pcol[j];
            *pmat++ = sum;
        }
    }

    /* regression vector for fitting amplitudes */
    pvec = amp;
    pcoef = coef;
    for (row = 0; row < 3; row++) {
        pcoef += stride;
        sum = 0.0;
        for (j = 1; j < numcoef; j++)
            sum += coef[j] * pcoef[j];
        *pvec++ = sum;
    }

    /* solve linear equation system for amplitudes */
    error = linsolve(3, matrix, amp);
    if (error != 0) {
        PyEval_RestoreThread(_save);
        return error;
    }

    /* calculate offset from zero Chebyshev coefficients */
    pcoef = coef + stride;
    temp = *coef;
    for (n = 0; n < 3; n++) {
        temp -= amp[n] * (*pcoef);
        pcoef += stride;
    }
    *off = temp;

    /* calculate fitted data */
    if (fitt != NULL) {
        if (fitt_stride == sizeof(double)) {
            double *pfitt = (double *)fitt;
            temp = *off;
            for (t = 0; t < numdata; t++)
                *pfitt++ = temp;
            pbuff = buff;
            for (n = 0; n < 3; n++) {
                pfitt = (double *)fitt;
                temp = amp[n];
                for (t = 0; t < numdata; t++)
                    *pfitt++ += temp * (*pbuff++);
            }
        } else {
            char *pfitt = fitt;
            temp = *off;
            for (t = 0; t < numdata; t++) {
                *(double *)pfitt = temp;
                pfitt += fitt_stride;
            }
            pbuff = buff;
            for (n = 0; n < 3; n++) {
                pfitt = fitt;
                temp = amp[n];
                for (t = 0; t < numdata; t++) {
                    *(double *)pfitt += temp * (*pbuff++);
                    pfitt += fitt_stride;
                }
            }
        }
    }

    PyEval_RestoreThread(_save);
    return 0;
}
Exemplo n.º 5
0
/*
Fit multiple exponential function.
*/
int fitexps(
    char *data,      /* data array of doubles */
    int data_stride, /* number of bytes to move from one data value to next */
    int numdata,     /* number of double values in data array */
    double *poly,    /* precalculated normalized Chebyshev polynomial Tj(t) */
    double *coef,    /* buffer for dnj of shape (numexps+1, numcoef+1) */
    int numcoef,     /* number of coefficients */
    int numexps,     /* number of exponentials to fit */
    double deltat,   /* duration between data points */
    int startcoef,   /* start coefficient. usually equals numexps-1 */
    double *buff,    /* working buffer of shape (numexps, numdata) */
    double *result,  /* buffer to receive fitted parameters
                        offset, amp[numexps], tau[numexps], frq[numexps] */
    char *fitt,      /* buffer to receive fitted data in double [numpoints] */
    int fitt_stride) /* number bytes to move from one fitted value to next */
{
    PyThreadState *_save = NULL;
    Py_complex xroots[MAXEXPS];
    Py_complex xcoefs[MAXEXPS+1];
    double matrix[MAXEXPS*MAXEXPS];
    double vector[MAXEXPS];
    double *pbuff;
    double *ppoly;
    double *pcoef;
    double *pmat;
    double *pvec;
    double *prow;
    double *pcol;
    double *pdn0;
    double *pdn1;
    double *off = result;
    double *amp = result + 1;
    double *rat = result + 1 + numexps;
    double *frq = result + 1 + numexps + numexps;
    double sum, temp, frqn, ratn;
    int j, t, n, N, row, col, error;
    int stride = numcoef + 1;

    /* discrete Chebyshev coefficients dj */
    ppoly = poly;
    pcoef = coef;
    if (data_stride == sizeof(double)) {
        double *pdata;
        for (j = 0; j < numcoef; j++) {
            pdata = (double *)data;
            sum = 0.0;
            for (t = 0; t < numdata; t++) {
                sum += (*pdata++) * (*ppoly++);
            }
            *pcoef++ = sum;
        }
    } else {
        char *pdata;
        for (j = 0; j < numcoef; j++) {
            pdata = data;
            sum = 0.0;
            for (t = 0; t < numdata; t++) {
                sum += (*((double *) pdata)) * (*ppoly++);
                pdata += data_stride;
            }
            *pcoef++ = sum;
        }
    }

    _save = PyEval_SaveThread();

    /* integral coefficients dnj */
    N = numdata - 1;
    pdn0 = coef;
    pdn1 = coef + stride;
    for (n = 0; n < numexps; n++) {
        pdn0[numcoef] = 0.0;
        pdn1[0] = 0.0;
        for (j = 1; j < numcoef; j++) {
            pdn1[j] = ((N + j + 2) * pdn0[j + 1] / (2*j + 3) - pdn0[j] -
                       (N - j + 1) * pdn0[j - 1] / (2*j - 1)) / 2.0;
        }
        pdn0 += stride;
        pdn1 += stride;
    }

    /* regression matrix */
    pmat = matrix;
    pcol = coef;
    for (col = 0; col < numexps; col++) {
        pcol += stride;
        prow = coef;
        for (row = 0; row < numexps; row++) {
            prow += stride;
            sum = 0.0;
            for (j = startcoef; j < numcoef; j++) {
                sum += prow[j] * pcol[j];
            }
            *pmat++ = sum;
        }
    }

    /* regression vector */
    pvec = vector;
    pcoef = coef;
    for (row = 0; row < numexps; row++) {
        pcoef += stride;
        sum = 0.0;
        for (j = startcoef; j < numcoef; j++) {
            sum += coef[j] * pcoef[j];
        }
        *pvec++ = sum;
    }

    /* solve linear equation system */
    error = linsolve(numexps, matrix, vector);
    if (error != 0) {
        PyEval_RestoreThread(_save);
        return error;
    }

    /* roots of polynomial */
    for (n = 0; n < numexps; n++) {
        xcoefs[n].real = -vector[numexps - n - 1];
        xcoefs[n].imag = 0.0;
    }
    xcoefs[numexps].real = 1.0;
    xcoefs[numexps].imag = 0.0;
    error = polyroots(numexps+1, xcoefs, xroots);
    if (error != 0) {
        PyEval_RestoreThread(_save);
        return error;
    }

    /* decay rate and frequency of harmonics */
    for (n = 0; n < numexps; n++) {
        temp = xroots[n].real + 1.0;
        rat[n] = -log(temp*temp + xroots[n].imag*xroots[n].imag) / 2.0;
        frq[n] = atan2(xroots[n].imag, temp);
    }

    /* fitting amplitudes */
    /* Chebyshev transform signal for each exponential component */
    pcoef = coef + (numcoef + 1);
    pbuff = buff;
    for (n = 0; n < numexps; n++) {
        frqn = frq[n];
        ratn = rat[n];
        if (frqn == 0.0) {
            *pbuff++ = 1.0;
            for (t = 1; t < numdata; t++) {
                *pbuff++ = exp(-ratn*t);
            }
        } else if (frqn > 0) {
            *pbuff++ = 1.0;
            for (t = 1; t < numdata; t++) {
                *pbuff++ = exp(-ratn*t) * cos(frqn*t);
            }
        } else {
            *pbuff++ = 0.0;
            for (t = 1; t < numdata; t++) {
                *pbuff++ = -exp(-ratn*t) * sin(frqn*t);
            }
        }
        rat[n] = deltat / ratn;
        frq[n] /= deltat;

        /* forward Chebyshev transform */
        ppoly = poly;
        for (j = 0; j < numcoef; j++) {
            pbuff -= numdata;
            sum = 0.0;
            for (t = 0; t < numdata; t++) {
                sum += (*pbuff++) * (*ppoly++);
            }
            *pcoef++ = sum;
        }
        pcoef++;
    }

    /* regression matrix for fitting amplitudes */
    pmat = matrix;
    pcol = coef;
    for (col = 0; col < numexps; col++) {
        pcol += stride;
        prow = coef;
        for (row = 0; row < numexps; row++) {
            prow += stride;
            sum = 0.0;
            for (j = 1; j < numcoef; j++) {
                sum += prow[j] * pcol[j];
            }
            *pmat++ = sum;
        }
    }

    /* regression vector for fitting amplitudes */
    pvec = amp;
    pcoef = coef;
    for (row = 0; row < numexps; row++) {
        pcoef += stride;
        sum = 0.0;
        for (j = 1; j < numcoef; j++) {
            sum += coef[j] * pcoef[j];
        }
        *pvec++ = sum;
    }

    /* solve linear equation system for amplitudes */
    error = linsolve(numexps, matrix, amp);
    if (error != 0) {
        PyEval_RestoreThread(_save);
        return error;
    }

    /* calculate offset from zero Chebyshev coefficients */
    pcoef = coef + stride;
    temp = *coef;
    for (n = 0; n < numexps; n++) {
        temp -= amp[n] * (*pcoef);
        pcoef += stride;
    }
    *off = temp;

    /* calculate fitted data */
    if (fitt != NULL) {
        if (fitt_stride == sizeof(double)) {
            double *pfitt = (double *)fitt;
            temp = *off;
            for (t = 0; t < numdata; t++) {
                *pfitt++ = temp;
            }
            pbuff = buff;
            for (n = 0; n < numexps; n++) {
                pfitt = (double *)fitt;
                temp = amp[n];
                for (t = 0; t < numdata; t++) {
                    *pfitt++ += temp * (*pbuff++);
                }
            }
        } else {
            char *pfitt = fitt;
            temp = *off;
            for (t = 0; t < numdata; t++) {
                *(double *)pfitt = temp;
                pfitt += fitt_stride;
            }
            pbuff = buff;
            for (n = 0; n < numexps; n++) {
                pfitt = fitt;
                temp = amp[n];
                for (t = 0; t < numdata; t++) {
                    *(double *)pfitt += temp * (*pbuff++);
                    pfitt += fitt_stride;
                }
            }
        }
    }

    PyEval_RestoreThread(_save);
    return 0;
}
Real DistCircle3Circle3<Real>::GetSquared ()
{
    Vector3<Real> diff = mCircle1->Center - mCircle0->Center;
    Real u0u1 = mCircle0->Direction0.Dot(mCircle1->Direction0);
    Real u0v1 = mCircle0->Direction0.Dot(mCircle1->Direction1);
    Real v0u1 = mCircle0->Direction1.Dot(mCircle1->Direction0);
    Real v0v1 = mCircle0->Direction1.Dot(mCircle1->Direction1);

    Real a0 = -diff.Dot(mCircle0->Direction0);
    Real a1 = -mCircle1->Radius*u0u1;
    Real a2 = -mCircle1->Radius*u0v1;
    Real a3 = diff.Dot(mCircle0->Direction1);
    Real a4 = mCircle1->Radius*v0u1;
    Real a5 = mCircle1->Radius*v0v1;

    Real b0 = -diff.Dot(mCircle1->Direction0);
    Real b1 = mCircle0->Radius*u0u1;
    Real b2 = mCircle0->Radius*v0u1;
    Real b3 = diff.Dot(mCircle1->Direction1);
    Real b4 = -mCircle0->Radius*u0v1;
    Real b5 = -mCircle0->Radius*v0v1;

    // Compute polynomial p0 = p00+p01*z+p02*z^2.
    Polynomial1<Real> p0(2);
    p0[0] = a2*b1 - a5*b2;
    p0[1] = a0*b4 - a3*b5;
    p0[2] = a5*b2 - a2*b1 + a1*b4 - a4*b5;

    // Compute polynomial p1 = p10+p11*z.
    Polynomial1<Real> p1(1);
    p1[0] = a0*b1 - a3*b2;
    p1[1] = a1*b1 - a5*b5 + a2*b4 - a4*b2;

    // Compute polynomial q0 = q00+q01*z+q02*z^2.
    Polynomial1<Real> q0(2);
    q0[0] = a0*a0 + a2*a2 + a3*a3 + a5*a5;
    q0[1] = ((Real)2)*(a0*a1 + a3*a4);
    q0[2] = a1*a1 - a2*a2 + a4*a4 - a5*a5;

    // Compute polynomial q1 = q10+q11*z.
    Polynomial1<Real> q1(1);
    q1[0] = ((Real)2)*(a0*a2 + a3*a5);
    q1[1] = ((Real)2)*(a1*a2 + a4*a5);

    // Compute coefficients of r0 = r00+r02*z^2.
    Polynomial1<Real> r0(2);
    r0[0] = b0*b0;
    r0[1] = (Real)0;
    r0[2] = b3*b3 - b0*b0;

    // Compute polynomial r1 = r11*z.
    Polynomial1<Real> r1(1);
    r1[0] = (Real)0;
    r1[1] = ((Real)2)*b0*b3;

    // Compute polynomial g0 = g00+g01*z+g02*z^2+g03*z^3+g04*z^4.
    Polynomial1<Real> g0(4);
    g0[0] = p0[0]*p0[0] + p1[0]*p1[0] - q0[0]*r0[0];
    g0[1] = ((Real)2)*(p0[0]*p0[1] + p1[0]*p1[1]) - q0[1]*r0[0] - q1[0]*r1[1];
    g0[2] = p0[1]*p0[1] + ((Real)2)*p0[0]*p0[2] - p1[0]*p1[0] +
        p1[1]*p1[1] - q0[2]*r0[0] - q0[0]*r0[2] - q1[1]*r1[1];
    g0[3] = ((Real)2)*(p0[1]*p0[2] - p1[0]*p1[1]) - q0[1]*r0[2] + q1[0]*r1[1];
    g0[4] = p0[2]*p0[2] - p1[1]*p1[1] - q0[2]*r0[2] + q1[1]*r1[1];

    // Compute polynomial g1 = g10+g11*z+g12*z^2+g13*z^3.
    Polynomial1<Real> g1(3);
    g1[0] = ((Real)2)*p0[0]*p1[0] - q1[0]*r0[0];
    g1[1] = ((Real)2)*(p0[1]*p1[0] + p0[0]*p1[1]) - q1[1]*r0[0] - q0[0]*r1[1];
    g1[2] = ((Real)2)*(p0[2]*p1[0] + p0[1]*p1[1]) - q1[0]*r0[2] - q0[1]*r1[1];
    g1[3] = ((Real)2)*p0[2]*p1[1] - q1[1]*r0[2] - q0[2]*r1[1];

    // Compute polynomial h = sum_{i=0}^8 h_i z^i.
    Polynomial1<Real> h(8);
    h[0] = g0[0]*g0[0] - g1[0]*g1[0];
    h[1] = ((Real)2)*(g0[0]*g0[1] - g1[0]*g1[1]);
    h[2] = g0[1]*g0[1] + g1[0]*g1[0] - g1[1]*g1[1] +
        ((Real)2)*(g0[0]*g0[2] - g1[0]*g1[2]);
    h[3] = ((Real)2)*(g0[1]*g0[2] + g0[0]*g0[3] + g1[0]*g1[1] -
        g1[1]*g1[2] - g1[0]*g1[3]);
    h[4] = g0[2]*g0[2] + g1[1]*g1[1] - g1[2]*g1[2] +
        ((Real)2)*(g0[1]*g0[3] + g0[0]*g0[4] + g1[0]*g1[2] -
        g1[1]*g1[3]);
    h[5] = ((Real)2)*(g0[2]*g0[3] + g0[1]*g0[4] + g1[1]*g1[2] +
        g1[0]*g1[3] - g1[2]*g1[3]);
    h[6] = g0[3]*g0[3] + g1[2]*g1[2] - g1[3]*g1[3] +
        ((Real)2)*(g0[2]*g0[4] + g1[1]*g1[3]);
    h[7] = ((Real)2)*(g0[3]*g0[4] + g1[2]*g1[3]);
    h[8] = g0[4]*g0[4] + g1[3]*g1[3];

    PolynomialRoots<Real> polyroots(Math<Real>::ZERO_TOLERANCE);
    polyroots.FindB(h, (Real)-1.01, (Real)1.01, 6);
    int count = polyroots.GetCount();
    const Real* roots = polyroots.GetRoots();

    Real minSqrDist = Math<Real>::MAX_REAL;
    Real cs0, sn0, cs1, sn1;

    for (int i = 0; i < count; ++i)
    {
        cs1 = roots[i];
        if (cs1 < (Real)-1)
        {
            cs1 = (Real)-1;
        }
        else if (cs1 > (Real)1)
        {
            cs1 = (Real)1;
        }

        // You can also try sn1 = -g0(cs1)/g1(cs1) to avoid the sqrt call,
        // but beware when g1 is nearly zero.  For now I use g0 and g1 to
        // determine the sign of sn1.
        sn1 = Math<Real>::Sqrt(Math<Real>::FAbs((Real)1 - cs1*cs1));

        Real g0cs1 = g0(cs1);
        Real g1cs1 = g1(cs1);
        Real product = g0cs1*g1cs1;
        if (product > (Real)0)
        {
            sn1 = -sn1;
        }
        else if (product < (Real)0)
        {
            // sn1 already has correct sign
        }
        else if (g1cs1 != (Real)0)
        {
            // g0 == 0.0
            // assert( sn1 == 0.0 );
        }
        else // g1 == 0.0
        {
            // TO DO:  When g1 = 0, there is no constraint on sn1.
            // What should be done here?  In this case, cs1 is a root
            // to the quartic equation g0(cs1) = 0.  Is there some
            // geometric significance?
            assertion(false, "Unexpected case\n");
        }

        Real m00 = a0 + a1*cs1 + a2*sn1;
        Real m01 = a3 + a4*cs1 + a5*sn1;
        Real m10 = b2*sn1 + b5*cs1;
        Real m11 = b1*sn1 + b4*cs1;
        Real det = m00*m11 - m01*m10;
        if (Math<Real>::FAbs(det) >= Math<Real>::ZERO_TOLERANCE)
        {
            Real invDet = ((Real)1)/det;
            Real lambda = -(b0*sn1 + b3*cs1);
            cs0 = lambda*m00*invDet;
            sn0 = -lambda*m01*invDet;

            // Unitize in case of numerical error.  Remove if you feel
            // confident of the accuracy for cs0 and sn0.
            Real tmp = Math<Real>::InvSqrt(cs0*cs0 + sn0*sn0);
            cs0 *= tmp;
            sn0 *= tmp;

            Vector3<Real> closest0 = mCircle0->Center +
                mCircle0->Radius*(cs0*mCircle0->Direction0 +
                sn0*mCircle0->Direction1);

            Vector3<Real> closest1 = mCircle1->Center +
                mCircle1->Radius*(cs1*mCircle1->Direction0 +
                sn1*mCircle1->Direction1);

            diff = closest1 - closest0;

            Real sqrDist = diff.SquaredLength();
            if (sqrDist < minSqrDist)
            {
                minSqrDist = sqrDist;
                mClosestPoint0 = closest0;
                mClosestPoint1 = closest1;
            }
        }
        else
        {
            // TO DO:  Handle this case.  Is there some geometric
            // significance?
            assertion(false, "Unexpected case\n");
        }
    }

    return minSqrDist;
}