Hi Robert,
Attached is a solution for the problem of finding intersection point described
in the test example.
I only copied the code from KdTree.cpp where the final calculations on
triangles are done.
Of course I had to do some small changes (e.g. continue -> return).
Look at the similarities between "struct TriangleIntersector" in
LineSegmentIntersector.cpp and "struct IntersectKdTree" in KdTree.cpp.
Probably you do not want to check in my file into svn/trunk because this piece
of code could or maybe should be shared. But I do not know the correct way to
do it.
Regards,
Lars.
-----Original Message-----
From: [email protected]
[mailto:[email protected]] On Behalf Of Nilsson
Lars
Sent: den 2 februari 2010 10:21
To: OpenSceneGraph Submissions
Subject: Re: [osg-submissions] LineSegmentIntersector using KdTree sometimes
fails
Hi Robert,
You are quite right, not to merge the changes in the IntersectKdTree::intersect
method. I have tested our application/datasets against svn/trunk 11008 with the
merged KdTree files.
It solved that problem.
However, during the testing, I found another combination that fails to find
intersections. This time KdTree works fine, but the other way (without KdTree)
it fails.
It could easily be reproduced by just changing a little in the test example,
which is updated and attached.
I do not have any solution to this problem yet. Shall I continue with this
thread or submit in a new one, if I find a way to solve the new problem.
Regards,
Lars.
/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
*
* This library is open source and may be redistributed and/or modified under
* the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
* (at your option) any later version. The full license is in LICENSE file
* included with this distribution, and on the openscenegraph.org website.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* OpenSceneGraph Public License for more details.
*/
#include <osgUtil/LineSegmentIntersector>
#include <osg/Geometry>
#include <osg/Notify>
#include <osg/io_utils>
#include <osg/TriangleFunctor>
#include <osg/KdTree>
#include <osg/Timer>
using namespace osgUtil;
namespace LineSegmentIntersectorUtils
{
struct TriangleIntersection
{
TriangleIntersection(unsigned int index, const osg::Vec3& normal, float
r1, const osg::Vec3* v1, float r2, const osg::Vec3* v2, float r3, const
osg::Vec3* v3):
_index(index),
_normal(normal),
_r1(r1),
_v1(v1),
_r2(r2),
_v2(v2),
_r3(r3),
_v3(v3) {}
unsigned int _index;
const osg::Vec3 _normal;
float _r1;
const osg::Vec3* _v1;
float _r2;
const osg::Vec3* _v2;
float _r3;
const osg::Vec3* _v3;
protected:
TriangleIntersection& operator = (const TriangleIntersection&) { return
*this; }
};
typedef std::multimap<float,TriangleIntersection> TriangleIntersections;
struct TriangleIntersector
{
osg::Vec3 _s;
osg::Vec3 _d;
float _length;
int _index;
float _ratio;
bool _hit;
TriangleIntersections _intersections;
TriangleIntersector()
{
_length = 0.0f;
_index = 0;
_ratio = 0.0f;
_hit = false;
}
void set(const osg::Vec3d& start, osg::Vec3d& end, float ratio=FLT_MAX)
{
_hit=false;
_index = 0;
_ratio = ratio;
_s = start;
_d = end - start;
_length = _d.length();
_d /= _length;
}
inline void operator () (const osg::Vec3& v0,const osg::Vec3& v1,const
osg::Vec3& v2, bool treatVertexDataAsTemporary)
{
// note v1,v2,v3 has been changed to v0,v1,v2
++_index;
if (v0==v1 || v1==v2 || v0==v2) return;
osg::Vec3 T = _s - v0;
osg::Vec3 E2 = v2 - v0;
osg::Vec3 E1 = v1 - v0;
osg::Vec3 P = _d ^ E2;
float det = P * E1;
float r,r0,r1,r2;
const float esplison = 1e-10f;
if (det>esplison)
{
float u = (P*T);
if (u<0.0 || u>det) return;
osg::Vec3 Q = T ^ E1;
float v = (Q*_d);
if (v<0.0 || v>det) return;
if ((u+v)> det) return;
float inv_det = 1.0f/det;
float t = (Q*E2)*inv_det;
if (t<0.0 || t>_length) return;
u *= inv_det;
v *= inv_det;
r0 = 1.0f-u-v;
r1 = u;
r2 = v;
// r = t * _inverse_length;
r = t / _length;
}
else if (det<-esplison)
{
float u = (P*T);
if (u>0.0 || u<det) return;
osg::Vec3 Q = T ^ E1;
float v = (Q*_d);
if (v>0.0 || v<det) return;
if ((u+v) < det) return;
float inv_det = 1.0f/det;
float t = (Q*E2)*inv_det;
if (t<0.0 || t>_length) return;
u *= inv_det;
v *= inv_det;
r0 = 1.0f-u-v;
r1 = u;
r2 = v;
// r = t * _inverse_length;
r = t / _length;
}
else
{
return;
}
// osg::Vec3 in = v0*r0 + v1*r1 + v2*r2;
osg::Vec3 normal = E1^E2;
normal.normalize();
if (treatVertexDataAsTemporary)
{
_intersections.insert(std::pair<const
float,TriangleIntersection>(r,TriangleIntersection(_index-1,normal,r0,0,r1,0,r2,0)));
}
else
{
_intersections.insert(std::pair<const
float,TriangleIntersection>(r,TriangleIntersection(_index-1,normal,r0,&v0,r1,&v1,r2,&v2)));
}
_hit = true;
}
};
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// LineSegmentIntersector
//
LineSegmentIntersector::LineSegmentIntersector(const osg::Vec3d& start, const
osg::Vec3d& end):
_parent(0),
_start(start),
_end(end)
{
}
LineSegmentIntersector::LineSegmentIntersector(CoordinateFrame cf, const
osg::Vec3d& start, const osg::Vec3d& end):
Intersector(cf),
_parent(0),
_start(start),
_end(end)
{
}
LineSegmentIntersector::LineSegmentIntersector(CoordinateFrame cf, double x,
double y):
Intersector(cf),
_parent(0)
{
switch(cf)
{
case WINDOW : _start.set(x,y,0.0); _end.set(x,y,1.0); break;
case PROJECTION : _start.set(x,y,-1.0); _end.set(x,y,1.0); break;
case VIEW : _start.set(x,y,0.0); _end.set(x,y,1.0); break;
case MODEL : _start.set(x,y,0.0); _end.set(x,y,1.0); break;
}
}
Intersector* LineSegmentIntersector::clone(osgUtil::IntersectionVisitor& iv)
{
if (_coordinateFrame==MODEL && iv.getModelMatrix()==0)
{
osg::ref_ptr<LineSegmentIntersector> lsi = new
LineSegmentIntersector(_start, _end);
lsi->_parent = this;
return lsi.release();
}
// compute the matrix that takes this Intersector from its CoordinateFrame
into the local MODEL coordinate frame
// that geometry in the scene graph will always be in.
osg::Matrix matrix;
switch (_coordinateFrame)
{
case(WINDOW):
if (iv.getWindowMatrix()) matrix.preMult( *iv.getWindowMatrix() );
if (iv.getProjectionMatrix()) matrix.preMult(
*iv.getProjectionMatrix() );
if (iv.getViewMatrix()) matrix.preMult( *iv.getViewMatrix() );
if (iv.getModelMatrix()) matrix.preMult( *iv.getModelMatrix() );
break;
case(PROJECTION):
if (iv.getProjectionMatrix()) matrix.preMult(
*iv.getProjectionMatrix() );
if (iv.getViewMatrix()) matrix.preMult( *iv.getViewMatrix() );
if (iv.getModelMatrix()) matrix.preMult( *iv.getModelMatrix() );
break;
case(VIEW):
if (iv.getViewMatrix()) matrix.preMult( *iv.getViewMatrix() );
if (iv.getModelMatrix()) matrix.preMult( *iv.getModelMatrix() );
break;
case(MODEL):
if (iv.getModelMatrix()) matrix = *iv.getModelMatrix();
break;
}
osg::Matrix inverse;
inverse.invert(matrix);
osg::ref_ptr<LineSegmentIntersector> lsi = new
LineSegmentIntersector(_start * inverse, _end * inverse);
lsi->_parent = this;
return lsi.release();
}
bool LineSegmentIntersector::enter(const osg::Node& node)
{
return !node.isCullingActive() || intersects( node.getBound() );
}
void LineSegmentIntersector::leave()
{
// do nothing
}
void LineSegmentIntersector::intersect(osgUtil::IntersectionVisitor& iv,
osg::Drawable* drawable)
{
osg::Vec3d s(_start), e(_end);
if ( !intersectAndClip( s, e, drawable->getBound() ) ) return;
if (iv.getDoDummyTraversal()) return;
osg::KdTree* kdTree = iv.getUseKdTreeWhenAvailable() ?
dynamic_cast<osg::KdTree*>(drawable->getShape()) : 0;
if (kdTree)
{
osg::KdTree::LineSegmentIntersections intersections;
intersections.reserve(4);
if (kdTree->intersect(s,e,intersections))
{
// osg::notify(osg::NOTICE)<<"Got KdTree intersections"<<std::endl;
for(osg::KdTree::LineSegmentIntersections::iterator itr =
intersections.begin();
itr != intersections.end();
++itr)
{
osg::KdTree::LineSegmentIntersection& lsi = *(itr);
// get ratio in s,e range
double ratio = lsi.ratio;
// remap ratio into _start, _end range
double remap_ratio = ((s-_start).length() + ratio *
(e-s).length() )/(_end-_start).length();
Intersection hit;
hit.ratio = remap_ratio;
hit.matrix = iv.getModelMatrix();
hit.nodePath = iv.getNodePath();
hit.drawable = drawable;
hit.primitiveIndex = lsi.primitiveIndex;
hit.localIntersectionPoint = _start*(1.0-remap_ratio) +
_end*remap_ratio;
// osg::notify(osg::NOTICE)<<"KdTree: ratio="<<hit.ratio<<"
("<<hit.localIntersectionPoint<<")"<<std::endl;
hit.localIntersectionNormal = lsi.intersectionNormal;
hit.indexList.reserve(3);
hit.ratioList.reserve(3);
if (lsi.r0!=0.0f)
{
hit.indexList.push_back(lsi.p0);
hit.ratioList.push_back(lsi.r0);
}
if (lsi.r1!=0.0f)
{
hit.indexList.push_back(lsi.p1);
hit.ratioList.push_back(lsi.r1);
}
if (lsi.r2!=0.0f)
{
hit.indexList.push_back(lsi.p2);
hit.ratioList.push_back(lsi.r2);
}
insertIntersection(hit);
}
}
return;
}
osg::TriangleFunctor<LineSegmentIntersectorUtils::TriangleIntersector> ti;
ti.set(s,e);
drawable->accept(ti);
if (ti._hit)
{
osg::Geometry* geometry = drawable->asGeometry();
for(LineSegmentIntersectorUtils::TriangleIntersections::iterator thitr
= ti._intersections.begin();
thitr != ti._intersections.end();
++thitr)
{
// get ratio in s,e range
double ratio = thitr->first;
// remap ratio into _start, _end range
double remap_ratio = ((s-_start).length() + ratio * (e-s).length()
)/(_end-_start).length();
LineSegmentIntersectorUtils::TriangleIntersection& triHit =
thitr->second;
Intersection hit;
hit.ratio = remap_ratio;
hit.matrix = iv.getModelMatrix();
hit.nodePath = iv.getNodePath();
hit.drawable = drawable;
hit.primitiveIndex = triHit._index;
hit.localIntersectionPoint = _start*(1.0-remap_ratio) +
_end*remap_ratio;
// osg::notify(osg::NOTICE)<<"Conventional: ratio="<<hit.ratio<<"
("<<hit.localIntersectionPoint<<")"<<std::endl;
hit.localIntersectionNormal = triHit._normal;
if (geometry)
{
osg::Vec3Array* vertices =
dynamic_cast<osg::Vec3Array*>(geometry->getVertexArray());
if (vertices)
{
osg::Vec3* first = &(vertices->front());
if (triHit._v1)
{
hit.indexList.push_back(triHit._v1-first);
hit.ratioList.push_back(triHit._r1);
}
if (triHit._v2)
{
hit.indexList.push_back(triHit._v2-first);
hit.ratioList.push_back(triHit._r2);
}
if (triHit._v3)
{
hit.indexList.push_back(triHit._v3-first);
hit.ratioList.push_back(triHit._r3);
}
}
}
insertIntersection(hit);
}
}
}
void LineSegmentIntersector::reset()
{
Intersector::reset();
_intersections.clear();
}
bool LineSegmentIntersector::intersects(const osg::BoundingSphere& bs)
{
// if bs not valid then return true based on the assumption that an invalid
sphere is yet to be defined.
if (!bs.valid()) return true;
osg::Vec3d sm = _start - bs._center;
double c = sm.length2()-bs._radius*bs._radius;
if (c<0.0) return true;
osg::Vec3d se = _end-_start;
double a = se.length2();
double b = (sm*se)*2.0;
double d = b*b-4.0*a*c;
if (d<0.0) return false;
d = sqrt(d);
double div = 1.0/(2.0*a);
double r1 = (-b-d)*div;
double r2 = (-b+d)*div;
if (r1<=0.0 && r2<=0.0) return false;
if (r1>=1.0 && r2>=1.0) return false;
// passed all the rejection tests so line must intersect bounding sphere,
return true.
return true;
}
bool LineSegmentIntersector::intersectAndClip(osg::Vec3d& s, osg::Vec3d&
e,const osg::BoundingBox& bbInput)
{
osg::Vec3d bb_min(bbInput._min);
osg::Vec3d bb_max(bbInput._max);
#if 1
double epsilon = 1e-4;
bb_min.x() -= epsilon;
bb_min.y() -= epsilon;
bb_min.z() -= epsilon;
bb_max.x() += epsilon;
bb_max.y() += epsilon;
bb_max.z() += epsilon;
#endif
// compate s and e against the xMin to xMax range of bb.
if (s.x()<=e.x())
{
// trivial reject of segment wholely outside.
if (e.x()<bb_min.x()) return false;
if (s.x()>bb_max.x()) return false;
if (s.x()<bb_min.x())
{
// clip s to xMin.
s = s+(e-s)*(bb_min.x()-s.x())/(e.x()-s.x());
}
if (e.x()>bb_max.x())
{
// clip e to xMax.
e = s+(e-s)*(bb_max.x()-s.x())/(e.x()-s.x());
}
}
else
{
if (s.x()<bb_min.x()) return false;
if (e.x()>bb_max.x()) return false;
if (e.x()<bb_min.x())
{
// clip s to xMin.
e = s+(e-s)*(bb_min.x()-s.x())/(e.x()-s.x());
}
if (s.x()>bb_max.x())
{
// clip e to xMax.
s = s+(e-s)*(bb_max.x()-s.x())/(e.x()-s.x());
}
}
// compate s and e against the yMin to yMax range of bb.
if (s.y()<=e.y())
{
// trivial reject of segment wholely outside.
if (e.y()<bb_min.y()) return false;
if (s.y()>bb_max.y()) return false;
if (s.y()<bb_min.y())
{
// clip s to yMin.
s = s+(e-s)*(bb_min.y()-s.y())/(e.y()-s.y());
}
if (e.y()>bb_max.y())
{
// clip e to yMax.
e = s+(e-s)*(bb_max.y()-s.y())/(e.y()-s.y());
}
}
else
{
if (s.y()<bb_min.y()) return false;
if (e.y()>bb_max.y()) return false;
if (e.y()<bb_min.y())
{
// clip s to yMin.
e = s+(e-s)*(bb_min.y()-s.y())/(e.y()-s.y());
}
if (s.y()>bb_max.y())
{
// clip e to yMax.
s = s+(e-s)*(bb_max.y()-s.y())/(e.y()-s.y());
}
}
// compate s and e against the zMin to zMax range of bb.
if (s.z()<=e.z())
{
// trivial reject of segment wholely outside.
if (e.z()<bb_min.z()) return false;
if (s.z()>bb_max.z()) return false;
if (s.z()<bb_min.z())
{
// clip s to zMin.
s = s+(e-s)*(bb_min.z()-s.z())/(e.z()-s.z());
}
if (e.z()>bb_max.z())
{
// clip e to zMax.
e = s+(e-s)*(bb_max.z()-s.z())/(e.z()-s.z());
}
}
else
{
if (s.z()<bb_min.z()) return false;
if (e.z()>bb_max.z()) return false;
if (e.z()<bb_min.z())
{
// clip s to zMin.
e = s+(e-s)*(bb_min.z()-s.z())/(e.z()-s.z());
}
if (s.z()>bb_max.z())
{
// clip e to zMax.
s = s+(e-s)*(bb_max.z()-s.z())/(e.z()-s.z());
}
}
// osg::notify(osg::NOTICE)<<"clampped segment "<<s<<" "<<e<<std::endl;
// if (s==e) return false;
return true;
}
#include <iostream>
#include <osg/Geode>
#include <osg/KdTree>
#include <osgUtil/LineSegmentIntersector>
int main(int argc, char *argv[])
{
for (int i=0; i<2; ++i) {
const bool useKdTree = (i == 0);
const float Z = 3000.0;
osg::ref_ptr<osg::Vec3Array> vertices = new osg::Vec3Array;
vertices->push_back(osg::Vec3(86093.9, 30625.9, Z));
vertices->push_back(osg::Vec3(86077.8, 30669.0, Z));
vertices->push_back(osg::Vec3(83483.4, 29653.5, Z));
vertices->push_back(osg::Vec3(83000.0, 30000.0, Z));
vertices->push_back(osg::Vec3(80000.0, 30000.0, Z)); // at least 5 vertices
are needed to invoke KdTree (default)
osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
geometry->setVertexArray(vertices.get());
geometry->addPrimitiveSet(new
osg::DrawArrays(osg::PrimitiveSet::TRIANGLE_STRIP, 0, vertices->size()));
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
geode->addDrawable(geometry.get());
if (useKdTree) {
osg::ref_ptr<osg::KdTreeBuilder> kdTreeBuilder = new osg::KdTreeBuilder;
geode->accept(*(kdTreeBuilder.get()));
}
const osg::Vec2d XY(84718.3, 30129.7);
const osg::Vec3d start(XY, 0.0);
const osg::Vec3d end(XY, 1e9);
osg::ref_ptr<osgUtil::LineSegmentIntersector> intersector = new
osgUtil::LineSegmentIntersector(start, end);
osgUtil::IntersectionVisitor iv(intersector.get());
iv.reset();
geode->accept(iv);
osgUtil::LineSegmentIntersector::Intersections& intersections =
intersector->getIntersections();
std::cout << "KdTree " << (useKdTree ? "on " : "off ");
if (intersections.empty()) std::cout << "No intersections ";
else std::cout << intersections.begin()->localIntersectionPoint[2];
std::cout << std::endl;
}
return 0;
}
_______________________________________________
osg-submissions mailing list
[email protected]
http://lists.openscenegraph.org/listinfo.cgi/osg-submissions-openscenegraph.org
