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

Revision 8414, 14.8 kB (checked in by robert, 7 years ago)

Added update traversal to run prior to doing kdtree build to make sure that
costs in build osgTerrain databases isn't incurred during the build traversal.

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