walk.cpp

Go to the documentation of this file.
00001 /*
00002  * walk.cpp
00003  *
00004  * Copyright (C) 2010  Thomas A. Vaughan
00005  * All rights reserved.
00006  *
00007  * Simple test of the kdtree API, focusing on walking the structure externally.
00008  */
00009 
00010 // includes --------------------------------------------------------------------
00011 #include "common/wave_ex.h"
00012 #include "kdtree/kdtree.h"
00013 #include "perf/perf.h"
00014 
00015 #include <math.h>
00016 #include <iostream>
00017 
00018 
00019 
00020 static const float s_max                        = 5000.0;
00021 
00022 
00023 ////////////////////////////////////////////////////////////////////////////////
00024 //
00025 //      static helper methods
00026 //
00027 ////////////////////////////////////////////////////////////////////////////////
00028 
00029 class Test : public kdtree::Entry {
00030 public:
00031         ~Test(void) throw() { }
00032 };
00033 
00034 
00035 static float
00036 getRandom
00037 (
00038 IN float q
00039 )
00040 {
00041         ASSERT(q > 0, "bad q: %f", q);
00042 
00043         return (q * rand()) / RAND_MAX;
00044 }
00045 
00046 
00047 
00048 static void
00049 getRandomRect
00050 (
00051 IN const rect3d_t& bounds,
00052 IN float size,
00053 OUT rect3d_t& r
00054 )
00055 {
00056         ASSERT(bounds.isValid(), "invalid");
00057 
00058         r.x0 = bounds.x0 + getRandom(bounds.x1 - bounds.x0 - size);
00059         r.y0 = bounds.y0 + getRandom(bounds.y1 - bounds.y0 - size);
00060         r.z0 = bounds.z0 + getRandom(bounds.z1 - bounds.z0 - size);
00061 
00062         r.x1 = r.x0 + size;
00063         r.y1 = r.y0 + size;
00064         r.z1 = r.z0 + size;
00065 }
00066 
00067 
00068 
00069 static kdtree::eAction
00070 countCallback
00071 (
00072 IN void * context,
00073 IN kdtree::Entry * e,
00074 IN const rect3d_t& r
00075 )
00076 {
00077         long * pCount = (long *) context;
00078         ASSERT(pCount, "null context?");
00079 
00080         *pCount = (*pCount) + 1;
00081 
00082         return kdtree::eAction_Continue;
00083 }
00084 
00085 
00086 /*
00087 static kdtree::eAction
00088 printCallback
00089 (
00090 IN void * context,
00091 IN kdtree::entry_t& e
00092 )
00093 {
00094         long * pCount = (long *) context;
00095         ASSERT(pCount, "null context?");
00096 
00097         DPRINTF("Found entry in search rect");
00098         e.bounds.dump("  entry bounds");
00099 
00100         *pCount = (*pCount) + 1;
00101 
00102         return kdtree::eAction_Continue;
00103 }
00104  */
00105 
00106 
00107 static kdtree::eAction
00108 findCallback
00109 (
00110 IN void * context,
00111 IN kdtree::Entry * e,
00112 IN const rect3d_t& r
00113 )
00114 {
00115         if (e == context) {
00116                 // we have a match!
00117                 return kdtree::eAction_Halt;
00118         }
00119 
00120         // not a match...
00121         return kdtree::eAction_Continue;
00122 }
00123 
00124 
00125 
00126 static kdtree::eAction
00127 removeCallback
00128 (
00129 IN void * context,
00130 IN kdtree::Entry * e,
00131 IN const rect3d_t& r
00132 )
00133 {
00134         if (e == context) {
00135                 // matches!
00136                 return (kdtree::eAction)
00137                     (kdtree::eAction_Remove | kdtree::eAction_Halt);
00138         }
00139 
00140         // not a match...
00141         return kdtree::eAction_Continue;
00142 }
00143 
00144 
00145 
00146 static smart_ptr<kdtree::Node>
00147 populateTree
00148 (
00149 IN int nCount,
00150 IN int subdivide
00151 )
00152 {
00153         perf::Timer timer("populateTree");
00154         ASSERT(nCount > 0, "bad count: %d", nCount);
00155         ASSERT(subdivide > 0, "Bad subdivide: %d", subdivide);
00156 
00157         rect3d_t bounds;
00158 
00159         DPRINTF(
00160             "Populating tree with %d entries, subdivide at %d entries per node",
00161             nCount, subdivide);
00162 
00163         bounds.x0 = -s_max;
00164         bounds.x1 = +s_max;
00165         bounds.y0 = -(s_max / 2.5);
00166         bounds.y1 = +(s_max / 2.5);
00167         bounds.z0 = -(s_max / 5);
00168         bounds.z1 = +(s_max / 5);
00169         //bounds.dump("bounds");
00170 
00171         smart_ptr<kdtree::Node> root = kdtree::Node::create(bounds);
00172         ASSERT(root, "failed to create root node");
00173 
00174         for (int i = 0; i < nCount; ++i) {
00175                 rect3d_t r;
00176                 getRandomRect(bounds, getRandom(10), r);
00177                 //e.bounds.dump("  adding");
00178                 smart_ptr<kdtree::Entry> e;
00179 
00180                 {
00181                         perf::Timer timer("addEntry");
00182                         root->addStaticEntry(e, r);
00183                 }
00184         }
00185 
00186         DPRINTF("Tree depth: %d", root->getTreeDepth());
00187         DPRINTF("Node count: %d", root->getNodeCount());
00188         DPRINTF("Entry count: %d", root->getStaticEntryCount());
00189         if (root->getStaticEntryCount() != nCount) {
00190                 WAVE_EX(wex);
00191                 wex << "Count of entries in tree does not match what was added";
00192         }
00193 
00194         // okay, subdivide
00195         DPRINTF("Subdividing...");
00196         {
00197                 perf::Timer timer("subdivide");
00198                 root->subdivide(kdtree::eStrategy_Naive, subdivide);
00199         }
00200         DPRINTF("Tree depth: %d", root->getTreeDepth());
00201         DPRINTF("Node count: %d", root->getNodeCount());
00202         DPRINTF("Entry count: %d", root->getStaticEntryCount());
00203 
00204         long count = 0;
00205         {
00206                 perf::Timer timer("full iteration");
00207                 root->iterateStaticEntries(bounds, countCallback, (void *) &count);
00208         }
00209         if (count != nCount) {
00210                 WAVE_EX(wex);
00211                 wex << "Iteration should return all entities!";
00212         }
00213 
00214         rect3d_t sub;
00215         getRandomRect(bounds, 1000, sub);
00216         sub.dump("Search rect");
00217         count = 0;
00218         {
00219                 // warning: don't use the print callback here, it is slow
00220                 //      (writing to console)
00221                 perf::Timer timer("sub iteration");
00222                 root->iterateStaticEntries(sub, countCallback, (void *) &count);
00223         }
00224         DPRINTF("Found %ld entries in sub rect", count);
00225 
00226         // all done
00227         return root;
00228 }
00229 
00230 
00231 
00232 static void
00233 walkNearToFar
00234 (
00235 IN const kdtree::Node * node,
00236 IN const point3d_t& start,
00237 IN const point3d_t& end,
00238 IN const point3d_t& n           // normalized direction of walk
00239 )
00240 throw()
00241 {
00242         ASSERT(node, "null");
00243 
00244         // look at the given node and its children (if any)
00245         //  - walk farthest child
00246         //  - walk local spanning objects
00247         //  - walk closest child
00248 
00249         const kdtree::Node * left = node->getLeftChild();
00250         const kdtree::Node * right = node->getRightChild();
00251 
00252         const kdtree::plane_t& plane = node->getSortPlane();
00253         kdtree::eSort sort = kdtree::sortPoint(plane, end);
00254 
00255         // determine farthest, closest nodes
00256         const kdtree::Node * farthest = left;
00257         const kdtree::Node * closest = right;
00258         if (kdtree::eSort_Left == sort) {
00259                 farthest = right;
00260                 closest = left;
00261         }
00262 
00263         // walk closest first
00264         if (closest) {
00265                 walkNearToFar(closest, start, end, n);
00266         }
00267 
00268         // walk straddling objects
00269         const kdtree::Node::vec_entries_t& vec = node->getStaticEntries();
00270         if (vec.size()) {
00271                 DPRINTF("%d entries in this node", (int) vec.size());
00272         }
00273         for (kdtree::Node::vec_entries_t::const_iterator i = vec.begin();
00274              i != vec.end(); ++i) {
00275                 const kdtree::Node::entry_rec_t& er = *i;
00276                 point3d_t mid = er.rect.getMidpoint();
00277                 mid = mid - start;
00278                 float d = fabs(dotProduct(n, mid));
00279                 DPRINTF("Object at distance: %f", d);
00280         }
00281 
00282         // walk farthest
00283         if (farthest) {
00284                 walkNearToFar(farthest, start, end, n);
00285         }
00286 }
00287 
00288 
00289 
00290 ////////////////////////////////////////////////////////////////////////////////
00291 //
00292 //      entry point
00293 //
00294 ////////////////////////////////////////////////////////////////////////////////
00295 
00296 int
00297 main
00298 (
00299 IN int argc,
00300 IN const char * argv[]
00301 )
00302 {
00303         int nCount = 50 * 1000;
00304         int nDivide = 32;
00305         if (argc > 1) {
00306                 nCount = atoi(argv[1]);
00307         } else {
00308                 DPRINTF("No count provided, so using count = %d", nCount);
00309         }
00310         if (argc > 2) {
00311                 nDivide = atoi(argv[2]);
00312         } else {
00313                 DPRINTF("No subdivide threshold specified, using %d", nDivide);
00314         }
00315         int retval = 0;
00316 
00317         try {
00318                 perf::Timer timer("Total test time");
00319 
00320                 smart_ptr<kdtree::Node> root = populateTree(nCount, nDivide);
00321                 ASSERT(root, "failed to create kd-tree");
00322 
00323                 point3d_t start(0, 0, 0);
00324                 point3d_t end(s_max, s_max, s_max);
00325 
00326                 point3d_t n = end - start;
00327                 normalize(n);
00328                 n.dump("Walk direction");
00329 
00330                 walkNearToFar(root, start, end, n);
00331 
00332         } catch (std::exception& e) {
00333                 DPRINTF("Exception: %s", e.what());
00334                 retval = 1;
00335         }
00336 
00337         perf::dumpTimingSummary(std::cerr);
00338 
00339         return retval;
00340 }
00341