gdiam.c

任意给定三维空间的点集

    //printf( "addPairHeap( ?, %p, %p, ? )\n", left, right );
    pair_diam.update_diam( p, q, proj );
    //printf( "old_diam; %g\n", diam );
    //diam = max( diam, pnt_distance( p, q ) );
    //printf( "new_diam; %g\n", diam );
    
    GFSPPair  pair;
    
    pair.init( left, right, proj,
               pnt_distance( p, q, proj ) );
    if  ( pair.maxDiam() <= pair_diam.distance )
        return;
    //printf( "pair.maxDiam: %g, father.maxDiam: %g\n",
    //        pair.maxDiam(),  father.maxDiam() );
    //printf( "pair_diam.distance: %g\n", pair_diam.distance );
    //pnt_dump( proj );
    heap.push( pair );
}


void  GTreeDiamAlg::split_pair_proj( GFSPPair  & pair,
                                     g_heap_pairs_p  & heap, double  eps,
                                     gdiam_point  proj )
{
    bool  f_is_left_splitable, f_is_right_splitable;
    
    //printf( "pair.maxDiam: %g, limit: %g\n",
    //        pair.maxDiam(), ((1.0 + eps) * pair_diam.distance ));

    if  ( pair.maxDiam() <= ((1.0 + eps) * pair_diam.distance ))
        return;
    
    if  ( pair.isBelowThreshold( threshold_brute ) ) {
        pair.directDiameter( tree, pair_diam, proj );
        return;
    }

    //printf( "Curr pair: (%p, %p)\n", pair.get_left(), pair.get_right() );
    f_is_left_splitable = pair.get_left()->isSplitable();
    f_is_right_splitable = pair.get_right()->isSplitable();
    assert( f_is_left_splitable  ||  f_is_right_splitable );
    if ( f_is_left_splitable )
        tree.split_node( pair.get_left() );
    if ( f_is_right_splitable )
        tree.split_node( pair.get_right() );
    
    if  ( f_is_left_splitable 
          &&  f_is_right_splitable ) {
        addPairHeap( heap, 
                     pair.get_left()->get_left(),
                     pair.get_right()->get_left(),
                     proj, pair );
        addPairHeap( heap, 
                     pair.get_left()->get_left(),
                     pair.get_right()->get_right(),
                     proj, pair );
        // to avoid exponential blowup
        if  ( pair.get_left() != pair.get_right() )
            addPairHeap( heap, 
                         pair.get_left()->get_right(),
                         pair.get_right()->get_left(),
                         proj, pair );
        addPairHeap( heap, 
                     pair.get_left()->get_right(),
                     pair.get_right()->get_right(),
                     proj, pair );
        return;
    }
    if  ( f_is_left_splitable ) {
        addPairHeap( heap,
                     pair.get_left()->get_left(),
                     pair.get_right(), proj, pair );
        addPairHeap( heap, 
                     pair.get_left()->get_right(),
                     pair.get_right(), proj, pair );
        return;
    }
    if  ( f_is_right_splitable ) {
        addPairHeap( heap, 
                     pair.get_left(),
                     pair.get_right()->get_left(), proj, pair );
        addPairHeap( heap, 
                     pair.get_left(),
                     pair.get_right()->get_right(), proj, pair );
        return;
    }
}


