obb.c

任意给定三维空间的点集

    double  XfMin[3], XfMax[3], XfTemp;

    /* initialize min & max with first point's values */
    XfMax[ 0 ] = XfMin[ 0 ] = vec1.dot_prod( arr[ 0 ] );
    XfMax[ 1 ] = XfMin[ 1 ] = vec2.dot_prod( arr[ 0 ] );
    XfMax[ 2 ] = XfMin[ 2 ] = vec3.dot_prod( arr[ 0 ] );

    /* check subsequent points */
    for  ( int ind = 1; ind < arr.size(); ind++){
        XfTemp = vec1.dot_prod( arr[ ind ] );
        XfMin[ 0 ] = min( XfMin[ 0 ], XfTemp );
        XfMax[ 0 ] = max( XfMax[ 0 ], XfTemp );

        XfTemp = vec2.dot_prod( arr[ ind ] );
        XfMin[ 1 ] = min( XfMin[ 1 ], XfTemp );
        XfMax[ 1 ] = max( XfMax[ 1 ], XfTemp );

        XfTemp = vec3.dot_prod( arr[ ind ] );
        XfMin[ 2 ] = min( XfMin[ 2 ], XfTemp );
        XfMax[ 2 ] = max( XfMax[ 2 ], XfTemp );
    }

    /* Now the orthogonal bounding box in transformed
       coordinates is in XfMin and XfMax. */
    Point3d  evec[ 3 ];

    evec[ 0 ] = vec1;
    evec[ 1 ] = vec2;
    evec[ 2 ] = vec3;

    base_pnt = compute_pnt( evec, XfMin );
    
    side_0 = vec1;
    side_0.scale( XfMax[ 0 ] - XfMin[ 0 ] );

    side_1 = vec2;
    side_1.scale( XfMax[ 1 ] - XfMin[ 1 ] );

    side_2 = vec3;
    side_2.scale( XfMax[ 2 ] - XfMin[ 2 ] );    
}

/* beginning of MAGIC code */

typedef double REAL;
const REAL epsilon = 1e-06f;

// roots r0 < r1 < r2
#define distinctRoots 0
// root r0
#define singleRoot    1
// roots r0 = r1 < r2
#define doubleRoot01  2
// roots r0 < r1 = r2
#define doubleRoot12  4
// roots r0 = r1 = r2
#define tripleRoot    6

