Quadrotor

//----------------------------------------------------------------------

// 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);

        }

}