gdiam.c

任意给定三维空间的点集

/*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*
 * gdiam.C -
 *   Computes diameter, and a tight-fitting bounding box of a
 *   point-set in 3d.
 *
 * Copyright 2000 Sariel Har-Peled ([e-mail])
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2, 
 * or (at your option) any later version.
 * 
 * Code is based on the paper:
 *   A Practical Approach for Computing the Diameter of a 
 *        Point-Set (in ACM Sym. on Computation Geometry
 *                   SoCG 2001)
 *   Sariel Har-Peled (http://www.uiuc.edu/~sariel)
 *--------------------------------------------------------------
 * History
 * 3/28/01 - 
 *     This is a more robust version of the code. It should
 *     handlereally abnoxious inputs well (i.e., points with equal
 *     coordinates, etc.
\*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*/

#include  <stdlib.h>
#include  <stdio.h>
#include  <assert.h>
#include  <memory.h>
#include  <math.h>

#include  <vector>
#include  <algorithm>

#include  "gdiam.h"

/*--- Constants ---*/

#define  HEAP_LIMIT  10000
#define  HEAP_DELTA  10000
#define  PI 3.14159265958979323846

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

typedef long double  ldouble;

class GFSPPair;

/*void  computeExtremePairDir( const ArrPoint3d  & arr,
                             const Point3d  & dir,
                             GPointPair  & pp,
                             int  start,
                             int  end
                             );
*/

class   GFSPTreeNode {
private:
    GBBox    bbox;
    //int  left_ind, p_pnt_right; // range in the array
    gdiam_point   * p_pnt_left, * p_pnt_right;
    gdiam_real  bbox_diam, bbox_diam_proj;
    GFSPTreeNode  * left, * right;
    gdiam_point_t  center;

public:
    void  dump() const {
        printf( "dump...\n" );
        //printf( "\tNode: Range: [%d, %d]\n", left_ind, p_pnt_right );
    }
    int  size() const {
        return  (int)( p_pnt_right - p_pnt_left );
    }
    const gdiam_point  getCenter() const {
        return   (gdiam_point)center;
    }
    int  nodes_number() const {
        int  sum;

        if  ( ( left == NULL )  &&  ( right == NULL ) )
            return  1;
        sum = 1;

        if  ( left != NULL )
            sum += left->nodes_number();
        if  ( right != NULL )
            sum += right->nodes_number();

        return  sum;
    }

        
    GFSPTreeNode( gdiam_point   * _p_pnt_left, 
                  gdiam_point   * _p_pnt_right ) {
        bbox.init();
        left = right = NULL;
        p_pnt_left = _p_pnt_left;
        p_pnt_right = _p_pnt_right;
        bbox_diam = bbox_diam_proj = -1;
    }

    gdiam_real  maxDiam() const {
        return  bbox_diam;
    }
    gdiam_real  maxDiamProj() const {
        return  bbox_diam_proj;
    }

    GFSPTreeNode  * get_left() {
        return  left;
    }
    GFSPTreeNode  * get_right() {
        return   right;
    }

    bool  isSplitable() const {
        return  (size() > 1);
    }

    const gdiam_point   * ptr_pnt_left() {
        return  p_pnt_left;
    }
    const gdiam_point   * ptr_pnt_rand() {
        return  p_pnt_left + ( rand() % ( p_pnt_right - p_pnt_left + 1 ) );
    }
    const gdiam_point   * ptr_pnt_right() {
        return  p_pnt_right;
    }


    friend class  GFSPTree;

    const GBBox  & getBBox() const {
        return  bbox;
    }
};


class  GFSPTree { 
private:
    gdiam_point  * arr;
    GFSPTreeNode  * root;

public:
    void  init( const gdiam_point  * _arr,
                int  size ) {
        arr = (gdiam_point *)malloc( sizeof( gdiam_point ) * size );
        assert( arr != NULL );

        for  ( int  ind = 0; ind < size; ind++ ) {
            arr[ ind ] = _arr[ ind ];
        }

        root = build_node( arr, arr + size - 1 );
    }

    static void  terminate( GFSPTreeNode  * node ) {
        if  ( node == NULL )
            return;
        terminate( node->left );
        terminate( node->right );

        node->left = node->right = NULL;

        delete node;
    }
    void  term() {
        free( arr );
        arr = NULL;

        GFSPTree::terminate( root );
        root = NULL;
    }

    const gdiam_point   * getPoints() const {
        return  arr;
    }

    int  nodes_number() const {
        return  root->nodes_number();
    }

