void BroadPhase2DHashGrid::_cull(const Point2i p_cell,const Rect2& p_aabb,const Point2& p_from, const Point2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index) {
PosKey pk;
pk.x=p_cell.x;
pk.y=p_cell.y;
uint32_t idx = pk.hash() % hash_table_size;
PosBin *pb = hash_table[idx];
while (pb) {
if (pb->key == pk) {
break;
}
pb=pb->next;
}
if (!pb)
return;
for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) {
if (index>=p_max_results)
break;
if (E->key()->pass==pass)
continue;
E->key()->pass=pass;
if (use_aabb && !p_aabb.intersects(E->key()->aabb))
continue;
if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to))
continue;
p_results[index]=E->key()->owner;
p_result_indices[index]=E->key()->subindex;
index++;
}
for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) {
if (index>=p_max_results)
break;
if (E->key()->pass==pass)
continue;
if (use_aabb && !p_aabb.intersects(E->key()->aabb)) {
continue;
}
if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to))
continue;
E->key()->pass=pass;
p_results[index]=E->key()->owner;
p_result_indices[index]=E->key()->subindex;
index++;
}
}
void BroadPhase2DHashGrid::_exit_grid( Element* p_elem, const Rect2& p_rect,bool p_static) {
Point2i from = (p_rect.pos/cell_size).floor();
Point2i to = ((p_rect.pos+p_rect.size)/cell_size).floor();
for(int i=from.x;i<=to.x;i++) {
for(int j=from.y;j<=to.y;j++) {
PosKey pk;
pk.x=i;
pk.y=j;
uint32_t idx = pk.hash() % hash_table_size;
PosBin *pb = hash_table[idx];
while (pb) {
if (pb->key == pk) {
break;
}
pb=pb->next;
}
ERR_CONTINUE(!pb); //should exist!!
bool exited=false;
if (p_static) {
if (pb->static_object_set[p_elem].dec()==0) {
pb->static_object_set.erase(p_elem);
exited=true;
}
} else {
if (pb->object_set[p_elem].dec()==0) {
pb->object_set.erase(p_elem);
exited=true;
}
}
if (exited) {
for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) {
if (E->key()->owner==p_elem->owner)
continue;
_unpair_attempt(p_elem,E->key());
}
if (!p_static) {
for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) {
if (E->key()->owner==p_elem->owner)
continue;
_unpair_attempt(p_elem,E->key());
}
}
}
if (pb->object_set.empty() && pb->static_object_set.empty()) {
if (hash_table[idx]==pb) {
hash_table[idx]=pb->next;
} else {
PosBin *px = hash_table[idx];
while (px) {
if (px->next==pb) {
px->next=pb->next;
break;
}
px=px->next;
}
ERR_CONTINUE(!px);
}
memdelete(pb);
}
}
}
}
void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) {
Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); //use magic number to avoid floating point issues
if (sz.width * sz.height > large_object_min_surface) {
//large object, do not use grid, must check against all elements
for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
if (E->key() == p_elem->self)
continue; // do not pair against itself
if (E->get().owner == p_elem->owner)
continue;
if (E->get()._static && p_static)
continue;
_pair_attempt(p_elem, &E->get());
}
large_elements[p_elem].inc();
return;
}
Point2i from = (p_rect.position / cell_size).floor();
Point2i to = ((p_rect.position + p_rect.size) / cell_size).floor();
for (int i = from.x; i <= to.x; i++) {
for (int j = from.y; j <= to.y; j++) {
PosKey pk;
pk.x = i;
pk.y = j;
uint32_t idx = pk.hash() % hash_table_size;
PosBin *pb = hash_table[idx];
while (pb) {
if (pb->key == pk) {
break;
}
pb = pb->next;
}
bool entered = false;
if (!pb) {
//does not exist, create!
pb = memnew(PosBin);
pb->key = pk;
pb->next = hash_table[idx];
hash_table[idx] = pb;
}
if (p_static) {
if (pb->static_object_set[p_elem].inc() == 1) {
entered = true;
}
} else {
if (pb->object_set[p_elem].inc() == 1) {
entered = true;
}
}
if (entered) {
for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {
if (E->key()->owner == p_elem->owner)
continue;
_pair_attempt(p_elem, E->key());
}
if (!p_static) {
for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {
if (E->key()->owner == p_elem->owner)
continue;
_pair_attempt(p_elem, E->key());
}
}
}
}
}
//pair separatedly with large elements
for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
if (E->key() == p_elem)
continue; // do not pair against itself
if (E->key()->owner == p_elem->owner)
continue;
if (E->key()->_static && p_static)
continue;
_pair_attempt(E->key(), p_elem);
}
}
void BroadPhase2DHashGrid::_enter_grid( Element* p_elem, const Rect2& p_rect,bool p_static) {
Point2i from = (p_rect.pos/cell_size).floor();
Point2i to = ((p_rect.pos+p_rect.size)/cell_size).floor();
for(int i=from.x;i<=to.x;i++) {
for(int j=from.y;j<=to.y;j++) {
PosKey pk;
pk.x=i;
pk.y=j;
uint32_t idx = pk.hash() % hash_table_size;
PosBin *pb = hash_table[idx];
while (pb) {
if (pb->key == pk) {
break;
}
pb=pb->next;
}
bool entered=false;
if (!pb) {
//does not exist, create!
pb = memnew( PosBin );
pb->key=pk;
pb->next=hash_table[idx];
hash_table[idx]=pb;
}
if (p_static) {
if (pb->static_object_set[p_elem].inc()==1) {
entered=true;
}
} else {
if (pb->object_set[p_elem].inc()==1) {
entered=true;
}
}
if (entered) {
for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) {
if (E->key()->owner==p_elem->owner)
continue;
_pair_attempt(p_elem,E->key());
}
if (!p_static) {
for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) {
if (E->key()->owner==p_elem->owner)
continue;
_pair_attempt(p_elem,E->key());
}
}
}
}
}
}
void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) {
Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI);
if (sz.width * sz.height > large_object_min_surface) {
//unpair all elements, instead of checking all, just check what is already paired, so we at least save from checking static vs static
Map<Element *, PairData *>::Element *E = p_elem->paired.front();
while (E) {
Map<Element *, PairData *>::Element *next = E->next();
_unpair_attempt(p_elem, E->key());
E = next;
}
if (large_elements[p_elem].dec() == 0) {
large_elements.erase(p_elem);
}
return;
}
Point2i from = (p_rect.position / cell_size).floor();
Point2i to = ((p_rect.position + p_rect.size) / cell_size).floor();
for (int i = from.x; i <= to.x; i++) {
for (int j = from.y; j <= to.y; j++) {
PosKey pk;
pk.x = i;
pk.y = j;
uint32_t idx = pk.hash() % hash_table_size;
PosBin *pb = hash_table[idx];
while (pb) {
if (pb->key == pk) {
break;
}
pb = pb->next;
}
ERR_CONTINUE(!pb); //should exist!!
bool exited = false;
if (p_static) {
if (pb->static_object_set[p_elem].dec() == 0) {
pb->static_object_set.erase(p_elem);
exited = true;
}
} else {
if (pb->object_set[p_elem].dec() == 0) {
pb->object_set.erase(p_elem);
exited = true;
}
}
if (exited) {
for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {
if (E->key()->owner == p_elem->owner)
continue;
_unpair_attempt(p_elem, E->key());
}
if (!p_static) {
for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {
if (E->key()->owner == p_elem->owner)
continue;
_unpair_attempt(p_elem, E->key());
}
}
}
if (pb->object_set.empty() && pb->static_object_set.empty()) {
if (hash_table[idx] == pb) {
hash_table[idx] = pb->next;
} else {
PosBin *px = hash_table[idx];
while (px) {
if (px->next == pb) {
px->next = pb->next;
break;
}
px = px->next;
}
ERR_CONTINUE(!px);
}
memdelete(pb);
}
}
}
for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
if (E->key() == p_elem)
continue; // do not pair against itself
if (E->key()->owner == p_elem->owner)
continue;
if (E->key()->_static && p_static)
continue;
//unpair from large elements
_unpair_attempt(p_elem, E->key());
}
}