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;
}
Rect3 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;
}
_VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_size, int &count) {
_VolumeSW_BVH *bvh = memnew(_VolumeSW_BVH);
if (p_size == 1) {
//leaf
bvh->aabb = p_elements[0].aabb;
bvh->left = NULL;
bvh->right = NULL;
bvh->face_index = p_elements->face_index;
count++;
return bvh;
} else {
bvh->face_index = -1;
}
Rect3 aabb;
for (int i = 0; i < p_size; i++) {
if (i == 0)
aabb = p_elements[i].aabb;
else
aabb.merge_with(p_elements[i].aabb);
}
bvh->aabb = aabb;
switch (aabb.get_longest_axis_index()) {
case 0: {
SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareX> sort_x;
sort_x.sort(p_elements, p_size);
} break;
case 1: {
SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareY> sort_y;
sort_y.sort(p_elements, p_size);
} break;
case 2: {
SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareZ> sort_z;
sort_z.sort(p_elements, p_size);
} break;
}
int split = p_size / 2;
bvh->left = _volume_sw_build_bvh(p_elements, split, count);
bvh->right = _volume_sw_build_bvh(&p_elements[split], p_size - split, count);
//printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);
count++;
return bvh;
}
void GridMap::_octant_update(const OctantKey &p_key) {
ERR_FAIL_COND(!octant_map.has(p_key));
Octant &g = *octant_map[p_key];
if (!g.dirty)
return;
Ref<Mesh> mesh;
_octant_clear_navmesh(p_key);
PhysicsServer::get_singleton()->body_clear_shapes(g.static_body);
if (g.collision_debug.is_valid()) {
VS::get_singleton()->mesh_clear(g.collision_debug);
}
PoolVector<Vector3> col_debug;
/*
* foreach item in this octant,
* set item's multimesh's instance count to number of cells which have this item
* and set said multimesh bounding box to one containing all cells which have this item
*/
for (Map<int, Octant::ItemInstances>::Element *E = g.items.front(); E; E = E->next()) {
Octant::ItemInstances &ii = E->get();
ii.multimesh->set_instance_count(ii.cells.size());
Rect3 aabb;
Rect3 mesh_aabb = ii.mesh.is_null() ? Rect3() : ii.mesh->get_aabb();
Vector3 ofs(cell_size * 0.5 * int(center_x), cell_size * 0.5 * int(center_y), cell_size * 0.5 * int(center_z));
//print_line("OCTANT, CELLS: "+itos(ii.cells.size()));
int idx = 0;
// foreach cell containing this item type
for (Set<IndexKey>::Element *F = ii.cells.front(); F; F = F->next()) {
IndexKey ik = F->get();
Map<IndexKey, Cell>::Element *C = cell_map.find(ik);
ERR_CONTINUE(!C);
Vector3 cellpos = Vector3(ik.x, ik.y, ik.z);
Transform xform;
if (clip && ((clip_above && cellpos[clip_axis] > clip_floor) || (!clip_above && cellpos[clip_axis] < clip_floor))) {
xform.basis.set_zero();
} else {
xform.basis.set_orthogonal_index(C->get().rot);
}
xform.set_origin(cellpos * cell_size + ofs);
xform.basis.scale(Vector3(cell_scale, cell_scale, cell_scale));
ii.multimesh->set_instance_transform(idx, xform);
//ii.multimesh->set_instance_transform(idx,Transform() );
//ii.multimesh->set_instance_color(idx,Color(1,1,1,1));
//print_line("MMINST: "+xform);
if (idx == 0) {
aabb = xform.xform(mesh_aabb);
} else {
aabb.merge_with(xform.xform(mesh_aabb));
}
// add the item's shape at given xform to octant's static_body
if (ii.shape.is_valid()) {
// add the item's shape
PhysicsServer::get_singleton()->body_add_shape(g.static_body, ii.shape->get_rid(), xform);
if (g.collision_debug.is_valid()) {
ii.shape->add_vertices_to_array(col_debug, xform);
}
//print_line("PHIS x: "+xform);
}
// add the item's navmesh at given xform to GridMap's Navigation ancestor
if (navigation) {
if (ii.navmesh.is_valid()) {
int nm_id = navigation->navmesh_create(ii.navmesh, xform, this);
Octant::NavMesh nm;
nm.id = nm_id;
nm.xform = xform;
g.navmesh_ids[ik] = nm;
}
}
idx++;
}
//ii.multimesh->set_aabb(aabb);
}
if (col_debug.size()) {
Array arr;
arr.resize(VS::ARRAY_MAX);
arr[VS::ARRAY_VERTEX] = col_debug;
VS::get_singleton()->mesh_add_surface_from_arrays(g.collision_debug, VS::PRIMITIVE_LINES, arr);
SceneTree *st = SceneTree::get_singleton();
if (st) {
VS::get_singleton()->mesh_surface_set_material(g.collision_debug, 0, st->get_debug_collision_material()->get_rid());
}
}
g.dirty = false;
}
Rect3 Rect3::merge(const Rect3 &p_with) const {
Rect3 aabb = *this;
aabb.merge_with(p_with);
return aabb;
}