Changeset 8556
- Timestamp:
- 07/10/08 17:50:10
- Files:
-
- OpenSceneGraph/trunk/include/osg/KdTree (modified) (6 diffs)
- OpenSceneGraph/trunk/src/osg/KdTree.cpp (modified) (14 diffs)
Legend:
- Unmodified
- Added
- Removed
- Modified
- Copied
- Moved
OpenSceneGraph/trunk/include/osg/KdTree
r8553 r8556 92 92 value_type second; 93 93 }; 94 95 96 struct KdLeaf 97 { 98 KdLeaf(): 99 first(0), 100 second(0) {} 101 102 KdLeaf(value_type f, value_type s): 103 first(f), 104 second(s) {} 105 106 osg::BoundingBox bb; 107 108 value_type first; 109 value_type second; 110 }; 111 94 112 95 struct Triangle 113 96 { … … 131 114 typedef std::vector< unsigned int > AxisStack; 132 115 typedef std::vector< KdNode > KdNodeList; 133 typedef std::vector< KdLeaf > KdLeafList;134 135 /// note, leafNum is negative to distinguish from nodeNum136 int addLeaf(const KdLeaf& leaf) { int num = _kdLeaves.size(); _kdLeaves.push_back(leaf); return -(num+1); }137 138 int replaceLeaf(int leafNum, const KdLeaf& leaf)139 {140 int num = -leafNum-1;141 142 if (num>static_cast<int>(_kdLeaves.size())-1)143 {144 osg::notify(osg::NOTICE)<<"Warning: replaceChild("<<leafNum<<", leaf), num = "<<num<<" _kdLeaves.size()="<<_kdLeaves.size()<<std::endl;145 return leafNum;146 }147 148 _kdLeaves[num] = leaf; return leafNum;149 }150 151 /// note, leafNum is negative to distinguish from nodeNum152 KdLeaf& getLeaf(int leafNum)153 {154 int num = -leafNum-1;155 if (num<0 || num>static_cast<int>(_kdLeaves.size())-1)156 {157 osg::notify(osg::NOTICE)<<"Warning: getLeaf("<<leafNum<<", num = "<<num<<") _kdLeaves.size()="<<_kdLeaves.size()<<std::endl;158 }159 160 return _kdLeaves[num];161 }162 163 const KdLeaf& getLeaf(int leafNum) const164 {165 int num = -leafNum-1;166 if (num<0 || num>static_cast<int>(_kdLeaves.size())-1)167 {168 osg::notify(osg::NOTICE)<<"Warning: getLeaf("<<leafNum<<", num = "<<num<<") _kdLeaves.size()="<<_kdLeaves.size()<<std::endl;169 }170 171 return _kdLeaves[num];172 }173 116 174 117 int addNode(const KdNode& node) … … 201 144 osg::BoundingBox& getBoundingBox(int nodeNum) 202 145 { 203 if (nodeNum<0) 204 { 205 int num = -nodeNum-1; 206 return _kdLeaves[num].bb; 207 } 208 else 209 { 210 return _kdNodes[nodeNum].bb; 211 } 146 return _kdNodes[nodeNum].bb; 212 147 } 213 148 … … 217 152 int divide(BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level); 218 153 219 bool intersect(const KdLeaf& leaf, const osg::Vec3& start, const osg::Vec3& end, LineSegmentIntersections& intersections) const; 220 bool intersect(const KdNode& node, const osg::Vec3& start, const osg::Vec3& end, const osg::Vec3& s, const osg::Vec3& e, LineSegmentIntersections& intersections) const; 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; 221 176 bool intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const; 222 177 … … 231 186 AxisStack _axisStack; 232 187 KdNodeList _kdNodes; 233 KdLeafList _kdLeaves;234 188 235 189 osg::ref_ptr<osg::Vec3Array> _vertices; … … 237 191 Indices _primitiveIndices; 238 192 239 BoundingBoxList _boundingBoxes;240 193 TriangleList _triangles; 241 194 CenterList _centers; OpenSceneGraph/trunk/src/osg/KdTree.cpp
r8554 r8556 21 21 using namespace osg; 22 22 23 // #define VERBOSE_OUTPUT23 //#define VERBOSE_OUTPUT 24 24 25 25 //////////////////////////////////////////////////////////////////////////////// … … 43 43 bb.expandBy((*(_kdTree->_vertices))[p2]); 44 44 bb.expandBy((*(_kdTree->_vertices))[p3]); 45 _kdTree->_boundingBoxes.push_back(bb); 46 45 47 46 _kdTree->_centers.push_back(bb.center()); 48 49 47 _kdTree->_primitiveIndices.push_back(i); 50 48 … … 61 59 KdTree::BuildOptions::BuildOptions(): 62 60 _numVerticesProcessed(0), 63 _targetNumTrianglesPerLeaf( 4),61 _targetNumTrianglesPerLeaf(2), 64 62 _maxNumLevels(32) 65 63 { … … 100 98 #endif 101 99 102 _kdNodes.reserve(estimatedSize); 103 _kdLeaves.reserve(estimatedSize); 100 _kdNodes.reserve(estimatedSize*5); 104 101 105 102 computeDivisions(options); … … 109 106 unsigned int estimatedNumTriangles = vertices->size()*2; 110 107 _primitiveIndices.reserve(estimatedNumTriangles); 111 _boundingBoxes.reserve(estimatedNumTriangles);112 108 _triangles.reserve(estimatedNumTriangles); 113 109 _centers.reserve(estimatedNumTriangles); … … 121 117 _primitiveIndices.reserve(vertices->size()); 122 118 123 124 125 KdLeaf leaf(0, _primitiveIndices.size()); 126 127 int leafNum = addLeaf(leaf); 119 KdNode node(-1, _primitiveIndices.size()); 120 node.bb = _bb; 121 122 int nodeNum = addNode(node); 128 123 129 124 osg::BoundingBox bb = _bb; 130 int nodeNum = divide(options, bb, leafNum, 0);125 nodeNum = divide(options, bb, nodeNum, 0); 131 126 132 127 #ifdef VERBOSE_OUTPUT … … 180 175 int KdTree::divide(BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level) 181 176 { 182 if (_axisStack.size()<=level) return nodeIndex; 183 184 int axis = _axisStack[level]; 185 186 #ifdef VERBOSE_OUTPUT 187 //osg::notify(osg::NOTICE)<<"divide("<<nodeIndex<<", "<<level<< "), axis="<<axis<<std::endl; 188 #endif 189 190 if (nodeIndex>=0) 191 { 192 #ifdef VERBOSE_OUTPUT 193 osg::notify(osg::NOTICE)<<" divide node"<<std::endl; 194 #endif 195 KdNode& node = getNode(nodeIndex); 196 return nodeIndex; 197 } 198 else 199 { 200 201 if (getLeaf(nodeIndex).second<=options._targetNumTrianglesPerLeaf) 202 { 177 KdNode& node = getNode(nodeIndex); 178 179 bool needToDivide = level < _axisStack.size() && 180 (node.first<0 && node.second>options._targetNumTrianglesPerLeaf); 181 182 if (!needToDivide) 183 { 184 if (node.first<0) 185 { 186 int istart = -node.first-1; 187 int iend = istart+node.second-1; 188 203 189 // leaf is done, now compute bound on it. 204 KdLeaf& leaf = getLeaf(nodeIndex); 205 leaf.bb.init(); 206 int iend = leaf.first+leaf.second; 207 for(int i=leaf.first; i<iend; ++i) 190 node.bb.init(); 191 for(int i=istart; i<=iend; ++i) 208 192 { 209 193 const Triangle& tri = _triangles[_primitiveIndices[i]]; … … 211 195 const osg::Vec3& v2 = (*_vertices)[tri._p2]; 212 196 const osg::Vec3& v3 = (*_vertices)[tri._p3]; 213 leaf.bb.expandBy(v1); 214 leaf.bb.expandBy(v2); 215 leaf.bb.expandBy(v3); 216 } 217 218 return nodeIndex; 219 } 197 node.bb.expandBy(v1); 198 node.bb.expandBy(v2); 199 node.bb.expandBy(v3); 200 201 float epsilon = 1e-6; 202 node.bb._min.x() -= epsilon; 203 node.bb._min.y() -= epsilon; 204 node.bb._min.z() -= epsilon; 205 node.bb._max.x() += epsilon; 206 node.bb._max.y() += epsilon; 207 node.bb._max.z() += epsilon; 208 } 209 210 #ifdef VERBOSE_OUTPUT 211 if (!node.bb.valid()) 212 { 213 osg::notify(osg::NOTICE)<<"After reset "<<node.first<<","<<node.second<<std::endl; 214 osg::notify(osg::NOTICE)<<" bb._min ("<<node.bb._min<<")"<<std::endl; 215 osg::notify(osg::NOTICE)<<" bb._max ("<<node.bb._max<<")"<<std::endl; 216 } 217 else 218 { 219 osg::notify(osg::NOTICE)<<"Set bb for nodeIndex = "<<nodeIndex<<std::endl; 220 } 221 #endif 222 } 223 224 return nodeIndex; 225 226 } 227 228 int axis = _axisStack[level]; 229 230 #ifdef VERBOSE_OUTPUT 231 osg::notify(osg::NOTICE)<<"divide("<<nodeIndex<<", "<<level<< "), axis="<<axis<<std::endl; 232 #endif 233 234 if (node.first<0) 235 { 236 // leaf node as first <= 0, so look at dividing it. 237 238 int istart = -node.first-1; 239 int iend = istart+node.second-1; 220 240 221 241 //osg::notify(osg::NOTICE)<<" divide leaf"<<std::endl; 222 242 223 int nodeNum = addNode(KdNode());224 225 243 float original_min = bb._min[axis]; 226 244 float original_max = bb._max[axis]; … … 228 246 float mid = (original_min+original_max)*0.5f; 229 247 230 { 231 KdLeaf& leaf = getLeaf(nodeIndex); 232 248 int originalLeftChildIndex = 0; 249 int originalRightChildIndex = 0; 250 251 { 233 252 //osg::Vec3Array* vertices = kdTree._vertices.get(); 234 int end = leaf.first+leaf.second-1; 235 int left = leaf.first; 236 int right = leaf.first+leaf.second-1; 253 int left = istart; 254 int right = iend; 237 255 238 256 while(left<right) … … 258 276 } 259 277 260 Kd Leaf leftLeaf(leaf.first, (right-leaf.first)+1);261 Kd Leaf rightLeaf(left, (end-left)+1);262 263 #if 0 264 osg::notify(osg::NOTICE)<<"In leaf.first ="<<leaf.first <<" leaf.second ="<<leaf.second<<std::endl;278 KdNode leftLeaf(-istart-1, (right-istart)+1); 279 KdNode rightLeaf(-left-1, (iend-left)+1); 280 281 #if 0 282 osg::notify(osg::NOTICE)<<"In node.first ="<<node.first <<" node.second ="<<node.second<<std::endl; 265 283 osg::notify(osg::NOTICE)<<" leftLeaf.first ="<<leftLeaf.first <<" leftLeaf.second ="<<leftLeaf.second<<std::endl; 266 284 osg::notify(osg::NOTICE)<<" rightLeaf.first="<<rightLeaf.first<<" rightLeaf.second="<<rightLeaf.second<<std::endl; 267 285 osg::notify(osg::NOTICE)<<" left="<<left<<" right="<<right<<std::endl; 268 286 269 if ( leaf.second != (leftLeaf.second +rightLeaf.second))270 { 271 osg::notify(osg::NOTICE)<<"*** Error in size, leaf.second="<< leaf.second287 if (node.second != (leftLeaf.second +rightLeaf.second)) 288 { 289 osg::notify(osg::NOTICE)<<"*** Error in size, leaf.second="<<node.second 272 290 <<", leftLeaf.second="<<leftLeaf.second 273 291 <<", rightLeaf.second="<<rightLeaf.second<<std::endl; … … 275 293 else 276 294 { 277 osg::notify(osg::NOTICE)<<"Size OK, leaf.second="<< leaf.second295 osg::notify(osg::NOTICE)<<"Size OK, leaf.second="<<node.second 278 296 <<", leftLeaf.second="<<leftLeaf.second 279 297 <<", rightLeaf.second="<<rightLeaf.second<<std::endl; 280 298 } 281 299 #endif 300 282 301 if (leftLeaf.second<=0) 283 302 { 284 303 //osg::notify(osg::NOTICE)<<"LeftLeaf empty"<<std::endl; 285 getNode(nodeNum).first= 0;286 getNode(nodeNum).second = replaceLeaf(nodeIndex,rightLeaf);304 originalLeftChildIndex = 0; 305 originalRightChildIndex = addNode(rightLeaf); 287 306 } 288 307 else if (rightLeaf.second<=0) 289 308 { 290 309 //osg::notify(osg::NOTICE)<<"RightLeaf empty"<<std::endl; 291 getNode(nodeNum).first = replaceLeaf(nodeIndex,leftLeaf);292 getNode(nodeNum).second= 0;310 originalLeftChildIndex = addNode(leftLeaf); 311 originalRightChildIndex = 0; 293 312 } 294 313 else 295 314 { 296 getNode(nodeNum).first = replaceLeaf(nodeIndex, leftLeaf); 297 getNode(nodeNum).second = addLeaf(rightLeaf); 298 } 299 } 300 301 302 int originalLeftChildIndex = getNode(nodeNum).first; 303 int originalRightChildIndex = getNode(nodeNum).second; 304 305 315 originalLeftChildIndex = addNode(leftLeaf); 316 originalRightChildIndex = addNode(rightLeaf); 317 } 318 } 319 320 306 321 float restore = bb._max[axis]; 307 322 bb._max[axis] = mid; 308 323 309 324 //osg::notify(osg::NOTICE)<<" divide leftLeaf "<<kdTree.getNode(nodeNum).first<<std::endl; 310 int leftChildIndex = divide(options, bb, originalLeftChildIndex, level+1);325 int leftChildIndex = originalLeftChildIndex!=0 ? divide(options, bb, originalLeftChildIndex, level+1) : 0; 311 326 312 327 bb._max[axis] = restore; … … 316 331 317 332 //osg::notify(osg::NOTICE)<<" divide rightLeaf "<<kdTree.getNode(nodeNum).second<<std::endl; 318 int rightChildIndex = divide(options, bb, originalRightChildIndex, level+1);333 int rightChildIndex = originalRightChildIndex!=0 ? divide(options, bb, originalRightChildIndex, level+1) : 0; 319 334 320 335 bb._min[axis] = restore; 321 336 322 getNode(nodeNum).first = leftChildIndex; 323 getNode(nodeNum).second = rightChildIndex; 324 325 getNode(nodeNum).bb.init(); 326 if (leftChildIndex!=0) getNode(nodeNum).bb.expandBy(getBoundingBox(leftChildIndex)); 327 if (rightChildIndex!=0) getNode(nodeNum).bb.expandBy(getBoundingBox(rightChildIndex)); 328 329 return nodeNum; 330 } 331 } 332 333 bool KdTree::intersect(const KdLeaf& leaf, const osg::Vec3& start, const osg::Vec3& end, LineSegmentIntersections& intersections) const 334 { 335 osg::Vec3 _s = start; 336 osg::Vec3 _d = end - start; 337 float _length = _d.length(); 338 _d /= _length; 339 340 341 //osg::notify(osg::NOTICE)<<"KdTree::intersect("<<&leaf<<")"<<std::endl; 342 bool intersects = false; 343 int iend = leaf.first + leaf.second; 344 for(int i=leaf.first; i<iend; ++i) 345 { 346 const Triangle& tri = _triangles[_primitiveIndices[i]]; 347 const osg::Vec3& v1 = (*_vertices)[tri._p1]; 348 const osg::Vec3& v2 = (*_vertices)[tri._p2]; 349 const osg::Vec3& v3 = (*_vertices)[tri._p3]; 350 // osg::notify(osg::NOTICE)<<" tri("<<tri._p1<<","<<tri._p2<<","<<tri._p3<<")"<<std::endl; 351 352 if (v1==v2 || v2==v3 || v1==v3) continue; 353 354 osg::Vec3 v12 = v2-v1; 355 osg::Vec3 n12 = v12^_d; 356 float ds12 = (_s-v1)*n12; 357 float d312 = (v3-v1)*n12; 358 if (d312>=0.0f) 359 { 360 if (ds12<0.0f) continue; 361 if (ds12>d312) continue; 362 } 363 else // d312 < 0 364 { 365 if (ds12>0.0f) continue; 366 if (ds12<d312) continue; 367 } 368 369 osg::Vec3 v23 = v3-v2; 370 osg::Vec3 n23 = v23^_d; 371 float ds23 = (_s-v2)*n23; 372 float d123 = (v1-v2)*n23; 373 if (d123>=0.0f) 374 { 375 if (ds23<0.0f) continue; 376 if (ds23>d123) continue; 377 } 378 else // d123 < 0 379 { 380 if (ds23>0.0f) continue; 381 if (ds23<d123) continue; 382 } 383 384 osg::Vec3 v31 = v1-v3; 385 osg::Vec3 n31 = v31^_d; 386 float ds31 = (_s-v3)*n31; 387 float d231 = (v2-v3)*n31; 388 if (d231>=0.0f) 389 { 390 if (ds31<0.0f) continue; 391 if (ds31>d231) continue; 392 } 393 else // d231 < 0 394 { 395 if (ds31>0.0f) continue; 396 if (ds31<d231) continue; 397 } 398 399 400 float r3; 401 if (ds12==0.0f) r3=0.0f; 402 else if (d312!=0.0f) r3 = ds12/d312; 403 else continue; // the triangle and the line must be parallel intersection. 404 405 float r1; 406 if (ds23==0.0f) r1=0.0f; 407 else if (d123!=0.0f) r1 = ds23/d123; 408 else continue; // the triangle and the line must be parallel intersection. 409 410 float r2; 411 if (ds31==0.0f) r2=0.0f; 412 else if (d231!=0.0f) r2 = ds31/d231; 413 else continue; // the triangle and the line must be parallel intersection. 414 415 float total_r = (r1+r2+r3); 416 if (total_r!=1.0f) 417 { 418 if (total_r==0.0f) continue; // the triangle and the line must be parallel intersection. 419 float inv_total_r = 1.0f/total_r; 420 r1 *= inv_total_r; 421 r2 *= inv_total_r; 422 r3 *= inv_total_r; 423 } 424 425 osg::Vec3 in = v1*r1+v2*r2+v3*r3; 426 if (!in.valid()) 427 { 428 osg::notify(osg::WARN)<<"Warning:: Picked up error in TriangleIntersect"<<std::endl; 429 osg::notify(osg::WARN)<<" ("<<v1<<",\t"<<v2<<",\t"<<v3<<")"<<std::endl; 430 osg::notify(osg::WARN)<<" ("<<r1<<",\t"<<r2<<",\t"<<r3<<")"<<std::endl; 431 continue; 432 } 433 434 float d = (in-_s)*_d; 435 436 if (d<0.0f) continue; 437 if (d>_length) continue; 438 439 osg::Vec3 normal = v12^v23; 440 normal.normalize(); 441 442 float r = d/_length; 443 444 LineSegmentIntersection intersection; 445 intersection.ratio = r; 446 intersection.primitiveIndex = _primitiveIndices[i]; 447 intersection.intersectionPoint = in; 448 intersection.intersectionNormal = normal; 449 450 intersection.indexList.push_back(tri._p1); 451 intersection.indexList.push_back(tri._p2); 452 intersection.indexList.push_back(tri._p3); 453 454 intersection.ratioList.push_back(r1); 455 intersection.ratioList.push_back(r2); 456 intersection.ratioList.push_back(r3); 457 458 intersections.insert(intersection); 459 460 // osg::notify(osg::NOTICE)<<" got intersection ("<<in<<") ratio="<<r<<std::endl; 461 462 intersects = true; 463 } 464 465 return intersects; 466 } 467 468 bool KdTree::intersect(const KdNode& node, const osg::Vec3& start, const osg::Vec3& end, const osg::Vec3& s, const osg::Vec3& e, LineSegmentIntersections& intersections) const 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 365 } 366 else 367 { 368 osg::notify(osg::NOTICE)<<"NOT expecting to get here"<<std::endl; 369 } 370 371 return nodeIndex; 372 373 } 374 375 bool KdTree::intersect(const KdNode& node, const RayData& rayData, osg::Vec3 ls, osg::Vec3 le, LineSegmentIntersections& intersections) const 469 376 { 470 377 //osg::notify(osg::NOTICE)<<" Intersect "<<&node<<std::endl; 471 //osg::Vec3 ls(start), le(end); 472 osg::Vec3 ls(s), le(e); 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 473 389 int numIntersectionsBefore = intersections.size(); 474 if (intersectAndClip(ls, le, node.bb)) 475 { 476 if (node.first>0) intersect(getNode(node.first), start, end, ls, le, intersections); 477 else if (node.first<0) intersect(getLeaf(node.first), start, end, intersections); 478 479 if (node.second>0) intersect(getNode(node.second), start, end, ls, le, intersections); 480 else if (node.second<0) intersect(getLeaf(node.second), start, end, intersections); 481 } 390 391 if (node.first<0) 392 { 393 // treat as a leaf 394 395 //osg::notify(osg::NOTICE)<<"KdTree::intersect("<<&leaf<<")"<<std::endl; 396 int istart = -node.first-1; 397 int iend = istart + node.second; 398 399 for(int i=istart; i<iend; ++i) 400 { 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; 486 continue; 487 } 488 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; 495 normal.normalize(); 496 497 float r = d* rayData._inverse_length; 498 499 LineSegmentIntersection intersection; 500 intersection.ratio = r; 501 intersection.primitiveIndex = _primitiveIndices[i]; 502 intersection.intersectionPoint = in; 503 intersection.intersectionNormal = normal; 504 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 515 // osg::notify(osg::NOTICE)<<" got intersection ("<<in<<") ratio="<<r<<std::endl; 516 } 517 } 518 else 519 { 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 482 524 return numIntersectionsBefore != intersections.size(); 483 525 } … … 485 527 bool KdTree::intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const 486 528 { 529 //return true; 530 531 //if (!bb.valid()) return true; 532 487 533 // compate s and e against the xMin to xMax range of bb. 488 534 if (s.x()<=e.x()) … … 608 654 bool KdTree::intersect(const osg::Vec3& start, const osg::Vec3& end, LineSegmentIntersections& intersections) const 609 655 { 610 osg::Vec3 s(start), e(end);611 return intersect(getNode(0), start, end, s,e, intersections);656 RayData rayData(start,end); 657 return intersect(getNode(0), rayData, start, end, intersections); 612 658 } 613 659
