gdiam.c

任意给定三维空间的点集

        //        y_a = a_slope * x_a + a_const  (since (x_a, y_a) in l_a
        //        a_const = y_a - a_slope * x_a
        a_const = y_a - a_slope * x_a;
        //printf( "a_const: %g, y_a: %g, a_slope: %g, x_a: %g\n",
        //        a_const, y_a, a_slope, x_a );

        b_const = y_b - b_slope * x_b;
        //printf( "b_const: %g, y_b: %g, b_slope: %g, x_b: %g\n",
        //        b_const, y_b, b_slope, x_b );

        x_out = - ( a_const - b_const ) / (a_slope - b_slope);
        y_out = a_slope * x_out + a_const;
        /* 
        x = (y_b - y_a + x_a * a_slope - x_b * a_slope)
            / (a_slope - b_slope); 
        y = ((y_a - x_a * a_slope) / a_slope - (y_b - x_b * b_slope) 
             / b_slope) / 
            (1.0 / a_slope - 1.0 / b_slope ); 
        printf( "gill: (%g, %g)\nMine: (%g, %g)\n",
                x, y, x_out, y_out );
        */
        //printf( "a_angle: %g, b_angle: %g\n",
        //        a_angle / PI, b_angle/ PI );
        //printf( "a_slope: %g, b_slope: %g\n", 
        //        a_slope, b_slope );
        //printf( "on line_b: %g\n",
        //        b_slope * x_out + b_const - y_out );
        x = x_out;
        y = y_out;
        //printf( "line_a : y = %g * x + %g\n", 
        //        a_slope, a_const );
        //printf( "line_b : y = %g * x + %g\n", 
        //        b_slope, b_const );
        //printf( "a: (%g, %g)\n"
        //        "b: (%g, %g)\n",
        //        x_a, y_a, x_b, y_b );

        //printf( "out: (%g, %g)\n", x, y );
    }
    

    void  get_bbox( int   a_ind, gdiam_real   a_angle,
                    int   b_ind, gdiam_real   b_angle,
                    int   c_ind, gdiam_real   c_angle,
                    int   d_ind, gdiam_real   d_angle,
                    bbox_2d_info   & bbox,
                    gdiam_real  & area ) {
        /*
        printf( "get_bbox: (%d, %g,\n"
                "%d, %g\n"
                "%d, %g\n"
                "%d, %g )\n", 
                a_ind, a_angle / PI,
                b_ind, b_angle / PI,
                c_ind, c_angle / PI,
                d_ind, d_angle / PI );*/
                                
        compute_crossing( a_ind, a_angle, b_ind, b_angle,
                          bbox.vertices[ 0 ][ 0 ],
                          bbox.vertices[ 0 ][ 1 ] );
        compute_crossing( b_ind, b_angle, c_ind, c_angle,
                          bbox.vertices[ 1 ][ 0 ],
                          bbox.vertices[ 1 ][ 1 ] );
        compute_crossing( c_ind, c_angle, d_ind, d_angle,
                          bbox.vertices[ 2 ][ 0 ],
                          bbox.vertices[ 2 ][ 1 ] );
        compute_crossing( d_ind, d_angle, a_ind, a_angle,
                          bbox.vertices[ 3 ][ 0 ],
                          bbox.vertices[ 3 ][ 1 ] );

        area = pnt_distance_2d( bbox.vertices[ 0 ],
                              bbox.vertices[ 1 ] )
            * pnt_distance_2d( bbox.vertices[ 0 ],
                               bbox.vertices[ 3 ] );
    }

    /* Given one angle (bbx direction), find the other three */
    void  get_angles( int  ind,
                      gdiam_real   & angle_1,
                      gdiam_real   & angle_2,
                      gdiam_real   & angle_3 ) {
        double   angle;

        angle = angles[ ind ];
        angle_1 = angle + PI * 0.5;
        //printf( "angle = %g\n", angle / PI );
        //printf( "angle1 = %g\n", angle_1 / PI );

        if   ( angle_1 >= (2.0 * PI) )
            angle_1 -= 2.0 * PI;
        //printf( "angle1 = %g\n", angle_1 / PI );

        
        angle_2 = angle + PI;
        if ( angle_2 >= (2.0 * PI) )
            angle_2 -= 2.0 * PI;

        angle_3 = angle + 1.5 * PI;
        if ( angle_3 >= (2.0 * PI) )
            angle_3 -= 2.0 * PI;
    }

    void  compute_min_bbox_inner( bbox_2d_info   & min_bbox,
                                  gdiam_real  & min_area ) {
        int        u, v, s, t;
        gdiam_real     ang1, ang2, ang3, tmp_area;
        bbox_2d_info  tmp_bbox;

        angles = (gdiam_real *)malloc( sizeof( gdiam_real ) 
                                           * (int)ch.size() );
        assert( angles != NULL );

        /* Pre-computing all edge directions */
        for (u = 0; u < (int)ch.size(); u ++)
            compute_edge_dir( u );

        /* Initializing the search */
        //printf( "_angles[ 0 ]: %g\n", angles[ 0 ] );
        get_angles( 0, ang1, ang2, ang3 );
        
        //printf( "_angles[ 0 ]: %g\n", angles[ 0 ] );
        s = find_vertex( 0, ang1 );
        //printf( "_angles[ 0 ]: %g\n", angles[ 0 ] );
        v = find_vertex( s, ang2 );
        t = find_vertex( v, ang3 );
        
        //printf( "_angles[ 0 ]: %g\n", angles[ 0 ] );
        get_bbox( 0, angles[ 0 ],
                  s, ang1,
                  v, ang2,
                  t, ang3,
                  min_bbox, min_area );
        //min_bbox.dump();
  
        //printf( "min_area: %g\n", min_area );
        /* Performing a double rotating calipers */
        for  ( u = 1; u < (int)ch.size(); u ++ ) {
            get_angles( u, ang1, ang2, ang3 );
            s = find_vertex( s, ang1 );
            v = find_vertex( v, ang2 );
            t = find_vertex( t, ang3 );
            get_bbox( u, angles[ u ],
                      s, ang1,
                      v, ang2,
                      t, ang3,
                      tmp_bbox, tmp_area );
            //tmp_bbox.dump();
            //printf( "tmp_area: %g\n", tmp_area );
            if  ( min_area > tmp_area ) {
                min_area = tmp_area;
                min_bbox = tmp_bbox;
            }
        }
        free( angles );
        angles = NULL;        
    }

    void  compute_min_bbox( vec_point_2d   & points,
                            bbox_2d_info   & min_bbox,
                            gdiam_real  & min_area ) {        
        //printf( "before cunvex hull\n" );
        convex_hull( points, ch );
        //printf( "cunvex hull done!\n" );
        //dump_ch();
        compute_min_bbox_inner( min_bbox, min_area );
        //min_bbox.dump();
    }

};

