# Thread: Some per poly collision detection code

1. ## Some per poly collision detection code (+screenie)

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,
{
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();

{
CCWPITDEBUG(	trace << "Returning on first point" << "\n";	)
return	FALSE;
}

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

{
CCWPITDEBUG(	trace << "Returning on second point" << "\n";	)
return	FALSE;
}

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

{
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,
{
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();

{
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();

{
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();

{
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;

//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,
{
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