source: proiecte/pmake3d/make3d_original/Make3dSingleImageStanford_version0.1/third_party/ann_1.1.1/src/kd_util.cpp @ 37

//----------------------------------------------------------------------
// File:                        kd_util.cpp
// Programmer:          Sunil Arya and David Mount
// Description:         Common utilities for kd-trees
// Last modified:       01/04/05 (Version 1.0)
//----------------------------------------------------------------------
// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and
// David Mount.  All Rights Reserved.
// 
// This software and related documentation is part of the Approximate
// Nearest Neighbor Library (ANN).  This software is provided under
// the provisions of the Lesser GNU Public License (LGPL).  See the
// file ../ReadMe.txt for further information.
// 
// The University of Maryland (U.M.) and the authors make no
// representations about the suitability or fitness of this software for
// any purpose.  It is provided "as is" without express or implied
// warranty.
//----------------------------------------------------------------------
// History:
//      Revision 0.1  03/04/98
//              Initial release
//----------------------------------------------------------------------

#include "kd_util.h"                                    // kd-utility declarations

#include <ANN/ANNperf.h>                                // performance evaluation

//----------------------------------------------------------------------
// The following routines are utility functions for manipulating
// points sets, used in determining splitting planes for kd-tree
// construction.
//----------------------------------------------------------------------

//----------------------------------------------------------------------
//      NOTE: Virtually all point indexing is done through an index (i.e.
//      permutation) array pidx.  Consequently, a reference to the d-th
//      coordinate of the i-th point is pa[pidx[i]][d].  The macro PA(i,d)
//      is a shorthand for this.
//----------------------------------------------------------------------
                                                                                // standard 2-d indirect indexing
#define PA(i,d)                 (pa[pidx[(i)]][(d)])
                                                                                // accessing a single point
#define PP(i)                   (pa[pidx[(i)]])

//----------------------------------------------------------------------
//      annAspectRatio
//              Compute the aspect ratio (ratio of longest to shortest side)
//              of a rectangle.
//----------------------------------------------------------------------

double annAspectRatio(
        int                                     dim,                    // dimension
        const ANNorthRect       &bnd_box)               // bounding cube
{
        ANNcoord length = bnd_box.hi[0] - bnd_box.lo[0];
        ANNcoord min_length = length;                           // min side length
        ANNcoord max_length = length;                           // max side length
        for (int d = 0; d < dim; d++) {
                length = bnd_box.hi[d] - bnd_box.lo[d];
                if (length < min_length) min_length = length;
                if (length > max_length) max_length = length;
        }
        return max_length/min_length;
}

//----------------------------------------------------------------------
//      annEnclRect, annEnclCube
//              These utilities compute the smallest rectangle and cube enclosing
//              a set of points, respectively.
//----------------------------------------------------------------------

void annEnclRect(
        ANNpointArray           pa,                             // point array
        ANNidxArray                     pidx,                   // point indices
        int                                     n,                              // number of points
        int                                     dim,                    // dimension
        ANNorthRect                     &bnds)                  // bounding cube (returned)
{
        for (int d = 0; d < dim; d++) {         // find smallest enclosing rectangle
                ANNcoord lo_bnd = PA(0,d);              // lower bound on dimension d
                ANNcoord hi_bnd = PA(0,d);              // upper bound on dimension d
                for (int i = 0; i < n; i++) {
                        if (PA(i,d) < lo_bnd) lo_bnd = PA(i,d);
                        else if (PA(i,d) > hi_bnd) hi_bnd = PA(i,d);
                }
                bnds.lo[d] = lo_bnd;
                bnds.hi[d] = hi_bnd;
        }
}

void annEnclCube(                                               // compute smallest enclosing cube
        ANNpointArray           pa,                             // point array
        ANNidxArray                     pidx,                   // point indices
        int                                     n,                              // number of points
        int                                     dim,                    // dimension
        ANNorthRect                     &bnds)                  // bounding cube (returned)
{
        int d;
                                                                                // compute smallest enclosing rect
        annEnclRect(pa, pidx, n, dim, bnds);

        ANNcoord max_len = 0;                           // max length of any side
        for (d = 0; d < dim; d++) {                     // determine max side length
                ANNcoord len = bnds.hi[d] - bnds.lo[d];
                if (len > max_len) {                    // update max_len if longest
                        max_len = len;
                }
        }
        for (d = 0; d < dim; d++) {                     // grow sides to match max
                ANNcoord len = bnds.hi[d] - bnds.lo[d];
                ANNcoord half_diff = (max_len - len) / 2;
                bnds.lo[d] -= half_diff;
                bnds.hi[d] += half_diff;
        }
}