void   dump_points( gdiam_point  * in_arr,
                    int  size ) 
{
    for  ( int  ind = 0; ind < size; ind++ ) {
        printf( "%d: (%g, %g, %g)\n", ind,
                in_arr[ ind ][ 0 ],
                in_arr[ ind ][ 1 ],
                in_arr[ ind ][ 2 ] );
    }
}

#define  ZERO_EPS  (1e-6)

static void   construct_base_inner( gdiam_point  pnt1,
                             gdiam_point  pnt2,
                             gdiam_point  pnt3 )
{
    pnt_normalize( pnt1 );
    pnt_normalize( pnt2 );
    pnt_normalize( pnt3 );

    if  ( ( pnt_length( pnt1 ) < ZERO_EPS )
          &&  ( pnt_length( pnt2 ) < ZERO_EPS )
          &&  ( pnt_length( pnt3 ) < ZERO_EPS ) ){
        assert( false );
    }

    if  ( ( pnt_length( pnt1 ) < ZERO_EPS )
          &&  ( pnt_length( pnt2 ) < ZERO_EPS ) ) {
        gdiam_generate_orthonormal_base( pnt3, pnt1, pnt2 );
        return;
    }
    if  ( ( pnt_length( pnt1 ) < ZERO_EPS )
          &&  ( pnt_length( pnt3 ) < ZERO_EPS ) ) {
        gdiam_generate_orthonormal_base( pnt2, pnt1, pnt3 );
        return;
    }
    if  ( ( pnt_length( pnt2 ) < ZERO_EPS )
          &&  ( pnt_length( pnt3 ) < ZERO_EPS ) ) {
        gdiam_generate_orthonormal_base( pnt1, pnt2, pnt3 );
        return;
    }
    if  ( pnt_length( pnt1 ) < ZERO_EPS ) {
        pnt_cross_prod( pnt2, pnt3, pnt1 );
        return;
    }
    if  ( pnt_length( pnt2 ) < ZERO_EPS ) {
        pnt_cross_prod( pnt1, pnt3, pnt2 );
        return;
    }
    if  ( pnt_length( pnt3 ) < ZERO_EPS ) {
        pnt_cross_prod( pnt1, pnt2, pnt3 );
        return;
    }
}