    gdiam_real   brute_diameter( int  a_lo, int  a_hi,
                                 int  b_lo, int  b_hi,
                                 GPointPair  & diam ) const { 
        double  max_dist;
        
        max_dist = 0;
        for  ( int  ind = a_lo; ind <= a_hi; ind++ )
            for  ( int  jnd = b_lo; jnd <= b_hi; jnd++ )
                diam.update_diam( arr[ ind ], arr[ jnd ] );
        
        return  diam.distance;
    }
    gdiam_real   brute_diameter( int  a_lo, int  a_hi,
                                 int  b_lo, int  b_hi,
                                 GPointPair  & diam,
                                 const gdiam_point  dir ) const { 
        double  max_dist;

        max_dist = 0;
        for  ( int  ind = a_lo; ind <= a_hi; ind++ )
            for  ( int  jnd = b_lo; jnd <= b_hi; jnd++ )
                diam.update_diam( arr[ ind ], arr[ jnd ], dir );
        
        return  diam.distance;
    }
    gdiam_real   brute_diameter( const gdiam_point  * a_lo, 
                           const gdiam_point  * a_hi,
                           const gdiam_point  * b_lo, 
                           const gdiam_point  * b_hi,
                           GPointPair  & diam ) const { 
        double  max_dist;

        max_dist = 0;
        for  ( const gdiam_point  * ind = a_lo; ind <= a_hi; ind++ )
            for  ( const gdiam_point   * jnd = b_lo; jnd <= b_hi; jnd++ )
                diam.update_diam( *ind, *jnd );
        
        return  diam.distance;
    }
    gdiam_real   brute_diameter( const gdiam_point  * a_lo, 
                                 const gdiam_point  * a_hi,
                                 const gdiam_point  * b_lo, 
                                 const gdiam_point  * b_hi,
                                 GPointPair  & diam,
                                 const gdiam_point  dir ) const { 
        double  max_dist;

        max_dist = 0;
        for  ( const gdiam_point  * ind = a_lo; ind <= a_hi; ind++ )
            for  ( const gdiam_point   * jnd = b_lo; jnd <= b_hi; jnd++ )
                diam.update_diam( *ind, *jnd, dir );
        
        return  diam.distance;
    }

    const gdiam_point  getPoint( int  ind ) const {
        return  arr[ ind ];
    }

    GFSPTreeNode  * getRoot() {
        return  root;
    }

    GFSPTreeNode  * build_node( gdiam_point  * left,
                                gdiam_point  * right ) {
        if  ( left > right ) {
            printf( "what!?\n" );
            fflush( stdout );
            assert( left <= right );
        }
        while  ( ( right > left ) 
                 &&  ( pnt_isEqual( *right, *left ) ) )
            right--;
        GFSPTreeNode  * node = new  GFSPTreeNode( left, right );

        node->bbox.init();
        for  ( const gdiam_point  * ind = left; ind <= right; ind++ ) 
            node->bbox.bound( *ind );
        node->bbox_diam = node->bbox.get_diam();

        node->bbox.center( node->center );

        return  node;
    }
    
    // return how many elements on left side.
    int    split_array( gdiam_point   * left, 
                        gdiam_point   * right, 
                        int  dim, const gdiam_real  & val ) {
        const gdiam_point   * start_left = left;

        while  ( left < right ) {
            if  ( (*left)[ dim ] < val )
                left++;
            else
                if  ( (*right)[ dim ] >= val )
                    right--;
                else {
                    gdiam_exchange( *right, *left );
                }
        }

        return  left - start_left;
    }


    void   split_node( GFSPTreeNode  * node ) 
    {
        int  dim, left_size;
        gdiam_real  split_val;

        if  ( node->left != NULL )
            return;

        GBBox  & bb( node->bbox );

        dim = bb.getLongestDim();
        split_val = ( bb.min_coord( dim ) + bb.max_coord( dim ) ) / 2.0;

        left_size = split_array( node->p_pnt_left, node->p_pnt_right, 
                              dim, split_val );
        if  ( left_size <= 0.0 ) {
            printf( "bb: %g   %g\n",
                    bb.min_coord( dim ), bb.max_coord( dim ) );
            printf( "left: %p, right: %p\n",
                    node->p_pnt_left, node->p_pnt_right );
            assert( left_size > 0 );
        }
        if  ( left_size >= (node->p_pnt_right - node->p_pnt_left + 1 ) ) {
            printf( "left size too large?\n" );
            fflush( stdout );
            assert( left_size < (node->p_pnt_right - node->p_pnt_left + 1 ) );
        }

        node->left = build_node( node->p_pnt_left, 
                                 node->p_pnt_left + left_size - 1 );
        node->right = build_node( node->p_pnt_left + left_size, 
                                  node->p_pnt_right );
    }

