obb.c

任意给定三维空间的点集

/*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*
 * obb.C -
 *     
\*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*/

#include  <stdlib.h>
#include  <stdio.h>
#include  <assert.h>
#include  <math.h>
#include  <time.h>
#include  <string.h>

//#include  "dmalloc.h"

#include  "_generic.h"
#include  "sarray.h"
#include  "real.h"
#include  "point.h"
#include  "misc.h"

/*--- Constants ---*/


/*--- Start of Code ---*/

/* from MAGIC */
void EigenSymmetric3(double mat[3][3], double eval[3],
                     double evec[3][3]);

#define X 0
#define Y 1
#define Z 2

inline void  Zero(double pt[3]){
    pt[X] = 0.0;
    pt[Y] = 0.0;
    pt[Z] = 0.0;
}


static Point3d    compute_pnt( Point3d  * evecs,
                               double  * coff )
{
    Point3d  pnt;

    pnt.add_scale( evecs[ 0 ], coff[ 0 ] );
    pnt.add_scale( evecs[ 1 ], coff[ 1 ] );
    pnt.add_scale( evecs[ 2 ], coff[ 2 ] );

    return  pnt;
}


void  extractOrthonormalBase(Point3d  & p_a,
                             Point3d  & p_b,
                             Point3d  & p_c )
{
    Point3d  evec[ 3 ];

    evec[ 0 ] = p_a;
    evec[ 1 ] = p_b;
    evec[ 2 ] = p_c;

    if  ( evec[ 0 ].length() < evec[ 1 ].length() )
        exchange( evec[ 0 ], evec[ 1 ] );
    if  ( evec[ 0 ].length() < evec[ 2 ].length() )
        exchange( evec[ 0 ], evec[ 2 ] );
    if  ( evec[ 1 ].length() < evec[ 2 ].length() )
        exchange( evec[ 1 ], evec[ 2 ] );

    // The vectors are sorted by length...
    if  ( evec[ 0 ].length() == 0.0 ) {
        p_a = Point3d( 1, 0, 0 );
        p_b = Point3d( 0, 1, 0 );
        p_c = Point3d( 0, 0, 1 );
        return;
    }

    evec[ 0 ].normalize();
    //evec[ 1 ].add_scale( evec[ 0 ], - evec[ 0 ].dot_prod( evec[ 1 ] ) );

    evec[ 1 ].normalize();
    evec[ 2 ].normalize();
    
    //printf( "dot prod 0,1: %g\n", evec[ 0 ].dot_prod( evec[ 1 ] ) );

    //printf( "dot prod 0,2: %g\n", evec[ 0 ].dot_prod( evec[ 2 ] ) );
    evec[ 2 ].add_scale( evec[ 0 ], - evec[ 0 ].dot_prod( evec[ 2 ] ) );
    printf( "dot prod 0,2: %g\n", evec[ 0 ].dot_prod( evec[ 2 ] ) );

    evec[ 2 ].add_scale( evec[ 1 ], - evec[ 1 ].dot_prod( evec[ 2 ] ) );
    printf( "dot prod 1,2: %g\n", evec[ 1 ].dot_prod( evec[ 2 ] ) );

    evec[ 2 ].print();
    printf( "\n\n" );

    evec[ 2 ].normalize();

    // The vectors are sorted by length...
    if  ( evec[ 1 ].length() == 0.0 ) {
        assert( false );
        return;

    }
    if  ( ( evec[ 0 ].length() != 0.0 )
          &&  ( evec[ 1 ].length() != 0.0 ) 
          &&  ( evec[ 2 ].length() == 0.0 ) ) {
        evec[ 2 ] = cross_prod( evec[ 0 ], evec[ 1 ] );
        evec[ 2 ].normalize();
    }

    /*
    printf( "\n\n res:\n" );

    evec[ 0 ].print();
    printf( "\n" );
    evec[ 1 ].print();
    printf( "\n" );
    evec[ 2 ].print();
    printf( "\n" );
    */
    p_a = evec[ 0 ];
    p_b = evec[ 1 ];
    p_c = evec[ 2 ];
}




void  EigenSymetric3vec( double  mCoVars[3][3],
                         Point3d  & p_a,
                         Point3d  & p_b,
                         Point3d  & p_c )
{
    //double  XfTemp;
    Point3d  evec[ 3 ];
    double  EigenVals[3];
    double  mXForm[3][3];

    /* put eigenvectors into mXForm */
    //EigenSymmetric3(mCoVars, EigenVals, mXForm);
    Jacobi_EigenSymmetric3( mCoVars, EigenVals, mXForm );

    //printf( "eigen values: %g %g %g\n",
    //         EigenVals[ 0 ],
    //        EigenVals[ 1 ],
    //        EigenVals[ 2 ] );


    /* Normalize the eigenvectors to make unit axes. */
    for  ( int  i = 0; i < 3; i++ ) {
        //printf( "___%d: %g %g %g\n", i,
        //        mXForm[i][0],
        //        mXForm[i][1],
        //       mXForm[i][2] );

        evec[ i ] = Point3d( mXForm[i][0], 
                             mXForm[i][1],
                             mXForm[i][2] );
        evec[ i ].normalize_big();
    }

    p_a = evec[ 0 ];
    p_b = evec[ 1 ];
    p_c = evec[ 2 ];
}


