gdiam.h

任意给定三维空间的点集

/*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*
 * gdiam.h -
 *     Implmeents an algorithm for computing a diameter,
 * 
 * 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.
 *   Sariel Har-Peled (http://www.uiuc.edu/~sariel)
\*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*/

#ifndef  __GDIAM__H
#define  __GDIAM__H

#define  GDIAM_DIM   3
typedef  double  gdiam_real;
typedef  gdiam_real    gdiam_point_t[ GDIAM_DIM ];
typedef  gdiam_real    gdiam_point_2d_t[ 2 ];
typedef  gdiam_real  * gdiam_point_2d;
typedef  gdiam_real  * gdiam_point;
typedef  const gdiam_real  * gdiam_point_cnt;

#ifndef __MINMAX_DEFINED
#define __MINMAX_DEFINED
/*
template <class T> const inline   T& min( const T& t1, const T& t2 )
{
         return t1>t2 ? t2 : t1;
}

template <class T> inline  T& max( const T& t1, const T& t2 )
{
         return t1>t2 ? t1 : t2;
}
*/

template <class T>
inline T  min( T t1, T t2 )
{
         return t1>t2 ? t2 : t1;
}

template <class T>
inline T  max( T t1, T t2 )
{
         return t1>t2 ? t1 : t2;
}
#endif /* MIN_MAX */

template <class T>
inline void     gdiam_exchange( T   & a, T    & b )
{
    T   tmp = a;

    a = b;
    b = tmp;
}

inline gdiam_real   pnt_length( const gdiam_point  pnt ) 
{
    return  sqrt( pnt[ 0 ] * pnt[ 0 ] + pnt[ 1 ] * pnt[ 1 ]
                  + pnt[ 2 ] * pnt[ 2 ] );
}

inline void  pnt_normalize( gdiam_point  pnt )
{
    gdiam_real  len = pnt_length( pnt );
    if  ( len == 0.0 )
        return;

    pnt[ 0 ] /= len;
    pnt[ 1 ] /= len;
    pnt[ 2 ] /= len;
}

inline    void  pnt_copy( gdiam_point_t   dest,
                    gdiam_point_t   src )
    {
        dest[ 0 ] = src[ 0 ];
        dest[ 1 ] = src[ 1 ];
        dest[ 2 ] = src[ 2 ];
    }
inline void  pnt_zero( gdiam_point  dst ) {
    dst[ 0 ] = dst[ 1 ] = dst[ 2 ] = 0;
}
inline void  pnt_dump( gdiam_point_cnt  pnt ) {
    printf( "(%g, %g, %g)\n", pnt[ 0 ], pnt[ 1 ], pnt[ 2 ] );
}


inline gdiam_real  pnt_dot_prod( gdiam_point_cnt  a,
                                 gdiam_point_cnt  b )
{
    return  a[ 0 ] * b[ 0 ]
        + a[ 1 ] * b[ 1 ]
        + a[ 2 ] * b[ 2 ];
}

inline void   pnt_cross_prod( const gdiam_point   a, 
                              const gdiam_point   b,
                              const gdiam_point   out )
{    
    out[ 0 ] = a[ 1 ] * b[ 2 ] - a[ 2 ] * b[ 1 ];
    out[ 1 ] = - ( a[ 0 ] * b[ 2 ] - a[ 2 ] * b[ 0 ] );
    out[ 2 ] = a[ 0 ] * b[ 1 ] - a[ 1 ] * b[ 0 ];
}


inline gdiam_real   pnt_distance_2d( gdiam_point_2d  p, 
                                     gdiam_point_2d  q )
{
    gdiam_real  valx = (p[ 0 ] - q[ 0 ]);
    gdiam_real  valy = (p[ 1 ] - q[ 1 ]);

    return  sqrt( valx * valx + valy * valy );
}


inline gdiam_real   pnt_distance( gdiam_point  p, gdiam_point  q )
{
    gdiam_real  valx = (p[ 0 ] - q[ 0 ]);
    gdiam_real  valy = (p[ 1 ] - q[ 1 ]);
    gdiam_real  valz = (p[ 2 ] - q[ 2 ]);

    return  sqrt( valx * valx + valy * valy + valz * valz );
}

inline gdiam_real   pnt_distance( gdiam_point  p, gdiam_point  q,
                                  gdiam_point_cnt  dir )
{
    gdiam_real  valx = (p[ 0 ] - q[ 0 ]);
    gdiam_real  valy = (p[ 1 ] - q[ 1 ]);
    gdiam_real  valz = (p[ 2 ] - q[ 2 ]);

    gdiam_real  len, proj_len;

    len = sqrt( valx * valx + valy * valy + valz * valz );

    proj_len = dir[ 0 ] * valx + dir[ 1 ] * valy + dir[ 2 ] * valz;

    return  sqrt( len * len - proj_len * proj_len );
}

inline void  pnt_init( gdiam_point  pnt, 
                       gdiam_real  x,
                       gdiam_real  y,
                       gdiam_real  z )
{
    pnt[ 0 ] = x;
    pnt[ 1 ] = y;
    pnt[ 2 ] = z;
}


inline void  pnt_init_normalize( gdiam_point  pnt, 
                                 gdiam_real  x,
                                 gdiam_real  y,
                                 gdiam_real  z )
{
    pnt[ 0 ] = x;
    pnt[ 1 ] = y;
    pnt[ 2 ] = z;

    pnt_normalize( pnt );
}