void  GTreeDiamAlg::split_pair( GFSPPair  & pair,
                                g_heap_pairs_p  & heap,
                                double  eps )
{
    bool  f_is_left_splitable, f_is_right_splitable;
    
    if  ( pair.maxDiam() <= ((1.0 + eps) * pair_diam.distance ))
        return;
    
    if  ( pair.isBelowThreshold( threshold_brute ) ) {
        pair.directDiameter( tree, pair_diam );
        return;
    }
    
    f_is_left_splitable = pair.get_left()->isSplitable();
    f_is_right_splitable = pair.get_right()->isSplitable();
    assert( f_is_left_splitable  ||  f_is_right_splitable );
    if ( f_is_left_splitable )
        tree.split_node( pair.get_left() );
    if ( f_is_right_splitable )
        tree.split_node( pair.get_right() );
    
    if  ( f_is_left_splitable 
          &&  f_is_right_splitable ) {
        addPairHeap( heap, 
                     pair.get_left()->get_left(),
                     pair.get_right()->get_left() );
        addPairHeap( heap, 
                     pair.get_left()->get_left(),
                     pair.get_right()->get_right() );
        // to avoid exponential blowup
        if  ( pair.get_left() != pair.get_right() )
            addPairHeap( heap, 
                         pair.get_left()->get_right(),
                         pair.get_right()->get_left() );
        addPairHeap( heap, 
                     pair.get_left()->get_right(),
                     pair.get_right()->get_right() );
        return;
    }
    if  ( f_is_left_splitable ) {
        addPairHeap( heap,
                     pair.get_left()->get_left(),
                     pair.get_right() );
        addPairHeap( heap, 
                     pair.get_left()->get_right(),
                     pair.get_right() );
        return;
    }
    if  ( f_is_right_splitable ) {
        addPairHeap( heap, 
                     pair.get_left(),
                     pair.get_right()->get_left() );
        addPairHeap( heap, 
                     pair.get_left(),
                     pair.get_right()->get_right() );
        return;
    }
}


GPointPair   gdiam_approx_diam( gdiam_point  * start, int  size,
                                gdiam_real  eps )
{
    //gdiam_real  diam;
    GPointPair  pair;

    GTreeDiamAlg  * pAlg = new  GTreeDiamAlg( start, size, UDM_SIMPLE );
    pAlg->compute_by_heap( eps );

    pair = pAlg->getDiameter();

    delete  pAlg;

    return  pair;
}


GPointPair   gdiam_approx_diam_UDM( gdiam_point  * start, int  size,
                                    gdiam_real  eps )
{
    //gdiam_real  diam;
    GPointPair  pair;

    GTreeDiamAlg  * pAlg = new  GTreeDiamAlg( start, size, UDM_BIG_SAMPLE );
    pAlg->compute_by_heap( eps );

    pair = pAlg->getDiameter();

    delete  pAlg;

    return  pair;
}


gdiam_point  * gdiam_convert( gdiam_real  * start, int  size )
{
    gdiam_point  * p_arr;

    assert( start != NULL );
    assert( size > 0 );

    p_arr = (gdiam_point *)malloc( sizeof( gdiam_point ) * size );
    assert( p_arr != NULL );

    for  ( int  ind = 0; ind < size; ind++ ) 
        p_arr[ ind ] = (gdiam_point)&(start[ ind * 3 ]);

    return  p_arr;
}


GPointPair   gdiam_approx_diam_pair( gdiam_real  * start, int  size,
                                     gdiam_real  eps )
{
    gdiam_point  * p_arr;
    GPointPair  pair;

    p_arr = gdiam_convert( start, size );
    pair = gdiam_approx_diam( p_arr, size, eps );
    free( p_arr );

    return  pair;
}


GPointPair   gdiam_approx_diam_pair_UDM( gdiam_real  * start, int  size,
                                         gdiam_real  eps )
{
    gdiam_point  * p_arr;
    GPointPair  pair;

    p_arr = gdiam_convert( start, size );
    pair = gdiam_approx_diam_UDM( p_arr, size, eps );
    free( p_arr );

    return  pair;
}


