diff options
Diffstat (limited to '')
| -rw-r--r-- | servers/physics_2d/broad_phase_2d_hash_grid.cpp | 665 |
1 files changed, 665 insertions, 0 deletions
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp new file mode 100644 index 000000000..10da376df --- /dev/null +++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp @@ -0,0 +1,665 @@ +/*************************************************************************/ +/* broad_phase_2d_hash_grid.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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" + +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) { + + + 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) { + + + 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); + + } + } + + } + +} + + +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; +} + +void BroadPhase2DHashGrid::_cull(const Point2i p_cell,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; + 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; + + 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.floor() / cell_size; + Point2i end = p_to.floor() / cell_size; + 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(pos,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(pos,p_results,p_max_results,p_result_indices,cullcount); + + if (reached_x && reached_y) + break; + + } + + return cullcount; +} + + +int BroadPhase2DHashGrid::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) { + + + return 0; +} + +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); + + 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; + } + } + + */ |