godot/servers/physics_2d/broad_phase_2d_hash_grid.cpp

/*************************************************************************/
/*  broad_phase_2d_hash_grid.cpp                                         */
/*************************************************************************/
/*                       This file is part of:                           */
/*                           GODOT ENGINE                                */
/*                    http://www.godotengine.org                         */
/*************************************************************************/
/* Copyright (c) 2007-2017 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;
	}
    }

    */