root/OpenSceneGraph/trunk/examples/osgkdtree/osgkdtree.cpp @ 8411

Revision 8411, 14.7 kB (checked in by robert, 6 years ago)

Added timing code

Line 
1/* OpenSceneGraph example, osgintersection.
2*
3*  Permission is hereby granted, free of charge, to any person obtaining a copy
4*  of this software and associated documentation files (the "Software"), to deal
5*  in the Software without restriction, including without limitation the rights
6*  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7*  copies of the Software, and to permit persons to whom the Software is
8*  furnished to do so, subject to the following conditions:
9*
10*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
11*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
12*  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
13*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
14*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
15*  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
16*  THE SOFTWARE.
17*/
18
19 
20#include <osgDB/ReadFile>
21
22#define _GLIBCXX_DEBUG
23
24#include <osg/ArgumentParser>
25#include <osg/ApplicationUsage>
26#include <osg/Timer>
27#include <osg/CoordinateSystemNode>
28#include <osg/Notify>
29#include <osg/io_utils>
30#include <osg/Geometry>
31
32
33#include <osgUtil/IntersectionVisitor>
34#include <osgUtil/LineSegmentIntersector>
35
36#include <osgSim/LineOfSight>
37#include <osgSim/HeightAboveTerrain>
38#include <osgSim/ElevationSlice>
39
40#include <iostream>
41
42
43namespace osg
44{
45
46class KDNode
47{
48    public:
49   
50        KDNode():
51            _leftChild(0),
52            _rightChild(0) {}
53   
54        KDNode(const KDNode& rhs):
55            _leftChild(rhs._leftChild),
56            _rightChild(rhs._rightChild) {}
57
58        KDNode& operator = (const KDNode& rhs)
59        {
60            _leftChild = rhs._leftChild;
61            _rightChild = rhs._rightChild;
62            return *this;
63        }
64
65        typedef short value_type;
66
67        value_type _leftChild;
68        value_type _rightChild;
69};
70
71class KDLeaf : public osg::Referenced
72{
73    public:
74   
75        KDLeaf() {}
76   
77        typedef unsigned int index_type;
78        typedef std::vector< index_type > Indices;
79
80        Indices _vertexIndices;       
81   
82    protected:
83   
84        virtual ~KDLeaf() {}
85};
86
87class KDTree : public osg::Shape
88{
89    public:
90   
91   
92        KDTree() {}
93       
94        KDTree(const KDTree& rhs, const CopyOp& copyop=CopyOp::SHALLOW_COPY):
95            Shape(rhs,copyop) {}
96
97        META_Shape(osg, KDTree)
98   
99        typedef std::vector< unsigned int > AxisStack;
100        typedef std::vector< KDNode > KDNodeList;
101        typedef std::vector< osg::ref_ptr<KDLeaf> > KDLeafList;
102       
103       
104        /// note, leafNum is negative to distinguish from nodeNum
105        int addLeaf(KDLeaf* leaf) { int num = _kdLeaves.size(); _kdLeaves.push_back(leaf); return -(num+1); }
106
107        int replaceLeaf(int leafNum, KDLeaf* leaf)
108        {
109            int num = -leafNum-1;
110           
111            if (num>_kdLeaves.size()-1)
112            {
113                osg::notify(osg::NOTICE)<<"Warning: replaceChild("<<leafNum<<", leaf), num = "<<num<<" _kdLeaves.size()="<<_kdLeaves.size()<<std::endl;
114                return leafNum;
115            }
116           
117            _kdLeaves[num] = leaf; return leafNum;
118        }
119
120        /// note, leafNum is negative to distinguish from nodeNum
121        KDLeaf* getLeaf(int leafNum)
122        {
123            int num = -leafNum-1;
124            if (num<0 || num>_kdLeaves.size()-1)
125            {
126                osg::notify(osg::NOTICE)<<"Warning: getLeaf("<<leafNum<<", num = "<<num<<") _kdLeaves.size()="<<_kdLeaves.size()<<std::endl;
127                return 0;
128            }
129
130            return _kdLeaves[num].get();
131        }
132       
133        int addNode(const KDNode& node)
134        {
135            int num = _kdNodes.size();
136            _kdNodes.push_back(node);
137            return num;
138        }
139
140        /// note, nodeNum is positive to distinguish from leftNum
141        KDNode& getNode(int nodeNum)
142        {
143            if (nodeNum<0 || nodeNum>_kdNodes.size()-1)
144            {
145                osg::notify(osg::NOTICE)<<"Warning: getNode("<<nodeNum<<") _kdNodes.size()="<<_kdNodes.size()<<std::endl;
146            }
147            return _kdNodes[nodeNum];
148        }
149
150
151        osg::observer_ptr<osg::Geometry> _geometry;
152
153        osg::BoundingBox                _bb;
154
155        AxisStack                       _axisStack;
156        KDNodeList                      _kdNodes;
157        KDLeafList                      _kdLeaves;
158};
159
160class KDTreeTraverser
161{
162    public:
163   
164   
165        std::ostream& output(unsigned int level)
166        {
167            for(unsigned int i=0; i<level; ++i)
168            {
169                osg::notify(osg::NOTICE)<<"  ";
170            }
171            return osg::notify(osg::NOTICE);
172        }
173   
174
175        void traverse(KDLeaf& leaf, unsigned int level)
176        {
177            output(level)<<"leaf("<<level<<") { ";
178
179            for(unsigned int i=0; i<leaf._vertexIndices.size(); ++i)
180            {
181                if (i==0) osg::notify(osg::NOTICE)<<leaf._vertexIndices[i];
182                else osg::notify(osg::NOTICE)<<", "<<leaf._vertexIndices[i];               
183            }
184
185            osg::notify(osg::NOTICE)<<"}"<<std::endl;;
186           
187        }
188
189        void traverse(KDTree& tree, KDNode::value_type nodeIndex, unsigned int level)
190        {
191            output(level)<<"traverse("<<nodeIndex<<", "<< level<<") { "<<std::endl;
192           
193            if (nodeIndex>=0)
194            {       
195                KDNode& node = tree._kdNodes[nodeIndex];
196                if (node._leftChild) traverse(tree,node._leftChild,level+1);
197                else output(level+1)<<"empty left child()"<<std::endl;
198               
199                if (node._rightChild) traverse(tree,node._rightChild,level+1);
200                else output(level+1)<<"empty right child()"<<std::endl;
201            }
202            else 
203            {
204                KDNode::value_type leafIndex = -nodeIndex-1;
205                KDLeaf& leaf = *(tree._kdLeaves[leafIndex]);
206                traverse(leaf, level);
207            }
208
209            output(level)<<"}"<<std::endl;;
210        }
211
212
213        void traverse(KDTree& tree)
214        {
215            osg::notify(osg::NOTICE)<<"traverse(tree)"<<std::endl;
216            if (!tree._kdNodes.empty())
217            {
218                traverse(tree,0,0);
219            }
220            else if (!tree._kdLeaves.empty())
221            {
222                traverse(*tree._kdLeaves.front(), 0);
223            }
224        }
225   
226};
227
228
229class KDTreeBuilder : public osg::NodeVisitor
230{
231    public:
232   
233        KDTreeBuilder():
234            osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN),
235            _maxNumLevels(24),
236            _targetNumVerticesPerLeaf(8),
237            _numVerticesProcessed(0)
238        {           
239        }
240   
241   
242        void apply(osg::Geode& geode)
243        {
244            for(unsigned int i=0; i<geode.getNumDrawables(); ++i)
245            {
246                osg::Geometry* geom = geode.getDrawable(i)->asGeometry();
247                if (geom)
248                {
249                    geom->setShape(createKDTree(geom));
250                }   
251            }
252        }
253   
254        KDTree* createKDTree(osg::Geometry* geometry);
255       
256        void computeDivisions(KDTree& kdTree);
257       
258        int divide(KDTree& kdTree, osg::BoundingBox& bb, int nodeIndex, unsigned int level);
259
260        unsigned int _maxNumLevels;
261        unsigned int _targetNumVerticesPerLeaf;
262       
263        unsigned int _numVerticesProcessed;   
264
265};
266
267
268KDTree* KDTreeBuilder::createKDTree(osg::Geometry* geometry)
269{
270#if VERBOSE_OUTPUT   
271    osg::notify(osg::NOTICE)<<"osg::KDTreeBuilder::createKDTree()"<<std::endl;
272#endif
273
274    osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(geometry->getVertexArray());
275    if (!vertices) return 0;
276
277    osg::ref_ptr<KDTree> kdTree = new KDTree;
278    kdTree->_geometry = geometry;
279    kdTree->_bb = kdTree->_geometry->getBound();
280   
281   
282    unsigned int estimatedSize = (unsigned int)(float(vertices->size())/float(_targetNumVerticesPerLeaf)*1.5);
283
284#if VERBOSE_OUTPUT   
285    osg::notify(osg::NOTICE)<<"kdTree->_kdNodes.reserve()="<<estimatedSize<<std::endl<<std::endl;
286#endif
287
288    kdTree->_kdNodes.reserve(estimatedSize);
289    kdTree->_kdLeaves.reserve(estimatedSize);
290   
291    computeDivisions(*kdTree);
292
293
294    _numVerticesProcessed += vertices->size();
295
296    // create initial leaf list   
297    KDLeaf* leaf = new KDLeaf;
298    leaf->_vertexIndices.reserve(vertices->size());
299    for(unsigned int i=0; i<vertices->size(); ++i)
300    {
301        leaf->_vertexIndices.push_back(i);
302    }
303
304    osg::BoundingBox bb = kdTree->_bb;
305   
306    int leafNum = kdTree->addLeaf(leaf);
307    int nodeNum = divide(*kdTree, bb, leafNum, 0);
308   
309#if VERBOSE_OUTPUT   
310    osg::notify(osg::NOTICE)<<"Root nodeNum="<<nodeNum<<std::endl;
311#endif
312   
313#if VERBOSE_OUTPUT   
314   
315    KDTreeTraverser traverser;
316    traverser.traverse(*kdTree);
317   
318    osg::notify(osg::NOTICE)<<"Final kdTree->_kdNodes.size()="<<kdTree->_kdNodes.size()<<std::endl;
319    osg::notify(osg::NOTICE)<<"Final kdTree->_kdLeaves.size()="<<kdTree->_kdLeaves.size()<<std::endl;
320
321    osg::notify(osg::NOTICE)<<"osg::KDTreeBuilder::createKDTree() completed"<<std::endl<<std::endl;
322#endif
323
324    return kdTree.release();
325}   
326
327void KDTreeBuilder::computeDivisions(KDTree& kdTree)
328{
329
330
331    osg::Vec3 dimensions(kdTree._bb.xMax()-kdTree._bb.xMin(),
332                         kdTree._bb.yMax()-kdTree._bb.yMin(),
333                         kdTree._bb.zMax()-kdTree._bb.zMin());
334
335#if VERBOSE_OUTPUT   
336    osg::notify(osg::NOTICE)<<"computeDivisions("<<_maxNumLevels<<") "<<dimensions<< " { "<<std::endl;
337#endif
338
339    kdTree._axisStack.reserve(_maxNumLevels);
340 
341    int level = 0;
342    for(unsigned int level=0; level<_maxNumLevels; ++level)
343    {
344        int axis = 0;
345        if (dimensions[0]>=dimensions[1])
346        {
347            if (dimensions[0]>=dimensions[2]) axis = 0;
348            else axis = 2;
349        }
350        else if (dimensions[1]>=dimensions[2]) axis = 1;
351        else axis = 2;
352
353        kdTree._axisStack.push_back(axis);
354        dimensions[axis] /= 2.0f;
355
356#if VERBOSE_OUTPUT   
357        osg::notify(osg::NOTICE)<<"  "<<level<<", "<<dimensions<<", "<<axis<<std::endl;
358#endif
359    }
360
361#if VERBOSE_OUTPUT   
362    osg::notify(osg::NOTICE)<<"}"<<std::endl;
363#endif
364}
365
366int KDTreeBuilder::divide(KDTree& kdTree, osg::BoundingBox& bb, int nodeIndex, unsigned int level)
367{
368    if (kdTree._axisStack.size()<=level) return nodeIndex;
369
370
371    int axis = kdTree._axisStack[level];
372
373#if VERBOSE_OUTPUT   
374    //osg::notify(osg::NOTICE)<<"divide("<<nodeIndex<<", "<<level<< "), axis="<<axis<<std::endl;
375#endif
376
377    if (nodeIndex>=0)
378    {
379#if VERBOSE_OUTPUT   
380        osg::notify(osg::NOTICE)<<"  divide node"<<std::endl;
381#endif
382        KDNode& node = kdTree.getNode(nodeIndex);
383        return nodeIndex;
384    }
385    else
386    {   
387        if (kdTree.getLeaf(nodeIndex)->_vertexIndices.size()<=_targetNumVerticesPerLeaf) return nodeIndex;
388   
389        //osg::notify(osg::NOTICE)<<"  divide leaf"<<std::endl;
390       
391        int nodeNum = kdTree.addNode(KDNode());
392
393        float original_min = bb._min[axis];
394        float original_max = bb._max[axis];
395
396        float mid = (original_min+original_max)*0.5f;
397
398        {
399            osg::ref_ptr<KDLeaf> leaf = kdTree.getLeaf(nodeIndex);
400
401            // create new node, and add two leaves to it.
402            osg::ref_ptr<KDLeaf> leftLeaf = new KDLeaf;
403            osg::ref_ptr<KDLeaf> rightLeaf = new KDLeaf;
404
405
406            osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(kdTree._geometry->getVertexArray());
407
408            //osg::notify(osg::NOTICE)<<"  divide leaf->_vertexIndices.size()="<<leaf->_vertexIndices.size()<<std::endl;
409
410            unsigned int estimatedSize = leaf->_vertexIndices.size();
411            leftLeaf->_vertexIndices.reserve(estimatedSize);
412            rightLeaf->_vertexIndices.reserve(estimatedSize);
413
414            for(unsigned int i=0; i<leaf->_vertexIndices.size(); ++i)
415            {
416                unsigned int vi = leaf->_vertexIndices[i];
417                osg::Vec3& v = (*vertices)[vi];
418                if (v[axis] <= mid) leftLeaf->_vertexIndices.push_back(vi);
419                else rightLeaf->_vertexIndices.push_back(vi);
420            }
421
422            if (leftLeaf->_vertexIndices.empty())
423            {
424                //osg::notify(osg::NOTICE)<<"LeftLeaf empty"<<std::endl;
425                kdTree.getNode(nodeNum)._leftChild = 0;
426                kdTree.getNode(nodeNum)._rightChild = kdTree.replaceLeaf(nodeIndex, rightLeaf.get());
427            }
428            else if (rightLeaf->_vertexIndices.empty())
429            {
430                //osg::notify(osg::NOTICE)<<"RightLeaf empty"<<std::endl;
431                kdTree.getNode(nodeNum)._leftChild = kdTree.replaceLeaf(nodeIndex, leftLeaf.get());
432                kdTree.getNode(nodeNum)._rightChild = 0;
433            }
434            else
435            {
436                kdTree.getNode(nodeNum)._leftChild = kdTree.replaceLeaf(nodeIndex, leftLeaf.get());
437                kdTree.getNode(nodeNum)._rightChild = kdTree.addLeaf(rightLeaf.get());
438            }
439
440
441        }
442       
443        int originalLeftChildIndex = kdTree.getNode(nodeNum)._leftChild;
444        int originalRightChildIndex = kdTree.getNode(nodeNum)._rightChild;
445
446
447        float restore = bb._max[axis];
448        bb._max[axis] = mid;
449
450        //osg::notify(osg::NOTICE)<<"  divide leftLeaf "<<kdTree.getNode(nodeNum)._leftChild<<std::endl;
451        int leftChildIndex = divide(kdTree, bb, originalLeftChildIndex, level+1);
452
453        bb._max[axis] = restore;
454       
455        restore = bb._min[axis];
456        bb._min[axis] = mid;
457
458        //osg::notify(osg::NOTICE)<<"  divide rightLeaf "<<kdTree.getNode(nodeNum)._rightChild<<std::endl;
459        int rightChildIndex = divide(kdTree, bb, originalRightChildIndex, level+1);
460       
461        bb._min[axis] = restore;
462       
463        kdTree.getNode(nodeNum)._leftChild = leftChildIndex;
464        kdTree.getNode(nodeNum)._rightChild = rightChildIndex;
465       
466        return nodeNum;       
467    }
468
469
470}
471
472}
473
474int main(int argc, char **argv)
475{
476    // use an ArgumentParser object to manage the program arguments.
477    osg::ArgumentParser arguments(&argc,argv);
478   
479    osg::ref_ptr<osg::Node> scene = osgDB::readNodeFiles(arguments);
480   
481    if (!scene)
482    {
483        std::cout<<"No model loaded, please specify a valid model on the command line."<<std::endl;
484        return 0;
485    }
486
487
488    osg::Timer_t start = osg::Timer::instance()->tick();
489   
490    osg::KDTreeBuilder builder;
491    scene->accept(builder);
492   
493    osg::Timer_t end = osg::Timer::instance()->tick();
494    double time = osg::Timer::instance()->delta_s(start,end);
495    osg::notify(osg::NOTICE)<<"Time to build "<<time*1000.0<<"ms "<<builder._numVerticesProcessed<<std::endl;
496    osg::notify(osg::NOTICE)<<"build speed "<<(double(builder._numVerticesProcessed)/time)/1000000.0<<"M vertices per second"<<std::endl;
497   
498   
499    return 0;
500}
Note: See TracBrowser for help on using the browser.