gdiam_bbox   gdiam_approx_const_mvbb( gdiam_point  * start, int  size,
                                      gdiam_real  eps, 
                                      GBBox  * p_ap_bbox )    
{
    //gdiam_real  diam;
    GPointPair  pair, pair_2;

    GTreeDiamAlg  * pAlg = new  GTreeDiamAlg( start, size, UDM_SIMPLE );
    pAlg->compute_by_heap( eps );

    pair = pAlg->getDiameter();

    /* Comput ethe resulting diameter */
    gdiam_point_t  dir, dir_2, dir_3;

    dir[ 0 ] = pair.p[ 0 ] - pair.q[ 0 ];
    dir[ 1 ] = pair.p[ 1 ] - pair.q[ 1 ];
    dir[ 2 ] = pair.p[ 2 ] - pair.q[ 2 ];    
    pnt_normalize( dir );

    //    printf( "Before computing second direction!\n" );
    //fflush( stdout );
    pAlg->compute_by_heap_proj( eps, dir );
    //printf( "second direction computed!\n" );
    //fflush( stdout );

    pair_2 = pAlg->getDiameter();
    dir_2[ 0 ] = pair_2.p[ 0 ] - pair_2.q[ 0 ];
    dir_2[ 1 ] = pair_2.p[ 1 ] - pair_2.q[ 1 ];
    dir_2[ 2 ] = pair_2.p[ 2 ] - pair_2.q[ 2 ];    

    gdiam_real prd;

    prd = pnt_dot_prod( dir, dir_2 );
    dir_2[ 0 ] = dir_2[ 0 ] - prd * dir[ 0 ];
    dir_2[ 1 ] = dir_2[ 1 ] - prd * dir[ 1 ];
    dir_2[ 2 ] = dir_2[ 2 ] - prd * dir[ 2 ];

    pnt_normalize( dir );
    pnt_normalize( dir_2 );

    pnt_cross_prod( dir, dir_2, dir_3 );
    pnt_normalize( dir_3 );

    gdiam_bbox  bbox;
    GBBox  ap_bbox;

    if  ( ( pnt_length( dir_2 ) < 1e-6 )
          &&  ( pnt_length( dir_3 ) < 1e-6 ) ) 
        gdiam_generate_orthonormal_base( dir, dir_2, dir_3 );

    if  ( ( pnt_length( dir ) == 0.0 ) 
          ||  ( pnt_length( dir_2 ) < 1e-6 )
          ||  ( pnt_length( dir_3 ) < 1e-6 ) ) {
        gdiam_generate_orthonormal_base( dir, dir_2, dir_3 );
        pnt_dump( dir );
        pnt_dump( dir_2 );
        pnt_dump( dir_3 );

        fflush( stdout );
        fflush( stderr );
        assert( false );
    }

    bbox.init( dir, dir_2, dir_3 );
    ap_bbox.init();
    for  ( int  ind = 0; ind < size; ind++ ) {
        bbox.bound( start[ ind ] );
        ap_bbox.bound( start[ ind ] );
    }

    //printf( "Axis parallel bounding box:\n" );
    //ap_bbox.dump();

    //printf( "Arbitrarily oriented bounding box:\n" );
    //bbox.dump();

    delete  pAlg;
    if  ( ap_bbox.volume() < bbox.volume() ) 
        bbox.init( ap_bbox );

    if  ( p_ap_bbox != NULL )
        *p_ap_bbox = ap_bbox;

    return  bbox;
}


gdiam_bbox   gdiam_approx_const_mvbb( gdiam_real  * start, int  size,
                                      gdiam_real  eps )
{
    gdiam_point  * p_arr;
    gdiam_bbox  gbbox;

    p_arr = gdiam_convert( start, size );
    gbbox = gdiam_approx_const_mvbb( p_arr, size, eps, NULL );
    free( p_arr );

    return   gbbox;
}

/*-----------------------------------------------------------------------
 * Code for computing best bounding box in a given drection 
\*-----------------------------------------------------------------------*/

