Thread: Some per poly collision detection code

I hashed this out over several days and tested it. I do a lot of ugly things in there just to get something produced and working (not worried about speed right now). I haven't been posting any progress so I decided to at least post some code and a screenshot.

The screenshot just shows a collision (the sphere) the center of the poly, and the poly itself that's being collided with.

Some ugly things I do:

• Too many normalize() calls
• Check all bsp leafs instead of only checking the ones I cross

I'm posting code here in the order of abstractdion, i.e, the first functions are the most basic/primitive and the scope of their functionality is smallest.

The last thing I post is a simple way to handle some physics in my physics manager (this is being re-done along with the collision detection mind you).

Code:

BOOL	CCWPointInTri(float	P1X,float	P1Y,float	P1Z,
					  float	P2X,float	P2Y,float	P2Z,
					  float	P3X,float	P3Y,float	P3Z,
					  float	NormalX,float	NormalY,float	NormalZ,
					  float	PointX,float PointY,float	PointZ,float Radius)
{
	Vector3	Point1(P1X,P1Y,P1Z);	Vector3	Point2(P2X,P2Y,P2Z);	Vector3	Point3(P3X,P3Y,P3Z);
	Vector3	Normal(NormalX,NormalY,NormalZ);
	Vector3 Point(PointX,PointY,PointZ);

	Vector3	LocalPoint = Point - Point1; //The point in local coordintes with respect to a vertex 

	Vector3	DiffVector	=	Point2 - Point1;
	Vector3	InwardVector(0,0,0);
	InwardVector	=	CrossProduct(&Normal,&DiffVector);
	InwardVector.Normalize();

	if(DotProduct(&InwardVector,&LocalPoint) <= -(Radius + globaltolerance))
	{
		CCWPITDEBUG(	trace << "Returning on first point" << "\n";	)
		return	FALSE;
	}

	LocalPoint	=	Point  - Point2;
	DiffVector	=	Point3 - Point2;
	InwardVector	=	CrossProduct(&Normal,&DiffVector);
	InwardVector.Normalize();


	if(DotProduct(&InwardVector,&LocalPoint) <= -(Radius + globaltolerance))
	{
		CCWPITDEBUG(	trace << "Returning on second point" << "\n";	)
		return	FALSE;
	}

	LocalPoint	=	Point  - Point3;
	DiffVector	=	Point1 - Point3;
	InwardVector	=	CrossProduct(&Normal,&DiffVector);
	InwardVector.Normalize();
	
	if(DotProduct(&InwardVector,&LocalPoint)<= -(Radius + globaltolerance))
	{
		CCWPITDEBUG(	trace << "Returning on third point" << "\n";	)
		return	FALSE;
	}

	return	TRUE;
}

