Added --points and --lines command line options that do a very simplistic conversion of geometry primitives to points or lines respectively, used to aid testing of intersectors

This commit is contained in:
Robert Osfield
2017-05-09 11:33:22 +01:00
parent 03f73d3aad
commit 6e1866ac18
5 changed files with 937 additions and 1441 deletions

View File

@@ -14,6 +14,7 @@
#include <osg/KdTree>
#include <osg/Geode>
#include <osg/TriangleIndexFunctor>
#include <osg/TemplatePrimitiveIndexFunctor>
#include <osg/Timer>
#include <osg/io_utils>
@@ -26,6 +27,7 @@ using namespace osg;
//
// BuildKdTree Declarartion - class used for building an single KdTree
struct BuildKdTree
{
BuildKdTree(KdTree& kdTree):
@@ -53,19 +55,54 @@ protected:
BuildKdTree& operator = (const BuildKdTree&) { return *this; }
};
////////////////////////////////////////////////////////////////////////////////
//
// Functor for collecting triangle indices from Geometry
struct TriangleIndicesCollector
struct PrimitiveIndicesCollector
{
TriangleIndicesCollector():
PrimitiveIndicesCollector():
_buildKdTree(0)
{
}
inline void operator () (unsigned int p0)
{
OSG_NOTICE<<" point ("<<p0<<")"<<std::endl;
const osg::Vec3& v0 = (*(_buildKdTree->_kdTree.getVertices()))[p0];
_buildKdTree->_kdTree.addPoint(p0);
osg::BoundingBox bb;
bb.expandBy(v0);
_buildKdTree->_primitiveIndices.push_back(_buildKdTree->_centers.size());
_buildKdTree->_centers.push_back(bb.center());
}
inline void operator () (unsigned int p0, unsigned int p1)
{
OSG_NOTICE<<" line ("<<p0<<", "<<p1<<")"<<std::endl;
const osg::Vec3& v0 = (*(_buildKdTree->_kdTree.getVertices()))[p0];
const osg::Vec3& v1 = (*(_buildKdTree->_kdTree.getVertices()))[p1];
// discard degenerate points
if (v0==v1)
{
//OSG_NOTICE<<"Disgarding degenerate triangle"<<std::endl;
return;
}
_buildKdTree->_kdTree.addLine(p0,p1);
osg::BoundingBox bb;
bb.expandBy(v0);
bb.expandBy(v1);
_buildKdTree->_primitiveIndices.push_back(_buildKdTree->_centers.size());
_buildKdTree->_centers.push_back(bb.center());
}
inline void operator () (unsigned int p0, unsigned int p1, unsigned int p2)
{
// OSG_NOTICE<<" triangle ("<<p0<<", "<<p1<<", "<<p2<<")"<<std::endl;
const osg::Vec3& v0 = (*(_buildKdTree->_kdTree.getVertices()))[p0];
const osg::Vec3& v1 = (*(_buildKdTree->_kdTree.getVertices()))[p1];
const osg::Vec3& v2 = (*(_buildKdTree->_kdTree.getVertices()))[p2];
@@ -77,16 +114,43 @@ struct TriangleIndicesCollector
return;
}
unsigned int i = _buildKdTree->_kdTree.addTriangle(KdTree::Triangle(p0,p1,p2));
_buildKdTree->_kdTree.addTriangle(p0,p1,p2);
osg::BoundingBox bb;
bb.expandBy(v0);
bb.expandBy(v1);
bb.expandBy(v2);
_buildKdTree->_primitiveIndices.push_back(_buildKdTree->_centers.size());
_buildKdTree->_centers.push_back(bb.center());
_buildKdTree->_primitiveIndices.push_back(i);
}
inline void operator () (unsigned int p0, unsigned int p1, unsigned int p2, unsigned int p3)
{
OSG_NOTICE<<" quad ("<<p0<<", "<<p1<<", "<<p2<<", "<<p3<<")"<<std::endl;
const osg::Vec3& v0 = (*(_buildKdTree->_kdTree.getVertices()))[p0];
const osg::Vec3& v1 = (*(_buildKdTree->_kdTree.getVertices()))[p1];
const osg::Vec3& v2 = (*(_buildKdTree->_kdTree.getVertices()))[p2];
const osg::Vec3& v3 = (*(_buildKdTree->_kdTree.getVertices()))[p3];
// discard degenerate points
if (v0==v1 || v1==v2 || v2==v0 || v3==v0 || v3==v1 || v3==v2)
{
//OSG_NOTICE<<"Disgarding degenerate quad"<<std::endl;
return;
}
_buildKdTree->_kdTree.addQuad(p0,p1,p2,p3);
osg::BoundingBox bb;
bb.expandBy(v0);
bb.expandBy(v1);
bb.expandBy(v2);
bb.expandBy(v3);
_buildKdTree->_primitiveIndices.push_back(_buildKdTree->_centers.size());
_buildKdTree->_centers.push_back(bb.center());
}
BuildKdTree* _buildKdTree;
@@ -129,11 +193,9 @@ bool BuildKdTree::build(KdTree::BuildOptions& options, osg::Geometry* geometry)
_primitiveIndices.reserve(estimatedNumTriangles);
_centers.reserve(estimatedNumTriangles);
_kdTree.getTriangles().reserve(estimatedNumTriangles);
osg::TriangleIndexFunctor<TriangleIndicesCollector> collectTriangleIndices;
collectTriangleIndices._buildKdTree = this;
geometry->accept(collectTriangleIndices);
osg::TemplatePrimitiveIndexFunctor<PrimitiveIndicesCollector> collectIndices;
collectIndices._buildKdTree = this;
geometry->accept(collectIndices);
_primitiveIndices.reserve(vertices->size());
@@ -145,14 +207,18 @@ bool BuildKdTree::build(KdTree::BuildOptions& options, osg::Geometry* geometry)
osg::BoundingBox bb = _bb;
nodeNum = divide(options, bb, nodeNum, 0);
// now reorder the triangle list so that it's in order as per the primitiveIndex list.
KdTree::TriangleList triangleList(_kdTree.getTriangles().size());
for(unsigned int i=0; i<_primitiveIndices.size(); ++i)
{
triangleList[i] = _kdTree.getTriangle(_primitiveIndices[i]);
}
OSG_NOTICE<<"After KdTree setup"<<std::endl;
osg::KdTree::Indices& primitiveIndices = _kdTree.getPrimitiveIndices();
_kdTree.getTriangles().swap(triangleList);
KdTree::Indices new_indices;
new_indices.reserve(_primitiveIndices.size());
for(Indices::iterator itr = _primitiveIndices.begin();
itr != _primitiveIndices.end();
++itr)
{
new_indices.push_back(primitiveIndices[*itr]);
}
primitiveIndices.swap(new_indices);
#ifdef VERBOSE_OUTPUT
@@ -217,18 +283,22 @@ int BuildKdTree::divide(KdTree::BuildOptions& options, osg::BoundingBox& bb, int
int istart = -node.first-1;
int iend = istart+node.second-1;
OSG_NOTICE<<"Computing bb on KdNode, numPrimitives="<<node.second<<std::endl;
// leaf is done, now compute bound on it.
node.bb.init();
for(int i=istart; i<=iend; ++i)
{
const KdTree::Triangle& tri = _kdTree.getTriangle(_primitiveIndices[i]);
const osg::Vec3& v0 = (*_kdTree.getVertices())[tri.p0];
const osg::Vec3& v1 = (*_kdTree.getVertices())[tri.p1];
const osg::Vec3& v2 = (*_kdTree.getVertices())[tri.p2];
node.bb.expandBy(v0);
node.bb.expandBy(v1);
node.bb.expandBy(v2);
unsigned int primitiveIndex = _kdTree.getPrimitiveIndices()[_primitiveIndices[i]];
unsigned int numPoints = _kdTree.getVertexIndices()[primitiveIndex++];
OSG_NOTICE<<" Primitive "<<primitiveIndex<<", numPoints="<<numPoints<<std::endl;
for(; numPoints>0; --numPoints)
{
unsigned int vi = _kdTree.getVertexIndices()[primitiveIndex++];
const osg::Vec3& v = (*_kdTree.getVertices())[vi];
node.bb.expandBy(v);
OSG_NOTICE<<" vi="<<vi<<", v="<<v<<std::endl;
}
}
if (node.bb.valid())
@@ -314,26 +384,6 @@ int BuildKdTree::divide(KdTree::BuildOptions& options, osg::BoundingBox& bb, int
KdTree::KdNode leftLeaf(-istart-1, (right-istart)+1);
KdTree::KdNode rightLeaf(-left-1, (iend-left)+1);
#if 0
OSG_NOTICE<<"In node.first ="<<node.first <<" node.second ="<<node.second<<std::endl;
OSG_NOTICE<<" leftLeaf.first ="<<leftLeaf.first <<" leftLeaf.second ="<<leftLeaf.second<<std::endl;
OSG_NOTICE<<" rightLeaf.first="<<rightLeaf.first<<" rightLeaf.second="<<rightLeaf.second<<std::endl;
OSG_NOTICE<<" left="<<left<<" right="<<right<<std::endl;
if (node.second != (leftLeaf.second +rightLeaf.second))
{
OSG_NOTICE<<"*** Error in size, leaf.second="<<node.second
<<", leftLeaf.second="<<leftLeaf.second
<<", rightLeaf.second="<<rightLeaf.second<<std::endl;
}
else
{
OSG_NOTICE<<"Size OK, leaf.second="<<node.second
<<", leftLeaf.second="<<leftLeaf.second
<<", rightLeaf.second="<<rightLeaf.second<<std::endl;
}
#endif
if (leftLeaf.second<=0)
{
//OSG_NOTICE<<"LeftLeaf empty"<<std::endl;
@@ -421,316 +471,6 @@ int BuildKdTree::divide(KdTree::BuildOptions& options, osg::BoundingBox& bb, int
}
////////////////////////////////////////////////////////////////////////////////
//
// IntersectKdTree
//
struct IntersectKdTree
{
IntersectKdTree(const osg::Vec3Array& vertices,
const KdTree::KdNodeList& nodes,
const KdTree::TriangleList& triangles,
KdTree::LineSegmentIntersections& intersections,
const osg::Vec3d& s, const osg::Vec3d& e):
_vertices(vertices),
_kdNodes(nodes),
_triangles(triangles),
_intersections(intersections),
_s(s),
_e(e)
{
_d = e - s;
_length = _d.length();
_inverse_length = _length!=0.0f ? 1.0f/_length : 0.0;
_d *= _inverse_length;
_d_invX = _d.x()!=0.0f ? _d/_d.x() : osg::Vec3(0.0f,0.0f,0.0f);
_d_invY = _d.y()!=0.0f ? _d/_d.y() : osg::Vec3(0.0f,0.0f,0.0f);
_d_invZ = _d.z()!=0.0f ? _d/_d.z() : osg::Vec3(0.0f,0.0f,0.0f);
}
void intersect(const KdTree::KdNode& node, const osg::Vec3& s, const osg::Vec3& e) const;
bool intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const;
const osg::Vec3Array& _vertices;
const KdTree::KdNodeList& _kdNodes;
const KdTree::TriangleList& _triangles;
KdTree::LineSegmentIntersections& _intersections;
osg::Vec3 _s;
osg::Vec3 _e;
osg::Vec3 _d;
float _length;
float _inverse_length;
osg::Vec3 _d_invX;
osg::Vec3 _d_invY;
osg::Vec3 _d_invZ;
protected:
IntersectKdTree& operator = (const IntersectKdTree&) { return *this; }
};
void IntersectKdTree::intersect(const KdTree::KdNode& node, const osg::Vec3& ls, const osg::Vec3& le) const
{
if (node.first<0)
{
// treat as a leaf
//OSG_NOTICE<<"KdTree::intersect("<<&leaf<<")"<<std::endl;
int istart = -node.first-1;
int iend = istart + node.second;
for(int i=istart; i<iend; ++i)
{
//const Triangle& tri = _triangles[_primitiveIndices[i]];
const KdTree::Triangle& tri = _triangles[i];
// OSG_NOTICE<<" tri("<<tri.p1<<","<<tri.p2<<","<<tri.p3<<")"<<std::endl;
const osg::Vec3& v0 = _vertices[tri.p0];
const osg::Vec3& v1 = _vertices[tri.p1];
const osg::Vec3& v2 = _vertices[tri.p2];
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) continue;
osg::Vec3 Q = T ^ E1;
float v = (Q*_d);
if (v<0.0 || v>det) continue;
if ((u+v)> det) continue;
float inv_det = 1.0f/det;
float t = (Q*E2)*inv_det;
if (t<0.0 || t>_length) continue;
u *= inv_det;
v *= inv_det;
r0 = 1.0f-u-v;
r1 = u;
r2 = v;
r = t * _inverse_length;
}
else if (det<-esplison)
{
float u = (P*T);
if (u>0.0 || u<det) continue;
osg::Vec3 Q = T ^ E1;
float v = (Q*_d);
if (v>0.0 || v<det) continue;
if ((u+v) < det) continue;
float inv_det = 1.0f/det;
float t = (Q*E2)*inv_det;
if (t<0.0 || t>_length) continue;
u *= inv_det;
v *= inv_det;
r0 = 1.0f-u-v;
r1 = u;
r2 = v;
r = t * _inverse_length;
}
else
{
continue;
}
osg::Vec3 in = v0*r0 + v1*r1 + v2*r2;
osg::Vec3 normal = E1^E2;
normal.normalize();
#if 1
_intersections.push_back(KdTree::LineSegmentIntersection());
KdTree::LineSegmentIntersection& intersection = _intersections.back();
intersection.ratio = r;
intersection.primitiveIndex = i;
intersection.intersectionPoint = in;
intersection.intersectionNormal = normal;
intersection.p0 = tri.p0;
intersection.p1 = tri.p1;
intersection.p2 = tri.p2;
intersection.r0 = r0;
intersection.r1 = r1;
intersection.r2 = r2;
#endif
// OSG_NOTICE<<" got intersection ("<<in<<") ratio="<<r<<std::endl;
}
}
else
{
if (node.first>0)
{
osg::Vec3 l(ls), e(le);
if (intersectAndClip(l,e, _kdNodes[node.first].bb))
{
intersect(_kdNodes[node.first], l, e);
}
}
if (node.second>0)
{
osg::Vec3 l(ls), e(le);
if (intersectAndClip(l,e, _kdNodes[node.second].bb))
{
intersect(_kdNodes[node.second], l, e);
}
}
}
}
bool IntersectKdTree::intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const
{
//return true;
//if (!bb.valid()) return true;
// 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.xMin()) return false;
if (s.x()>bb.xMax()) return false;
if (s.x()<bb.xMin())
{
// clip s to xMin.
s = s+_d_invX*(bb.xMin()-s.x());
}
if (e.x()>bb.xMax())
{
// clip e to xMax.
e = s+_d_invX*(bb.xMax()-s.x());
}
}
else
{
if (s.x()<bb.xMin()) return false;
if (e.x()>bb.xMax()) return false;
if (e.x()<bb.xMin())
{
// clip s to xMin.
e = s+_d_invX*(bb.xMin()-s.x());
}
if (s.x()>bb.xMax())
{
// clip e to xMax.
s = s+_d_invX*(bb.xMax()-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.yMin()) return false;
if (s.y()>bb.yMax()) return false;
if (s.y()<bb.yMin())
{
// clip s to yMin.
s = s+_d_invY*(bb.yMin()-s.y());
}
if (e.y()>bb.yMax())
{
// clip e to yMax.
e = s+_d_invY*(bb.yMax()-s.y());
}
}
else
{
if (s.y()<bb.yMin()) return false;
if (e.y()>bb.yMax()) return false;
if (e.y()<bb.yMin())
{
// clip s to yMin.
e = s+_d_invY*(bb.yMin()-s.y());
}
if (s.y()>bb.yMax())
{
// clip e to yMax.
s = s+_d_invY*(bb.yMax()-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.zMin()) return false;
if (s.z()>bb.zMax()) return false;
if (s.z()<bb.zMin())
{
// clip s to zMin.
s = s+_d_invZ*(bb.zMin()-s.z());
}
if (e.z()>bb.zMax())
{
// clip e to zMax.
e = s+_d_invZ*(bb.zMax()-s.z());
}
}
else
{
if (s.z()<bb.zMin()) return false;
if (e.z()>bb.zMax()) return false;
if (e.z()<bb.zMin())
{
// clip s to zMin.
e = s+_d_invZ*(bb.zMin()-s.z());
}
if (s.z()>bb.zMax())
{
// clip e to zMax.
s = s+_d_invZ*(bb.zMax()-s.z());
}
}
// OSG_NOTICE<<"clampped segment "<<s<<" "<<e<<std::endl;
// if (s==e) return false;
return true;
}
////////////////////////////////////////////////////////////////////////////////
//
// KdTree::BuildOptions
@@ -753,8 +493,7 @@ KdTree::KdTree()
KdTree::KdTree(const KdTree& rhs, const osg::CopyOp& copyop):
Shape(rhs, copyop),
_vertices(rhs._vertices),
_kdNodes(rhs._kdNodes),
_triangles(rhs._triangles)
_kdNodes(rhs._kdNodes)
{
}
@@ -764,27 +503,6 @@ bool KdTree::build(BuildOptions& options, osg::Geometry* geometry)
return build.build(options, geometry);
}
bool KdTree::intersect(const osg::Vec3d& start, const osg::Vec3d& end, LineSegmentIntersections& intersections) const
{
if (_kdNodes.empty())
{
OSG_NOTICE<<"Warning: _kdTree is empty"<<std::endl;
return false;
}
unsigned int numIntersectionsBefore = intersections.size();
IntersectKdTree intersector(*_vertices,
_kdNodes,
_triangles,
intersections,
start, end);
intersector.intersect(getNode(0), start, end);
return numIntersectionsBefore != intersections.size();
}
////////////////////////////////////////////////////////////////////////////////
//
// KdTreeBuilder