//----------------------------------------------------------------------
//      annBoxDistance - utility routine which computes distance from point to
//              box (Note: most distances to boxes are computed using incremental
//              distance updates, not this function.)
//----------------------------------------------------------------------

ANNdist annBoxDistance(                 // compute distance from point to box
        const ANNpoint          q,                              // the point
        const ANNpoint          lo,                             // low point of box
        const ANNpoint          hi,                             // high point of box
        int                                     dim)                    // dimension of space
{
        register ANNdist dist = 0.0;            // sum of squared distances
        register ANNdist t;

        for (register int d = 0; d < dim; d++) {
                if (q[d] < lo[d]) {                             // q is left of box
                        t = ANNdist(lo[d]) - ANNdist(q[d]);
                        dist = ANN_SUM(dist, ANN_POW(t));
                }
                else if (q[d] > hi[d]) {                // q is right of box
                        t = ANNdist(q[d]) - ANNdist(hi[d]);
                        dist = ANN_SUM(dist, ANN_POW(t));
                }
        }
        ANN_FLOP(4*dim)                                         // increment floating op count

        return dist;
}

//----------------------------------------------------------------------
//      annSpread - find spread along given dimension
//      annMinMax - find min and max coordinates along given dimension
//      annMaxSpread - find dimension of max spread
//----------------------------------------------------------------------

ANNcoord annSpread(                             // compute point spread along dimension
        ANNpointArray           pa,                             // point array
        ANNidxArray                     pidx,                   // point indices
        int                                     n,                              // number of points
        int                                     d)                              // dimension to check
{
        ANNcoord min = PA(0,d);                         // compute max and min coords
        ANNcoord max = PA(0,d);
        for (int i = 1; i < n; i++) {
                ANNcoord c = PA(i,d);
                if (c < min) min = c;
                else if (c > max) max = c;
        }
        return (max - min);                                     // total spread is difference
}

void annMinMax(                                 // compute min and max coordinates along dim
        ANNpointArray           pa,                             // point array
        ANNidxArray                     pidx,                   // point indices
        int                                     n,                              // number of points
        int                                     d,                              // dimension to check
        ANNcoord                        &min,                   // minimum value (returned)
        ANNcoord                        &max)                   // maximum value (returned)
{
        min = PA(0,d);                                          // compute max and min coords
        max = PA(0,d);
        for (int i = 1; i < n; i++) {
                ANNcoord c = PA(i,d);
                if (c < min) min = c;
                else if (c > max) max = c;
        }
}

int annMaxSpread(                                               // compute dimension of max spread
        ANNpointArray           pa,                             // point array
        ANNidxArray                     pidx,                   // point indices
        int                                     n,                              // number of points
        int                                     dim)                    // dimension of space
{
        int max_dim = 0;                                        // dimension of max spread
        ANNcoord max_spr = 0;                           // amount of max spread

        if (n == 0) return max_dim;                     // no points, who cares?

        for (int d = 0; d < dim; d++) {         // compute spread along each dim
                ANNcoord spr = annSpread(pa, pidx, n, d);
                if (spr > max_spr) {                    // bigger than current max
                        max_spr = spr;
                        max_dim = d;
                }
        }
        return max_dim;
}

//----------------------------------------------------------------------
//      annMedianSplit - split point array about its median
//              Splits a subarray of points pa[0..n] about an element of given
//              rank (median: n_lo = n/2) with respect to dimension d.  It places
//              the element of rank n_lo-1 correctly (because our splitting rule
//              takes the mean of these two).  On exit, the array is permuted so
//              that:
//
//              pa[0..n_lo-2][d] <= pa[n_lo-1][d] <= pa[n_lo][d] <= pa[n_lo+1..n-1][d].
//
//              The mean of pa[n_lo-1][d] and pa[n_lo][d] is returned as the
//              splitting value.
//
//              All indexing is done indirectly through the index array pidx.
//
//              This function uses the well known selection algorithm due to
//              C.A.R. Hoare.
//----------------------------------------------------------------------

                                                                                // swap two points in pa array
