804 lines
19 KiB
C++
804 lines
19 KiB
C++
/*************************************************************************/
|
|
/* broad_phase_2d_hash_grid.cpp */
|
|
/*************************************************************************/
|
|
/* This file is part of: */
|
|
/* GODOT ENGINE */
|
|
/* http://www.godotengine.org */
|
|
/*************************************************************************/
|
|
/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */
|
|
/* */
|
|
/* Permission is hereby granted, free of charge, to any person obtaining */
|
|
/* a copy of this software and associated documentation files (the */
|
|
/* "Software"), to deal in the Software without restriction, including */
|
|
/* without limitation the rights to use, copy, modify, merge, publish, */
|
|
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
|
/* permit persons to whom the Software is furnished to do so, subject to */
|
|
/* the following conditions: */
|
|
/* */
|
|
/* The above copyright notice and this permission notice shall be */
|
|
/* included in all copies or substantial portions of the Software. */
|
|
/* */
|
|
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
|
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
|
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
|
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
|
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
|
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
|
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
|
/*************************************************************************/
|
|
#include "broad_phase_2d_hash_grid.h"
|
|
#include "globals.h"
|
|
|
|
#define LARGE_ELEMENT_FI 1.01239812
|
|
|
|
void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element* p_with) {
|
|
|
|
Map<Element*,PairData*>::Element *E=p_elem->paired.find(p_with);
|
|
|
|
ERR_FAIL_COND(p_elem->_static && p_with->_static);
|
|
|
|
if (!E) {
|
|
|
|
PairData *pd = memnew( PairData );
|
|
p_elem->paired[p_with]=pd;
|
|
p_with->paired[p_elem]=pd;
|
|
} else {
|
|
E->get()->rc++;
|
|
}
|
|
|
|
}
|
|
|
|
void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element* p_with) {
|
|
|
|
Map<Element*,PairData*>::Element *E=p_elem->paired.find(p_with);
|
|
|
|
ERR_FAIL_COND(!E); //this should really be paired..
|
|
|
|
E->get()->rc--;
|
|
|
|
if (E->get()->rc==0) {
|
|
|
|
if (E->get()->colliding) {
|
|
//uncollide
|
|
if (unpair_callback) {
|
|
unpair_callback(p_elem->owner,p_elem->subindex,p_with->owner,p_with->subindex,E->get()->ud,unpair_userdata);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
memdelete(E->get());
|
|
p_elem->paired.erase(E);
|
|
p_with->paired.erase(p_elem);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {
|
|
|
|
for (Map<Element*,PairData*>::Element *E=p_elem->paired.front();E;E=E->next()) {
|
|
|
|
bool pairing = p_elem->aabb.intersects( E->key()->aabb );
|
|
|
|
if (pairing!=E->get()->colliding) {
|
|
|
|
if (pairing) {
|
|
|
|
if (pair_callback) {
|
|
E->get()->ud=pair_callback(p_elem->owner,p_elem->subindex,E->key()->owner,E->key()->subindex,pair_userdata);
|
|
}
|
|
} else {
|
|
|
|
if (unpair_callback) {
|
|
unpair_callback(p_elem->owner,p_elem->subindex,E->key()->owner,E->key()->subindex,E->get()->ud,unpair_userdata);
|
|
}
|
|
|
|
}
|
|
|
|
E->get()->colliding=pairing;
|
|
}
|
|
}
|
|
}
|
|
|
|
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.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());
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
//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::_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
|
|
for (Map<Element*,PairData*>::Element *E=p_elem->paired.front();E;E=E->next()) {
|
|
|
|
_unpair_attempt(p_elem,E->key());
|
|
}
|
|
|
|
if (large_elements[p_elem].dec()==0) {
|
|
large_elements.erase(p_elem);
|
|
}
|
|
return;
|
|
}
|
|
|
|
|
|
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);
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
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());
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex) {
|
|
|
|
current++;
|
|
|
|
Element e;
|
|
e.owner=p_object;
|
|
e._static=false;
|
|
e.subindex=p_subindex;
|
|
e.self=current;
|
|
e.pass=0;
|
|
|
|
element_map[current]=e;
|
|
return current;
|
|
|
|
}
|
|
|
|
void BroadPhase2DHashGrid::move(ID p_id, const Rect2& p_aabb) {
|
|
|
|
|
|
Map<ID,Element>::Element *E=element_map.find(p_id);
|
|
ERR_FAIL_COND(!E);
|
|
|
|
Element &e=E->get();
|
|
|
|
if (p_aabb==e.aabb)
|
|
return;
|
|
|
|
|
|
if (p_aabb!=Rect2()) {
|
|
|
|
_enter_grid(&e,p_aabb,e._static);
|
|
}
|
|
|
|
if (e.aabb!=Rect2()) {
|
|
|
|
_exit_grid(&e,e.aabb,e._static);
|
|
}
|
|
|
|
e.aabb=p_aabb;
|
|
|
|
_check_motion(&e);
|
|
|
|
e.aabb=p_aabb;
|
|
|
|
}
|
|
void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) {
|
|
|
|
Map<ID,Element>::Element *E=element_map.find(p_id);
|
|
ERR_FAIL_COND(!E);
|
|
|
|
Element &e=E->get();
|
|
|
|
if (e._static==p_static)
|
|
return;
|
|
|
|
if (e.aabb!=Rect2())
|
|
_exit_grid(&e,e.aabb,e._static);
|
|
|
|
e._static=p_static;
|
|
|
|
if (e.aabb!=Rect2()) {
|
|
_enter_grid(&e,e.aabb,e._static);
|
|
_check_motion(&e);
|
|
}
|
|
|
|
}
|
|
void BroadPhase2DHashGrid::remove(ID p_id) {
|
|
|
|
Map<ID,Element>::Element *E=element_map.find(p_id);
|
|
ERR_FAIL_COND(!E);
|
|
|
|
Element &e=E->get();
|
|
|
|
if (e.aabb!=Rect2())
|
|
_exit_grid(&e,e.aabb,e._static);
|
|
|
|
element_map.erase(p_id);
|
|
|
|
}
|
|
|
|
CollisionObject2DSW *BroadPhase2DHashGrid::get_object(ID p_id) const {
|
|
|
|
const Map<ID,Element>::Element *E=element_map.find(p_id);
|
|
ERR_FAIL_COND_V(!E,NULL);
|
|
return E->get().owner;
|
|
|
|
}
|
|
bool BroadPhase2DHashGrid::is_static(ID p_id) const {
|
|
|
|
const Map<ID,Element>::Element *E=element_map.find(p_id);
|
|
ERR_FAIL_COND_V(!E,false);
|
|
return E->get()._static;
|
|
|
|
}
|
|
int BroadPhase2DHashGrid::get_subindex(ID p_id) const {
|
|
|
|
const Map<ID,Element>::Element *E=element_map.find(p_id);
|
|
ERR_FAIL_COND_V(!E,-1);
|
|
return E->get().subindex;
|
|
}
|
|
|
|
template<bool use_aabb,bool use_segment>
|
|
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++;
|
|
|
|
}
|
|
}
|
|
|
|
int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
|
|
|
|
pass++;
|
|
|
|
Vector2 dir = (p_to-p_from);
|
|
if (dir==Vector2())
|
|
return 0;
|
|
//avoid divisions by zero
|
|
dir.normalize();
|
|
if (dir.x==0.0)
|
|
dir.x=0.000001;
|
|
if (dir.y==0.0)
|
|
dir.y=0.000001;
|
|
Vector2 delta = dir.abs();
|
|
|
|
delta.x=cell_size/delta.x;
|
|
delta.y=cell_size/delta.y;
|
|
|
|
Point2i pos = (p_from/cell_size).floor();
|
|
Point2i end = (p_to/cell_size).floor();
|
|
|
|
Point2i step = Vector2( SGN(dir.x), SGN(dir.y) );
|
|
|
|
Vector2 max;
|
|
|
|
if (dir.x<0)
|
|
max.x= (Math::floor(pos.x)*cell_size - p_from.x) / dir.x;
|
|
else
|
|
max.x= (Math::floor(pos.x + 1)*cell_size - p_from.x) / dir.x;
|
|
|
|
if (dir.y<0)
|
|
max.y= (Math::floor(pos.y)*cell_size - p_from.y) / dir.y;
|
|
else
|
|
max.y= (Math::floor(pos.y + 1)*cell_size - p_from.y) / dir.y;
|
|
|
|
int cullcount=0;
|
|
_cull<false,true>(pos,Rect2(),p_from,p_to,p_results,p_max_results,p_result_indices,cullcount);
|
|
|
|
bool reached_x=false;
|
|
bool reached_y=false;
|
|
|
|
while(true) {
|
|
|
|
if (max.x < max.y) {
|
|
|
|
max.x+=delta.x;
|
|
pos.x+=step.x;
|
|
} else {
|
|
|
|
max.y+=delta.y;
|
|
pos.y+=step.y;
|
|
|
|
}
|
|
|
|
if (step.x>0) {
|
|
if (pos.x>=end.x)
|
|
reached_x=true;
|
|
} else if (pos.x<=end.x) {
|
|
|
|
reached_x=true;
|
|
}
|
|
|
|
if (step.y>0) {
|
|
if (pos.y>=end.y)
|
|
reached_y=true;
|
|
} else if (pos.y<=end.y) {
|
|
|
|
reached_y=true;
|
|
}
|
|
|
|
_cull<false,true>(pos,Rect2(),p_from,p_to,p_results,p_max_results,p_result_indices,cullcount);
|
|
|
|
if (reached_x && reached_y)
|
|
break;
|
|
|
|
}
|
|
|
|
for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) {
|
|
|
|
if (cullcount>=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 (!E->key()->aabb.intersects_segment(p_from,p_to))
|
|
continue;
|
|
|
|
p_results[cullcount]=E->key()->owner;
|
|
p_result_indices[cullcount]=E->key()->subindex;
|
|
cullcount++;
|
|
|
|
|
|
}
|
|
|
|
return cullcount;
|
|
}
|
|
|
|
|
|
int BroadPhase2DHashGrid::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
|
|
|
|
pass++;
|
|
|
|
Point2i from = (p_aabb.pos/cell_size).floor();
|
|
Point2i to = ((p_aabb.pos+p_aabb.size)/cell_size).floor();
|
|
int cullcount=0;
|
|
|
|
for(int i=from.x;i<=to.x;i++) {
|
|
|
|
for(int j=from.y;j<=to.y;j++) {
|
|
|
|
_cull<true,false>(Point2i(i,j),p_aabb,Point2(),Point2(),p_results,p_max_results,p_result_indices,cullcount);
|
|
}
|
|
|
|
}
|
|
|
|
for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) {
|
|
|
|
if (cullcount>=p_max_results)
|
|
break;
|
|
if (E->key()->pass==pass)
|
|
continue;
|
|
|
|
E->key()->pass=pass;
|
|
|
|
if (!p_aabb.intersects(E->key()->aabb))
|
|
continue;
|
|
|
|
// if (!E->key()->aabb.intersects_segment(p_from,p_to))
|
|
// continue;
|
|
|
|
p_results[cullcount]=E->key()->owner;
|
|
p_result_indices[cullcount]=E->key()->subindex;
|
|
cullcount++;
|
|
|
|
|
|
}
|
|
return cullcount;
|
|
}
|
|
|
|
void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) {
|
|
|
|
pair_callback=p_pair_callback;
|
|
pair_userdata=p_userdata;
|
|
|
|
}
|
|
void BroadPhase2DHashGrid::set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata) {
|
|
|
|
unpair_callback=p_unpair_callback;
|
|
unpair_userdata=p_userdata;
|
|
|
|
}
|
|
|
|
void BroadPhase2DHashGrid::update() {
|
|
|
|
|
|
}
|
|
|
|
BroadPhase2DSW *BroadPhase2DHashGrid::_create() {
|
|
|
|
return memnew( BroadPhase2DHashGrid );
|
|
}
|
|
|
|
|
|
BroadPhase2DHashGrid::BroadPhase2DHashGrid() {
|
|
|
|
hash_table_size = GLOBAL_DEF("physics_2d/bp_hash_table_size",4096);
|
|
hash_table_size = Math::larger_prime(hash_table_size);
|
|
hash_table = memnew_arr( PosBin*, hash_table_size);
|
|
|
|
cell_size = GLOBAL_DEF("physics_2d/cell_size",128);
|
|
large_object_min_surface = GLOBAL_DEF("physics_2d/large_object_surface_treshold_in_cells",512);
|
|
|
|
for(int i=0;i<hash_table_size;i++)
|
|
hash_table[i]=NULL;
|
|
pass=1;
|
|
|
|
current=0;
|
|
}
|
|
|
|
BroadPhase2DHashGrid::~BroadPhase2DHashGrid() {
|
|
|
|
for(int i=0;i<hash_table_size;i++) {
|
|
while(hash_table[i]) {
|
|
PosBin *pb=hash_table[i];
|
|
hash_table[i]=pb->next;
|
|
memdelete(pb);
|
|
}
|
|
}
|
|
|
|
memdelete_arr( hash_table );
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
/* 3D version of voxel traversal:
|
|
|
|
public IEnumerable<Point3D> GetCellsOnRay(Ray ray, int maxDepth)
|
|
{
|
|
// Implementation is based on:
|
|
// "A Fast Voxel Traversal Algorithm for Ray Tracing"
|
|
// John Amanatides, Andrew Woo
|
|
// http://www.cse.yorku.ca/~amana/research/grid.pdf
|
|
// http://www.devmaster.net/articles/raytracing_series/A%20faster%20voxel%20traversal%20algorithm%20for%20ray%20tracing.pdf
|
|
|
|
// NOTES:
|
|
// * This code assumes that the ray's position and direction are in 'cell coordinates', which means
|
|
// that one unit equals one cell in all directions.
|
|
// * When the ray doesn't start within the voxel grid, calculate the first position at which the
|
|
// ray could enter the grid. If it never enters the grid, there is nothing more to do here.
|
|
// * Also, it is important to test when the ray exits the voxel grid when the grid isn't infinite.
|
|
// * The Point3D structure is a simple structure having three integer fields (X, Y and Z).
|
|
|
|
// The cell in which the ray starts.
|
|
Point3D start = GetCellAt(ray.Position); // Rounds the position's X, Y and Z down to the nearest integer values.
|
|
int x = start.X;
|
|
int y = start.Y;
|
|
int z = start.Z;
|
|
|
|
// Determine which way we go.
|
|
int stepX = Math.Sign(ray.Direction.X);
|
|
int stepY = Math.Sign(ray.Direction.Y);
|
|
int stepZ = Math.Sign(ray.Direction.Z);
|
|
|
|
// Calculate cell boundaries. When the step (i.e. direction sign) is positive,
|
|
// the next boundary is AFTER our current position, meaning that we have to add 1.
|
|
// Otherwise, it is BEFORE our current position, in which case we add nothing.
|
|
Point3D cellBoundary = new Point3D(
|
|
x + (stepX > 0 ? 1 : 0),
|
|
y + (stepY > 0 ? 1 : 0),
|
|
z + (stepZ > 0 ? 1 : 0));
|
|
|
|
// NOTE: For the following calculations, the result will be Single.PositiveInfinity
|
|
// when ray.Direction.X, Y or Z equals zero, which is OK. However, when the left-hand
|
|
// value of the division also equals zero, the result is Single.NaN, which is not OK.
|
|
|
|
// Determine how far we can travel along the ray before we hit a voxel boundary.
|
|
Vector3 tMax = new Vector3(
|
|
(cellBoundary.X - ray.Position.X) / ray.Direction.X, // Boundary is a plane on the YZ axis.
|
|
(cellBoundary.Y - ray.Position.Y) / ray.Direction.Y, // Boundary is a plane on the XZ axis.
|
|
(cellBoundary.Z - ray.Position.Z) / ray.Direction.Z); // Boundary is a plane on the XY axis.
|
|
if (Single.IsNaN(tMax.X)) tMax.X = Single.PositiveInfinity;
|
|
if (Single.IsNaN(tMax.Y)) tMax.Y = Single.PositiveInfinity;
|
|
if (Single.IsNaN(tMax.Z)) tMax.Z = Single.PositiveInfinity;
|
|
|
|
// Determine how far we must travel along the ray before we have crossed a gridcell.
|
|
Vector3 tDelta = new Vector3(
|
|
stepX / ray.Direction.X, // Crossing the width of a cell.
|
|
stepY / ray.Direction.Y, // Crossing the height of a cell.
|
|
stepZ / ray.Direction.Z); // Crossing the depth of a cell.
|
|
if (Single.IsNaN(tDelta.X)) tDelta.X = Single.PositiveInfinity;
|
|
if (Single.IsNaN(tDelta.Y)) tDelta.Y = Single.PositiveInfinity;
|
|
if (Single.IsNaN(tDelta.Z)) tDelta.Z = Single.PositiveInfinity;
|
|
|
|
// For each step, determine which distance to the next voxel boundary is lowest (i.e.
|
|
// which voxel boundary is nearest) and walk that way.
|
|
for (int i = 0; i < maxDepth; i++)
|
|
{
|
|
// Return it.
|
|
yield return new Point3D(x, y, z);
|
|
|
|
// Do the next step.
|
|
if (tMax.X < tMax.Y && tMax.X < tMax.Z)
|
|
{
|
|
// tMax.X is the lowest, an YZ cell boundary plane is nearest.
|
|
x += stepX;
|
|
tMax.X += tDelta.X;
|
|
}
|
|
else if (tMax.Y < tMax.Z)
|
|
{
|
|
// tMax.Y is the lowest, an XZ cell boundary plane is nearest.
|
|
y += stepY;
|
|
tMax.Y += tDelta.Y;
|
|
}
|
|
else
|
|
{
|
|
// tMax.Z is the lowest, an XY cell boundary plane is nearest.
|
|
z += stepZ;
|
|
tMax.Z += tDelta.Z;
|
|
}
|
|
}
|
|
|
|
*/
|