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;
}
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;
}
// 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;
}
/*
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;
}
/*
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;
}