//===========================================================================
int CubicRoots (REAL c0, REAL c1, REAL c2, REAL *r0, REAL *r1, REAL *r2)
{
    // polynomial is L^3-c2*L^2+c1*L-c0

    int maxiter = 50;
    REAL discr, temp, pval, pdval, b0, b1;
    int i;

    // find local extrema (if any) of p'(L) = 3*L^2-2*c2*L+c1
    discr = c2*c2-3*c1;
    if ( discr >= 0.0 ) {
        discr = (REAL)sqrt(discr);
        temp = (c2+discr)/3;
        pval = temp*(temp*(temp-c2)+c1)-c0;
        if ( pval >= 0.0 ) {
            // real root occurs before the positive local maximum
            (*r0) = (c2-discr)/3 - 1;  // initial guess for Newton's methods
            pval = 2*epsilon;
            for (i = 0; i < maxiter && fabs(pval) > epsilon; i++) {
                pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0;
                pdval = (*r0)*(3*(*r0)-2*c2)+c1;
                (*r0) -= pval/pdval;
            }

            // Other two roots are solutions to quadratic equation
            // L^2 + ((*r0)-c2)*L + [(*r0)*((*r0)-c2)+c1] = 0.
            b1 = (*r0)-c2;
            b0 = (*r0)*((*r0)-c2)+c1;
            discr = b1*b1-4*b0;
            if ( discr < -epsilon )
            {
                // single root r0
                return singleRoot;
            }
            else
            {
                int result = distinctRoots;

                // roots r0 <= r1 <= r2
                discr = (REAL)sqrt(fabs(discr));
                (*r1) = (REAL)0.5*(-b1-discr);
                (*r2) = (REAL)0.5*(-b1+discr);

                if ( fabs((*r0)-(*r1)) <= epsilon )
                {
                    (*r0) = (*r1);
                    result |= doubleRoot01;
                }
                if ( fabs((*r1)-(*r2)) <= epsilon )
                {
                    (*r1) = (*r2);
                    result |= doubleRoot12;
                }
                return result;
            }
        }
        else {
            // real root occurs after the negative local minimum
            (*r2) = temp + 1;  // initial guess for Newton's method
            pval = 2*epsilon;
            for (i = 0; i < maxiter && fabs(pval) > epsilon; i++) {
                pval = (*r2)*((*r2)*((*r2)-c2)+c1)-c0;
                pdval = (*r2)*(3*(*r2)-2*c2)+c1;
                (*r2) -= pval/pdval;
            }

            // Other two roots are solutions to quadratic equation
            // L^2 + (r2-c2)*L + [r2*(r2-c2)+c1] = 0.
            b1 = (*r2)-c2;
            b0 = (*r2)*((*r2)-c2)+c1;
            discr = b1*b1-4*b0;
            if ( discr < -epsilon )
            {
                // single root
                (*r0) = (*r2);
                return singleRoot;
            }
            else
            {
                int result = distinctRoots;

                // roots r0 <= r1 <= r2
                discr = (REAL)sqrt(fabs(discr));
                (*r0) = (REAL)0.5*(-b1-discr);
                (*r1) = (REAL)0.5*(-b1+discr);

                if ( fabs((*r0)-(*r1)) <= epsilon )
                {
                    (*r0) = (*r1);
                    result |= doubleRoot01;
                }
                if ( fabs((*r1)-(*r2)) <= epsilon )
                {
                    (*r1) = (*r2);
                    result |= doubleRoot12;
                }
                return result;
            }
        }
    }
    else {
        // p(L) has one real root
        (*r0) = c0;
        pval = 2*epsilon;
        for (i = 0; i < maxiter && fabs(pval) > epsilon; i++) {
            pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0;
            pdval = (*r0)*(3*(*r0)-2*c2)+c1;
            (*r0) -= pval/pdval;
        }
        return singleRoot;
    }
}
//---------------------------------------------------------------------------
void Tridiagonal3 (REAL mat[3][3], REAL diag[3], REAL subd[2])
{
    REAL a, b, c, d, e, f, ell, q;

    a = mat[0][0];
    b = mat[0][1];
    c = mat[0][2];
    d = mat[1][1];
    e = mat[1][2];
    f = mat[2][2];

    diag[0] = a;
    subd[2] = (REAL)0;
    if ( fabs(c) >= epsilon ) {
        ell = (REAL)sqrt(b*b+c*c);
        b /= ell;
        c /= ell;
        q = 2*b*e+c*(f-d);
        diag[1] = d+c*q;
        diag[2] = f-c*q;
        subd[0] = ell;
        subd[1] = e-b*q;
        mat[0][0] = (REAL)1; mat[0][1] = (REAL)0; mat[0][2] = (REAL)0;
        mat[1][0] = (REAL)0; mat[1][1] = b; mat[1][2] = c;
        mat[2][0] = (REAL)0; mat[2][1] = c; mat[2][2] = -b;
    }
    else {
        diag[1] = d;
        diag[2] = f;
        subd[0] = b;
        subd[1] = e;
        mat[0][0] = (REAL)1; mat[0][1] = (REAL)0; mat[0][2] = (REAL)0;
        mat[1][0] = (REAL)0; mat[1][1] = (REAL)1; mat[1][2] = (REAL)0;
        mat[2][0] = (REAL)0; mat[2][1] = (REAL)0; mat[2][2] = (REAL)1;
    }
}
//---------------------------------------------------------------------------
void PreEigenvector (REAL diag[3], REAL subd[2], REAL eval, REAL evec[3][3],
                     int *numvec)
{
    if ( fabs(subd[0]) >= epsilon )
    {
        if ( fabs(subd[1]) >= epsilon )
        {
            REAL diff = eval-diag[0];
            evec[(*numvec)][0] = subd[0]*subd[1];
            evec[(*numvec)][1] = subd[1]*diff;
            evec[(*numvec)][2] = diff*(eval-diag[1])-subd[0]*subd[0];
            (*numvec)++;
        }
        else
        {
            evec[(*numvec)][0] = subd[0];
            evec[(*numvec)][1] = eval-diag[0];
            evec[(*numvec)][2] = (REAL)0;
            (*numvec)++;
            if ( fabs(eval-diag[2]) < epsilon )
            {
                evec[(*numvec)][0] = (REAL)0;
                evec[(*numvec)][1] = (REAL)0;
                evec[(*numvec)][2] = (REAL)1;
                (*numvec)++;
            }
        }
    }
    else
    {
        if ( fabs(subd[1]) >= epsilon )
        {
            evec[(*numvec)][0] = (REAL)0;
            evec[(*numvec)][1] = subd[1];
            evec[(*numvec)][2] = eval-diag[1];
            (*numvec)++;
            if ( fabs(eval-diag[0]) < epsilon )
            {
                evec[(*numvec)][0] = (REAL)1;
                evec[(*numvec)][1] = (REAL)0;
                evec[(*numvec)][2] = (REAL)0;
            }
        }
        else
        {
            if ( fabs(eval-diag[0]) < epsilon )
            {
                evec[(*numvec)][0] = (REAL)1;
                evec[(*numvec)][1] = (REAL)0;
                evec[(*numvec)][2] = (REAL)0;
                (*numvec)++;
            }
            if ( fabs(eval-diag[1]) < epsilon )
            {
                evec[(*numvec)][0] = (REAL)0;
                evec[(*numvec)][1] = (REAL)1;
                evec[(*numvec)][2] = (REAL)0;
                (*numvec)++;
            }
            if ( fabs(eval-diag[2]) < epsilon )
            {
                evec[(*numvec)][0] = (REAL)0;
                evec[(*numvec)][1] = (REAL)0;
                evec[(*numvec)][2] = (REAL)1;
                (*numvec)++;
            }
        }
    }
}
//---------------------------------------------------------------------------
void EigenSymmetric3 (REAL mat[3][3], REAL eval[3], REAL evec[3][3])
{
    REAL diag[3], subd[2];
    REAL c0, c1, c2, sum;
    REAL prevec[3][3];  // matrix P, vectors stored as rows, not columns
    int k, row, col, flag, numvec;

    //   Householder reduction T = Q^t M Q
    //     Input:   
    //       mat, symmetric 3x3 matrix M
    //     Output:  
    //       mat, orthogonal matrix Q
    //       diag, diagonal entries of T
    //       subd, subdiagonal entries of T (T is symmetric)

    Tridiagonal3(mat,diag,subd);

    // eigenvalue construction
    c0 = diag[0]*diag[1]*diag[2]-diag[0]*subd[1]*subd[1]
             -diag[2]*subd[0]*subd[0];
    c1 = diag[0]*diag[1]+diag[0]*diag[2]+diag[1]*diag[2]
             -subd[0]*subd[0]-subd[1]*subd[1];
    c2 = diag[0]+diag[1]+diag[2];
    flag = CubicRoots(c0,c1,c2,eval + 0, eval + 1, eval + 2);

    // eigenvector construction
    numvec = 0;
    switch ( flag )
    {
    case distinctRoots:
        PreEigenvector(diag,subd,eval[0],prevec,&numvec);
        PreEigenvector(diag,subd,eval[1],prevec,&numvec);
        PreEigenvector(diag,subd,eval[2],prevec,&numvec);
        break;
    case doubleRoot01:
    case doubleRoot12:
        PreEigenvector(diag,subd,eval[0],prevec,&numvec);
        PreEigenvector(diag,subd,eval[2],prevec,&numvec);
        break;
    case tripleRoot:
        PreEigenvector(diag,subd,eval[0],prevec,&numvec);
        break;
    }

    // transform back to original coordinates by E^t = PQ where the
    // rows of E are the eigenvectors of M
    for (row = 0; row < 3; row++)
    {
        for (col = 0; col < 3; col++)
        {
            sum = (REAL)0;
            for (k = 0; k < 3; k++)
                sum += prevec[row][k]*mat[k][col];
            evec[row][col] = sum;
        }
    }
}
/* end of MAGIC code */