void   construct_base( gdiam_point  pnt1,
                       gdiam_point  pnt2,
                       gdiam_point  pnt3 )
{
    construct_base_inner( pnt1, pnt2, pnt3 );
    pnt_scale_and_add( pnt2, -pnt_dot_prod( pnt1, pnt2 ),
                       pnt1 );
    pnt_normalize( pnt2 );

    pnt_scale_and_add( pnt3, -pnt_dot_prod( pnt1, pnt3 ),
                       pnt1 );
    pnt_scale_and_add( pnt3, -pnt_dot_prod( pnt2, pnt3 ),
                       pnt2 );
    pnt_normalize( pnt3 );
}


class  ProjPointSet 
{
private:
    gdiam_point_t  base_x, base_y, base_proj;
    point2d  * arr;
    vec_point_2d   points, ch;
    gdiam_bbox   bbox;
    gdiam_point  * in_arr;
    int  size;
        
public:
    ~ProjPointSet() {
        term();
    }
    void  init( gdiam_point  dir,
                gdiam_point  * _in_arr,
                int  _size ) {
        arr = NULL;

        if  ( pnt_length( dir ) == 0.0 ) { 
            dump_points( _in_arr, _size );
            pnt_dump( dir );
            fflush( stdout );
            fflush( stderr );
            assert( pnt_length( dir ) > 0.0 );
        }
        size = _size;
        in_arr = _in_arr;
        assert( size > 0 );

        pnt_copy( base_proj, dir );
        gdiam_generate_orthonormal_base( base_proj, base_x, base_y );
        
        arr = (point2d *)malloc( sizeof( point2d ) * size );
        assert( arr != 0 );

        for  ( int  ind = 0; ind < size; ind++ ) {
            arr[ ind ].init( in_arr[ ind ], base_x, base_y );
            points.push_back( &( arr[ ind ] ) );
        }
    }