#define PASWAP(a,b) { int tmp = pidx[a]; pidx[a] = pidx[b]; pidx[b] = tmp; }

void annMedianSplit(
        ANNpointArray           pa,                             // points to split
        ANNidxArray                     pidx,                   // point indices
        int                                     n,                              // number of points
        int                                     d,                              // dimension along which to split
        ANNcoord                        &cv,                    // cutting value
        int                                     n_lo)                   // split into n_lo and n-n_lo
{
        int l = 0;                                                      // left end of current subarray
        int r = n-1;                                            // right end of current subarray
        while (l < r) {
                register int i = (r+l)/2;               // select middle as pivot
                register int k;

                if (PA(i,d) > PA(r,d))                  // make sure last > pivot
                        PASWAP(i,r)
                PASWAP(l,i);                                    // move pivot to first position

                ANNcoord c = PA(l,d);                   // pivot value
                i = l;
                k = r;
                for(;;) {                                               // pivot about c
                        while (PA(++i,d) < c) ;
                        while (PA(--k,d) > c) ;
                        if (i < k) PASWAP(i,k) else break;
                }
                PASWAP(l,k);                                    // pivot winds up in location k

                if (k > n_lo)      r = k-1;             // recurse on proper subarray
                else if (k < n_lo) l = k+1;
                else break;                                             // got the median exactly
        }
        if (n_lo > 0) {                                         // search for next smaller item
                ANNcoord c = PA(0,d);                   // candidate for max
                int k = 0;                                              // candidate's index
                for (int i = 1; i < n_lo; i++) {
                        if (PA(i,d) > c) {
                                c = PA(i,d);
                                k = i;
                        }
                }
                PASWAP(n_lo-1, k);                              // max among pa[0..n_lo-1] to pa[n_lo-1]
        }
                                                                                // cut value is midpoint value
        cv = (PA(n_lo-1,d) + PA(n_lo,d))/2.0;
}

//----------------------------------------------------------------------
//      annPlaneSplit - split point array about a cutting plane
//              Split the points in an array about a given plane along a
//              given cutting dimension.  On exit, br1 and br2 are set so
//              that:
//              
//                              pa[ 0 ..br1-1] <  cv
//                              pa[br1..br2-1] == cv
//                              pa[br2.. n -1] >  cv
//
//              All indexing is done indirectly through the index array pidx.
//
//----------------------------------------------------------------------

void annPlaneSplit(                             // split points by a plane
        ANNpointArray           pa,                             // points to split
        ANNidxArray                     pidx,                   // point indices
        int                                     n,                              // number of points
        int                                     d,                              // dimension along which to split
        ANNcoord                        cv,                             // cutting value
        int                                     &br1,                   // first break (values < cv)
        int                                     &br2)                   // second break (values == cv)
{
        int l = 0;
        int r = n-1;
        for(;;) {                                                       // partition pa[0..n-1] about cv
                while (l < n && PA(l,d) < cv) l++;
                while (r >= 0 && PA(r,d) >= cv) r--;
                if (l > r) break;
                PASWAP(l,r);
                l++; r--;
        }
        br1 = l;                                        // now: pa[0..br1-1] < cv <= pa[br1..n-1]
        r = n-1;
        for(;;) {                                                       // partition pa[br1..n-1] about cv
                while (l < n && PA(l,d) <= cv) l++;
                while (r >= br1 && PA(r,d) > cv) r--;
                if (l > r) break;
                PASWAP(l,r);
                l++; r--;
        }
        br2 = l;                                        // now: pa[br1..br2-1] == cv < pa[br2..n-1]
}


//----------------------------------------------------------------------
//      annBoxSplit - split point array about a orthogonal rectangle
//              Split the points in an array about a given orthogonal
//              rectangle.  On exit, n_in is set to the number of points
//              that are inside (or on the boundary of) the rectangle.
//
//              All indexing is done indirectly through the index array pidx.
//
//----------------------------------------------------------------------

