opc_treecollider.cpp

opcode是功能强大

 *	\param		b		[in] collision node from first tree
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_CollideBoxTri(const AABBNoLeafNode* b)
{
	// Perform triangle-box overlap test (applies mScale0 on the box first!)
	if(!TriBoxOverlap(b->mAABB.mCenter*mScale0, b->mAABB.mExtents*mScale0))	return;

	// Keep same triangle, deal with first child
	if(b->HasPosLeaf())	PrimTestIndexTri(b->GetPosPrimitive());
	else				_CollideBoxTri(b->GetPos());

	if(ContactFound()) return;

	// Keep same triangle, deal with second child
	if(b->HasNegLeaf())	PrimTestIndexTri(b->GetNegPrimitive());
	else				_CollideBoxTri(b->GetNeg());
}

//! Request triangle vertices from the app and transform them
#define FETCH_LEAF(prim_index, imesh, rot, trans)				\
	mLeafIndex = prim_index;									\
	/* Request vertices from the app */							\
	VertexPointers VP;	imesh->GetTriangle(VP, prim_index);		\
	/* Transform them in a common space */						\
	TransformPoint(mLeafVerts[0], *VP.Vertex[0], rot, trans);	\
	TransformPoint(mLeafVerts[1], *VP.Vertex[1], rot, trans);	\
	TransformPoint(mLeafVerts[2], *VP.Vertex[2], rot, trans);

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Recursive collision query for no-leaf AABB trees.
 *	\param		a	[in] collision node from first tree
 *	\param		b	[in] collision node from second tree
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b)
{
	// Perform BV-BV overlap test (uses )
	if(!BoxBoxOverlap(a->mAABB.mExtents, a->mAABB.mCenter, b->mAABB.mExtents, b->mAABB.mCenter))	return;

	// Catch leaf status
	BOOL BHasPosLeaf = b->HasPosLeaf();
	BOOL BHasNegLeaf = b->HasNegLeaf();

	if(a->HasPosLeaf())
	{
		FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mSR0to1, mT0to1)

		if(BHasPosLeaf)	PrimTestTriIndex(b->GetPosPrimitive());
		else			_CollideTriBox(b->GetPos());

		if(ContactFound()) return;

		if(BHasNegLeaf)	PrimTestTriIndex(b->GetNegPrimitive());
		else			_CollideTriBox(b->GetNeg());
	}
	else
	{
		if(BHasPosLeaf)
		{
			FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mSR1to0, mT1to0)

			_CollideBoxTri(a->GetPos());
		}
		else _Collide(a->GetPos(), b->GetPos());

		if(ContactFound()) return;

		if(BHasNegLeaf)
		{
			FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mSR1to0, mT1to0)

			_CollideBoxTri(a->GetPos());
		}
		else _Collide(a->GetPos(), b->GetNeg());
	}

	if(ContactFound()) return;

	if(a->HasNegLeaf())
	{
		FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mSR0to1, mT0to1)

		if(BHasPosLeaf)	PrimTestTriIndex(b->GetPosPrimitive());
		else			_CollideTriBox(b->GetPos());

		if(ContactFound()) return;

		if(BHasNegLeaf)	PrimTestTriIndex(b->GetNegPrimitive());
		else			_CollideTriBox(b->GetNeg());
	}
	else
	{
		if(BHasPosLeaf)
		{
			// ### That leaf has possibly already been fetched
			FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mSR1to0, mT1to0)

			_CollideBoxTri(a->GetNeg());
		}
		else _Collide(a->GetNeg(), b->GetPos());

		if(ContactFound()) return;

		if(BHasNegLeaf)
		{
			// ### That leaf has possibly already been fetched
			FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mSR1to0, mT1to0)

			_CollideBoxTri(a->GetNeg());
		}
		else _Collide(a->GetNeg(), b->GetNeg());
	}
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Quantized trees
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Recursive collision query for quantized AABB trees.
 *	\param		b0		[in] collision node from first tree
 *	\param		b1		[in] collision node from second tree
 *	\param		a		[in] extent from box A
 *	\param		Pa		[in] center from box A
 *	\param		b		[in] extent from box B
 *	\param		Pb		[in] center from box B
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const IceMaths::Point& a, const IceMaths::Point& Pa, const IceMaths::Point& b, const IceMaths::Point& Pb)
{
	// Perform BV-BV overlap test
	if(!BoxBoxOverlap(a, Pa, b, Pb))	return;

	if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }

	if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
	{
		// Dequantize box
		const QuantizedAABB* Box = &b0->GetNeg()->mAABB;
		const IceMaths::Point negPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
		const IceMaths::Point nega(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
		_Collide(b0->GetNeg(), b1, nega, negPa, b, Pb);

		if(ContactFound()) return;

		// Dequantize box
		Box = &b0->GetPos()->mAABB;
		const IceMaths::Point posPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
		const IceMaths::Point posa(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
		_Collide(b0->GetPos(), b1, posa, posPa, b, Pb);
	}
	else
	{
		// Dequantize box
		const QuantizedAABB* Box = &b1->GetNeg()->mAABB;
		const IceMaths::Point negPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
		const IceMaths::Point negb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
		_Collide(b0, b1->GetNeg(), a, Pa, negb, negPb);

		if(ContactFound()) return;

		// Dequantize box
		Box = &b1->GetPos()->mAABB;
		const IceMaths::Point posPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
		const IceMaths::Point posb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
		_Collide(b0, b1->GetPos(), a, Pa, posb, posPb);
	}
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Quantized no-leaf trees
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Recursive collision of a leaf node from A and a quantized branch from B.
 *	\param		leaf	[in] leaf triangle from first tree
 *	\param		b		[in] collision node from second tree
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b)
{
	// Dequantize box
	const QuantizedAABB* bb = &b->mAABB;
	const IceMaths::Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
	const IceMaths::Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);

	// Perform triangle-box overlap test (box comes from B, so apply mScale1 to it)
	if(!TriBoxOverlap(Pb*mScale1, eb*mScale1))	return;

	if(b->HasPosLeaf())	PrimTestTriIndex(b->GetPosPrimitive());
	else				_CollideTriBox(b->GetPos());

	if(ContactFound()) return;

	if(b->HasNegLeaf())	PrimTestTriIndex(b->GetNegPrimitive());
	else				_CollideTriBox(b->GetNeg());
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Recursive collision of a leaf node from B and a quantized branch from A.
 *	\param		b		[in] collision node from first tree
 *	\param		leaf	[in] leaf triangle from second tree
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_CollideBoxTri(const AABBQuantizedNoLeafNode* b)
{
	// Dequantize box
	const QuantizedAABB* bb = &b->mAABB;
	const IceMaths::Point Pa(float(bb->mCenter[0]) * mCenterCoeff0.x, float(bb->mCenter[1]) * mCenterCoeff0.y, float(bb->mCenter[2]) * mCenterCoeff0.z);
	const IceMaths::Point ea(float(bb->mExtents[0]) * mExtentsCoeff0.x, float(bb->mExtents[1]) * mExtentsCoeff0.y, float(bb->mExtents[2]) * mExtentsCoeff0.z);

	// Perform triangle-box overlap test  (box comes from A, so apply mScale0 to it)
	if(!TriBoxOverlap(Pa*mScale0, ea*mScale0))	return;

	if(b->HasPosLeaf())	PrimTestIndexTri(b->GetPosPrimitive());
	else				_CollideBoxTri(b->GetPos());

	if(ContactFound()) return;

	if(b->HasNegLeaf())	PrimTestIndexTri(b->GetNegPrimitive());
	else				_CollideBoxTri(b->GetNeg());
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Recursive collision query for quantized no-leaf AABB trees.
 *	\param		a	[in] collision node from first tree
 *	\param		b	[in] collision node from second tree
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b)
{
	// Dequantize box A
	const QuantizedAABB* ab = &a->mAABB;
	const IceMaths::Point Pa(float(ab->mCenter[0]) * mCenterCoeff0.x, float(ab->mCenter[1]) * mCenterCoeff0.y, float(ab->mCenter[2]) * mCenterCoeff0.z);
	const IceMaths::Point ea(float(ab->mExtents[0]) * mExtentsCoeff0.x, float(ab->mExtents[1]) * mExtentsCoeff0.y, float(ab->mExtents[2]) * mExtentsCoeff0.z);
	// Dequantize box B
	const QuantizedAABB* bb = &b->mAABB;
	const IceMaths::Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
	const IceMaths::Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);

	// Perform BV-BV overlap test (don't use scales)
	if(!BoxBoxOverlap(ea, Pa, eb, Pb))	return;

	// Catch leaf status
	BOOL BHasPosLeaf = b->HasPosLeaf();
	BOOL BHasNegLeaf = b->HasNegLeaf();

	if(a->HasPosLeaf())
	{
		FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mSR0to1, mT0to1)

		if(BHasPosLeaf)	PrimTestTriIndex(b->GetPosPrimitive());
		else			_CollideTriBox(b->GetPos());

		if(ContactFound()) return;

		if(BHasNegLeaf)	PrimTestTriIndex(b->GetNegPrimitive());
		else			_CollideTriBox(b->GetNeg());
	}
	else
	{
		if(BHasPosLeaf)
		{
			FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mSR1to0, mT1to0)

			_CollideBoxTri(a->GetPos());
		}
		else _Collide(a->GetPos(), b->GetPos());

		if(ContactFound()) return;

		if(BHasNegLeaf)
		{
			FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mSR1to0, mT1to0)

			_CollideBoxTri(a->GetPos());
		}
		else _Collide(a->GetPos(), b->GetNeg());
	}

	if(ContactFound()) return;

	if(a->HasNegLeaf())
	{
		FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mSR0to1, mT0to1)

		if(BHasPosLeaf)	PrimTestTriIndex(b->GetPosPrimitive());
		else			_CollideTriBox(b->GetPos());

		if(ContactFound()) return;

		if(BHasNegLeaf)	PrimTestTriIndex(b->GetNegPrimitive());
		else			_CollideTriBox(b->GetNeg());
	}
	else
	{
		if(BHasPosLeaf)
		{
			// ### That leaf has possibly already been fetched
			FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mSR1to0, mT1to0)

			_CollideBoxTri(a->GetNeg());
		}
		else _Collide(a->GetNeg(), b->GetPos());

		if(ContactFound()) return;

		if(BHasNegLeaf)
		{
			// ### That leaf has possibly already been fetched
			FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mSR1to0, mT1to0)

			_CollideBoxTri(a->GetNeg());
		}
		else _Collide(a->GetNeg(), b->GetNeg());
	}
}