    void  compute() {
        MinAreaRectangle  mar;
        bbox_2d_info   min_bbox;
        gdiam_real   min_area;
        double  x1, y1, x2, y2;
        gdiam_point_t  out_1, out_2;

        mar.compute_min_bbox( points, min_bbox, min_area );

        // We take the resulting min_area rectangle and lift it back to 3d...
        x1 = min_bbox.vertices[ 0 ][ 0 ] -
            min_bbox.vertices[ 1 ][ 0 ];
        y1 = min_bbox.vertices[ 0 ][ 1 ] -
            min_bbox.vertices[ 1 ][ 1 ];
        x2 = min_bbox.vertices[ 0 ][ 0 ] -
            min_bbox.vertices[ 3 ][ 0 ];
        y2 = min_bbox.vertices[ 0 ][ 1 ] -
            min_bbox.vertices[ 3 ][ 1 ];

        //printf( "prod: %g\n", x1 * x2 + y1 * y2 );
        
        double  len;

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

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

        pnt_zero( out_1 );
        pnt_scale_and_add( out_1, x1, base_x );
        pnt_scale_and_add( out_1, y1, base_y );
        pnt_normalize( out_1 );

        pnt_zero( out_2 );
        pnt_scale_and_add( out_2, x2, base_x );
        pnt_scale_and_add( out_2, y2, base_y );
        pnt_normalize( out_2 );

        construct_base( base_proj, out_1, out_2 );
        if  ( ( ! (fabs( pnt_dot_prod( base_proj, out_1 ) ) < 1e-6 ) )
              ||  ( ! (fabs( pnt_dot_prod( base_proj, out_2 ) ) < 1e-6 ) )
              ||  ( ! (fabs( pnt_dot_prod( out_1, out_2 ) ) < 1e-6 ) ) ) {
            printf( "should be all close to zero: %g, %g, %g\n",
                    pnt_dot_prod( base_proj, out_1 ),
                    pnt_dot_prod( base_proj, out_2 ),
                    pnt_dot_prod( out_1, out_2 ) );
            pnt_dump( base_proj );
            pnt_dump( out_1 );
            pnt_dump( out_2 );


            printf( "real points:\n" );
            dump_points( in_arr, size );

            printf( "points on CH:\n" );
            dump( points );

            printf( "BBox:\n" );
            min_bbox.dump();

            fflush( stdout );
            fflush( stderr );
            assert( fabs( pnt_dot_prod( base_proj, out_1 ) ) < 1e-6 );
            assert( fabs( pnt_dot_prod( base_proj, out_2 ) ) < 1e-6 );
            assert( fabs( pnt_dot_prod( out_1, out_2 ) ) < 1e-6 );
        }

        bbox.init( base_proj, out_1, out_2 );
        for  ( int  ind = 0; ind < size; ind++ ) 
            bbox.bound( in_arr[ ind ] );
    }

    gdiam_bbox   & get_bbox() {
        return  bbox;
    }    

    void  term() {
        if  ( arr != NULL ) {
            free( arr );
            arr = NULL;
        }
    }
    void  init() {
    }
};


gdiam_bbox   gdiam_mvbb_optimize( gdiam_point  * start, int  size,
                                  gdiam_bbox  bb_out, int  times  )
{
    gdiam_bbox  bb_tmp;
    
    //printf( "gdiam_mvbb_optimize called\n" );

    for  ( int  ind = 0; ind < times; ind++ ) {
        ProjPointSet  pps;

        if  ( pnt_length( bb_out.get_dir( ind % 3 ) ) == 0.0 ) {
            printf( "Dumping!\n" );
            bb_out.dump();
            fflush( stdout );
            continue;
        }

        pps.init( bb_out.get_dir( ind % 3 ), start, size );

        pps.compute();
        
        //printf( "Old volume: %g\n", bb_out.volume() );
        bb_tmp = pps.get_bbox();
        //printf( "New volume: %g\n", bb_tmp.volume() );
        //printf( "Old bounding box:\n" );
        //bb2.dump();

        if  ( bb_tmp.volume() < bb_out.volume() ) 
            bb_out = bb_tmp;
    }

    return  bb_out;
}


gdiam_bbox   gdiam_approx_mvbb( gdiam_point  * start, int  size,
                                gdiam_real  eps ) 
{
    gdiam_bbox  bb, bb2;

    bb = gdiam_approx_const_mvbb( start, size, 0.0, NULL );
    bb2 = gdiam_mvbb_optimize( start, size,  bb, 10 );

    //bb2.dump();

    return  bb2;
}


static int  gcd2( int  a, int  b ) 
{
    if  ( a == 0  ||  a == b ) 
        return  b;
    if  ( b == 0 ) 
        return  a;
    if  ( a > b ) 
        return  gcd2( a % b, b );
    else
        return  gcd2( a, b % a );
}


static int  gcd3( int  a, int  b, int  c ) 
{
    a = abs( a );
    b = abs( b );
    c = abs( c );
    if   ( a == 0 ) 
        return  gcd2( b, c );
    if   ( b == 0 ) 
        return  gcd2( a, c );
    if   ( c == 0 ) 
        return  gcd2( a, b );

    return  gcd2( a, gcd2( b, c ) );
}


static void  try_direction( gdiam_bbox  & bb, 
                            gdiam_bbox  & bb_out, 
                            gdiam_point  * start, 
                            int  size,