/*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=* * 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 2inline 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 ){