00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include <algorithm>
00026 #include <vector>
00027
00028 #include "../common/WAssert.h"
00029 #include "../common/WLogger.h"
00030
00031 #include "WKdTree.h"
00032
00033 WKdTree::WKdTree( int size, float *pointArray ) :
00034 m_size( size ), m_pointArray( pointArray )
00035 {
00036 WAssert( size > 2, "The current kD-tree implementation works only with at least 3 vertices." );
00037 m_tree.clear();
00038 m_tree.resize( size );
00039
00040 wlog::debug( "KdTree" ) << " Start building KdTree";
00041
00042 for( int i = 0; i < size; ++i )
00043 m_tree[i] = i;
00044
00045 int root = ( size - 1 ) / 2;
00046 std::nth_element( m_tree.begin(), m_tree.begin() + root, m_tree.end(), lessy( m_pointArray, 0 ) );
00047
00048 int rootLeft = ( root - 1 ) / 2;
00049 std::nth_element( m_tree.begin(), m_tree.begin() + rootLeft, m_tree.begin() + root - 1, lessy( m_pointArray, 1 ) );
00050
00051 int rootRight = ( size + root ) / 2;
00052 std::nth_element( m_tree.begin() + root + 1, m_tree.begin() + rootRight, m_tree.end(), lessy( m_pointArray, 1 ) );
00053
00054 WKdTreeThread *thread1 = new WKdTreeThread( m_pointArray, &m_tree, 0, rootLeft - 1, 2 );
00055 WKdTreeThread *thread2 = new WKdTreeThread( m_pointArray, &m_tree, rootLeft + 1, root - 1, 2 );
00056 WKdTreeThread *thread3 = new WKdTreeThread( m_pointArray, &m_tree, root + 1, rootRight - 1, 2 );
00057 WKdTreeThread *thread4 = new WKdTreeThread( m_pointArray, &m_tree, rootRight + 1, size - 1, 2 );
00058
00059 wlog::debug( "KdTree" ) << "Start threads";
00060
00061 thread1->run();
00062 thread2->run();
00063 thread3->run();
00064 thread4->run();
00065
00066 wlog::debug( "KdTree" ) << "All threads started";
00067
00068 thread1->wait();
00069 thread2->wait();
00070 thread3->wait();
00071 thread4->wait();
00072
00073 wlog::debug( "KdTree" ) << "All threads finished";
00074
00075 delete thread1;
00076 delete thread2;
00077 delete thread3;
00078 delete thread4;
00079 }
00080
00081 WKdTree::~WKdTree()
00082 {
00083 }
00084
00085 void WKdTree::buildTree( int left, int right, int axis )
00086 {
00087 if( left >= right )
00088 return;
00089
00090 int div = ( left + right ) / 2;
00091 std::nth_element( m_tree.begin() + left, m_tree.begin() + div, m_tree.begin() + right, lessy( m_pointArray, axis ) );
00092
00093 buildTree( left, div - 1, ( axis + 1 ) % 3 );
00094 buildTree( div + 1, right, ( axis + 1 ) % 3 );
00095 }
00096
00097 WKdTreeThread::WKdTreeThread( float* pointArray, std::vector< unsigned int >* tree, int left, int right, int axis ) :
00098 WThreadedRunner(), m_tree( tree ), m_pointArray( pointArray ), m_left( left ), m_right( right ), m_axis( axis )
00099 {
00100 }
00101
00102 void WKdTreeThread::threadMain()
00103 {
00104 buildTree( m_left, m_right, m_axis );
00105 wlog::debug( "KdTree" ) << "thread finished";
00106 }
00107
00108 void WKdTreeThread::buildTree( int left, int right, int axis )
00109 {
00110 if( left >= right )
00111 return;
00112
00113 int div = ( left + right ) / 2;
00114 std::nth_element( m_tree->begin() + left, m_tree->begin() + div, m_tree->begin() + right, lessy( m_pointArray, axis ) );
00115
00116 buildTree( left, div - 1, ( axis + 1 ) % 3 );
00117 buildTree( div + 1, right, ( axis + 1 ) % 3 );
00118 }