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

Revision 8520, 15.7 kB (checked in by robert, 5 years ago)

Added Vec3Array arrange pointer to avoid dynamic cast

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