int OctreePointSet::RayCast(const Ray3D& r,Real radius) const
{
vector<int> raynodes;
FattenedRayLookup(r,radius,raynodes);
Vector3 temp;
Real r2 = radius*radius;
for(size_t i=0;i<raynodes.size();i++) {
const vector<Vector3>& pts = pointLists[raynodes[i]];
const vector<int>& bids = idLists[raynodes[i]];
Real closest = Inf;
int result = -1;
for(size_t k=0;k<pts.size();k++) {
Real t = r.closestPoint(pts[k],temp);
if(pts[k].distanceSquared(temp) <= r2) {
if(t < closest) {
closest = t;
result = bids[k];
}
}
}
if(result >= 0)
return result;
}
return -1;
}
void OctreePointSet::RayQuery(const Ray3D& r,Real radius,vector<Vector3>& points,vector<int>& ids) const
{
points.resize(0);
ids.resize(0);
vector<int> raynodes;
FattenedRayLookup(r,radius,raynodes);
Vector3 temp;
Real r2 = radius*radius;
for(size_t i=0;i<raynodes.size();i++) {
const vector<Vector3>& pts = pointLists[raynodes[i]];
const vector<int>& bids = idLists[raynodes[i]];
for(size_t k=0;k<pts.size();k++) {
r.closestPoint(pts[k],temp);
if(pts[k].distanceSquared(temp) <= r2) {
points.push_back(pts[k]);
ids.push_back(bids[k]);
}
}
}
}