opc_sweepandprune.cpp

opcode是功能强大

			Current = Current->mNext;
		}
	}
}

void SAP_PairData::DumpPairs(PairCallback callback, void* user_data) const
{
	if(!callback)	return;

	// ### Ugly and slow
	for(udword i=0;i<mNbObjects;i++)
	{
		SAP_Element* Current = mArray[i];
		while(Current)
		{
			ASSERT(Current->mID<mNbObjects);

			if(!(callback)(i, Current->mID, user_data))	return;
			Current = Current->mNext;
		}
	}
}




























///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Constructor.
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SweepAndPrune::SweepAndPrune()
{
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Destructor.
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SweepAndPrune::~SweepAndPrune()
{
}

void SweepAndPrune::GetPairs(Pairs& pairs) const
{
	mPairs.DumpPairs(pairs);
}

void SweepAndPrune::GetPairs(PairCallback callback, void* user_data) const
{
	mPairs.DumpPairs(callback, user_data);
}

bool SweepAndPrune::Init(udword nb_objects, const IceMaths::AABB** boxes)
{
	// 1) Create sorted lists
	mNbObjects = nb_objects;

	mBoxes = new SAP_Box[nb_objects];
//	for(udword i=0;i<nb_objects;i++)	mBoxes[i].Box = *boxes[i];

	float* Data = new float[nb_objects*2];

	for(udword Axis=0;Axis<3;Axis++)
	{
		mList[Axis] = new SAP_EndPoint[nb_objects*2];

		for(udword i=0;i<nb_objects;i++)
		{
			Data[i*2+0] = boxes[i]->GetMin(Axis);
			Data[i*2+1] = boxes[i]->GetMax(Axis);
		}
		RadixSort RS;
		const udword* Sorted = RS.Sort(Data, nb_objects*2).GetRanks();

		SAP_EndPoint* PreviousEndPoint = null;

		for(udword i=0;i<nb_objects*2;i++)
		{
			udword SortedIndex	= *Sorted++;
			float SortedCoord	= Data[SortedIndex];
			udword BoxIndex		= SortedIndex>>1;

			ASSERT(BoxIndex<nb_objects);

			SAP_EndPoint* CurrentEndPoint = &mList[Axis][SortedIndex];
			CurrentEndPoint->Value		= SortedCoord;
//			CurrentEndPoint->IsMax		= SortedIndex&1;		// ### could be implicit ?
//			CurrentEndPoint->ID			= BoxIndex;				// ### could be implicit ?
			CurrentEndPoint->SetData(BoxIndex, SortedIndex&1);	// ### could be implicit ?
			CurrentEndPoint->Previous	= PreviousEndPoint;
			CurrentEndPoint->Next		= null;
			if(PreviousEndPoint)	PreviousEndPoint->Next = CurrentEndPoint;

			if(CurrentEndPoint->IsMax())	mBoxes[BoxIndex].Max[Axis] = CurrentEndPoint;
			else							mBoxes[BoxIndex].Min[Axis] = CurrentEndPoint;

			PreviousEndPoint = CurrentEndPoint;
		}
	}

	DELETEARRAY(Data);

	CheckListsIntegrity();

	// 2) Quickly find starting pairs

	mPairs.Init(nb_objects);

	{
		Pairs P;
		CompleteBoxPruning(nb_objects, boxes, P, IceMaths::Axes(IceMaths::AXES_XZY));
		for(udword i=0;i<P.GetNbPairs();i++)
		{
			const Pair* PP = P.GetPair(i);

			udword id0 = PP->id0;
			udword id1 = PP->id1;

			if(id0!=id1 && boxes[id0]->Intersect(*boxes[id1]))
			{
				mPairs.AddPair(id0, id1);
			}
			else ASSERT(0);
		}
	}

	return true;
}

bool SweepAndPrune::CheckListsIntegrity()
{
	for(udword Axis=0;Axis<3;Axis++)
	{
		// Find list head
		SAP_EndPoint* Current = mList[Axis];
		while(Current->Previous)	Current = Current->Previous;

		udword Nb = 0;

		SAP_EndPoint* Previous = null;
		while(Current)
		{
			Nb++;

			if(Previous)
			{
				ASSERT(Previous->Value <= Current->Value);
				if(Previous->Value > Current->Value)	return false;
			}

			ASSERT(Current->Previous==Previous);
			if(Current->Previous!=Previous)	return false;

			Previous = Current;
			Current = Current->Next;
		}

		ASSERT(Nb==mNbObjects*2);
	}
	return true;
}

inline_ BOOL Intersect(const IceMaths::AABB& a, const SAP_Box& b)
{
	if(b.Max[0]->Value < a.GetMin(0) || a.GetMax(0) < b.Min[0]->Value
	|| b.Max[1]->Value < a.GetMin(1) || a.GetMax(1) < b.Min[1]->Value
	|| b.Max[2]->Value < a.GetMin(2) || a.GetMax(2) < b.Min[2]->Value)	return FALSE;

	return TRUE;
}



bool SweepAndPrune::UpdateObject(udword i, const IceMaths::AABB& box)
{
	for(udword Axis=0;Axis<3;Axis++)
	{
//		udword Base = (udword)&mList[Axis][0];

		// Update min
		{
			SAP_EndPoint* const CurrentMin = mBoxes[i].Min[Axis];
			ASSERT(!CurrentMin->IsMax());

			const float Limit = box.GetMin(Axis);
			if(Limit == CurrentMin->Value)
			{
			}
			else if(Limit < CurrentMin->Value)
			{
				CurrentMin->Value = Limit;

				// Min is moving left:
				SAP_EndPoint* NewPos = CurrentMin;
				ASSERT(NewPos);

				SAP_EndPoint* tmp;
				while((tmp = NewPos->Previous) && tmp->Value > Limit)
				{
					NewPos = tmp;

					if(NewPos->IsMax())
					{
						// Our min passed a max => start overlap
						//udword SortedIndex = (udword(CurrentMin) - Base)/sizeof(NS_EndPoint);
						const udword id0 = CurrentMin->GetBoxID();
						const udword id1 = NewPos->GetBoxID();

						if(id0!=id1 && Intersect(box, mBoxes[id1]))	mPairs.AddPair(id0, id1);
					}
				}

				CurrentMin->InsertBefore(NewPos);
			}
			else// if(Limit > CurrentMin->Value)
			{
				CurrentMin->Value = Limit;

				// Min is moving right:
				SAP_EndPoint* NewPos = CurrentMin;
				ASSERT(NewPos);

				SAP_EndPoint* tmp;
				while((tmp = NewPos->Next) && tmp->Value < Limit)
				{
					NewPos = tmp;

					if(NewPos->IsMax())
					{
						// Our min passed a max => stop overlap
						const udword id0 = CurrentMin->GetBoxID();
						const udword id1 = NewPos->GetBoxID();

						if(id0!=id1)	mPairs.RemovePair(id0, id1);
					}
				}

				CurrentMin->InsertAfter(NewPos);
			}
		}

		// Update max
		{
			SAP_EndPoint* const CurrentMax = mBoxes[i].Max[Axis];
			ASSERT(CurrentMax->IsMax());

			const float Limit = box.GetMax(Axis);
			if(Limit == CurrentMax->Value)
			{
			}
			else if(Limit > CurrentMax->Value)
			{
				CurrentMax->Value = Limit;

				// Max is moving right:
				SAP_EndPoint* NewPos = CurrentMax;
				ASSERT(NewPos);

				SAP_EndPoint* tmp;
				while((tmp = NewPos->Next) && tmp->Value < Limit)
				{
					NewPos = tmp;

					if(!NewPos->IsMax())
					{
						// Our max passed a min => start overlap
						const udword id0 = CurrentMax->GetBoxID();
						const udword id1 = NewPos->GetBoxID();

						if(id0!=id1 && Intersect(box, mBoxes[id1]))	mPairs.AddPair(id0, id1);
					}
				}

				CurrentMax->InsertAfter(NewPos);
			}
			else// if(Limit < CurrentMax->Value)
			{
				CurrentMax->Value = Limit;

				// Max is moving left:
				SAP_EndPoint* NewPos = CurrentMax;
				ASSERT(NewPos);

				SAP_EndPoint* tmp;
				while((tmp = NewPos->Previous) && tmp->Value > Limit)
				{
					NewPos = tmp;

					if(!NewPos->IsMax())
					{
						// Our max passed a min => stop overlap
						const udword id0 = CurrentMax->GetBoxID();
						const udword id1 = NewPos->GetBoxID();

						if(id0!=id1)	mPairs.RemovePair(id0, id1);
					}
				}

				CurrentMax->InsertBefore(NewPos);
			}
		}
	}

	return true;
}