void  gdiam_generate_orthonormal_base( gdiam_point  in,
                                       gdiam_point  out1,
                                       gdiam_point  out2 )
{
    assert( pnt_length( in ) > 0.0 );

    pnt_normalize( in );

    // stupid cases..
    if  ( in[ 0 ] == 0.0 ) {
        if  ( in[ 1 ] == 0.0 ) {
            pnt_init_normalize( out1, 1, 0, 0 );
            pnt_init_normalize( out2, 0, 1, 0 );
            return;
        }
        if  ( in[ 2 ] == 0.0 ) {
            pnt_init_normalize( out1, 1, 0, 0 );
            pnt_init_normalize( out2, 0, 0, 1 );
            return;
        }        
        pnt_init_normalize( out1, 0, -in[ 2 ], in[ 1 ] );
        pnt_init_normalize( out2, 1, 0, 0 );
        return;
    }
    if  ( ( in[ 1 ] == 0.0 )  &&  ( in[ 2 ] == 0.0 ) )  {
        pnt_init_normalize( out1, 0, 1, 0 );
        pnt_init_normalize( out2, 0, 0, 1 );
        return;
    }
    if  ( in[ 1 ] == 0.0 ) {
        pnt_init_normalize( out1, -in[ 2 ], 0, in[ 0 ] );
        pnt_init_normalize( out2, 0, 1, 0 );
        return;
    }    
    if  ( in[ 2 ] == 0.0 ) {
        pnt_init_normalize( out1, -in[ 1 ], in[ 0 ], 0 );
        pnt_init_normalize( out2, 0, 0, 1 );
        return;
    }

    // all entries in the vector are not zero.
    pnt_init_normalize( out1, -in[ 1 ], in[ 0 ], 0 );
    pnt_cross_prod( in, out1, out2 );
    
    pnt_normalize( out2 );
}


class  point2d
{
public:
    gdiam_real  x, y;
    gdiam_point  src;

    void  init( gdiam_point  pnt,
                gdiam_point  base_x,
                gdiam_point  base_y ) {
        src = pnt;
        x = pnt_dot_prod( pnt, base_x );
        y = pnt_dot_prod( pnt, base_y );
    }

    gdiam_real  dist( const point2d  & pnt ) {
        return  sqrt( ( x - pnt.x ) * ( x - pnt.x )
                      + ( y - pnt.y ) * ( y - pnt.y ) );
    }

    void  dump() const {
        printf( "(%g, %g)\n", x, y );
    }
    bool  equal( const point2d  & pnt ) const {
        return  ( ( x == pnt.x )  &&  ( y == pnt.y ) );
    }
    bool  equal_real( const point2d  & pnt ) const {
        return  ( ( fabs( x - pnt.x ) < 1e-8 )
                  && ( fabs( y - pnt.y ) < 1e-8 ) );
    }
};

typedef  point2d  * point2d_ptr;
class  vec_point_2d : public std::vector<point2d_ptr> {};



inline ldouble     Area( const point2d  & a, 
                        const point2d  & b,
                        const point2d  & c )
{
    double  x1, y1, x2, y2, len;

    x1 = b.x - a.x;
    y1 = b.y - a.y;

    x2 = c.x - a.x;
    y2 = c.y - a.y;

    if  ( ( x1 != 0.0 )  ||  ( y1 != 0.0 ) ) {
        len = sqrt( x1 * x1 + y1 * y1 );
        x1 /= len;
        y1 /= len;
    }

    if  ( ( x2 != 0.0 )  ||  ( y2 != 0.0 ) ) {
        len = sqrt( x2 * x2 + y2 * y2 );
        x2 /= len;
        y2 /= len;
    }

    ldouble  area;

    area = x1 * y2 - x2 * y1;
    //area = (ldouble)a.x * (ldouble)b.y - (ldouble)a.y * (ldouble)b.x +
    //    (ldouble)a.y * (ldouble)c.x - (ldouble)a.x * (ldouble)c.y +
    //    (ldouble)b.x * (ldouble)c.y - (ldouble)c.x * (ldouble)b.y;

    //printf( "area: %g\n", area );
    return  area;
}


inline int     AreaSign( const point2d  & a, 
                         const point2d  & b,
                         const point2d  & c )
{
    ldouble  area;

    area = a.x * b.y - a.y * b.x +
        a.y * c.x - a.x * c.y +
        b.x * c.y - c.x * b.y;