Changeset 8557
- Timestamp:
- 07/11/08 18:48:39
- Files:
-
- OpenSceneGraph/trunk/include/osg/KdTree (modified) (7 diffs)
- OpenSceneGraph/trunk/src/osg/KdTree.cpp (modified) (16 diffs)
Legend:
- Unmodified
- Added
- Removed
- Modified
- Copied
- Moved
OpenSceneGraph/trunk/include/osg/KdTree
r8556 r8557 17 17 #include <osg/Shape> 18 18 #include <osg/Geometry> 19 20 #include <map> 19 21 20 22 namespace osg … … 51 53 LineSegmentIntersection(): 52 54 ratio(-1.0), 55 p0(0), 56 p1(0), 57 p2(0), 58 r0(0.0f), 59 r1(0.0f), 60 r2(0.0f), 53 61 primitiveIndex(0) {} 54 62 … … 61 69 osg::Vec3d intersectionPoint; 62 70 osg::Vec3 intersectionNormal; 63 IndexList indexList; 64 RatioList ratioList; 71 72 unsigned int p0; 73 unsigned int p1; 74 unsigned int p2; 75 float r0; 76 float r1; 77 float r2; 78 65 79 unsigned int primitiveIndex; 66 80 }; 67 81 68 82 69 typedef std:: multiset<LineSegmentIntersection> LineSegmentIntersections;83 typedef std::vector<LineSegmentIntersection> LineSegmentIntersections; 70 84 71 85 /** compute the intersection of a line segment and the kdtree, return true if an intersection has been found.*/ … … 73 87 74 88 75 76 89 typedef int value_type; 77 typedef std::vector< value_type > Indices;78 90 79 91 struct KdNode … … 95 107 struct Triangle 96 108 { 97 Triangle(unsigned int p1, unsigned int p2, unsigned int p3): 98 _p1(p1), _p2(p2), _p3(p3) {} 109 Triangle(): 110 p0(0),p1(0),p2(0) {} 111 112 Triangle(unsigned int ip0, unsigned int ip1, unsigned int ip2): 113 p0(ip0), p1(ip1), p2(ip2) {} 99 114 100 115 bool operator < (const Triangle& rhs) const 101 116 { 102 if ( _p1<rhs._p1) return true;103 if ( _p1>rhs._p1) return false;104 if ( _p2<rhs._p2) return true;105 if ( _p2>rhs._p2) return false;106 return _p3<rhs._p3;117 if (p0<rhs.p0) return true; 118 if (p0>rhs.p0) return false; 119 if (p1<rhs.p1) return true; 120 if (p1>rhs.p1) return false; 121 return p2<rhs.p2; 107 122 } 108 123 109 unsigned int _p1;110 unsigned int _p2;111 unsigned int _p3;124 unsigned int p0; 125 unsigned int p1; 126 unsigned int p2; 112 127 }; 113 128 114 typedef std::vector< unsigned int > AxisStack;115 129 typedef std::vector< KdNode > KdNodeList; 130 typedef std::vector< Triangle > TriangleList; 116 131 117 132 int addNode(const KdNode& node) … … 122 137 } 123 138 124 /// note, nodeNum is positive to distinguish from leftNum 125 KdNode& getNode(int nodeNum) 139 KdNode& getNode(int nodeNum) { return _kdNodes[nodeNum]; } 140 const KdNode& getNode(int nodeNum) const { return _kdNodes[nodeNum]; } 141 142 KdNodeList& getNodes() { return _kdNodes; } 143 const KdNodeList& getNodes() const { return _kdNodes; } 144 145 void setVertices(osg::Vec3Array* vertices) { _vertices = vertices; } 146 const osg::Vec3Array* getVertices() const { return _vertices.get(); } 147 148 unsigned int addTriangle(const Triangle& tri) 126 149 { 127 if (nodeNum<0 || nodeNum>static_cast<int>(_kdNodes.size())-1) 128 { 129 osg::notify(osg::NOTICE)<<"Warning: getNode("<<nodeNum<<") _kdNodes.size()="<<_kdNodes.size()<<std::endl; 130 } 131 return _kdNodes[nodeNum]; 150 unsigned int num = _triangles.size(); 151 _triangles.push_back(tri); 152 return num; 132 153 } 133 154 134 /// note, nodeNum is positive to distinguish from leftNum 135 const KdNode& getNode(int nodeNum) const 136 { 137 if (nodeNum<0 || nodeNum>static_cast<int>(_kdNodes.size())-1) 138 { 139 osg::notify(osg::NOTICE)<<"Warning: getNode("<<nodeNum<<") _kdNodes.size()="<<_kdNodes.size()<<std::endl; 140 } 141 return _kdNodes[nodeNum]; 142 } 143 144 osg::BoundingBox& getBoundingBox(int nodeNum) 145 { 146 return _kdNodes[nodeNum].bb; 147 } 155 Triangle& getTriangle(unsigned int i) { return _triangles[i]; } 156 const Triangle& getTriangle(unsigned int i) const { return _triangles[i]; } 157 158 TriangleList& getTriangles() { return _triangles; } 159 const TriangleList& getTriangles() const { return _triangles; } 148 160 149 161 150 void computeDivisions(BuildOptions& options); 151 152 int divide(BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level); 153 154 struct RayData 155 { 156 RayData(const osg::Vec3& s, const osg::Vec3& e): 157 _s(s), 158 _e(e) 159 { 160 _d = e - s; 161 _length = _d.length(); 162 _inverse_length = 1.0f/_length; 163 _d *= _inverse_length; 164 } 165 166 167 osg::Vec3 _s; 168 osg::Vec3 _e; 169 170 osg::Vec3 _d; 171 float _length; 172 float _inverse_length; 173 }; 174 175 bool intersect(const KdNode& node, const RayData& rayData, osg::Vec3 s, osg::Vec3 e, LineSegmentIntersections& intersections) const; 176 bool intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const; 177 178 typedef std::vector< osg::BoundingBox > BoundingBoxList; 179 typedef std::vector< Triangle > TriangleList; 180 typedef std::vector< osg::Vec3 > CenterList; 181 182 osg::observer_ptr<osg::Geometry> _geometry; 183 184 osg::BoundingBox _bb; 185 186 AxisStack _axisStack; 187 KdNodeList _kdNodes; 162 protected: 188 163 189 164 osg::ref_ptr<osg::Vec3Array> _vertices; 190 191 Indices _primitiveIndices; 192 165 KdNodeList _kdNodes; 193 166 TriangleList _triangles; 194 CenterList _centers;195 167 196 168 }; … … 211 183 212 184 osg::ref_ptr<osg::KdTree> _kdTreePrototype; 185 186 213 187 214 188 protected: OpenSceneGraph/trunk/src/osg/KdTree.cpp
r8556 r8557 25 25 //////////////////////////////////////////////////////////////////////////////// 26 26 // 27 // Functor for collecting triangle indices from Geometry 28 29 struct TriangleIndicesCollector 30 { 31 TriangleIndicesCollector(): 32 _kdTree(0) 33 { 34 } 35 36 inline void operator () (unsigned int p1, unsigned int p2, unsigned int p3) 37 { 38 unsigned int i = _kdTree->_triangles.size(); 39 _kdTree->_triangles.push_back(KdTree::Triangle(p1,p2,p3)); 40 41 osg::BoundingBox bb; 42 bb.expandBy((*(_kdTree->_vertices))[p1]); 43 bb.expandBy((*(_kdTree->_vertices))[p2]); 44 bb.expandBy((*(_kdTree->_vertices))[p3]); 45 46 _kdTree->_centers.push_back(bb.center()); 47 _kdTree->_primitiveIndices.push_back(i); 48 49 } 50 51 KdTree* _kdTree; 27 // BuildKdTree Declarartion - class used for building an single KdTree 28 29 struct BuildKdTree 30 { 31 BuildKdTree(KdTree& kdTree): 32 _kdTree(kdTree) {} 33 34 typedef std::vector< osg::Vec3 > CenterList; 35 typedef std::vector< unsigned int > Indices; 36 typedef std::vector< unsigned int > AxisStack; 37 38 bool build(KdTree::BuildOptions& options, osg::Geometry* geometry); 39 40 void computeDivisions(KdTree::BuildOptions& options); 41 42 int divide(KdTree::BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level); 43 44 KdTree& _kdTree; 45 46 osg::BoundingBox _bb; 47 AxisStack _axisStack; 48 Indices _primitiveIndices; 49 CenterList _centers; 52 50 53 51 }; … … 55 53 //////////////////////////////////////////////////////////////////////////////// 56 54 // 57 // KdTree 58 59 KdTree::BuildOptions::BuildOptions(): 60 _numVerticesProcessed(0), 61 _targetNumTrianglesPerLeaf(2), 62 _maxNumLevels(32) 63 { 64 } 55 // Functor for collecting triangle indices from Geometry 56 57 struct TriangleIndicesCollector 58 { 59 TriangleIndicesCollector(): 60 _buildKdTree(0) 61 { 62 } 63 64 inline void operator () (unsigned int p0, unsigned int p1, unsigned int p2) 65 { 66 const osg::Vec3& v0 = (*(_buildKdTree->_kdTree.getVertices()))[p0]; 67 const osg::Vec3& v1 = (*(_buildKdTree->_kdTree.getVertices()))[p1]; 68 const osg::Vec3& v2 = (*(_buildKdTree->_kdTree.getVertices()))[p2]; 69 70 // discard degenerate points 71 if (v0==v1 || v1==v2 || v1==v2) 72 { 73 //osg::notify(osg::NOTICE)<<"Disgarding degenerate triangle"<<std::endl; 74 return; 75 } 76 77 unsigned int i = _buildKdTree->_kdTree.addTriangle(KdTree::Triangle(p0,p1,p2)); 78 79 osg::BoundingBox bb; 80 bb.expandBy(v0); 81 bb.expandBy(v1); 82 bb.expandBy(v2); 83 84 _buildKdTree->_centers.push_back(bb.center()); 85 _buildKdTree->_primitiveIndices.push_back(i); 86 87 } 88 89 BuildKdTree* _buildKdTree; 90 91 }; 92 65 93 66 94 //////////////////////////////////////////////////////////////////////////////// 67 95 // 68 // KdTree 69 70 KdTree::KdTree() 71 { 72 } 73 74 KdTree::KdTree(const KdTree& rhs, const osg::CopyOp& copyop): 75 Shape(rhs) 76 { 77 } 78 79 bool KdTree::build(BuildOptions& options, osg::Geometry* geometry) 80 { 96 // BuildKdTree Implementation 97 98 bool BuildKdTree::build(KdTree::BuildOptions& options, osg::Geometry* geometry) 99 { 100 81 101 #ifdef VERBOSE_OUTPUT 82 osg::notify(osg::NOTICE)<<"osg::KDTreeBuilder::createKDTree()"<<std::endl; 102 osg::notify(osg::NOTICE)<<"osg::KDTreeBuilder::createKDTree()"<<std::endl;146 83 103 #endif 84 104 … … 88 108 if (vertices->size() <= options._targetNumTrianglesPerLeaf) return false; 89 109 90 _geometry = geometry; 91 _bb = _geometry->getBound(); 92 _vertices = vertices; 110 _bb = geometry->getBound(); 111 _kdTree.setVertices(vertices); 93 112 94 113 unsigned int estimatedSize = (unsigned int)(2.0*float(vertices->size())/float(options._targetNumTrianglesPerLeaf)); … … 98 117 #endif 99 118 100 _kd Nodes.reserve(estimatedSize*5);119 _kdTree.getNodes().reserve(estimatedSize*5); 101 120 102 121 computeDivisions(options); … … 106 125 unsigned int estimatedNumTriangles = vertices->size()*2; 107 126 _primitiveIndices.reserve(estimatedNumTriangles); 108 _triangles.reserve(estimatedNumTriangles);109 127 _centers.reserve(estimatedNumTriangles); 110 128 111 129 _kdTree.getTriangles().reserve(estimatedNumTriangles); 112 130 113 131 osg::TriangleIndexFunctor<TriangleIndicesCollector> collectTriangleIndices; 114 collectTriangleIndices._ kdTree = this;132 collectTriangleIndices._buildKdTree = this; 115 133 geometry->accept(collectTriangleIndices); 116 134 117 135 _primitiveIndices.reserve(vertices->size()); 118 136 119 Kd Node node(-1, _primitiveIndices.size());137 KdTree::KdNode node(-1, _primitiveIndices.size()); 120 138 node.bb = _bb; 121 139 122 int nodeNum = addNode(node);140 int nodeNum = _kdTree.addNode(node); 123 141 124 142 osg::BoundingBox bb = _bb; 125 143 nodeNum = divide(options, bb, nodeNum, 0); 126 144 145 // now reorder the triangle list so that it's in order as per the primitiveIndex list. 146 KdTree::TriangleList triangleList(_kdTree.getTriangles().size()); 147 for(unsigned int i=0; i<_primitiveIndices.size(); ++i) 148 { 149 triangleList[i] = _kdTree.getTriangle(_primitiveIndices[i]); 150 } 151 152 _kdTree.getTriangles().swap(triangleList); 153 154 127 155 #ifdef VERBOSE_OUTPUT 128 156 osg::notify(osg::NOTICE)<<"Root nodeNum="<<nodeNum<<std::endl; … … 134 162 135 163 136 return !_kd Nodes.empty();137 } 138 139 void KdTree::computeDivisions(BuildOptions& options)164 return !_kdTree.getNodes().empty(); 165 } 166 167 void BuildKdTree::computeDivisions(KdTree::BuildOptions& options) 140 168 { 141 169 osg::Vec3 dimensions(_bb.xMax()-_bb.xMin(), … … 173 201 } 174 202 175 int KdTree::divide(BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level)176 { 177 Kd Node& node =getNode(nodeIndex);203 int BuildKdTree::divide(KdTree::BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level) 204 { 205 KdTree::KdNode& node = _kdTree.getNode(nodeIndex); 178 206 179 207 bool needToDivide = level < _axisStack.size() && … … 191 219 for(int i=istart; i<=iend; ++i) 192 220 { 193 const Triangle& tri = _triangles[_primitiveIndices[i]]; 194 const osg::Vec3& v1 = (*_vertices)[tri._p1]; 195 const osg::Vec3& v2 = (*_vertices)[tri._p2]; 196 const osg::Vec3& v3 = (*_vertices)[tri._p3]; 221 const KdTree::Triangle& tri = _kdTree.getTriangle(_primitiveIndices[i]); 222 const osg::Vec3& v0 = (*_kdTree.getVertices())[tri.p0]; 223 const osg::Vec3& v1 = (*_kdTree.getVertices())[tri.p1]; 224 const osg::Vec3& v2 = (*_kdTree.getVertices())[tri.p2]; 225 node.bb.expandBy(v0); 197 226 node.bb.expandBy(v1); 198 227 node.bb.expandBy(v2); 199 node.bb.expandBy(v3);200 228 201 229 float epsilon = 1e-6; … … 248 276 int originalLeftChildIndex = 0; 249 277 int originalRightChildIndex = 0; 278 bool insitueDivision = false; 250 279 251 280 { … … 276 305 } 277 306 278 Kd Node leftLeaf(-istart-1, (right-istart)+1);279 Kd Node rightLeaf(-left-1, (iend-left)+1);307 KdTree::KdNode leftLeaf(-istart-1, (right-istart)+1); 308 KdTree::KdNode rightLeaf(-left-1, (iend-left)+1); 280 309 281 310 #if 0 … … 303 332 //osg::notify(osg::NOTICE)<<"LeftLeaf empty"<<std::endl; 304 333 originalLeftChildIndex = 0; 305 originalRightChildIndex = addNode(rightLeaf); 334 //originalRightChildIndex = addNode(rightLeaf); 335 originalRightChildIndex = nodeIndex; 336 insitueDivision = true; 306 337 } 307 338 else if (rightLeaf.second<=0) 308 339 { 309 340 //osg::notify(osg::NOTICE)<<"RightLeaf empty"<<std::endl; 310 originalLeftChildIndex = addNode(leftLeaf); 341 // originalLeftChildIndex = addNode(leftLeaf); 342 originalLeftChildIndex = nodeIndex; 311 343 originalRightChildIndex = 0; 344 insitueDivision = true; 312 345 } 313 346 else 314 347 { 315 originalLeftChildIndex = addNode(leftLeaf);316 originalRightChildIndex = addNode(rightLeaf);348 originalLeftChildIndex = _kdTree.addNode(leftLeaf); 349 originalRightChildIndex = _kdTree.addNode(rightLeaf); 317 350 } 318 351 } … … 335 368 bb._min[axis] = restore; 336 369 337 getNode(nodeIndex).first = leftChildIndex; 338 getNode(nodeIndex).second = rightChildIndex; 339 340 getNode(nodeIndex).bb.init(); 341 if (leftChildIndex!=0) getNode(nodeIndex).bb.expandBy(getBoundingBox(leftChildIndex)); 342 if (rightChildIndex!=0) getNode(nodeIndex).bb.expandBy(getBoundingBox(rightChildIndex)); 343 344 if (!getNode(nodeIndex).bb.valid()) 345 { 346 osg::notify(osg::NOTICE)<<"leftChildIndex="<<leftChildIndex<<" && originalLeftChildIndex="<<originalLeftChildIndex<<std::endl; 347 osg::notify(osg::NOTICE)<<"rightChildIndex="<<rightChildIndex<<" && originalRightChildIndex="<<originalRightChildIndex<<std::endl; 348 349 osg::notify(osg::NOTICE)<<"Invalid BB leftChildIndex="<<leftChildIndex<<", "<<rightChildIndex<<std::endl; 350 osg::notify(osg::NOTICE)<<" bb._min ("<<getNode(nodeIndex).bb._min<<")"<<std::endl; 351 osg::notify(osg::NOTICE)<<" bb._max ("<<getNode(nodeIndex).bb._max<<")"<<std::endl; 352 353 if (leftChildIndex!=0) 354 { 355 osg::notify(osg::NOTICE)<<" getBoundingBox(leftChildIndex) min = "<<getBoundingBox(leftChildIndex)._min<<std::endl; 356 osg::notify(osg::NOTICE)<<" max = "<<getBoundingBox(leftChildIndex)._max<<std::endl; 357 } 358 if (rightChildIndex!=0) 359 { 360 osg::notify(osg::NOTICE)<<" getBoundingBox(rightChildIndex) min = "<<getBoundingBox(rightChildIndex)._min<<std::endl; 361 osg::notify(osg::NOTICE)<<" max = "<<getBoundingBox(rightChildIndex)._max<<std::endl; 362 } 363 } 364 370 371 if (!insitueDivision) 372 { 373 // take a second reference to node we are working on as the std::vector<> resize could 374 // have invalidate the previous node ref. 375 KdTree::KdNode& newNodeRef = _kdTree.getNode(nodeIndex); 376 377 newNodeRef.first = leftChildIndex; 378 newNodeRef.second = rightChildIndex; 379 380 insitueDivision = true; 381 382 newNodeRef.bb.init(); 383 if (leftChildIndex!=0) newNodeRef.bb.expandBy(_kdTree.getNode(leftChildIndex).bb); 384 if (rightChildIndex!=0) newNodeRef.bb.expandBy(_kdTree.getNode(rightChildIndex).bb); 385 386 if (!newNodeRef.bb.valid()) 387 { 388 osg::notify(osg::NOTICE)<<"leftChildIndex="<<leftChildIndex<<" && originalLeftChildIndex="<<originalLeftChildIndex<<std::endl; 389 osg::notify(osg::NOTICE)<<"rightChildIndex="<<rightChildIndex<<" && originalRightChildIndex="<<originalRightChildIndex<<std::endl; 390 391 osg::notify(osg::NOTICE)<<"Invalid BB leftChildIndex="<<leftChildIndex<<", "<<rightChildIndex<<std::endl; 392 osg::notify(osg::NOTICE)<<" bb._min ("<<newNodeRef.bb._min<<")"<<std::endl; 393 osg::notify(osg::NOTICE)<<" bb._max ("<<newNodeRef.bb._max<<")"<<std::endl; 394 395 if (leftChildIndex!=0) 396 { 397 osg::notify(osg::NOTICE)<<" getNode(leftChildIndex).bb min = "<<_kdTree.getNode(leftChildIndex).bb._min<<std::endl; 398 osg::notify(osg::NOTICE)<<" max = "<<_kdTree.getNode(leftChildIndex).bb._max<<std::endl; 399 } 400 if (rightChildIndex!=0) 401 { 402 osg::notify(osg::NOTICE)<<" getNode(rightChildIndex).bb min = "<<_kdTree.getNode(rightChildIndex).bb._min<<std::endl; 403 osg::notify(osg::NOTICE)<<" max = "<<_kdTree.getNode(rightChildIndex).bb._max<<std::endl; 404 } 405 } 406 } 365 407 } 366 408 else … … 373 415 } 374 416 375 bool KdTree::intersect(const KdNode& node, const RayData& rayData, osg::Vec3 ls, osg::Vec3 le, LineSegmentIntersections& intersections) const 376 { 377 //osg::notify(osg::NOTICE)<<" Intersect "<<&node<<std::endl; 378 if (!intersectAndClip(ls, le, node.bb)) return false; 379 380 #if 0 381 { 382 osg::notify(osg::NOTICE)<<"Failed intersectAndClip("<<s<<","<<e<<")"<<std::endl; 383 osg::notify(osg::NOTICE)<<" bb._min ("<<node.bb._min<<")"<<std::endl; 384 osg::notify(osg::NOTICE)<<" bb._max ("<<node.bb._max<<")"<<std::endl; 385 return false; 386 } 387 #endif 388 389 int numIntersectionsBefore = intersections.size(); 390 417 //////////////////////////////////////////////////////////////////////////////// 418 // 419 // IntersectKdTree 420 // 421 struct IntersectKdTree 422 { 423 IntersectKdTree(const osg::Vec3Array& vertices, 424 const KdTree::KdNodeList& nodes, 425 const KdTree::TriangleList& triangles, 426 KdTree::LineSegmentIntersections& intersections, 427 const osg::Vec3& s, const osg::Vec3& e): 428 _vertices(vertices), 429 _kdNodes(nodes), 430 _triangles(triangles), 431 _intersections(intersections), 432 _s(s), 433 _e(e) 434 { 435 _d = e - s; 436 _length = _d.length(); 437 _inverse_length = 1.0f/_length; 438 _d *= _inverse_length; 439 } 440 441 void intersect(const KdTree::KdNode& node, const osg::Vec3& s, const osg::Vec3& e) const; 442 bool intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const; 443 444 const osg::Vec3Array& _vertices; 445 const KdTree::KdNodeList& _kdNodes; 446 const KdTree::TriangleList& _triangles; 447 KdTree::LineSegmentIntersections& _intersections; 448 449 osg::Vec3 _s; 450 osg::Vec3 _e; 451 452 osg::Vec3 _d; 453 float _length; 454 float _inverse_length; 455 }; 456 457 458 void IntersectKdTree::intersect(const KdTree::KdNode& node, const osg::Vec3& ls, const osg::Vec3& le) const 459 { 391 460 if (node.first<0) 392 461 { 393 462 // treat as a leaf 394 463 395 464 //osg::notify(osg::NOTICE)<<"KdTree::intersect("<<&leaf<<")"<<std::endl; 396 465 int istart = -node.first-1; … … 399 468 for(int i=istart; i<iend; ++i) 400 469 { 401 const Triangle& tri = _triangles[_primitiveIndices[i]]; 402 const osg::Vec3& v1 = (*_vertices)[tri._p1]; 403 const osg::Vec3& v2 = (*_vertices)[tri._p2]; 404 const osg::Vec3& v3 = (*_vertices)[tri._p3]; 405 // osg::notify(osg::NOTICE)<<" tri("<<tri._p1<<","<<tri._p2<<","<<tri._p3<<")"<<std::endl; 406 407 if (v1==v2 || v2==v3 || v1==v3) continue; 408 409 osg::Vec3 v12 = v2-v1; 410 osg::Vec3 n12 = v12^rayData._d; 411 float ds12 = (rayData._s-v1)*n12; 412 float d312 = (v3-v1)*n12; 413 if (d312>=0.0f) 414 { 415 if (ds12<0.0f) continue; 416 if (ds12>d312) continue; 417 } 418 else // d312 < 0 419 { 420 if (ds12>0.0f) continue; 421 if (ds12<d312) continue; 422 } 423 424 osg::Vec3 v23 = v3-v2; 425 osg::Vec3 n23 = v23^rayData._d; 426 float ds23 = (rayData._s-v2)*n23; 427 float d123 = (v1-v2)*n23; 428 if (d123>=0.0f) 429 { 430 if (ds23<0.0f) continue; 431 if (ds23>d123) continue; 432 } 433 else // d123 < 0 434 { 435 if (ds23>0.0f) continue; 436 if (ds23<d123) continue; 437 } 438 439 osg::Vec3 v31 = v1-v3; 440 osg::Vec3 n31 = v31^rayData._d; 441 float ds31 = (rayData._s-v3)*n31; 442 float d231 = (v2-v3)*n31; 443 if (d231>=0.0f) 444 { 445 if (ds31<0.0f) continue; 446 if (ds31>d231) continue; 447 } 448 else // d231 < 0 449 { 450 if (ds31>0.0f) continue; 451 if (ds31<d231) continue; 452 } 453 454 455 float r3; 456 if (ds12==0.0f) r3=0.0f; 457 else if (d312!=0.0f) r3 = ds12/d312; 458 else continue; // the triangle and the line must be parallel intersection. 459 460 float r1; 461 if (ds23==0.0f) r1=0.0f; 462 else if (d123!=0.0f) r1 = ds23/d123; 463 else continue; // the triangle and the line must be parallel intersection. 464 465 float r2; 466 if (ds31==0.0f) r2=0.0f; 467 else if (d231!=0.0f) r2 = ds31/d231; 468 else continue; // the triangle and the line must be parallel intersection. 469 470 float total_r = (r1+r2+r3); 471 if (total_r!=1.0f) 472 { 473 if (total_r==0.0f) continue; // the triangle and the line must be parallel intersection. 474 float inv_total_r = 1.0f/total_r; 475 r1 *= inv_total_r; 476 r2 *= inv_total_r; 477 r3 *= inv_total_r; 478 } 479 480 osg::Vec3 in = v1*r1+v2*r2+v3*r3; 481 if (!in.valid()) 482 { 483 osg::notify(osg::WARN)<<"Warning:: Picked up error in TriangleIntersect"<<std::endl; 484 osg::notify(osg::WARN)<<" ("<<v1<<",\t"<<v2<<",\t"<<v3<<")"<<std::endl; 485 osg::notify(osg::WARN)<<" ("<<r1<<",\t"<<r2<<",\t"<<r3<<")"<<std::endl; 470 //const Triangle& tri = _triangles[_primitiveIndices[i]]; 471 const KdTree::Triangle& tri = _triangles[i]; 472 // osg::notify(osg::NOTICE)<<" tri("<<tri.p1<<","<<tri.p2<<","<<tri.p3<<")"<<std::endl; 473 474 const osg::Vec3& v0 = _vertices[tri.p0]; 475 const osg::Vec3& v1 = _vertices[tri.p1]; 476 const osg::Vec3& v2 = _vertices[tri.p2]; 477 478 osg::Vec3 T = _s - v0; 479 osg::Vec3 E2 = v2 - v0; 480 osg::Vec3 E1 = v1 - v0; 481 482 osg::Vec3 P = _d ^ E2; 483 484 float det = P * E1; 485 486 float r,r0,r1,r2; 487 488 const float esplison = 1e-10; 489 if (det>esplison) 490 { 491 float u = (P*T); 492 if (u<0.0 || u>det) continue; 493 494 osg::Vec3 Q = T ^ E1; 495 float v = (Q*_d); 496 if (v<0.0 || v>det) continue; 497 498 if ((u+v)> det) continue; 499 500 float inv_det = 1.0f/det; 501 float t = (Q*E2)*inv_det; 502 if (t<0.0 || t>_length) continue; 503 504 u *= inv_det; 505 v *= inv_det; 506 507 r0 = 1.0f-u-v; 508 r1 = u; 509 r2 = v; 510 r = t * _inverse_length; 511 } 512 else if (det<-esplison) 513 { 514 515 float u = (P*T); 516 if (u>0.0 || u<det) continue; 517 518 osg::Vec3 Q = T ^ E1; 519 float v = (Q*_d); 520 if (v>0.0 || v<det) continue; 521 522 if ((u+v) < det) continue; 523 524 float inv_det = 1.0f/det; 525 float t = (Q*E2)*inv_det; 526 if (t<0.0 || t>_length) continue; 527 528 u *= inv_det; 529 v *= inv_det; 530 531 r0 = 1.0f-u-v; 532 r1 = u; 533 r2 = v; 534 r = t * _inverse_length; 535 } 536 else 537 { 486 538 continue; 487 539 } 488 540 489 float d = (in-rayData._s)*rayData._d; 490 491 if (d<0.0f) continue; 492 if (d>rayData._length) continue; 493 494 osg::Vec3 normal = v12^v23; 541 osg::Vec3 in = v0*r0 + v1*r1 + v2*r2; 542 osg::Vec3 normal = E1^E2; 495 543 normal.normalize(); 496 497 float r = d* rayData._inverse_length; 498 499 LineSegmentIntersection intersection; 544 545 #if 1 546 _intersections.push_back(KdTree::LineSegmentIntersection()); 547 KdTree::LineSegmentIntersection& intersection = _intersections.back(); 548 500 549 intersection.ratio = r; 501 intersection.primitiveIndex = _primitiveIndices[i];550 intersection.primitiveIndex = i; 502 551 intersection.intersectionPoint = in; 503 552 intersection.intersectionNormal = normal; 504 553 505 intersection.indexList.push_back(tri._p1); 506 intersection.indexList.push_back(tri._p2); 507 intersection.indexList.push_back(tri._p3); 508 509 intersection.ratioList.push_back(r1); 510 intersection.ratioList.push_back(r2); 511 intersection.ratioList.push_back(r3); 512 513 intersections.insert(intersection); 514 554 intersection.p0 = tri.p0; 555 intersection.p1 = tri.p1; 556 intersection.p2 = tri.p2; 557 intersection.r0 = r0; 558 intersection.r1 = r1; 559 intersection.r2 = r1; 560 561 #endif 515 562 // osg::notify(osg::NOTICE)<<" got intersection ("<<in<<") ratio="<<r<<std::endl; 516 563 } … … 518 565 else 519 566 { 520 if (node.first>0) intersect(getNode(node.first), rayData, ls, le, intersections); 521 if (node.second>0) intersect(getNode(node.second), rayData, ls, le, intersections); 522 } 523 524 return numIntersectionsBefore != intersections.size(); 525 } 526 527 bool KdTree::intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const 567 if (node.first>0) 568 { 569 osg::Vec3 l(ls), e(le); 570 if (intersectAndClip(l,e, _kdNodes[node.first].bb)) 571 { 572 intersect(_kdNodes[node.first], l, e); 573 } 574 } 575 if (node.second>0) 576 { 577 osg::Vec3 l(ls), e(le); 578 if (intersectAndClip(l,e, _kdNodes[node.second].bb)) 579 { 580 intersect(_kdNodes[node.second], l, e); 581 } 582 } 583 } 584 } 585 586 bool IntersectKdTree::intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const 528 587 { 529 588 //return true; … … 652 711 } 653 712 713 714 //////////////////////////////////////////////////////////////////////////////// 715 // 716 // KdTree::BuildOptions 717 718 KdTree::BuildOptions::BuildOptions(): 719 _numVerticesProcessed(0), 720 _targetNumTrianglesPerLeaf(4), 721 _maxNumLevels(32) 722 { 723 } 724 725 //////////////////////////////////////////////////////////////////////////////// 726 // 727 // KdTree 728 729 KdTree::KdTree() 730 { 731 } 732 733 KdTree::KdTree(const KdTree& rhs, const osg::CopyOp& copyop): 734 Shape(rhs) 735 { 736 } 737 738 bool KdTree::build(BuildOptions& options, osg::Geometry* geometry) 739 { 740 BuildKdTree build(*this); 741 return build.build(options, geometry); 742 } 743 654 744 bool KdTree::intersect(const osg::Vec3& start, const osg::Vec3& end, LineSegmentIntersections& intersections) const 655 745 { 656 RayData rayData(start,end); 657 return intersect(getNode(0), rayData, start, end, intersections); 746 int numIntersectionsBefore = intersections.size(); 747 748 IntersectKdTree intersector(*_vertices, 749 _kdNodes, 750 _triangles, 751 intersections, 752 start, end); 753 754 intersector.intersect(getNode(0), start, end); 755 756 return numIntersectionsBefore != intersections.size(); 658 757 } 659 758
