/*
* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "b2BroadPhase.h"
#include <algorithm>

#include <cstring>

// Notes:
// - we use bound arrays instead of linked lists for cache coherence.
// - we use quantized integral values for fast compares.
// - we use short indices rather than pointers to save memory.
// - we use a stabbing count for fast overlap queries (less than order N).
// - we also use a time stamp on each proxy to speed up the registration of
//   overlap query results.
// - where possible, we compare bound indices instead of values to reduce
//   cache misses (TODO_ERIN).
// - no broadphase is perfect and neither is this one: it is not great for huge
//   worlds (use a multi-SAP instead), it is not great for large objects.

bool b2BroadPhase::s_validate = false;

struct b2BoundValues
{
	uint16 lowerValues[2];
	uint16 upperValues[2];
};

static int32 BinarySearch(b2Bound* bounds, int32 count, uint16 value)
{
	int32 low = 0;
	int32 high = count - 1;
	while (low <= high)
	{
		int32 mid = (low + high) >> 1;
		if (bounds[mid].value > value)
		{
			high = mid - 1;
		}
		else if (bounds[mid].value < value)
		{
			low = mid + 1;
		}
		else
		{
			return (uint16)mid;
		}
	}
	
	return low;
}

b2BroadPhase::b2BroadPhase(const b2AABB& worldAABB, b2PairCallback* callback)
{
	m_pairManager.Initialize(this, callback);

	b2Assert(worldAABB.IsValid());
	m_worldAABB = worldAABB;
	m_proxyCount = 0;

	b2Vec2 d = worldAABB.upperBound - worldAABB.lowerBound;
	m_quantizationFactor.x = float32(B2BROADPHASE_MAX) / d.x;
	m_quantizationFactor.y = float32(B2BROADPHASE_MAX) / d.y;

	for (uint16 i = 0; i < b2_maxProxies - 1; ++i)
	{
		m_proxyPool[i].SetNext(i + 1);
		m_proxyPool[i].timeStamp = 0;
		m_proxyPool[i].overlapCount = b2_invalid;
		m_proxyPool[i].userData = NULL;
	}
	m_proxyPool[b2_maxProxies-1].SetNext(b2_nullProxy);
	m_proxyPool[b2_maxProxies-1].timeStamp = 0;
	m_proxyPool[b2_maxProxies-1].overlapCount = b2_invalid;
	m_proxyPool[b2_maxProxies-1].userData = NULL;
	m_freeProxy = 0;

	m_timeStamp = 1;
	m_queryResultCount = 0;
}

b2BroadPhase::~b2BroadPhase()
{
}

// This one is only used for validation.
bool b2BroadPhase::TestOverlap(b2Proxy* p1, b2Proxy* p2)
{
	for (int32 axis = 0; axis < 2; ++axis)
	{
		b2Bound* bounds = m_bounds[axis];

		b2Assert(p1->lowerBounds[axis] < 2 * m_proxyCount);
		b2Assert(p1->upperBounds[axis] < 2 * m_proxyCount);
		b2Assert(p2->lowerBounds[axis] < 2 * m_proxyCount);
		b2Assert(p2->upperBounds[axis] < 2 * m_proxyCount);

		if (bounds[p1->lowerBounds[axis]].value > bounds[p2->upperBounds[axis]].value)
			return false;

		if (bounds[p1->upperBounds[axis]].value < bounds[p2->lowerBounds[axis]].value)
			return false;
	}

	return true;
}

bool b2BroadPhase::TestOverlap(const b2BoundValues& b, b2Proxy* p)
{
	for (int32 axis = 0; axis < 2; ++axis)
	{
		b2Bound* bounds = m_bounds[axis];

		b2Assert(p->lowerBounds[axis] < 2 * m_proxyCount);
		b2Assert(p->upperBounds[axis] < 2 * m_proxyCount);

		if (b.lowerValues[axis] > bounds[p->upperBounds[axis]].value)
			return false;

		if (b.upperValues[axis] < bounds[p->lowerBounds[axis]].value)
			return false;
	}

	return true;
}

void b2BroadPhase::ComputeBounds(uint16* lowerValues, uint16* upperValues, const b2AABB& aabb)
{
	b2Assert(aabb.upperBound.x >= aabb.lowerBound.x);
	b2Assert(aabb.upperBound.y >= aabb.lowerBound.y);

	b2Vec2 minVertex = b2Clamp(aabb.lowerBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);
	b2Vec2 maxVertex = b2Clamp(aabb.upperBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);

	// Bump lower bounds downs and upper bounds up. This ensures correct sorting of
	// lower/upper bounds that would have equal values.
	// TODO_ERIN implement fast float to uint16 conversion.
	lowerValues[0] = (uint16)(m_quantizationFactor.x * (minVertex.x - m_worldAABB.lowerBound.x)) & (B2BROADPHASE_MAX - 1);
	upperValues[0] = (uint16)(m_quantizationFactor.x * (maxVertex.x - m_worldAABB.lowerBound.x)) | 1;

	lowerValues[1] = (uint16)(m_quantizationFactor.y * (minVertex.y - m_worldAABB.lowerBound.y)) & (B2BROADPHASE_MAX - 1);
	upperValues[1] = (uint16)(m_quantizationFactor.y * (maxVertex.y - m_worldAABB.lowerBound.y)) | 1;
}

void b2BroadPhase::IncrementTimeStamp()
{
	if (m_timeStamp == B2BROADPHASE_MAX)
	{
		for (uint16 i = 0; i < b2_maxProxies; ++i)
		{
			m_proxyPool[i].timeStamp = 0;
		}
		m_timeStamp = 1;
	}
	else
	{
		++m_timeStamp;
	}
}

void b2BroadPhase::IncrementOverlapCount(int32 proxyId)
{
	b2Proxy* proxy = m_proxyPool + proxyId;
	if (proxy->timeStamp < m_timeStamp)
	{
		proxy->timeStamp = m_timeStamp;
		proxy->overlapCount = 1;
	}
	else
	{
		proxy->overlapCount = 2;
		b2Assert(m_queryResultCount < b2_maxProxies);
		m_queryResults[m_queryResultCount] = (uint16)proxyId;
		++m_queryResultCount;
	}
}

void b2BroadPhase::Query(int32* lowerQueryOut, int32* upperQueryOut,
					   uint16 lowerValue, uint16 upperValue,
					   b2Bound* bounds, int32 boundCount, int32 axis)
{
	int32 lowerQuery = BinarySearch(bounds, boundCount, lowerValue);
	int32 upperQuery = BinarySearch(bounds, boundCount, upperValue);

	// Easy case: lowerQuery <= lowerIndex(i) < upperQuery
	// Solution: search query range for min bounds.
	for (int32 i = lowerQuery; i < upperQuery; ++i)
	{
		if (bounds[i].IsLower())
		{
			IncrementOverlapCount(bounds[i].proxyId);
		}
	}

	// Hard case: lowerIndex(i) < lowerQuery < upperIndex(i)
	// Solution: use the stabbing count to search down the bound array.
	if (lowerQuery > 0)
	{
		int32 i = lowerQuery - 1;
		int32 s = bounds[i].stabbingCount;

		// Find the s overlaps.
		while (s)
		{
			b2Assert(i >= 0);

			if (bounds[i].IsLower())
			{
				b2Proxy* proxy = m_proxyPool + bounds[i].proxyId;
				if (lowerQuery <= proxy->upperBounds[axis])
				{
					IncrementOverlapCount(bounds[i].proxyId);
					--s;
				}
			}
			--i;
		}
	}

	*lowerQueryOut = lowerQuery;
	*upperQueryOut = upperQuery;
}

uint16 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData)
{
	b2Assert(m_proxyCount < b2_maxProxies);
	b2Assert(m_freeProxy != b2_nullProxy);

	uint16 proxyId = m_freeProxy;
	b2Proxy* proxy = m_proxyPool + proxyId;
	m_freeProxy = proxy->GetNext();

	proxy->overlapCount = 0;
	proxy->userData = userData;

	int32 boundCount = 2 * m_proxyCount;

	uint16 lowerValues[2], upperValues[2];
	ComputeBounds(lowerValues, upperValues, aabb);

	for (int32 axis = 0; axis < 2; ++axis)
	{
		b2Bound* bounds = m_bounds[axis];
		int32 lowerIndex, upperIndex;
		Query(&lowerIndex, &upperIndex, lowerValues[axis], upperValues[axis], bounds, boundCount, axis);

		memmove(bounds + upperIndex + 2, bounds + upperIndex, (boundCount - upperIndex) * sizeof(b2Bound));
		memmove(bounds + lowerIndex + 1, bounds + lowerIndex, (upperIndex - lowerIndex) * sizeof(b2Bound));

		// The upper index has increased because of the lower bound insertion.
		++upperIndex;

		// Copy in the new bounds.
		bounds[lowerIndex].value = lowerValues[axis];
		bounds[lowerIndex].proxyId = proxyId;
		bounds[upperIndex].value = upperValues[axis];
		bounds[upperIndex].proxyId = proxyId;

		bounds[lowerIndex].stabbingCount = lowerIndex == 0 ? 0 : bounds[lowerIndex-1].stabbingCount;
		bounds[upperIndex].stabbingCount = bounds[upperIndex-1].stabbingCount;

		// Adjust the stabbing count between the new bounds.
		for (int32 index = lowerIndex; index < upperIndex; ++index)
		{
			++bounds[index].stabbingCount;
		}

		// Adjust the all the affected bound indices.
		for (int32 index = lowerIndex; index < boundCount + 2; ++index)
		{
			b2Proxy* proxy = m_proxyPool + bounds[index].proxyId;
			if (bounds[index].IsLower())
			{
				proxy->lowerBounds[axis] = (uint16)index;
			}
			else
			{
				proxy->upperBounds[axis] = (uint16)index;
			}
		}
	}

	++m_proxyCount;

	b2Assert(m_queryResultCount < b2_maxProxies);

	// Create pairs if the AABB is in range.
	for (int32 i = 0; i < m_queryResultCount; ++i)
	{
		b2Assert(m_queryResults[i] < b2_maxProxies);
		b2Assert(m_proxyPool[m_queryResults[i]].IsValid());

		m_pairManager.AddBufferedPair(proxyId, m_queryResults[i]);
	}

	m_pairManager.Commit();

	if (s_validate)
	{
		Validate();
	}

	// Prepare for next query.
	m_queryResultCount = 0;
	IncrementTimeStamp();

	return proxyId;
}

void b2BroadPhase::DestroyProxy(int32 proxyId)
{
	b2Assert(0 < m_proxyCount && m_proxyCount <= b2_maxProxies);
	b2Proxy* proxy = m_proxyPool + proxyId;
	b2Assert(proxy->IsValid());

	int32 boundCount = 2 * m_proxyCount;

	for (int32 axis = 0; axis < 2; ++axis)
	{
		b2Bound* bounds = m_bounds[axis];

		int32 lowerIndex = proxy->lowerBounds[axis];
		int32 upperIndex = proxy->upperBounds[axis];
		uint16 lowerValue = bounds[lowerIndex].value;
		uint16 upperValue = bounds[upperIndex].value;

		memmove(bounds + lowerIndex, bounds + lowerIndex + 1, (upperIndex - lowerIndex - 1) * sizeof(b2Bound));
		memmove(bounds + upperIndex-1, bounds + upperIndex + 1, (boundCount - upperIndex - 1) * sizeof(b2Bound));

		// Fix bound indices.
		for (int32 index = lowerIndex; index < boundCount - 2; ++index)
		{
			b2Proxy* proxy = m_proxyPool + bounds[index].proxyId;
			if (bounds[index].IsLower())
			{
				proxy->lowerBounds[axis] = (uint16)index;
			}
			else
			{
				proxy->upperBounds[axis] = (uint16)index;
			}
		}

		// Fix stabbing count.
		for (int32 index = lowerIndex; index < upperIndex - 1; ++index)
		{
			--bounds[index].stabbingCount;
		}

		// Query for pairs to be removed. lowerIndex and upperIndex are not needed.
		Query(&lowerIndex, &upperIndex, lowerValue, upperValue, bounds, boundCount - 2, axis);
	}

	b2Assert(m_queryResultCount < b2_maxProxies);

	for (int32 i = 0; i < m_queryResultCount; ++i)
	{
		b2Assert(m_proxyPool[m_queryResults[i]].IsValid());
		m_pairManager.RemoveBufferedPair(proxyId, m_queryResults[i]);
	}

	m_pairManager.Commit();

	// Prepare for next query.
	m_queryResultCount = 0;
	IncrementTimeStamp();

	// Return the proxy to the pool.
	proxy->userData = NULL;
	proxy->overlapCount = b2_invalid;
	proxy->lowerBounds[0] = b2_invalid;
	proxy->lowerBounds[1] = b2_invalid;
	proxy->upperBounds[0] = b2_invalid;
	proxy->upperBounds[1] = b2_invalid;

	proxy->SetNext(m_freeProxy);
	m_freeProxy = (uint16)proxyId;
	--m_proxyCount;

	if (s_validate)
	{
		Validate();
	}
}

void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb)
{
	if (proxyId == b2_nullProxy || b2_maxProxies <= proxyId)
	{
		b2Assert(false);
		return;
	}

	if (aabb.IsValid() == false)
	{
		b2Assert(false);
		return;
	}

	int32 boundCount = 2 * m_proxyCount;

	b2Proxy* proxy = m_proxyPool + proxyId;

	// Get new bound values
	b2BoundValues newValues;
	ComputeBounds(newValues.lowerValues, newValues.upperValues, aabb);

	// Get old bound values
	b2BoundValues oldValues;
	for (int32 axis = 0; axis < 2; ++axis)
	{
		oldValues.lowerValues[axis] = m_bounds[axis][proxy->lowerBounds[axis]].value;
		oldValues.upperValues[axis] = m_bounds[axis][proxy->upperBounds[axis]].value;
	}

	for (int32 axis = 0; axis < 2; ++axis)
	{
		b2Bound* bounds = m_bounds[axis];

		int32 lowerIndex = proxy->lowerBounds[axis];
		int32 upperIndex = proxy->upperBounds[axis];

		uint16 lowerValue = newValues.lowerValues[axis];
		uint16 upperValue = newValues.upperValues[axis];

		int32 deltaLower = lowerValue - bounds[lowerIndex].value;
		int32 deltaUpper = upperValue - bounds[upperIndex].value;

		bounds[lowerIndex].value = lowerValue;
		bounds[upperIndex].value = upperValue;

		//
		// Expanding adds overlaps
		//

		// Should we move the lower bound down?
		if (deltaLower < 0)
		{
			int32 index = lowerIndex;
			while (index > 0 && lowerValue < bounds[index-1].value)
			{
				b2Bound* bound = bounds + index;
				b2Bound* prevBound = bound - 1;

				int32 prevProxyId = prevBound->proxyId;
				b2Proxy* prevProxy = m_proxyPool + prevBound->proxyId;

				++prevBound->stabbingCount;

				if (prevBound->IsUpper() == true)
				{
					if (TestOverlap(newValues, prevProxy))
					{
						m_pairManager.AddBufferedPair(proxyId, prevProxyId);
					}

					++prevProxy->upperBounds[axis];
					++bound->stabbingCount;
				}
				else
				{
					++prevProxy->lowerBounds[axis];
					--bound->stabbingCount;
				}

				--proxy->lowerBounds[axis];
				b2Swap(*bound, *prevBound);
				--index;
			}
		}

		// Should we move the upper bound up?
		if (deltaUpper > 0)
		{
			int32 index = upperIndex;
			while (index < boundCount-1 && bounds[index+1].value <= upperValue)
			{
				b2Bound* bound = bounds + index;
				b2Bound* nextBound = bound + 1;
				int32 nextProxyId = nextBound->proxyId;
				b2Proxy* nextProxy = m_proxyPool + nextProxyId;

				++nextBound->stabbingCount;

				if (nextBound->IsLower() == true)
				{
					if (TestOverlap(newValues, nextProxy))
					{
						m_pairManager.AddBufferedPair(proxyId, nextProxyId);
					}

					--nextProxy->lowerBounds[axis];
					++bound->stabbingCount;
				}
				else
				{
					--nextProxy->upperBounds[axis];
					--bound->stabbingCount;
				}

				++proxy->upperBounds[axis];
				b2Swap(*bound, *nextBound);
				++index;
			}
		}

		//
		// Shrinking removes overlaps
		//

		// Should we move the lower bound up?
		if (deltaLower > 0)
		{
			int32 index = lowerIndex;
			while (index < boundCount-1 && bounds[index+1].value <= lowerValue)
			{
				b2Bound* bound = bounds + index;
				b2Bound* nextBound = bound + 1;

				int32 nextProxyId = nextBound->proxyId;
				b2Proxy* nextProxy = m_proxyPool + nextProxyId;

				--nextBound->stabbingCount;

				if (nextBound->IsUpper())
				{
					if (TestOverlap(oldValues, nextProxy))
					{
						m_pairManager.RemoveBufferedPair(proxyId, nextProxyId);
					}

					--nextProxy->upperBounds[axis];
					--bound->stabbingCount;
				}
				else
				{
					--nextProxy->lowerBounds[axis];
					++bound->stabbingCount;
				}

				++proxy->lowerBounds[axis];
				b2Swap(*bound, *nextBound);
				++index;
			}
		}

		// Should we move the upper bound down?
		if (deltaUpper < 0)
		{
			int32 index = upperIndex;
			while (index > 0 && upperValue < bounds[index-1].value)
			{
				b2Bound* bound = bounds + index;
				b2Bound* prevBound = bound - 1;

				int32 prevProxyId = prevBound->proxyId;
				b2Proxy* prevProxy = m_proxyPool + prevProxyId;

				--prevBound->stabbingCount;

				if (prevBound->IsLower() == true)
				{
					if (TestOverlap(oldValues, prevProxy))
					{
						m_pairManager.RemoveBufferedPair(proxyId, prevProxyId);
					}

					++prevProxy->lowerBounds[axis];
					--bound->stabbingCount;
				}
				else
				{
					++prevProxy->upperBounds[axis];
					++bound->stabbingCount;
				}

				--proxy->upperBounds[axis];
				b2Swap(*bound, *prevBound);
				--index;
			}
		}
	}

	if (s_validate)
	{
		Validate();
	}
}

void b2BroadPhase::Commit()
{
	m_pairManager.Commit();
}

int32 b2BroadPhase::Query(const b2AABB& aabb, void** userData, int32 maxCount)
{
	uint16 lowerValues[2];
	uint16 upperValues[2];
	ComputeBounds(lowerValues, upperValues, aabb);

	int32 lowerIndex, upperIndex;

	Query(&lowerIndex, &upperIndex, lowerValues[0], upperValues[0], m_bounds[0], 2*m_proxyCount, 0);
	Query(&lowerIndex, &upperIndex, lowerValues[1], upperValues[1], m_bounds[1], 2*m_proxyCount, 1);

	b2Assert(m_queryResultCount < b2_maxProxies);

	int32 count = 0;
	for (int32 i = 0; i < m_queryResultCount && count < maxCount; ++i, ++count)
	{
		b2Assert(m_queryResults[i] < b2_maxProxies);
		b2Proxy* proxy = m_proxyPool + m_queryResults[i];
		b2Assert(proxy->IsValid());
		userData[i] = proxy->userData;
	}

	// Prepare for next query.
	m_queryResultCount = 0;
	IncrementTimeStamp();

	return count;
}

void b2BroadPhase::Validate()
{
	for (int32 axis = 0; axis < 2; ++axis)
	{
		b2Bound* bounds = m_bounds[axis];

		int32 boundCount = 2 * m_proxyCount;
		uint16 stabbingCount = 0;

		for (int32 i = 0; i < boundCount; ++i)
		{
			b2Bound* bound = bounds + i;
			b2Assert(i == 0 || bounds[i-1].value <= bound->value);
			b2Assert(bound->proxyId != b2_nullProxy);
			b2Assert(m_proxyPool[bound->proxyId].IsValid());

			if (bound->IsLower() == true)
			{
				b2Assert(m_proxyPool[bound->proxyId].lowerBounds[axis] == i);
				++stabbingCount;
			}
			else
			{
				b2Assert(m_proxyPool[bound->proxyId].upperBounds[axis] == i);
				--stabbingCount;
			}

			b2Assert(bound->stabbingCount == stabbingCount);
		}
	}
}


int32 b2BroadPhase::QuerySegment(const b2Segment& segment, void** userData, int32 maxCount, SortKeyFunc sortKey)
{
	float32 maxLambda = 1;

	float32 dx = (segment.p2.x-segment.p1.x)*m_quantizationFactor.x;
	float32 dy = (segment.p2.y-segment.p1.y)*m_quantizationFactor.y;

	int32 sx = dx<-B2_FLT_EPSILON ? -1 : (dx>B2_FLT_EPSILON ? 1 : 0);
	int32 sy = dy<-B2_FLT_EPSILON ? -1 : (dy>B2_FLT_EPSILON ? 1 : 0);

	b2Assert(sx!=0||sy!=0);

	float32 p1x = (segment.p1.x-m_worldAABB.lowerBound.x)*m_quantizationFactor.x;
	float32 p1y = (segment.p1.y-m_worldAABB.lowerBound.y)*m_quantizationFactor.y;

	uint16 startValues[2];
	uint16 startValues2[2];

	int32 xIndex;
	int32 yIndex;

	uint16 proxyId;
	b2Proxy* proxy;
	
	// TODO_ERIN implement fast float to uint16 conversion.
	startValues[0] = (uint16)(p1x) & (B2BROADPHASE_MAX - 1);
	startValues2[0] = (uint16)(p1x) | 1;

	startValues[1] = (uint16)(p1y) & (B2BROADPHASE_MAX - 1);
	startValues2[1] = (uint16)(p1y) | 1;

	//First deal with all the proxies that contain segment.p1
	int32 lowerIndex;
	int32 upperIndex;
	Query(&lowerIndex,&upperIndex,startValues[0],startValues2[0],m_bounds[0],2*m_proxyCount,0);
	if(sx>=0)	xIndex = upperIndex-1;
	else		xIndex = lowerIndex;
	Query(&lowerIndex,&upperIndex,startValues[1],startValues2[1],m_bounds[1],2*m_proxyCount,1);
	if(sy>=0)	yIndex = upperIndex-1;
	else		yIndex = lowerIndex;

	//If we are using sortKey, then sort what we have so far, filtering negative keys
	if(sortKey)
	{
		//Fill keys
		for(int32 i=0;i<m_queryResultCount;i++)
		{
			m_querySortKeys[i] = sortKey(m_proxyPool[m_queryResults[i]].userData);
		}
		//Bubble sort keys
		//Sorting negative values to the top, so we can easily remove them
		int32 i = 0;
		while(i<m_queryResultCount-1)
		{
			float32 a = m_querySortKeys[i];
			float32 b = m_querySortKeys[i+1];
			if((a<0)?(b>=0):(a>b&&b>=0))
			{
				m_querySortKeys[i+1] = a;
				m_querySortKeys[i]   = b;
				uint16 tempValue = m_queryResults[i+1];
				m_queryResults[i+1] = m_queryResults[i];
				m_queryResults[i] = tempValue;
				i--;
				if(i==-1) i=1;
			}
			else
			{
				i++;
			}
		}
		//Skim off negative values
		while(m_queryResultCount>0 && m_querySortKeys[m_queryResultCount-1]<0)
			m_queryResultCount--;
	}

	//Now work through the rest of the segment
	for (;;)
	{
		float32 xProgress = 0;
		float32 yProgress = 0;
		//Move on to the next bound
		xIndex += sx>=0?1:-1;
		if(xIndex<0||xIndex>=m_proxyCount*2)
			break;
		if(sx!=0)
			xProgress = ((float32)m_bounds[0][xIndex].value-p1x)/dx;
		//Move on to the next bound
		yIndex += sy>=0?1:-1;
		if(yIndex<0||yIndex>=m_proxyCount*2)
			break;
		if(sy!=0)
			yProgress = ((float32)m_bounds[1][yIndex].value-p1y)/dy;
		for(;;)
		{
			if(sy==0||(sx!=0&&xProgress<yProgress))
			{
				if(xProgress>maxLambda)
					break;

				//Check that we are entering a proxy, not leaving
				if(sx>0?m_bounds[0][xIndex].IsLower():m_bounds[0][xIndex].IsUpper()){
					//Check the other axis of the proxy
					proxyId = m_bounds[0][xIndex].proxyId;
					proxy = m_proxyPool+proxyId;
					if(sy>=0)
					{
						if(proxy->lowerBounds[1]<=yIndex-1&&proxy->upperBounds[1]>=yIndex)
						{
							//Add the proxy
							if(sortKey)
							{
								AddProxyResult(proxyId,proxy,maxCount,sortKey);
							}
							else
							{
								m_queryResults[m_queryResultCount] = proxyId;
								++m_queryResultCount;
							}
						}
					}
					else
					{
						if(proxy->lowerBounds[1]<=yIndex&&proxy->upperBounds[1]>=yIndex+1)
						{
							//Add the proxy
							if(sortKey)
							{
								AddProxyResult(proxyId,proxy,maxCount,sortKey);
							}
							else
							{
								m_queryResults[m_queryResultCount] = proxyId;
								++m_queryResultCount;
							}
						}
					}
				}

				//Early out
				if(sortKey && m_queryResultCount==maxCount && m_queryResultCount>0 && xProgress>m_querySortKeys[m_queryResultCount-1])
					break;

				//Move on to the next bound
				if(sx>0)
				{
					xIndex++;
					if(xIndex==m_proxyCount*2)
						break;
				}
				else
				{
					xIndex--;
					if(xIndex<0)
						break;
				}
				xProgress = ((float32)m_bounds[0][xIndex].value - p1x) / dx;
			}
			else
			{
				if(yProgress>maxLambda)
					break;

				//Check that we are entering a proxy, not leaving
				if(sy>0?m_bounds[1][yIndex].IsLower():m_bounds[1][yIndex].IsUpper()){
					//Check the other axis of the proxy
					proxyId = m_bounds[1][yIndex].proxyId;
					proxy = m_proxyPool+proxyId;
					if(sx>=0)
					{
						if(proxy->lowerBounds[0]<=xIndex-1&&proxy->upperBounds[0]>=xIndex)
						{
							//Add the proxy
							if(sortKey)
							{
								AddProxyResult(proxyId,proxy,maxCount,sortKey);
							}
							else
							{
								m_queryResults[m_queryResultCount] = proxyId;
								++m_queryResultCount;
							}
						}
					}
					else
					{
						if(proxy->lowerBounds[0]<=xIndex&&proxy->upperBounds[0]>=xIndex+1)
						{
							//Add the proxy
							if(sortKey)
							{
								AddProxyResult(proxyId,proxy,maxCount,sortKey);
							}
							else
							{
								m_queryResults[m_queryResultCount] = proxyId;
								++m_queryResultCount;
							}
						}
					}
				}

				//Early out
				if(sortKey && m_queryResultCount==maxCount && m_queryResultCount>0 && yProgress>m_querySortKeys[m_queryResultCount-1])
					break;

				//Move on to the next bound
				if(sy>0)
				{
					yIndex++;
					if(yIndex==m_proxyCount*2)
						break;
				}
				else
				{
					yIndex--;
					if(yIndex<0)
						break;
				}
				yProgress = ((float32)m_bounds[1][yIndex].value - p1y) / dy;
			}
		}

		break;
	}

	int32 count = 0;
	for(int32 i=0;i < m_queryResultCount && count<maxCount; ++i, ++count)
	{
		b2Assert(m_queryResults[i] < b2_maxProxies);
		b2Proxy* proxy = m_proxyPool + m_queryResults[i];
		b2Assert(proxy->IsValid());
		userData[i] = proxy->userData;
	}

	// Prepare for next query.
	m_queryResultCount = 0;
	IncrementTimeStamp();
	
	return count;

}
void b2BroadPhase::AddProxyResult(uint16 proxyId, b2Proxy* proxy, int32 maxCount, SortKeyFunc sortKey)
{
	float32 key = sortKey(proxy->userData);
	//Filter proxies on positive keys
	if(key<0)
		return;
	//Merge the new key into the sorted list.
	//float32* p = std::lower_bound(m_querySortKeys,m_querySortKeys+m_queryResultCount,key);
	float32* p = m_querySortKeys;
	while(*p<key&&p<m_querySortKeys+m_queryResultCount)
		p++;
	int32 i = (int32)(p-m_querySortKeys);
	if(maxCount==m_queryResultCount&&i==m_queryResultCount)
		return;
	if(maxCount==m_queryResultCount)
		m_queryResultCount--;
	//std::copy_backward
	for(int32 j=m_queryResultCount+1;j>i;--j){
		m_querySortKeys[j] = m_querySortKeys[j-1];
		m_queryResults[j]  = m_queryResults[j-1];
	}
	m_querySortKeys[i] = key;
	m_queryResults[i] = proxyId;
	m_queryResultCount++;
}