#include "AdeptHeaders.hpp"
#include "CollisionGrid.hpp"
#include "Tile.hpp"
#include "CollisionVolume.hpp"
#include "Application.hpp"
#include "EntityManager.hpp"
#include "map.hpp"
#include <MLR\MLRFootStep.hpp>
#include <MLR\MLRShape.hpp>

//#define SPEW_CULL_INFO "your_name_here"

//############################################################################
//############################  CollisionGrid  ################################
//############################################################################

CollisionGrid*
	CollisionGrid::Instance = NULL;

CollisionGrid::ClassData
	*CollisionGrid::DefaultData = NULL;

DECLARE_TIMER(static, Ray_Cast);
DWORD Ray_Cast_Count;
DWORD Collision_Callbacks;
DWORD Collider_Count;
DWORD Collider_Collidee_Count;
DWORD Collider_Collider_Count;
bool CollisionGrid::s_ignoreOBBCollisions = false;
static bool __stdcall CheckIgnoreOBBCollisions() {return CollisionGrid::s_ignoreOBBCollisions;}
static void __stdcall EnableIgnoreOBBCollisions() {CollisionGrid::s_ignoreOBBCollisions = !CollisionGrid::s_ignoreOBBCollisions;}

#if 1
#define COLLISION_LOGIC(string) LOGIC("Collision::" string)
#else
#define COLLISION_LOGIC(string)
#endif

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CollisionGrid::InitializeClass()
{
	Verify(!DefaultData);
	DefaultData =
		new ClassData(
			CollisionGridClassID,
			"Adept::CollisionGrid",
			ElementRenderer::GridElement::DefaultData,
			reinterpret_cast<Factory>(&Make)
		);
	Register_Object(DefaultData);

#if !defined(NO_STATS)
	AddStatistic("Colliders", "entities", gos_DWORD, &Collider_Count, Stat_AutoReset);
	AddStatistic("Collider vs. Collidee", "tests", gos_DWORD, &Collider_Collidee_Count, Stat_AutoReset);
	AddStatistic("Collider vs. Collider", "tests", gos_DWORD, &Collider_Collider_Count, Stat_AutoReset);
	AddStatistic("Collision Callbacks", "callbacks", gos_DWORD, &Collision_Callbacks, Stat_AutoReset);
	AddStatistic("Ray Casts", "casts", gos_DWORD, &Ray_Cast_Count, Stat_AutoReset);
#endif

	Initialize_Timer(Ray_Cast, "Ray Casting");

	AddDebuggerMenuItem("Libraries\\Adept\\Ignore OBB Collisions", CheckIgnoreOBBCollisions, EnableIgnoreOBBCollisions, 0 );
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CollisionGrid::TerminateClass()
{
	Unregister_Object(DefaultData);
	delete DefaultData;
	DefaultData = NULL;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CollisionGrid::TestInstance()
{
	Verify(IsDerivedFrom(DefaultData));
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
CollisionGrid::CollisionGrid(
	BYTE row_count,
	BYTE column_count,
	Stuff::Scalar row_offset,
	Stuff::Scalar column_offset,
	Stuff::Scalar total_row_size,
	Stuff::Scalar total_column_size,
	Stuff::Scalar water_depth
):
	GridElement(
		row_count,
		column_count,
		row_offset,
		column_offset,
		total_row_size,
		total_column_size,
		DefaultData
	),
	m_lineColliders(NULL),
	m_solidColliders(NULL),
	m_collidingTiles(NULL)
{
	int total = row_count*column_count;
	m_tileArray = new SlotOf<Tile*>*[total];
	m_waterLevel.normal.x = 0.0f;
	m_waterLevel.normal.y = 1.0f;
	m_waterLevel.normal.z = 0.0f;
	m_waterLevel.offset = water_depth;
	for (int i=0; i<total; ++i)
	{
		m_tileArray[i] = new SlotOf<Tile*>(NULL);
		Check_Object(m_tileArray[i]);
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
CollisionGrid::~CollisionGrid()
{
	Check_Object(this);

	int i;
	for (i=0; i<m_list.GetLength(); ++i)
	{
		Check_Object(m_tileArray[i]);
		delete m_tileArray[i];
	}
	delete m_tileArray;
	for (i=0; i<m_list.GetLength(); ++i)
		m_list[i] = NULL;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CollisionGrid::AttachIndexedChild(
		BYTE row,
		BYTE column,
		Tile *tile
	)
{
	Check_Object(this);

	int index = row*m_columnCellCount+column;
	Verify(index < m_list.GetLength());
	Verify(!m_list[index]);
	m_list[index] = tile;
	m_tileArray[index]->Add(tile);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Tile*
	CollisionGrid::GetTile(
		BYTE row,
		BYTE column
	)
{
	Check_Object(this);

	int index = row*m_columnCellCount+column;
	Verify(index < m_list.GetLength());
	return m_tileArray[index]->GetCurrent();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

Tile*
	CollisionGrid::GetTile(WORD index)
{
	Check_Object(this);

	Verify(index < m_list.GetLength());
	return m_tileArray[index]->GetCurrent();
}


//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Entity*
	CollisionGrid::ProjectLine(Entity::CollisionQuery *query)
{
	Check_Object(this);
	Check_Object(query);
	Verify(query->m_line->m_length >= 0.0f);
	Start_Timer(Ray_Cast);
	RAYCAST_LOGIC("Project Line");
	Set_Statistic(Ray_Cast_Count, Ray_Cast_Count+1);

	//
	//------------------
	// Set up the normal
	//------------------
	//
	*query->m_normal = query->m_line->m_direction;
	#if defined(BSP_BUG)
		if (debug_bsp)
		{
			SPEW((BSP_BUG, "\nLength is %f", query->m_line->m_length));
		}
	#endif

	//
	//------------------------------------
	// Set up the strike against the water
	//------------------------------------
	//
	Entity *entity = NULL;
	if (query->m_collisionMask&Entity::WaterSurfaceFlag)
	{
		Stuff::Scalar product;
		Stuff::Scalar to_water = query->m_line->GetDistanceTo(m_waterLevel, &product);
		query->m_material = WaterMaterial;
		if (product<0.0f && to_water>=0.0f && to_water<=query->m_line->m_length)
		{
			*query->m_normal = m_waterLevel.normal;
			query->m_line->m_length = to_water;
			//
			// We know we at least intersected with the water, and
			// we need to store the map as the otherEntity in order to 
			// function correctly higher up where it has to have an entity
			// to believe it hit anything.
			//
			entity = Adept::Map::GetInstance();
		}
	}

	//
	//------------------------------------
	// Have the grid iterate the map zones
	//------------------------------------
	//
	Entity * temp = static_cast<Entity*>(IterateLine(query, Tile::TestTile));
	if (temp)
		entity = temp;
	
	Stop_Timer(Ray_Cast);
	return entity;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
bool
CollisionGrid::MakeFootStep(const LinearMatrix4D& where, Scalar radius, MidLevelRenderer::MLRTexture *foot_texture)
{
	if(MidLevelRenderer::MLRFootStep::Instance==NULL || MidLevelRenderer::MLRFootStep::Instance->ShowSteps()==false)
	{
		return false;
	}

	Point3D pos;

	pos = where;

	Scalar row_scale = 1.0f/m_rowCellSize;
	Scalar column_scale = 1.0f/m_columnCellSize;

	//
	//-------------------------------
	// Figure out the grid boundaries
	//-------------------------------
	//
	Scalar min_z = (pos.z - m_rowOffset - radius) * row_scale;
	BYTE minTileRow = (min_z < 0.0f) ? (BYTE)0 : Truncate_Float_To_Byte(min_z);

	Scalar max_z = (pos.z - m_rowOffset + radius) * row_scale;
	BYTE maxTileRow = Truncate_Float_To_Byte(max_z);
	if (maxTileRow >= m_rowCellCount)
		maxTileRow = static_cast<BYTE>(m_rowCellCount - 1);

	Scalar min_x = (pos.x - m_columnOffset - radius) * column_scale;
	BYTE minTileColumn = (min_x < 0.0f) ? (BYTE)0 : Truncate_Float_To_Byte(min_x);
	Scalar max_x = (pos.x - m_columnOffset + radius) * column_scale;
	BYTE maxTileColumn = Truncate_Float_To_Byte(max_x);
	if (maxTileColumn >= m_columnCellCount)
		maxTileColumn = static_cast<BYTE>(m_columnCellCount - 1);


	gos_PushCurrentHeap(MidLevelRenderer::ShapeHeap);
	MidLevelRenderer::MLRShape *shape = new MidLevelRenderer::MLRShape(8);
	gos_PopCurrentHeap();

	BYTE x, z;
	for(z=minTileRow;z<=maxTileRow;z++)
	{
		for(x=minTileColumn;x<=maxTileColumn;x++)
		{
			Tile *tile = GetTile(z, x);

			if ((tile != 0) &&
				(tile->m_ground != 0))
			{
				tile->m_ground->MakeFootStep(shape, where, radius, foot_texture);
			}
		}
	}
	if(shape->GetNum()==0)
	{
		shape->DetachReference();
	}
	else
	{
		MidLevelRenderer::MLRFootStep::Instance->AddAStep(shape, pos);
	}

	return false;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CollisionGrid::FindCollisions(Time till)
{
	Check_Object(this);

	//
	//------------------------------------
	// Clear out the colliding tile chains
	//------------------------------------
	//
	ChainIteratorOf<Tile*> tiles(&m_collidingTiles);
	Tile *tile;
	Entity *collider;
	unsigned collider_count;
	{
		COLLISION_LOGIC("Tile Assignment");
		while ((tile = tiles.GetCurrent()) != NULL)
		{
			Check_Object(tile);
			ChainIteratorOf<Entity*> colliders(&tile->m_colliders);
			while ((collider = colliders.GetCurrent()) != NULL)
			{
				Check_Object(collider);
				colliders.Remove();
			}
			tiles.Remove();
		}

		//
		//----------------------------------------------------------------
		// Iterate the solid colliders and attach them to the proper tiles
		//----------------------------------------------------------------
		//
		Scalar row_scale = 1.0f/m_rowCellSize;
		Scalar column_scale = 1.0f/m_columnCellSize;
		ChainIteratorOf<Entity*> colliders(&m_solidColliders);
		collider_count = 0;
		while ((collider = colliders.ReadAndNext()) != NULL)
		{
			Check_Object(collider);
			Verify(collider->IsACollider());
			Verify(collider->GetInterestLevel() != Entity::DormantInterestLevel);

			//
			//-------------------------------------------
			// Determine the primary bounds object to use
			//-------------------------------------------
			//
			Set_Statistic(Collider_Count, Collider_Count+1);
			++collider_count;
			ElementRenderer::Element *element = collider->GetElement();
			Check_Object(element);
			CollisionVolume *bounds = collider->GetHierarchicalVolume();
			if (!bounds)
				bounds = collider->GetSolidVolume();
			else
			{
				CollisionVolume *solid = collider->GetSolidVolume();
				Check_Object(solid);
				solid->m_worldSpaceBounds.Multiply(solid->m_localSpaceBounds, element->GetNewLocalToParent());
			}
			Check_Object(bounds);

			//
			//-----------------------------------------------------------------
			// Figure out the old dimension, then figure out the new dimensions
			// and union the two together.  The m_worldSpaceBounds variable is
			// now set up for the collision frame
			//-----------------------------------------------------------------
			//
			bounds->m_worldSpaceBounds.Multiply(bounds->m_localSpaceBounds, element->GetLocalToWorld());
			ExtentBox old_aabb(bounds->m_worldSpaceBounds);
			bounds->m_worldSpaceBounds.Multiply(bounds->m_localSpaceBounds, element->GetNewLocalToParent());
			ExtentBox new_aabb(bounds->m_worldSpaceBounds);
			new_aabb.Union(old_aabb, new_aabb);

			//
			//-------------------------------
			// Figure out the grid boundaries
			//-------------------------------
			//
			Scalar min_z = (new_aabb.minZ - m_rowOffset) * row_scale;
			collider->minTileRow = (min_z < 0.0f) ? (BYTE)0 : Truncate_Float_To_Byte(min_z);
			Scalar max_z = (new_aabb.maxZ - m_rowOffset) * row_scale;
			collider->maxTileRow = Truncate_Float_To_Byte(max_z);
			if (collider->maxTileRow >= m_rowCellCount)
				collider->maxTileRow = static_cast<BYTE>(m_rowCellCount - 1);

			Scalar min_x = (new_aabb.minX - m_columnOffset) * column_scale;
			collider->minTileColumn = (min_x < 0.0f) ? (BYTE)0 : Truncate_Float_To_Byte(min_x);
			Scalar max_x = (new_aabb.maxX - m_columnOffset) * column_scale;
			collider->maxTileColumn = Truncate_Float_To_Byte(max_x);
			if (collider->maxTileColumn >= m_columnCellCount)
				collider->maxTileColumn = static_cast<BYTE>(m_columnCellCount - 1);

			//
			//---------------------------------------------------------------------
			// Attach the collider to each tile that overlaps its extents.  If this
			// is the first time that tile is being used this frame, attach it to
			// the colliding tile list
			//---------------------------------------------------------------------
			//
			Verify(
				collider->minTileColumn <= collider->maxTileColumn
				 && collider->maxTileColumn < m_columnCellCount
			);
			Verify(
				collider->minTileRow <= collider->maxTileRow
				 && collider->maxTileRow < m_rowCellCount
			);
			for (BYTE z=collider->minTileRow; z<=collider->maxTileRow; ++z)
			{
				for (BYTE x=collider->minTileColumn; x<=collider->maxTileColumn; ++x)
				{
					Tile *tile = GetTile(z, x);
					Check_Object(tile);
					Verify(tile->m_row == z && tile->m_column == x);
					ChainIteratorOf<Entity*> temp(&tile->m_colliders);
					if (!temp.GetCurrent())
						m_collidingTiles.Add(tile);
					Verify(!tile->m_collidees.IsPlugMember(collider));
					Verify(!tile->m_colliders.IsPlugMember(collider));
					tile->m_colliders.Add(collider);
				}
			}
		}
	}

	//
	//---------------------------------------------------------------
	// Build an array to hold the collision results for each collider
	//---------------------------------------------------------------
	//
	if (!s_ignoreOBBCollisions)
	{
		COLLISION_LOGIC("Tests::Collider");
		DynamicArrayOf<DynamicArrayOf<Entity::CollisionData>*> collision_arrays(collider_count);
		DynamicArrayOf<Entity*> collider_list(collider_count);
		if (collider_count>0)
			memset(&collider_list[0], 0, collider_count*sizeof(Entity*));
		int i;
		int callback_count = 0;

		//
		//-----------------------------------------------------
		// Iterate the colliders in each of the colliding tiles
		//-----------------------------------------------------
		//
		tiles.First();
		while ((tile = tiles.ReadAndNext()) != NULL)
		{
			Check_Object(tile);
			ChainIteratorOf<Entity*> tile_colliders(&tile->m_colliders);
			while ((collider = tile_colliders.ReadAndNext()) != NULL)
			{
				Check_Object(collider);
				Verify(collider->IsACollider());
				Verify(collider->GetInterestLevel() != Entity::DormantInterestLevel);
				Verify(collider->CanOBBCollide());
				Entity::CollisionData collision_data;
				collision_data.m_worldIntersectionPoint = Point3D::Identity;
				collision_data.m_normal = UnitVector3D::Forward;
				collision_data.m_otherNormal = UnitVector3D::Forward;
				collision_data.m_volume = collider->GetSolidVolume();
				Check_Object(collision_data.m_volume);

				//
				//----------------------------------------------------------------
				// Check the colliders against each of the collidees, making that
				// this tile is at the minimum corner of the union of the collidee
				// tile extents
				//----------------------------------------------------------------
				//
				ChainIteratorOf<Entity*> collidees(&tile->m_collidees);
				while ((collision_data.m_otherEntity = collidees.ReadAndNext()) != NULL)
				{
					Check_Object(collision_data.m_otherEntity);
					Verify(!collision_data.m_otherEntity->IsACollider());
					Verify(collision_data.m_otherEntity->GetInterestLevel() != Entity::DormantInterestLevel);
					if (!collision_data.m_otherEntity->CanOBBCollide())
						continue;
					BYTE row = Max(collider->minTileRow, collision_data.m_otherEntity->minTileRow);
					Verify(row <= collider->maxTileRow && row <= collision_data.m_otherEntity->maxTileRow);
					BYTE col = Max(collider->minTileColumn, collision_data.m_otherEntity->minTileColumn);
					Verify(col <= collider->maxTileColumn && col <= collision_data.m_otherEntity->maxTileColumn);
					if (row != tile->m_row || col != tile->m_column)
						continue;

					//
					//----------------------
					// Find the other volume
					//----------------------
					//
					Set_Statistic(Collider_Collidee_Count, Collider_Collidee_Count+1);
					collision_data.m_otherVolume = collision_data.m_otherEntity->GetSolidVolume();
					if (!collision_data.m_otherVolume)
						collision_data.m_otherVolume = collision_data.m_otherEntity->GetHierarchicalVolume();
					Check_Object(collision_data.m_otherVolume);

					//
					//-------------------------------------------------------
					// Test for collision.  We can use local to world for the
					// collidee
					//-------------------------------------------------------
					//
					collision_data.m_otherVolume =
						collision_data.m_otherVolume->CollideOBB(
							collision_data.m_volume,
							collision_data.m_otherEntity,
							collision_data.m_otherEntity->GetLocalToWorld()
						);
					if (collision_data.m_otherVolume)
					{

						//
						//-------------------------------------------------------------
						// We have hit something, so look for this collider in the list
						//-------------------------------------------------------------
						//
						DynamicArrayOf<Entity::CollisionData> *collision_list = NULL;
						for (i=0; i<collider_count; ++i)
						{
							if (!collider_list[i])
							{
								collision_list = collision_arrays[i] = new DynamicArrayOf<Entity::CollisionData>;
								collider_list[i] = collider;
								Verify(i == callback_count);
								++callback_count;
								break;
							}
							else if (collider_list[i] == collider)
							{
								collision_list = collision_arrays[i];
								break;
							}
						}
						Verify(i < collider_count);

						//
						//----------------------------------------------
						// Grow the list, and stick this collision in it
						//----------------------------------------------
						//
						int impact_number = collision_list->GetLength();
						collision_list->SetLength(impact_number+1);
						(*collision_list)[impact_number] = collision_data;
					}
				}

				//
				//--------------------------------------------------------------------
				// Make an iterator for the remaining colliders, making that this tile
				// is at the minimum corner of the union of the collider tile extents
				//--------------------------------------------------------------------
				//
				ChainIteratorOf<Entity*> *remaining =
					Cast_Object(ChainIteratorOf<Entity*>*, tile_colliders.MakeClone());
				Check_Object(remaining);
				while ((collision_data.m_otherEntity = remaining->ReadAndNext()) != NULL)
				{
					Check_Object(collision_data.m_otherEntity);
					Verify(collision_data.m_otherEntity->IsACollider());
					Verify(collision_data.m_otherEntity->GetInterestLevel() != Entity::DormantInterestLevel);
					if (collision_data.m_otherEntity == collider->dontHit)
						continue;
					Verify(collision_data.m_otherEntity->CanOBBCollide());
					BYTE row = Max(collider->minTileRow, collision_data.m_otherEntity->minTileRow);
					Verify(row <= collider->maxTileRow && row <= collision_data.m_otherEntity->maxTileRow);
					BYTE col = Max(collider->minTileColumn, collision_data.m_otherEntity->minTileColumn);
					Verify(col <= collider->maxTileColumn && col <= collision_data.m_otherEntity->maxTileColumn);
					if (row != tile->m_row || col != tile->m_column)
						continue;


					//
					//---------------------------------------------------------
					// Do the obb test here.  We need to make sure we are using
					// the new local to parent for the collidee, as it's moving
					//---------------------------------------------------------
					//
					Set_Statistic(Collider_Collider_Count, Collider_Collider_Count+1);
					collision_data.m_otherVolume = collision_data.m_otherEntity->GetSolidVolume();
					Check_Object(collision_data.m_otherVolume);
					collision_data.m_otherVolume =
						collision_data.m_otherVolume->CollideOBB(
							collision_data.m_volume,
							collision_data.m_otherEntity,
							collision_data.m_otherEntity->GetElement()->GetNewLocalToParent()
						);
					if (collision_data.m_otherVolume)
					{

						//
						//-------------------------------------------------------------
						// We have hit something, so look for this collider in the list
						//-------------------------------------------------------------
						//
						DynamicArrayOf<Entity::CollisionData> *collision_list = NULL;
						for (i=0; i<collider_count; ++i)
						{
							if (!collider_list[i])
							{
								collision_list = collision_arrays[i] = new DynamicArrayOf<Entity::CollisionData>;
								collider_list[i] = collider;
								Verify(i == callback_count);
								++callback_count;
								break;
							}
							else if (collider_list[i] == collider)
							{
								collision_list = collision_arrays[i];
								break;
							}
						}
						Verify(i < collider_count);

						//
						//----------------------------------------------
						// Grow the list, and stick this collision in it
						//----------------------------------------------
						//
						int impact_number = collision_list->GetLength();
						collision_list->SetLength(impact_number+1);
						(*collision_list)[impact_number] = collision_data;

						//
						//-------------------------------------------
						// Now place an entry for the second collider
						//-------------------------------------------
						//
						Entity *them = collision_data.m_otherEntity;
						CollisionVolume *struck = collision_data.m_volume;
						collision_data.m_volume = collision_data.m_otherVolume;
						collision_data.m_otherVolume = struck;
						collision_data.m_otherEntity = collider;

						//
						//-------------------------------------------------------------
						// We have hit something, so look for this collider in the list
						//-------------------------------------------------------------
						//
						for (i=0; i<collider_count; ++i)
						{
							if (!collider_list[i])
							{
								collision_list = collision_arrays[i] = new DynamicArrayOf<Entity::CollisionData>;
								collider_list[i] = them;
								Verify(i == callback_count);
								++callback_count;
								break;
							}
							else if (collider_list[i] == them)
							{
								collision_list = collision_arrays[i];
								break;
							}
						}
						Verify(i < collider_count);

						//
						//----------------------------------------------
						// Grow the list, and stick this collision in it
						//----------------------------------------------
						//
						impact_number = collision_list->GetLength();
						collision_list->SetLength(impact_number+1);
						(*collision_list)[impact_number] = collision_data;
					}
				}
				delete remaining;
			}
		}

		//
		//--------------------------
		// Now, issure the callbacks
		//--------------------------
		//
		DynamicArrayOf<bool> moved(callback_count);
		DynamicArrayOf<LinearMatrix4D> positions(callback_count);
		{
			COLLISION_LOGIC("Callbacks::Collider");
			for (i=0; i<callback_count; ++i)
			{
				Set_Statistic(Collision_Callbacks, Collision_Callbacks+1);
				if (collider_list[i]->collisionAllowed)
				{
					moved[i] = collider_list[i]->CollisionHandler(&positions[i], collision_arrays[i]);
					#if defined(_ARMOR)
						if (collider_list[i]->newCollisions)
						{
							Check_Object(collider_list[i]->newCollisions);
							Verify(collider_list[i]->IsUsingPostCollision() && EntityManager::GetInstance()->IsInPostCollisionExecution(collider_list[i]));
						}

						//
						//-----------------------------------------------------------------
						// The following is a dangerous but useful tool for finding leaking
						// collisions.  It is NOT compatible with /goscheckmemory!!!!
						//-----------------------------------------------------------------
						//
						#if 0
							else
								Verify(!collision_arrays[i]->IsMarkedValid());
						#endif
					#endif
				}
				else
				{
					moved[i] = false;
					delete collision_arrays[i];
				}
			}
		}

		//
		//-------------------------------
		// Now, sync up the moved objects
		//-------------------------------
		//
		{
			COLLISION_LOGIC("Tests::Collider::Sync");
			for (i=0; i<callback_count; ++i)
			{
				if (moved[i])
					collider_list[i]->SetNewLocalToParent(positions[i]);
			}
		}
	}

	//
	//-------------------------------
	// Check each volumeless collider
	//-------------------------------
	//
	COLLISION_LOGIC("Tests::Ray");
	ChainIteratorOf<Entity*> line_colliders(&m_lineColliders);
	while ((collider = line_colliders.ReadAndNext()) != NULL)
	{
		Check_Object(collider);
		Verify(collider->IsACollider());
		Verify(collider->GetInterestLevel() != Entity::DormantInterestLevel);

		//
		//--------------------------------------------------------------------
		// We are not a collidee, so we should ray-trace from the old position
		// to the new one, but not if it is zero length
		//--------------------------------------------------------------------
		//
		Verify(!collider->GetSolidVolume());
		Line3D line; 
		line.m_origin = collider->GetLocalToWorld();
		ElementRenderer::Element *element = collider->GetElement();
		Check_Object(element);
		Point3D dest(element->GetNewLocalToParent());
		dest -= line.m_origin;
		line.m_length = dest.GetLength();
		if (Small_Enough(line.m_length))
			continue;

		//
		//------------------------------------------
		// Set up the ray direction and do the query
		//------------------------------------------
		//
		Scalar t = 1.0f / line.m_length;
		line.m_direction.x = dest.x * t;
		line.m_direction.y = dest.y * t;
		line.m_direction.z = dest.z * t;
		Entity::CollisionData collision_data;
		collision_data.m_otherNormal = UnitVector3D::Forward;
		Entity::CollisionQuery
			collision_query(
				&line,
				&collision_data.m_normal,
				collider->GetCollisionMask(),
				collider->dontHit
			);
		collision_data.m_otherEntity = ProjectLine(&collision_query);
		collision_data.m_otherVolume = NULL;
		collision_data.m_volume = NULL;
		if (!collision_data.m_otherEntity)
			continue;

		//
		//------------------------------------------------------
		// We hit something, so call the callback as appropriate
		//------------------------------------------------------
		//
		COLLISION_LOGIC("Callbacks::Ray");
		collision_data.m_material = collision_query.m_material;
		collision_data.m_timeSlice =
			(line.m_length * t) * collider->GetTimeSlice(till);
		line.FindEnd(&collision_data.m_worldIntersectionPoint);
		DynamicArrayOf<Entity::CollisionData> *array = new DynamicArrayOf<Entity::CollisionData>(1);
		(*array)[0] = collision_data;
		LinearMatrix4D dummy;
		collider->CollisionHandler(&dummy, array);
		#if defined(_ARMOR)
			if (collider->newCollisions)
			{
				Verify(collider->IsUsingPostCollision());
				Verify(
					EntityManager::GetInstance()->IsInPostCollisionExecution(collider)
				);
			}
		#endif
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CollisionGrid::FindEntitiesWithin(
		const Stuff::Sphere &sphere,
		WithinCallback callback,
		Entity *original_caller
	)
{
	Check_Object(this);
	Check_Object(&sphere);
	Check_Pointer(callback);
	//Check_Object(original_caller);
	COLLISION_LOGIC("Find Within");

	//
	//-------------------------------
	// Figure out the grid boundaries
	//-------------------------------
	//
	Scalar row_scale = 1.0f/m_rowCellSize;
	Scalar column_scale = 1.0f/m_columnCellSize;
	Scalar min_z = (sphere.center.z - sphere.radius - m_rowOffset) * row_scale;
	BYTE min_tile_row = (min_z < 0.0f) ? (BYTE)0 : Truncate_Float_To_Byte(min_z);
	Scalar max_z = (sphere.center.z + sphere.radius - m_rowOffset) * row_scale;
	BYTE max_tile_row = Truncate_Float_To_Byte(max_z);
	if (max_tile_row >= m_rowCellCount)
		max_tile_row = static_cast<BYTE>(m_rowCellCount - 1);

	Scalar min_x = (sphere.center.x - sphere.radius - m_columnOffset) * column_scale;
	BYTE min_tile_column = (min_x < 0.0f) ? (BYTE)0 : Truncate_Float_To_Byte(min_x);
	Scalar max_x = (sphere.center.x + sphere.radius - m_columnOffset) * column_scale;
	BYTE max_tile_column = Truncate_Float_To_Byte(max_x);
	if (max_tile_column >= m_columnCellCount)
		max_tile_column = static_cast<BYTE>(m_columnCellCount - 1);

	//
	//---------------
	// Walk the tiles
	//---------------
	//
	Verify(
		min_tile_column <= max_tile_column
		 && max_tile_column < m_columnCellCount
	);
	Verify(
		min_tile_row <= max_tile_row
		 && max_tile_row < m_rowCellCount
	);
	for (BYTE z=min_tile_row; z<=max_tile_row; ++z)
	{
		for (BYTE x=min_tile_column; x<=max_tile_column; ++x)
		{
			Tile *tile = GetTile(z, x);
			Check_Object(tile);
			Verify(tile->m_row == z && tile->m_column == x);
			ChainIteratorOf<Entity*> colliders(&tile->m_colliders);
			Entity *entity;
			while ((entity = colliders.ReadAndNext()) != NULL)
			{
				Check_Object(entity);
				Verify(entity->IsACollider());

				//
				// Update the collision volumes
				//
				//if (entity->entityElement)
				//{
				//	Check_Object(entity->entityElement);
				//	entity->entityElement->Sync();/*m_worldOBB.Multiply(entity->entityElement->m_localOBB, entity->GetLocalToParent());*/
				//}
				
				ElementRenderer::Element *element = entity->GetElement();
				Check_Object(element);
				Sphere other(element->GetWorldOBB());
				if (sphere.Intersects(other))
				{
					COLLISION_LOGIC("Find Within::Callbacks::Colliders");
					(*callback)(entity, original_caller, sphere);
				}
			}
			ChainIteratorOf<Entity*> collidees(&tile->m_collidees);
			while ((entity = collidees.ReadAndNext()) != NULL)
			{
				Check_Object(entity);
				Verify(!entity->IsACollider());

				//
				// Update the collision volumes
				//
				//if (entity->entityElement)
				//{
				//	Check_Object(entity->entityElement);
				//	entity->entityElement->Sync(); /*m_worldOBB.Multiply(entity->entityElement->m_localOBB, entity->GetLocalToParent());*/
				//}
				
				ElementRenderer::Element *element = entity->GetElement();
				Check_Object(element);
				OBB world_OBB;
				world_OBB.Multiply(element->m_localOBB, element->GetLocalToWorld());
				Sphere other(world_OBB);
				if (sphere.Intersects(other))
				{
					COLLISION_LOGIC("Find Within::Callbacks::Collidees");
					(*callback)(entity, original_caller, sphere);
				}
			}
		}
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
int
	CollisionGrid::EntitiesInTile(
		BYTE row,
		BYTE column
	)
{
	Tile *tile = GetTile(row, column);
	Check_Object(tile);
	ChainIteratorOf<Entity*> colliders(&tile->m_colliders);
	ChainIteratorOf<Entity*> collidees(&tile->m_collidees);
		
	return colliders.GetSize()+collidees.GetSize();
}
