pretty much, it’s all out in the public domain, for what it’s worth this is what I use, we can simplify as were only ever interested in point3, it also helps to index into the original mesh and or use faces centers not verts when you want to constrain ray tracing
#include <max.h>
#include <vector>
#include <float.h>
#define SAFE_DELETE(a) if( (a) != NULL ) delete (a); (a) = NULL;
#define SQ(x) ((x)*(x))
inline float dsqu(const Point3& a,const Point3& b)
{
Point3 c(a.x - b.x, a.y - b.y, a.z - b.z);
return c.LengthSquared();
}
struct kd3node : public MaxHeapOperators
{
Point3 pos; // point position
int dir; // direction of the slice
int index; // vert or face index in the mesh
kd3node *left;
kd3node *right;
kd3node(const Point3& p, const int d, const int i) : pos(p), dir(d), index(i), left(NULL), right(NULL) {}
~kd3node() { SAFE_DELETE(left); SAFE_DELETE(right); }
float slice() const { return pos[dir]; }
int nextdir() const { return (dir + 1) % 3; }
float delta(const Point3& p) const { return p[dir] - pos[dir]; }
float dsqu(const Point3& p) const { return ::dsqu(pos,p); }
};
class kd3tree : public MaxHeapOperators
{
enum buildtype { verts, faces };
int size;
kd3node *root;
public:
kd3tree() : root(NULL) {}
kd3tree(Mesh& mesh, bool useverts = true) : root(NULL) { useverts ? buildfromverts(mesh) : buildfromfaces(mesh); }
~kd3tree() { clear(); }
void clear() { SAFE_DELETE(root); size = 0; }
kd3node* findnearest(const Point3 &pos)
{
kd3node* result = root;
float dist_sq = result->dsqu(pos);
traverse(root, pos, &result, dist_sq);
return result;
}
int findnearest(const Point3 &pos, float range, BitArray& result)
{
result.SetSize(size);
return traverse(root, pos, range, result);
}
void buildfromverts(Mesh& mesh);
void buildfromfaces(Mesh& mesh);
int getSize() { return size; }
protected:
kd3node* insert(kd3node **nptr, const Point3 &pos, int i, int dir);
int insert(const Point3 &pos, int i) { return insert(&root, pos, i, 0) ? 0 : -1; }
void traverse(kd3node *node, const Point3 &pos, kd3node **result, float& result_dist_sq);
int traverse(kd3node *node, const Point3 &pos, float range, BitArray& result);
};
kd3node* kd3tree::insert(kd3node **nptr, const Point3 &pos, int i, int dir)
{
if(!*nptr)
{
*nptr = new kd3node(pos,dir,i);
size++;
return *nptr;
}
kd3node* n = *nptr;
return pos[n->dir] < n->slice() ? insert(&n->left,pos,i,n->nextdir()) : insert(&n->right,pos,i,n->nextdir());
}
// nearest traverse
void kd3tree::traverse(kd3node *node, const Point3 &pos, kd3node **result, float& result_dist_sq)
{
if(!node) return;
kd3node* near_node = node->right;
kd3node* far_node = node->left;
if(node->delta(pos) <= 0) // reverse the tree ?
{
near_node = node->left;
far_node = node->right;
}
if(near_node)
traverse(near_node, pos, result, result_dist_sq);
if(far_node)
traverse(far_node, pos, result, result_dist_sq);
// Check the distance of the point at the current node, compare it with our best so far
float dist_sq = node->dsqu(pos);
if (dist_sq < result_dist_sq)
{
*result = node;
result_dist_sq = dist_sq;
}
}
// range traverse
int kd3tree::traverse(kd3node *node, const Point3 &pos, float range, BitArray& result)
{
if(!node) return 0;
float dist_sq = node->dsqu(pos);
int ret, added_res = 0;
if(dist_sq <= SQ(range))
{
result.Set(node->index);
added_res = 1;
}
float dx = node->delta(pos);
ret = traverse(dx <= 0.0 ? node->left : node->right, pos, range, result);
if(ret >= 0 && fabs(dx) < range)
{
added_res += ret;
ret = traverse(dx <= 0.0 ? node->right : node->left, pos, range, result);
}
if(ret == -1)
return -1;
added_res += ret;
return added_res;
}
void kd3tree::buildfromverts(Mesh& mesh)
{
int numverts = mesh.numVerts;
Point3* verts = mesh.verts;
for(int v = 0; v < numverts; ++v)
insert(verts[v],v);
}
void kd3tree::buildfromfaces(Mesh& mesh)
{
int numfaces = mesh.numFaces;
for(int f = 0; f < numfaces; ++f)
insert(mesh.FaceCenter(f),f);
}
in reality it’s pretty simple stuff and should be part of the max core mesh interface reducing 100000 compares to 5 is not to be sniffed at 