BOOL	CWPointInTri(float	P1X,float	P1Y,float	P1Z,
					  float	P2X,float	P2Y,float	P2Z,
					  float	P3X,float	P3Y,float	P3Z,
					  float	NormalX,float	NormalY,float	NormalZ,
					  float	PointX,float PointY,float	PointZ,float Radius)
{
	Vector3	Point1(P1X,P1Y,P1Z);	Vector3	Point2(P2X,P2Y,P2Z);	Vector3	Point3(P3X,P3Y,P3Z);
	Vector3	Normal(-NormalX,-NormalY,-NormalZ);
	Vector3 Point(PointX,PointY,PointZ);

	Vector3	LocalPoint = Point - Point1; //The point in local coordintes with respect to a vertex 

	Vector3	DiffVector	=	Point2 - Point1;
	Vector3	InwardVector(0,0,0);
	InwardVector	=	CrossProduct(&Normal,&DiffVector);
	InwardVector.Normalize();

	if(DotProduct(&InwardVector,&LocalPoint) <= -(Radius + globaltolerance))
	{
		CCWPITDEBUG(	trace << "\nReturning on first point" << "\n";	)
		CCWPITDEBUG(	trace << "DP InVec, LocalPoint: " << DotProduct(&InwardVector,&LocalPoint) << "\n";)
		CCWPITDEBUG(	trace << "GlobalTolerance: " << globaltolerance << "\n"; )
		CCWPITDEBUG(	trace << "DP Diff, In: " << DotProduct(&DiffVector,&InwardVector) << "\n"; )
		
		CCWPITDEBUG(	trace << "LocalPointlength: " << LocalPoint.GetLength() << "\n"; )	//small? small == bad?
		CCWPITDEBUG(	trace << "DiffVector(edge) length: " << DiffVector.GetLength() << "\n"; )  //small? small == bad?

		//construct a temporary plane and see if it would work any better
		float	PlaneD = DotProduct(&InwardVector,&Point1); //point1 + point3 are both on plane
		
		CCWPITDEBUG(trace << "PlaneD: " << PlaneD << "\n"; )
		float	Dist = DotProduct(&InwardVector,&Point) - PlaneD;
		CCWPITDEBUG(trace << "Dist: " << Dist << "\n"; )
		return	FALSE;
	}

	LocalPoint	=	Point  - Point2;
	DiffVector	=	Point3 - Point2;
	InwardVector	=	CrossProduct(&Normal,&DiffVector);
	InwardVector.Normalize();


	if(DotProduct(&InwardVector,&LocalPoint) <= -(Radius+globaltolerance))
	{
		CCWPITDEBUG(	trace << "\nReturning on second point" << "\n";	)
		CCWPITDEBUG(	trace << "DP InVec, LocalPoint: " << DotProduct(&InwardVector,&LocalPoint) << "\n";)
		CCWPITDEBUG(	trace << "GlobalTolerance: " << globaltolerance << "\n"; )
		CCWPITDEBUG(	trace << "DP Diff, In: " << DotProduct(&DiffVector,&InwardVector) << "\n"; )
		
		CCWPITDEBUG(	trace << "LocalPointlength: " << LocalPoint.GetLength() << "\n"; )	//small? small == bad?
		CCWPITDEBUG(	trace << "DiffVector(edge) length: " << DiffVector.GetLength() << "\n"; )  //small? small == bad?

		//construct a temporary plane and see if it would work any better
		float	PlaneD = DotProduct(&InwardVector,&Point1); //point1 + point3 are both on plane
		
		CCWPITDEBUG(trace << "PlaneD: " << PlaneD << "\n"; )
		float	Dist = DotProduct(&InwardVector,&Point) - PlaneD;
		CCWPITDEBUG(trace << "Dist: " << Dist << "\n"; )
		return	FALSE;
	}

	LocalPoint	=	Point  - Point3;
	DiffVector	=	Point1 - Point3;
	InwardVector	=	CrossProduct(&Normal,&DiffVector);
	InwardVector.Normalize();
	
	if(DotProduct(&InwardVector,&LocalPoint) <= -(Radius + globaltolerance))
	{
		CCWPITDEBUG(	trace << "\nReturning on third point" << "\n";	)
		CCWPITDEBUG(	trace << "DP InVec, LocalPoint: " << DotProduct(&InwardVector,&LocalPoint) << "\n";)
		CCWPITDEBUG(	trace << "GlobalTolerance: " << globaltolerance << "\n"; )
		CCWPITDEBUG(	trace << "DP Diff, In: " << DotProduct(&DiffVector,&InwardVector) << "\n"; )
		
		CCWPITDEBUG(	trace << "LocalPointlength: " << LocalPoint.GetLength() << "\n"; )	//small? small == bad?
		CCWPITDEBUG(	trace << "DiffVector(edge) length: " << DiffVector.GetLength() << "\n"; )  //small? small == bad?

		//construct a temporary plane and see if it would work any better
		float	PlaneD = DotProduct(&InwardVector,&Point1); //point1 + point3 are both on plane
		
		CCWPITDEBUG(trace << "PlaneD: " << PlaneD << "\n"; )
		float	Dist = DotProduct(&InwardVector,&Point) - PlaneD;
		CCWPITDEBUG(trace << "Dist: " << Dist << "\n"; )
		return	FALSE;
	}

	return	TRUE;
}
	

//Same thing, different version
BOOL	CWPointInTri(float P1X,float P1Y,float P1Z, 
					 float P2X,float P2Y,float P2Z, 
					 float P3X,float P3Y,float P3Z, 
					 float NormalX,float NormalY,float NormalZ,
					 float PointX, float PointY, float PointZ)
{
	Vector3	P1(P1X,P1Y,P1Z);
	Vector3	P2(P2X,P2Y,P2Z);
	Vector3	P3(P3X,P3Y,P3Z);

	Vector3	Point(PointX,PointY,PointZ);
	Vector3	Normal(NormalX,NormalY,NormalZ);

	Vector3	Edge1 = P1 - P2;
	Vector3	Edge2 = P2 - P3;
	Vector3	Edge3 = P3 - P1;

	Vector3	PminusP2 = Point - P2;
	Vector3	PminusP3 = Point - P3;
	Vector3	PminusP1 = Point - P1;

	if(DotProduct(&Normal,&CrossProduct(&Edge1,&PminusP2)) >= globaltolerance)
	if(DotProduct(&Normal,&CrossProduct(&Edge2,&PminusP3)) >= globaltolerance)
	if(DotProduct(&Normal,&CrossProduct(&Edge3,&PminusP1)) >= globaltolerance)
		return true;

	return false;
}

Code:

