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

Revision 8526, 27.0 kB (checked in by robert, 5 years ago)

Implement an experiemental triangle kdtree building support

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#include <osg/TriangleIndexFunctor>
30
31
32#include <osgUtil/IntersectionVisitor>
33#include <osgUtil/LineSegmentIntersector>
34#include <osgUtil/UpdateVisitor>
35
36#include <osgSim/LineOfSight>
37#include <osgSim/HeightAboveTerrain>
38#include <osgSim/ElevationSlice>
39
40#include <iostream>
41
42
43namespace osg
44{
45
46typedef int value_type;
47typedef std::vector< value_type >   Indices;
48
49// #define VERBOSE_OUTPUT
50
51typedef std::pair< value_type, value_type> KDNode;
52typedef std::pair< value_type, value_type> KDLeaf;
53
54struct Triangle
55{
56    Triangle(unsigned int p1, unsigned int p2, unsigned int p3):
57        _p1(p1), _p2(p2), _p3(p3) {}
58       
59    bool operator < (const Triangle& rhs) const
60    {
61        if (_p1<rhs._p1) return true;
62        if (_p1>rhs._p1) return false;
63        if (_p2<rhs._p2) return true;
64        if (_p2>rhs._p2) return false;
65        return _p3<rhs._p3;
66    }
67   
68    unsigned int _p1;
69    unsigned int _p2;
70    unsigned int _p3;   
71};
72
73
74class KDPrimitiveLeaf : public osg::Referenced
75{
76    public:
77   
78      KDPrimitiveLeaf() {}
79     
80      Indices _indices;
81
82    protected:
83     
84      virtual ~KDPrimitiveLeaf() {}   
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   
100        typedef std::vector< unsigned int > AxisStack;
101        typedef std::vector< KDNode >       KDNodeList;
102
103        typedef std::vector< KDLeaf > KDLeafList;
104
105        /// note, leafNum is negative to distinguish from nodeNum
106        int addLeaf(const KDLeaf& leaf) { int num = _kdLeaves.size(); _kdLeaves.push_back(leaf); return -(num+1); }
107
108        int replaceLeaf(int leafNum, const 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            }
129
130            return _kdLeaves[num];
131        }
132       
133
134
135        /// note, leafNum is negative to distinguish from nodeNum
136        int addPrimitiveLeaf(KDPrimitiveLeaf* leaf) { int num = _primitiveList.size(); _primitiveList.push_back(leaf); return -(num+1); }
137
138        int replacePrimitiveLeaf(int leafNum, KDPrimitiveLeaf* leaf)
139        {
140            int num = -leafNum-1;
141           
142            if (num>_primitiveList.size()-1)
143            {
144                osg::notify(osg::NOTICE)<<"Warning: replaceChild("<<leafNum<<", leaf), num = "<<num<<" _primitiveList.size()="<<_primitiveList.size()<<std::endl;
145                return leafNum;
146            }
147           
148            _primitiveList[num] = leaf; return leafNum;
149        }
150
151
152        /// note, leafNum is negative to distinguish from nodeNum
153        KDPrimitiveLeaf* getPrimitiveLeaf(int leafNum)
154        {
155            int num = -leafNum-1;
156            if (num<0 || num>_primitiveList.size()-1)
157            {
158                osg::notify(osg::NOTICE)<<"Warning: getLeaf("<<leafNum<<", num = "<<num<<") _primitiveList.size()="<<_primitiveList.size()<<std::endl;
159            }
160
161            return _primitiveList[num].get();
162        }
163
164        int addNode(const KDNode& node)
165        {
166            int num = _kdNodes.size();
167            _kdNodes.push_back(node);
168            return num;
169        }
170
171        /// note, nodeNum is positive to distinguish from leftNum
172        KDNode& getNode(int nodeNum)
173        {
174            if (nodeNum<0 || nodeNum>_kdNodes.size()-1)
175            {
176                osg::notify(osg::NOTICE)<<"Warning: getNode("<<nodeNum<<") _kdNodes.size()="<<_kdNodes.size()<<std::endl;
177            }
178            return _kdNodes[nodeNum];
179        }
180
181
182        osg::observer_ptr<osg::Geometry>    _geometry;
183
184        osg::BoundingBox                    _bb;
185
186        AxisStack                           _axisStack;
187        KDNodeList                          _kdNodes;
188        KDLeafList                          _kdLeaves;
189
190        osg::ref_ptr<osg::Vec3Array>        _vertices;
191        Indices                             _vertexIndices;
192       
193        typedef std::vector< osg::ref_ptr<KDPrimitiveLeaf> > KDPrimitiveList;
194        KDPrimitiveList                     _primitiveList;
195};
196
197class KDTreeTraverser
198{
199    public:
200   
201   
202        std::ostream& output(unsigned int level)
203        {
204            for(unsigned int i=0; i<level; ++i)
205            {
206                osg::notify(osg::NOTICE)<<"  ";
207            }
208            return osg::notify(osg::NOTICE);
209        }
210   
211
212        void traverse(KDTree& tree, KDLeaf& leaf, unsigned int level)
213        {
214            output(level)<<"leaf("<<level<<") { ";
215
216            unsigned int end = leaf.first+leaf.second;
217            for(unsigned int i=leaf.first; i<end; ++i)
218            {
219                if (i==leaf.first) osg::notify(osg::NOTICE)<<tree._vertexIndices[i];
220                else osg::notify(osg::NOTICE)<<", "<<tree._vertexIndices[i];               
221            }
222
223            osg::notify(osg::NOTICE)<<"}"<<std::endl;;
224           
225        }
226
227        void traverse(KDTree& tree, KDPrimitiveLeaf& leaf, unsigned int level)
228        {
229            output(level)<<"leaf("<<level<<") size()="<<leaf._indices.size()<<"{ ";
230
231            for(unsigned int i=0; i<leaf._indices.size(); ++i)
232            {
233                if (i==0) osg::notify(osg::NOTICE)<<leaf._indices[i];
234                else osg::notify(osg::NOTICE)<<", "<<leaf._indices[i];               
235            }
236
237            osg::notify(osg::NOTICE)<<"}"<<std::endl;;
238           
239        }
240
241        void traverse(KDTree& tree, value_type nodeIndex, unsigned int level)
242        {
243            output(level)<<"traverse("<<nodeIndex<<", "<< level<<") { "<<std::endl;
244           
245            if (nodeIndex>=0)
246            {       
247                KDNode& node = tree._kdNodes[nodeIndex];
248                if (node.first) traverse(tree,node.first,level+1);
249                else output(level+1)<<"empty left child()"<<std::endl;
250               
251                if (node.second) traverse(tree,node.second,level+1);
252                else output(level+1)<<"empty right child()"<<std::endl;
253            }
254            else 
255            {
256                value_type leafIndex = -nodeIndex-1;
257               
258                if (leafIndex<tree._primitiveList.size())
259                {
260                    KDPrimitiveLeaf& leaf = *(tree._primitiveList[leafIndex]);
261                    traverse(tree, leaf, level);
262                }
263                else
264                {
265                    KDLeaf& leaf = tree._kdLeaves[leafIndex];
266                    traverse(tree, leaf, level);
267                }
268
269            }
270
271            output(level)<<"}"<<std::endl;;
272        }
273
274
275        void traverse(KDTree& tree)
276        {
277            osg::notify(osg::NOTICE)<<"traverse(tree)"<<std::endl;
278            if (!tree._kdNodes.empty())
279            {
280                traverse(tree,0,0);
281            }
282            else if (!tree._kdLeaves.empty())
283            {
284                traverse(tree, tree._kdLeaves.front(), 0);
285            }
286        }
287   
288};
289
290class KDTreeBuilder : public osg::NodeVisitor
291{
292    public:
293   
294        KDTreeBuilder():
295            osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN),
296            _maxNumLevels(16),
297            _targetNumVerticesPerLeaf(8),
298            _targetNumIndicesPerLeaf(15),
299            _numVerticesProcessed(0),
300            _processTriangles(true)
301        {           
302        }
303   
304   
305        void apply(osg::Geode& geode)
306        {
307            for(unsigned int i=0; i<geode.getNumDrawables(); ++i)
308            {
309                osg::Geometry* geom = geode.getDrawable(i)->asGeometry();
310                if (geom)
311                {
312                    geom->setShape(createKDTree(geom));
313                }   
314            }
315        }
316   
317        KDTree* createKDTree(osg::Geometry* geometry);
318       
319        void computeDivisions(KDTree& kdTree);
320       
321        int divideTriangles(KDTree& kdTree, osg::BoundingBox& bb, int nodeIndex, unsigned int level);
322        int dividePoints(KDTree& kdTree, osg::BoundingBox& bb, int nodeIndex, unsigned int level);
323
324        unsigned int _maxNumLevels;
325        unsigned int _targetNumVerticesPerLeaf;
326        unsigned int _targetNumIndicesPerLeaf;
327       
328        unsigned int _numVerticesProcessed;
329        bool         _processTriangles;
330       
331        inline void disposeKDPrimitiveLeaf(KDPrimitiveLeaf* leaf)
332        {
333            leaf->_indices.clear();
334            _leafRecycleList.push_back(leaf);
335        }
336       
337        inline KDPrimitiveLeaf* createKDPrimitiveLeaf()
338        {
339            if (_leafRecycleList.empty()) return new KDPrimitiveLeaf;
340            osg::ref_ptr<KDPrimitiveLeaf> leaf = _leafRecycleList.back();
341            _leafRecycleList.pop_back();
342            return leaf.release();
343        }
344       
345        typedef std::list< osg::ref_ptr<KDPrimitiveLeaf> > LeafRecyleList;
346        LeafRecyleList _leafRecycleList;
347
348};
349
350struct TriangleIndicesCollector
351{
352    TriangleIndicesCollector():
353        _indices(0)
354    {
355    }
356
357    inline void operator () (unsigned int v1, unsigned int v2, unsigned int v3)
358    {
359        _indices->push_back(v1);
360        _indices->push_back(v2);
361        _indices->push_back(v3);
362    }
363   
364    Indices* _indices;
365
366};
367
368KDTree* KDTreeBuilder::createKDTree(osg::Geometry* geometry)
369{
370#ifdef VERBOSE_OUTPUT   
371    osg::notify(osg::NOTICE)<<"osg::KDTreeBuilder::createKDTree()"<<std::endl;
372#endif
373
374    osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(geometry->getVertexArray());
375    if (!vertices) return 0;
376   
377    if (vertices->size() <= _targetNumVerticesPerLeaf) return 0;
378
379    osg::ref_ptr<KDTree> kdTree = new KDTree;
380    kdTree->_geometry = geometry;
381    kdTree->_bb = kdTree->_geometry->getBound();
382    kdTree->_vertices = vertices;
383   
384    unsigned int estimatedSize = (unsigned int)(2.0*float(vertices->size())/float(_targetNumVerticesPerLeaf));
385
386#ifdef VERBOSE_OUTPUT   
387    osg::notify(osg::NOTICE)<<"kdTree->_kdNodes.reserve()="<<estimatedSize<<std::endl<<std::endl;
388#endif
389
390    kdTree->_kdNodes.reserve(estimatedSize);
391    kdTree->_kdLeaves.reserve(estimatedSize);
392   
393    computeDivisions(*kdTree);
394
395
396    _numVerticesProcessed += vertices->size();
397
398    if (_processTriangles)
399    {
400
401        osg::TriangleIndexFunctor<TriangleIndicesCollector> collectTriangleIndices;
402        collectTriangleIndices._indices = &(kdTree->_vertexIndices);
403        geometry->accept(collectTriangleIndices);
404
405        KDPrimitiveLeaf* primitiveLeaf = createKDPrimitiveLeaf();
406        primitiveLeaf->_indices.insert(primitiveLeaf->_indices.end(),
407                                       kdTree->_vertexIndices.begin(),
408                                       kdTree->_vertexIndices.end());
409
410        // osg::notify(osg::NOTICE)<<"kdTree->_vertexIndices.size()="<<kdTree->_vertexIndices.size()<<std::endl;
411        int leafNum = kdTree->addPrimitiveLeaf(primitiveLeaf);
412
413        osg::BoundingBox bb = kdTree->_bb;
414        int nodeNum = divideTriangles(*kdTree, bb, leafNum, 0);
415   
416        std::cout<<"_vertexIndices.size() = "<<kdTree->_vertexIndices.size()<<std::endl;
417        std::cout<<"_primtiveLeaf.size() = "<<kdTree->_primitiveList.size()<<std::endl;
418        unsigned int numIndices = 0;
419        for(KDTree::KDPrimitiveList::iterator itr = kdTree->_primitiveList.begin();
420            itr != kdTree->_primitiveList.end();
421            ++itr)
422        {
423            numIndices += (*itr)->_indices.size();
424        }
425        std::cout<<"total numIndices = "<<numIndices<<std::endl;
426
427        typedef std::map<Triangle, unsigned int> TriangleMap;
428        TriangleMap triangleMap;
429        for(KDTree::KDPrimitiveList::iterator itr = kdTree->_primitiveList.begin();
430            itr != kdTree->_primitiveList.end();
431            ++itr)
432        {
433            Indices& indices = (*itr)->_indices;
434            for(unsigned int i=0; i<indices.size(); i+=3)
435            {
436                ++triangleMap[Triangle(indices[i],indices[i+1], indices[i+2])];
437            }
438        }
439
440        int totalNumTriangles = triangleMap.size();
441        int totalNumReferences = 0;
442        for(TriangleMap::iterator tm_itr = triangleMap.begin();
443            tm_itr != triangleMap.end();
444            ++tm_itr)
445        {   
446            totalNumReferences += tm_itr->second;
447        }
448        std::cout<<"Average number of references = "<<double(totalNumReferences)/double(totalNumTriangles)<<std::endl;
449
450    }
451    else
452    {
453        kdTree->_vertexIndices.reserve(vertices->size());
454        for(unsigned int i=0; i<vertices->size(); ++i)
455        {
456            kdTree->_vertexIndices.push_back(i);
457        }
458
459
460
461        KDLeaf leaf(0, kdTree->_vertexIndices.size());
462
463        int leafNum = kdTree->addLeaf(leaf);
464
465        osg::BoundingBox bb = kdTree->_bb;
466        int nodeNum = dividePoints(*kdTree, bb, leafNum, 0);
467
468    #ifdef VERBOSE_OUTPUT   
469        osg::notify(osg::NOTICE)<<"Root nodeNum="<<nodeNum<<std::endl;
470    #endif
471    }
472   
473   
474#ifdef VERBOSE_OUTPUT   
475   
476    KDTreeTraverser traverser;
477    traverser.traverse(*kdTree);
478   
479   
480    osg::notify(osg::NOTICE)<<"Final kdTree->_kdNodes.size()="<<kdTree->_kdNodes.size()<<std::endl;
481    osg::notify(osg::NOTICE)<<"Final kdTree->_kdLeaves.size()="<<kdTree->_kdLeaves.size()<<std::endl;
482
483    osg::notify(osg::NOTICE)<<"osg::KDTreeBuilder::createKDTree() completed"<<std::endl<<std::endl;
484#endif
485
486//    osg::notify(osg::NOTICE)<<"kdTree->_kdNodes.size()="<<kdTree->_kdNodes.size()<<"  estimated size = "<<estimatedSize<<std::endl;
487//    osg::notify(osg::NOTICE)<<"kdTree->_kdLeaves.size()="<<kdTree->_kdLeaves.size()<<"  estimated size = "<<estimatedSize<<std::endl<<std::endl;
488
489
490
491    return kdTree.release();
492}   
493
494void KDTreeBuilder::computeDivisions(KDTree& kdTree)
495{
496
497
498    osg::Vec3 dimensions(kdTree._bb.xMax()-kdTree._bb.xMin(),
499                         kdTree._bb.yMax()-kdTree._bb.yMin(),
500                         kdTree._bb.zMax()-kdTree._bb.zMin());
501
502#ifdef VERBOSE_OUTPUT   
503    osg::notify(osg::NOTICE)<<"computeDivisions("<<_maxNumLevels<<") "<<dimensions<< " { "<<std::endl;
504#endif
505
506    kdTree._axisStack.reserve(_maxNumLevels);
507 
508    int level = 0;
509    for(unsigned int level=0; level<_maxNumLevels; ++level)
510    {
511        int axis = 0;
512        if (dimensions[0]>=dimensions[1])
513        {
514            if (dimensions[0]>=dimensions[2]) axis = 0;
515            else axis = 2;
516        }
517        else if (dimensions[1]>=dimensions[2]) axis = 1;
518        else axis = 2;
519
520        kdTree._axisStack.push_back(axis);
521        dimensions[axis] /= 2.0f;
522
523#ifdef VERBOSE_OUTPUT   
524        osg::notify(osg::NOTICE)<<"  "<<level<<", "<<dimensions<<", "<<axis<<std::endl;
525#endif
526    }
527
528#ifdef VERBOSE_OUTPUT   
529    osg::notify(osg::NOTICE)<<"}"<<std::endl;
530#endif
531}
532
533int KDTreeBuilder::divideTriangles(KDTree& kdTree, osg::BoundingBox& bb, int nodeIndex, unsigned int level)
534{
535    if (kdTree._axisStack.size()<=level) return nodeIndex;
536
537
538    int axis = kdTree._axisStack[level];
539
540#ifdef VERBOSE_OUTPUT   
541    osg::notify(osg::NOTICE)<<"divideTriangles("<<nodeIndex<<", "<<level<< "), axis="<<axis<<std::endl;
542#endif
543
544    if (nodeIndex>=0)
545    {
546#ifdef VERBOSE_OUTPUT   
547        osg::notify(osg::NOTICE)<<"  divide node"<<std::endl;
548#endif
549        KDNode& node = kdTree.getNode(nodeIndex);
550        return nodeIndex;
551    }
552    else
553    {   
554
555        if (kdTree.getPrimitiveLeaf(nodeIndex)->_indices.size()<=_targetNumIndicesPerLeaf) return nodeIndex;
556
557#ifdef VERBOSE_OUTPUT   
558        osg::notify(osg::NOTICE)<<"  divide leaf"<<std::endl;
559#endif
560       
561        int nodeNum = kdTree.addNode(KDNode());
562
563        float original_min = bb._min[axis];
564        float original_max = bb._max[axis];
565
566        float mid = (original_min+original_max)*0.5f;
567
568        {
569            KDPrimitiveLeaf* leaf = kdTree.getPrimitiveLeaf(nodeIndex);
570
571            osg::Vec3Array* vertices = kdTree._vertices.get();
572            Indices& indices = leaf->_indices;
573
574#ifdef VERBOSE_OUTPUT   
575            osg::notify(osg::NOTICE)<<"  divide leaf->_indices.size()="<<leaf->_indices.size()<<std::endl;
576#endif
577
578            osg::ref_ptr<KDPrimitiveLeaf> leftLeaf = createKDPrimitiveLeaf();
579            osg::ref_ptr<KDPrimitiveLeaf> rightLeaf = createKDPrimitiveLeaf();
580
581            for(int i = 0; i<indices.size(); )
582            {
583                int numLeft = 0;
584                int numRight = 0;
585
586                int i1 = indices[i++];
587                if ((*vertices)[i1][axis]<=mid) ++numLeft;
588                else ++numRight;
589
590                int i2 = indices[i++];
591                if ((*vertices)[i2][axis]<=mid) ++numLeft;
592                else ++numRight;
593
594                int i3 = indices[i++];
595                if ((*vertices)[i3][axis]<=mid) ++numLeft;
596                else ++numRight;
597               
598                if (numLeft>0)
599                {
600                    leftLeaf->_indices.push_back(i1);                    leftLeaf->_indices.push_back(i2);
601                    leftLeaf->_indices.push_back(i3);
602                }
603                               
604                if (numRight>0)
605                {
606                    rightLeaf->_indices.push_back(i1);
607                    rightLeaf->_indices.push_back(i2);
608                    rightLeaf->_indices.push_back(i3);
609                }
610#if 0               
611                if (numRight>0 && numLeft>0)
612                {
613                    std::cout<<"In both"<<std::endl;
614                }
615                else
616                {
617                    std::cout<<"In one"<<std::endl;
618                }
619#endif
620            }
621
622            disposeKDPrimitiveLeaf(leaf);
623           
624 
625            if (leftLeaf->_indices.empty())
626            {
627#ifdef VERBOSE_OUTPUT   
628                osg::notify(osg::NOTICE)<<"LeftLeaf empty"<<std::endl;
629#endif
630                kdTree.getNode(nodeNum).first = 0;
631                kdTree.getNode(nodeNum).second = kdTree.replacePrimitiveLeaf(nodeIndex, rightLeaf.get());
632            }
633            else if (rightLeaf->_indices.empty())
634            {
635#ifdef VERBOSE_OUTPUT   
636                osg::notify(osg::NOTICE)<<"RightLeaf empty"<<std::endl;
637#endif
638                kdTree.getNode(nodeNum).first = kdTree.replacePrimitiveLeaf(nodeIndex, leftLeaf.get());
639                kdTree.getNode(nodeNum).second = 0;
640            }
641            else
642            {
643                kdTree.getNode(nodeNum).first = kdTree.replacePrimitiveLeaf(nodeIndex, leftLeaf.get());
644                kdTree.getNode(nodeNum).second = kdTree.addPrimitiveLeaf(rightLeaf.get());
645            }
646        }
647       
648
649       
650        int originalLeftChildIndex = kdTree.getNode(nodeNum).first;
651        int originalRightChildIndex = kdTree.getNode(nodeNum).second;
652
653
654        float restore = bb._max[axis];
655        bb._max[axis] = mid;
656
657        //osg::notify(osg::NOTICE)<<"  divide leftLeaf "<<kdTree.getNode(nodeNum).first<<std::endl;
658        int leftChildIndex = divideTriangles(kdTree, bb, originalLeftChildIndex, level+1);
659
660        bb._max[axis] = restore;
661       
662        restore = bb._min[axis];
663        bb._min[axis] = mid;
664
665        //osg::notify(osg::NOTICE)<<"  divide rightLeaf "<<kdTree.getNode(nodeNum).second<<std::endl;
666        int rightChildIndex = divideTriangles(kdTree, bb, originalRightChildIndex, level+1);
667       
668        bb._min[axis] = restore;
669       
670        kdTree.getNode(nodeNum).first = leftChildIndex;
671        kdTree.getNode(nodeNum).second = rightChildIndex;
672       
673        return nodeNum;       
674    }
675
676
677}
678
679
680
681int KDTreeBuilder::dividePoints(KDTree& kdTree, osg::BoundingBox& bb, int nodeIndex, unsigned int level)
682{
683    if (kdTree._axisStack.size()<=level) return nodeIndex;
684
685
686    int axis = kdTree._axisStack[level];
687
688#ifdef VERBOSE_OUTPUT   
689    //osg::notify(osg::NOTICE)<<"divide("<<nodeIndex<<", "<<level<< "), axis="<<axis<<std::endl;
690#endif
691
692    if (nodeIndex>=0)
693    {
694#ifdef VERBOSE_OUTPUT   
695        osg::notify(osg::NOTICE)<<"  divide node"<<std::endl;
696#endif
697        KDNode& node = kdTree.getNode(nodeIndex);
698        return nodeIndex;
699    }
700    else
701    {   
702
703        if (kdTree.getLeaf(nodeIndex).second<=_targetNumVerticesPerLeaf) return nodeIndex;
704
705        //osg::notify(osg::NOTICE)<<"  divide leaf"<<std::endl;
706       
707        int nodeNum = kdTree.addNode(KDNode());
708
709        float original_min = bb._min[axis];
710        float original_max = bb._max[axis];
711
712        float mid = (original_min+original_max)*0.5f;
713
714        {
715            KDLeaf& leaf = kdTree.getLeaf(nodeIndex);
716
717            osg::Vec3Array* vertices = kdTree._vertices.get();
718
719            //osg::notify(osg::NOTICE)<<"  divide leaf->_vertexIndices.size()="<<leaf->_vertexIndices.size()<<std::endl;
720
721            int end = leaf.first+leaf.second-1;
722            int left = leaf.first;
723            int right = leaf.first+leaf.second-1;
724           
725            while(left<right)
726            {
727                while(left<right && ((*vertices)[kdTree._vertexIndices[left]])[axis]<=mid) { ++left; }
728               
729                while(left<right && ((*vertices)[kdTree._vertexIndices[right]])[axis]>mid) { --right; }
730
731                if (left<right)
732                {
733                    std::swap(kdTree._vertexIndices[left], kdTree._vertexIndices[right]);
734                    ++left;
735                    --right;
736                }
737            }
738           
739            if (left==right)
740            {
741                if (((*vertices)[kdTree._vertexIndices[left]])[axis]<=mid) ++left;
742                else --right;
743            }
744           
745            KDLeaf leftLeaf(leaf.first, (right-leaf.first)+1);
746            KDLeaf rightLeaf(left, (end-left)+1);
747
748#if 0           
749            osg::notify(osg::NOTICE)<<"In  leaf.first     ="<<leaf.first     <<" leaf.second     ="<<leaf.second<<std::endl;
750            osg::notify(osg::NOTICE)<<"    leftLeaf.first ="<<leftLeaf.first <<" leftLeaf.second ="<<leftLeaf.second<<std::endl;
751            osg::notify(osg::NOTICE)<<"    rightLeaf.first="<<rightLeaf.first<<" rightLeaf.second="<<rightLeaf.second<<std::endl;
752            osg::notify(osg::NOTICE)<<"    left="<<left<<" right="<<right<<std::endl;
753
754            if (leaf.second != (leftLeaf.second +rightLeaf.second))
755            {
756                osg::notify(osg::NOTICE)<<"*** Error in size, leaf.second="<<leaf.second
757                                        <<", leftLeaf.second="<<leftLeaf.second
758                                        <<", rightLeaf.second="<<rightLeaf.second<<std::endl;
759            }
760            else
761            {
762                osg::notify(osg::NOTICE)<<"Size OK, leaf.second="<<leaf.second
763                                        <<", leftLeaf.second="<<leftLeaf.second
764                                        <<", rightLeaf.second="<<rightLeaf.second<<std::endl;
765            }
766#endif
767            if (leftLeaf.second<=0)
768            {
769                //osg::notify(osg::NOTICE)<<"LeftLeaf empty"<<std::endl;
770                kdTree.getNode(nodeNum).first = 0;
771                kdTree.getNode(nodeNum).second = kdTree.replaceLeaf(nodeIndex, rightLeaf);
772            }
773            else if (rightLeaf.second<=0)
774            {
775                //osg::notify(osg::NOTICE)<<"RightLeaf empty"<<std::endl;
776                kdTree.getNode(nodeNum).first = kdTree.replaceLeaf(nodeIndex, leftLeaf);
777                kdTree.getNode(nodeNum).second = 0;
778            }
779            else
780            {
781                kdTree.getNode(nodeNum).first = kdTree.replaceLeaf(nodeIndex, leftLeaf);
782                kdTree.getNode(nodeNum).second = kdTree.addLeaf(rightLeaf);
783            }
784        }
785
786       
787        int originalLeftChildIndex = kdTree.getNode(nodeNum).first;
788        int originalRightChildIndex = kdTree.getNode(nodeNum).second;
789
790
791        float restore = bb._max[axis];
792        bb._max[axis] = mid;
793
794        //osg::notify(osg::NOTICE)<<"  divide leftLeaf "<<kdTree.getNode(nodeNum).first<<std::endl;
795        int leftChildIndex = dividePoints(kdTree, bb, originalLeftChildIndex, level+1);
796
797        bb._max[axis] = restore;
798       
799        restore = bb._min[axis];
800        bb._min[axis] = mid;
801
802        //osg::notify(osg::NOTICE)<<"  divide rightLeaf "<<kdTree.getNode(nodeNum).second<<std::endl;
803        int rightChildIndex = dividePoints(kdTree, bb, originalRightChildIndex, level+1);
804       
805        bb._min[axis] = restore;
806       
807        kdTree.getNode(nodeNum).first = leftChildIndex;
808        kdTree.getNode(nodeNum).second = rightChildIndex;
809       
810        return nodeNum;       
811    }
812
813
814}
815
816}
817
818int main(int argc, char **argv)
819{
820    // use an ArgumentParser object to manage the program arguments.
821    osg::ArgumentParser arguments(&argc,argv);
822   
823    osg::KDTreeBuilder builder;
824   
825    while (arguments.read("--max",builder._maxNumLevels)) {}
826    while (arguments.read("--leaf",builder._targetNumIndicesPerLeaf)) {}
827    while (arguments.read("--points")) builder._processTriangles = false;
828    while (arguments.read("--triangles")) builder._processTriangles = true;
829
830    osg::ref_ptr<osg::Node> scene = osgDB::readNodeFiles(arguments);
831   
832    if (!scene)
833    {
834        std::cout<<"No model loaded, please specify a valid model on the command line."<<std::endl;
835        return 0;
836    }
837
838
839    osgUtil::UpdateVisitor updateVisitor;
840    updateVisitor.setFrameStamp(new osg::FrameStamp);
841    scene->accept(updateVisitor);
842    scene->getBound();
843
844
845    osg::Timer_t start = osg::Timer::instance()->tick();
846   
847   
848    scene->accept(builder);
849   
850    osg::Timer_t end = osg::Timer::instance()->tick();
851    double time = osg::Timer::instance()->delta_s(start,end);
852    osg::notify(osg::NOTICE)<<"Time to build "<<time*1000.0<<"ms "<<builder._numVerticesProcessed<<std::endl;
853    osg::notify(osg::NOTICE)<<"build speed "<<(double(builder._numVerticesProcessed)/time)/1000000.0<<"M vertices per second"<<std::endl;
854   
855   
856    return 0;
857}
Note: See TracBrowser for help on using the browser.