inline bool  pnt_isEqual( const gdiam_point  p,
                          const gdiam_point  q ) 
{
    // Assuming here the GDIAM_DIM == 3 !!!!
    return  ( ( p[ 0 ] == q[ 0 ] )
              &&  ( p[ 1 ] == q[ 1 ] )
              &&  ( p[ 2 ] == q[ 2 ] ) );
}

inline void  pnt_scale_and_add( gdiam_point  dest,
                         gdiam_real  coef,
                         gdiam_point_cnt   vec ) {
    dest[ 0 ] += coef * vec[ 0 ];
    dest[ 1 ] += coef * vec[ 1 ];
    dest[ 2 ] += coef * vec[ 2 ];
}

class  GPointPair
{
public:
    gdiam_real  distance;
    gdiam_point  p, q;

    GPointPair() : p(), q() {
        distance = 0.0;
    }

    void  init( const gdiam_point  _p, 
                const gdiam_point  _q ) {
        p = _p;
        q = _q;
        distance = pnt_distance( p, q );
    }
    void  init( const gdiam_point  _p, 
                const gdiam_point  _q,
                const gdiam_point  proj ) {
        p = _p;
        q = _q;
        distance = pnt_distance( p, q, proj );
    }

    void  init( const gdiam_point  pnt ) {
        distance = 0;
        p = q = pnt;
    }

    void  update_diam_simple( const gdiam_point  _p, const gdiam_point  _q ) {
        gdiam_real  new_dist;

        new_dist = pnt_distance( _p, _q );
        if  ( new_dist <= distance )
            return;
        
        //printf( "new_dist: %g\n", new_dist );
        distance = new_dist;
        p = _p;
        q = _q;
    }
    void  update_diam_simple( const gdiam_point  _p, const gdiam_point  _q,
                              const gdiam_point  dir ) {
        gdiam_real  new_dist;

        new_dist = pnt_distance( _p, _q, dir );
        if  ( new_dist <= distance )
            return;
        
        distance = new_dist;
        p = _p;
        q = _q;
    }

    void  update_diam( const gdiam_point  _p, const gdiam_point  _q ) { 
        //update_diam_simple( p, _p );
        //update_diam_simple( p, _q );
        //update_diam_simple( q, _p );
        //update_diam_simple( q, _q );
        update_diam_simple( _p, _q );
    }
    void  update_diam( const gdiam_point  _p, const gdiam_point  _q,
                       const gdiam_point  dir ) { 
        //update_diam_simple( p, _p );
        //update_diam_simple( p, _q );
        //update_diam_simple( q, _p );
        //update_diam_simple( q, _q );
        update_diam_simple( _p, _q, dir );
    }


    void  update_diam( GPointPair  & pp ) {
        //update_diam_simple( p, pp.p );
        //update_diam_simple( p, pp.q );
        //update_diam_simple( q, pp.p );
        //update_diam_simple( q, pp.q );
        update_diam_simple( pp.p, pp.q );
    }
};




class   GBBox {
private:
    gdiam_real  min_coords[ GDIAM_DIM ];
    gdiam_real  max_coords[ GDIAM_DIM ];

public:
    void  init( const GBBox   & a,
                const GBBox   & b ) {
        for  (int  ind = 0; ind < GDIAM_DIM; ind++ ) { 
            min_coords[ ind ] = min( a.min_coords[ ind ],
                                     b.min_coords[ ind ] );
            max_coords[ ind ] = max( a.max_coords[ ind ],
                                     b.max_coords[ ind ] );
        }
    }
    void  dump() const {
        gdiam_real  prod, diff;

        prod = 1.0;
        printf( "__________________________________________\n" );
        for  (int  ind = 0; ind < GDIAM_DIM; ind++ ) { 
            printf( "%d: [%g...%g]\n", ind, min_coords[ ind ],
                    max_coords[ ind ] );
            diff = max_coords[ ind ] - min_coords[ ind ];
            prod *= diff;
        }
        printf( "volume = %g\n", prod );
        printf( "\\__________________________________________\n" );
    }

    void  center( gdiam_point  out ) const {
        for  (int  ind = 0; ind < GDIAM_DIM; ind++ ) {
            out[ ind ] = ( min_coords[ ind ] + max_coords[ ind ] ) / 2.0;
        }
    }

    gdiam_real  volume() const {
        gdiam_real  prod, val;

        prod = 1;        
        for  ( int  ind = 0; ind < GDIAM_DIM; ind++ ) {
            val = length_dim( ind );
            prod *= val;
        }

        return  prod;
    }
    void  init() { 
        for  (int  ind = 0; ind < GDIAM_DIM; ind++ ) {
            min_coords[ ind ] = 1e20;
            max_coords[ ind ] = -1e20;
        }
    }
    /*
    void  dump() const {
        printf( "---(" );
        for  (int  ind = 0; ind < GDIAM_DIM; ind++ ) {
            printf( "[%g, %g] ",
                    min_coords[ ind ],
                    max_coords[ ind ] );
        }
        printf( ")\n" );
        }*/

    const gdiam_real  & min_coord( int  coord ) const {
        return  min_coords[ coord ];
    }
    const gdiam_real  & max_coord( int  coord ) const {
        return  max_coords[ coord ];
    }
    
    void  bound( const gdiam_point   pnt ) {