HitInfo	TraceSphereToBSPFaces(BSP *pBSP, Vector3	&Start,Vector3	&End,float	Radius, float	Epsilon)
{
	Vector3	FinalHitPoint = End;
	float	FinalHitFrac = 1.0f;
	Vector3	FinalTempHitPoint1;
	Vector3	FinalHitNormal(0,0,0);
	
	Vector3	EndMinusStart = End - Start;

	int		HitTri	=	-1;
	tBSPFace	*pHitFace = NULL;
	Vector3	P1,P2,P3;//temp verts on triangle

//	std::vector<int>	LeafIndices;
//	FindLeavesTouchedByMove(pBSP,Start,End,Radius,LeafIndices);

	//Iterate through every leaf
	for(int leaf = 0; leaf < pBSP->mNumLeafs; leaf++)
//	for(int leaf = 0; leaf < LeafIndices.size(); leaf++)
	{	
	//	tBSPLeaf	*pLeaf	=	&pBSP->mpLeafs[LeafIndices[leaf]];
		tBSPLeaf	*pLeaf  =   &pBSP->mpLeafs[leaf];
		for(int face = 0; face < pLeaf->facelist.size(); face++)
		{
			tBSPFace	*pFace = &pBSP->mpFaces[pLeaf->facelist[face]];
			float	StartD = DotProduct(&pFace->vNormal,&Start) - pFace->planeD - TestBall.mRadius;
				if(StartD > 0)
				{
					float	EndD = DotProduct(&pFace->vNormal,&End) - pFace->planeD - TestBall.mRadius;
					if(EndD < 0)
					{
						float	AbsStart = fabs(StartD);
						float	AbsEnd	 = fabs(EndD);
						float	Total	 = AbsStart + AbsEnd;
						float	tempFrac	 = (AbsStart - Epsilon)	/	Total;
						
						
						if(tempFrac < 0.0f)	//FIXME: should I just return?
						{
							tempFrac = 0.0f;
						}
						else	if(tempFrac >= 1.0f)//FIXME: better handling here?
						{
							tempFrac = 1.0f;
						}
						
						if(tempFrac < FinalHitFrac)
						{
							Vector3	tempHitPoint = Start + EndMinusStart * tempFrac;
							for(int tri = 0; tri < pFace->numMeshVerts / 3; tri++)
							{
								pBSP->GetBSPVertexesFromTri(leaf,face,tri,&P1,&P2,&P3);
								{
									if(CWPointInTri(P1.x,P1.y,P1.z,P2.x,P2.y,P2.z,P3.x,P3.y,P3.z,
										pFace->vNormal.x,pFace->vNormal.y,pFace->vNormal.z,tempHitPoint.x,tempHitPoint.y,tempHitPoint.z,TestBall.mRadius))
									{
										FinalHitPoint	=	Start + (End - Start) * tempFrac;//tempHitPoint;
										FinalHitFrac	=	tempFrac;
										FinalHitNormal  =   pFace->vNormal;
										
										break;
									}
								}
							}
						}
					}
				}
			}
		}
		
		
		HitInfo	RetVal;
		RetVal.HitPoint				=	FinalHitPoint;
		RetVal.InterpValue			=	FinalHitFrac;
		RetVal.HitNormal			=	FinalHitNormal;
		return	RetVal;
}

Code:

	
void	PhysicsManager::PHYSICS_RUNFRAMES()
{
	double	numframes = mTimeBuffer	*	mFPS;
	
	while(numframes >= 1.0f)
	{
		RENDER_PHYSICS_FRAME();
		numframes--;
		mTimeBuffer	-=	mSecondsPerFrame;
	}
}
	
	/*
		Renders/runs a single physics frame (each frame advances mSecondsPerFrame in time)
	*/
void	PhysicsManager::RENDER_PHYSICS_FRAME()
{	
	RigidBody	*A,*B;
	std::vector<RigidBody*>::iterator	ptr_outside;
	
	//Add weight and predict positions for all rigid bodies

	for(ptr_outside	=	mvRigidBodies.begin(); ptr_outside != mvRigidBodies.end(); ptr_outside++)
	{
		RigidBody &	A_ref = **(ptr_outside);
	
		A_ref.mLinearMomentum += Vector3(0,-30,0) * mSecondsPerFrame;
		A_ref.PredictNewPosAndOrientation(mSecondsPerFrame);	
	}

	for(ptr_outside	=	mvRigidBodies.begin(); ptr_outside != mvRigidBodies.end(); ptr_outside++)
	{
		RigidBody &	A_ref = **(ptr_outside);
		
		//Check to see if the object hits the world
		HitInfo	temp = TraceSphereToBSPFaces(gpBSP,A_ref.mPosition,A_ref.mPredictedPosition,A_ref.mRadius,5.0f);
			
		if(temp.InterpValue	< 1.0f)	//it collided with world
		{
			A_ref.mPosition			= temp.HitPoint;
			A_ref.mLinearVelocity	-= temp.HitNormal * DotProduct(&temp.HitNormal,&A_ref.mLinearVelocity) * 1.1;
			A_ref.mLinearMomentum = A_ref.mLinearVelocity * A_ref.mMass;
		}
		else
			A_ref.mPosition = A_ref.mPredictedPosition;
	}
}