void annBoxSplit(                               // split points by a box
        ANNpointArray           pa,                             // points to split
        ANNidxArray                     pidx,                   // point indices
        int                                     n,                              // number of points
        int                                     dim,                    // dimension of space
        ANNorthRect                     &box,                   // the box
        int                                     &n_in)                  // number of points inside (returned)
{
        int l = 0;
        int r = n-1;
        for(;;) {                                                       // partition pa[0..n-1] about box
                while (l < n && box.inside(dim, PP(l))) l++;
                while (r >= 0 && !box.inside(dim, PP(r))) r--;
                if (l > r) break;
                PASWAP(l,r);
                l++; r--;
        }
        n_in = l;                                       // now: pa[0..n_in-1] inside and rest outside
}

//----------------------------------------------------------------------
//      annSplitBalance - compute balance factor for a given plane split
//              Balance factor is defined as the number of points lying
//              below the splitting value minus n/2 (median).  Thus, a
//              median split has balance 0, left of this is negative and
//              right of this is positive.  (The points are unchanged.)
//----------------------------------------------------------------------

int annSplitBalance(                    // determine balance factor of a split
        ANNpointArray           pa,                             // points to split
        ANNidxArray                     pidx,                   // point indices
        int                                     n,                              // number of points
        int                                     d,                              // dimension along which to split
        ANNcoord                        cv)                             // cutting value
{
        int n_lo = 0;
        for(int i = 0; i < n; i++) {            // count number less than cv
                if (PA(i,d) < cv) n_lo++;
        }
        return n_lo - n/2;
}

//----------------------------------------------------------------------
//      annBox2Bnds - convert bounding box to list of bounds
//              Given two boxes, an inner box enclosed within a bounding
//              box, this routine determines all the sides for which the
//              inner box is strictly contained with the bounding box,
//              and adds an appropriate entry to a list of bounds.  Then
//              we allocate storage for the final list of bounds, and return
//              the resulting list and its size.
//----------------------------------------------------------------------

void annBox2Bnds(                                               // convert inner box to bounds
        const ANNorthRect       &inner_box,             // inner box
        const ANNorthRect       &bnd_box,               // enclosing box
        int                                     dim,                    // dimension of space
        int                                     &n_bnds,                // number of bounds (returned)
        ANNorthHSArray          &bnds)                  // bounds array (returned)
{
        int i;
        n_bnds = 0;                                                                     // count number of bounds
        for (i = 0; i < dim; i++) {
                if (inner_box.lo[i] > bnd_box.lo[i])    // low bound is inside
                                n_bnds++;
                if (inner_box.hi[i] < bnd_box.hi[i])    // high bound is inside
                                n_bnds++;
        }

        bnds = new ANNorthHalfSpace[n_bnds];            // allocate appropriate size

        int j = 0;
        for (i = 0; i < dim; i++) {                                     // fill the array
                if (inner_box.lo[i] > bnd_box.lo[i]) {
                                bnds[j].cd = i;
                                bnds[j].cv = inner_box.lo[i];
                                bnds[j].sd = +1;
                                j++;
                }
                if (inner_box.hi[i] < bnd_box.hi[i]) {
                                bnds[j].cd = i;
                                bnds[j].cv = inner_box.hi[i];
                                bnds[j].sd = -1;
                                j++;
                }
        }
}

//----------------------------------------------------------------------
//      annBnds2Box - convert list of bounds to bounding box
//              Given an enclosing box and a list of bounds, this routine
//              computes the corresponding inner box.  It is assumed that
//              the box points have been allocated already.
//----------------------------------------------------------------------

void annBnds2Box(
        const ANNorthRect       &bnd_box,               // enclosing box
        int                                     dim,                    // dimension of space
        int                                     n_bnds,                 // number of bounds
        ANNorthHSArray          bnds,                   // bounds array
        ANNorthRect                     &inner_box)             // inner box (returned)
{
        annAssignRect(dim, inner_box, bnd_box);         // copy bounding box to inner

        for (int i = 0; i < n_bnds; i++) {
                bnds[i].project(inner_box.lo);                  // project each endpoint
                bnds[i].project(inner_box.hi);
        }
}