int TriangleMesh::_create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, int p_depth, int &max_depth, int &max_alloc) {
if (p_depth > max_depth) {
max_depth = p_depth;
}
if (p_size == 1) {
return p_bb[p_from] - p_bvh;
} else if (p_size == 0) {
return -1;
}
AABB aabb;
aabb = p_bb[p_from]->aabb;
for (int i = 1; i < p_size; i++) {
aabb.merge_with(p_bb[p_from + i]->aabb);
}
int li = aabb.get_longest_axis_index();
switch (li) {
case Vector3::AXIS_X: {
SortArray<BVH *, BVHCmpX> sort_x;
sort_x.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
//sort_x.sort(&p_bb[p_from],p_size);
} break;
case Vector3::AXIS_Y: {
SortArray<BVH *, BVHCmpY> sort_y;
sort_y.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
//sort_y.sort(&p_bb[p_from],p_size);
} break;
case Vector3::AXIS_Z: {
SortArray<BVH *, BVHCmpZ> sort_z;
sort_z.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
//sort_z.sort(&p_bb[p_from],p_size);
} break;
}
int left = _create_bvh(p_bvh, p_bb, p_from, p_size / 2, p_depth + 1, max_depth, max_alloc);
int right = _create_bvh(p_bvh, p_bb, p_from + p_size / 2, p_size - p_size / 2, p_depth + 1, max_depth, max_alloc);
int index = max_alloc++;
BVH *_new = &p_bvh[index];
_new->aabb = aabb;
_new->center = aabb.position + aabb.size * 0.5;
_new->face_index = -1;
_new->left = left;
_new->right = right;
return index;
}
void TriangleMesh::create(const DVector<Vector3>& p_faces) {
valid=false;
int fc=p_faces.size();
ERR_FAIL_COND(!fc || ((fc%3) != 0));
fc/=3;
triangles.resize(fc);
bvh.resize(fc*3); //will never be larger than this (todo make better)
DVector<BVH>::Write bw = bvh.write();
{
//create faces and indices and base bvh
//except for the Set for repeated triangles, everything
//goes in-place.
DVector<Vector3>::Read r = p_faces.read();
DVector<Triangle>::Write w = triangles.write();
Map<Vector3,int> db;
for(int i=0;i<fc;i++) {
Triangle&f=w[i];
const Vector3 *v=&r[i*3];
for(int j=0;j<3;j++) {
int vidx=-1;
Vector3 vs=v[j].snapped(0.0001);
Map<Vector3,int>::Element *E=db.find(vs);
if (E) {
vidx=E->get();
} else {
vidx=db.size();
db[vs]=vidx;
}
f.indices[j]=vidx;
if (j==0)
bw[i].aabb.pos=vs;
else
bw[i].aabb.expand_to(vs);
}
f.normal=Face3(r[i*3+0],r[i*3+1],r[i*3+2]).get_plane().get_normal();
bw[i].left=-1;
bw[i].right=-1;
bw[i].face_index=i;
bw[i].center=bw[i].aabb.pos+bw[i].aabb.size*0.5;
}
vertices.resize(db.size());
DVector<Vector3>::Write vw = vertices.write();
for (Map<Vector3,int>::Element *E=db.front();E;E=E->next()) {
vw[E->get()]=E->key();
}
}
DVector<BVH*> bwptrs;
bwptrs.resize(fc);
DVector<BVH*>::Write bwp = bwptrs.write();
for(int i=0;i<fc;i++) {
bwp[i]=&bw[i];
}
max_depth=0;
int max_alloc=fc;
int max=_create_bvh(bw.ptr(),bwp.ptr(),0,fc,1,max_depth,max_alloc);
bw=DVector<BVH>::Write(); //clearup
bvh.resize(max_alloc); //resize back
valid=true;
}