Point3d    matrix_multiply( double  mCoVars[3][3],
                            Point3d  & vec )
{
    double  sum;
    Point3d  out;

    for  ( int  ind = 0; ind < 3; ind++ ) {
        sum = 0;
        for  ( int  jnd = 0; jnd < 3; jnd++ )
            sum += mCoVars[ ind ][ jnd ] * vec[ jnd ];

        out[ ind ] = sum;
    }

    return   out;
}


void MakeCovarOBB( const ArrPoint3d  & arr,
                   Point3d  & base_pnt,
                   Point3d  & side_0,
                   Point3d  & side_1,
                   Point3d  & side_2 )
{
    //double  boxpt[3], boxvecs[3][3];
    double  oon;
    double  mCoVars[3][3], mXForm[3][3];
    //double  ptAvg[3], ptAvgSq[3], ptAvgXS[3];
    Point3d  ptAvg, ptAvgSq, ptAvgXS;
    double  EigenVals[3];
    double  XfMin[3], XfMax[3];
    double  XfTemp;
    //int  i;
    
    assert( arr.size() > 0 );

    oon = 1.0 / (double)arr.size();

    /* Make averages for covariance matrix. */

    /* Intitialize averages. */
    ptAvg.zero();
    ptAvgSq.zero();
    ptAvgXS.zero();
   
    /* Sum all point data. */
    for  ( int  i = 0; i < arr.size(); i++ ) {
        ptAvg.add( arr[ i ] );

        ptAvgSq[X] += arr[i][X] * arr[i][X];
        ptAvgSq[Y] += arr[i][Y] * arr[i][Y];
        ptAvgSq[Z] += arr[i][Z] * arr[i][Z];

        ptAvgXS[X] += arr[i][X]*arr[i][Y];
        ptAvgXS[Y] += arr[i][X]*arr[i][Z];
        ptAvgXS[Z] += arr[i][Y]*arr[i][Z];
    }

    /* Divide sums to make average. */
    ptAvg.scale( oon );
    ptAvgSq.scale( oon );
    ptAvgXS.scale( oon );

    /* Compose covariance matrix. */
    mCoVars[0][0] = ptAvgSq[X] - ptAvg[X]*ptAvg[X];
    mCoVars[0][1] = ptAvgXS[X] - ptAvg[X]*ptAvg[Y];
    mCoVars[0][2] = ptAvgXS[Y] - ptAvg[X]*ptAvg[Z];

    mCoVars[1][0] = ptAvgXS[X] - ptAvg[Y]*ptAvg[X];
    mCoVars[1][1] = ptAvgSq[Y] - ptAvg[Y]*ptAvg[Y];
    mCoVars[1][2] = ptAvgXS[Z] - ptAvg[Y]*ptAvg[Z];

    mCoVars[2][0] = ptAvgXS[Y] - ptAvg[Z]*ptAvg[X];
    mCoVars[2][1] = ptAvgXS[Z] - ptAvg[Z]*ptAvg[Y];
    mCoVars[2][2] = ptAvgSq[Z] - ptAvg[Z]*ptAvg[Z];


    /*
    for  ( int  ind = 0; ind < 3; ind++ ) {
        printf( "[ " );
        for  ( int  jnd = 0; jnd < 3; jnd++ )
            printf( "%g ", mCoVars[ ind ][ jnd ] );
        printf( "]\n" );
    }
    */

    /* Find eigenvectors for the covariance matrix. */

    /* put eigenvectors into mXForm */
    //EigenSymmetric3(mCoVars, EigenVals, mXForm);
    Jacobi_EigenSymmetric3( mCoVars, EigenVals, mXForm );
    /*
      printf( "PCA eigen values: %g %g %g\n",
            EigenVals[ 0 ],
            EigenVals[ 1 ],
            EigenVals[ 2 ] );
    */


    /* Normalize the eigenvectors to make unit axes. */
    Point3d  evec[ 3 ];
    for  ( int  i = 0; i < 3; i++ ) {
        //XfTemp = (double)(1.0/sqrt(mXForm[i][0]*mXForm[i][0] +
        //                          mXForm[i][1]*mXForm[i][1] +
        //                          mXForm[i][2]*mXForm[i][2]));
        //mXForm[i][0] *= XfTemp;
        //mXForm[i][1] *= XfTemp;
        //mXForm[i][2] *= XfTemp;
        evec[ i ] = Point3d( mXForm[i][0], 
                             mXForm[i][1],
                             mXForm[i][2] );
        //printf( "vec:%d: ", i );
        //evec[ i ].print();
        //printf( "\n" );
        /*
        Point3d  evec_out;

        evec_out = multiply( mCoVars, evec[ i ] );
        printf( "mult:   " );
        evec_out.print();
        printf( "ratio: %g\n", evec_out.length() / evec[ i ].length() );
        */
    }


    /* find minimum/maximum of points in new coord
       system defined by the eigenvectors */

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

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

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

        XfTemp = evec[ 2 ].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. */
    base_pnt = compute_pnt( evec, XfMin );
    
    side_0 = evec[ 0 ];
    side_0.scale( XfMax[ 0 ] - XfMin[ 0 ] );

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

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


void  computeParallelogram(  const ArrPoint3d  & arr,
                             const Point3d  & vec1,
                             const Point3d  & vec2,
                             const Point3d  & vec3,
                             Point3d  & base_pnt,
                             Point3d  & side_0,
                             Point3d  & side_1,
                             Point3d  & side_2 )
{