    void  findExtremeInProjection( GFSPPair  & pair, 
                                   GPointPair  & max_pair_diam );
};

void  bbox_vertex( const GBBox  & bb, gdiam_point_t  & ver,
                   int  vert ) {
    ver[ 0 ] = ((vert & 0x1) != 0)? 
        bb.min_coord( 0 ) : bb.max_coord( 0 );
    ver[ 1 ] = ((vert & 0x2) != 0)? 
        bb.min_coord( 1 ) : bb.max_coord( 1 );
    ver[ 2 ] = ((vert & 0x4) != 0)? 
        bb.min_coord( 2 ) : bb.max_coord( 2 );
}


gdiam_real  bbox_proj_dist( const GBBox  & bb1, 
                            const GBBox  & bb2,
                            gdiam_point_cnt  proj )
{
    gdiam_point_t  a, b;
    gdiam_real  rl;

    rl = 0;
    for  ( int  ind = 0; ind < 8; ind++ ) {
        bbox_vertex( bb1, a, ind );

        //printf( "\n\n" );
        for  ( int  jnd = 0; jnd < 8; jnd++ ) {
            bbox_vertex( bb2, b, jnd );
            //pnt_dump( b );
            rl = max( rl, pnt_distance( a, b, proj ) );
        }
    }

    return  rl;            
}



class  GFSPPair 
{
private:
    GFSPTreeNode  * left, * right;
    double  max_diam;

public:
    void  init( GFSPTreeNode  * _left, GFSPTreeNode  * _right )
    {
        left = _left;
        right = _right;

        GBBox  bb;

        bb.init( left->getBBox(), right->getBBox() );
        //printf( "\t\tbbox: \n" );
        //bb.dump();
        max_diam = bb.get_diam();
        //printf( "\t\tmax_diam: %g\n", max_diam );
    }

    void  init( GFSPTreeNode  * _left, GFSPTreeNode  * _right,
                gdiam_point  proj_dir, gdiam_real dist )
    {
        left = _left;
        right = _right;

        GBBox  bb;

        bb.init( left->getBBox(), right->getBBox() );
        //printf( "\t\tbbox: \n" );
        //bb.dump();
        
        //max_diam = dist + _left->getBBox().get_diam()
        //    + _right->getBBox().get_diam();
        max_diam = max( max( bbox_proj_dist( _left->getBBox(),
                                        _right->getBBox(),
                                        proj_dir ),
                             bbox_proj_dist( _left->getBBox(),
                                             _left->getBBox(),
                                             proj_dir ) ),
                        bbox_proj_dist( _right->getBBox(),
                                        _right->getBBox(),
                                        proj_dir ) );
        
        //printf( "diam: %g, smart diam: %g\n",
        //        max_diam,
        //        bbox_proj_dist( _left->getBBox(),
        //                        _right->getBBox(),
        //                        proj_dir ) );

        //printf( "dist: %g, %g, %g:\n",
        //        dist, _left->getBBox().get_diam(),
        //         _right->getBBox().get_diam() );

        //bb.get_diam_proj( proj_dir );
        //printf( "\t\tmax_diam: %g\n", max_diam );
    }

    // error 2r/l - approximate cos of the max possible angle in the pair
    double  getProjectionError() const 
    {
        double  l, two_r;

        l = pnt_distance( left->getCenter(), right->getCenter() );
        two_r = max( left->maxDiam(), right->maxDiam() );

        if  ( l == 0.0 )
            return  10;

        return  two_r / l;
    }

    GFSPTreeNode  * get_left() {
        return  left;
        }
    GFSPTreeNode  * get_right() {
        return   right;
    }

    void  dump() const {
        printf( "[\n" );
        left->dump();
        right->dump();
        printf( "\tmax_diam; %g\n", max_diam );
    }

    bool  isBelowThreshold( int  threshold ) const {
        if  ( left->size() > threshold )
            return  false;
        if  ( right->size() > threshold )
            return  false;

        return  true;
    }

    double   directDiameter( const GFSPTree  & tree,
                             GPointPair  & diam ) const {
        return  tree.brute_diameter( left->ptr_pnt_left(),
                                     left->ptr_pnt_right(), 
                                     right->ptr_pnt_left(),
                                     right->ptr_pnt_right(),
                                     diam );