Some collision code for everyone

I spent three days trying to find all of the info I needed to do sphere collision. Some of the math itself is over my head. I know I'll learn these things in my classes, so I don't want to bother spending weeks figuring it out now. Since I spent so much time searching for this stuff, I decided to post my final code so that other less-than-math-freaks can learn from it (or just throw it into their code somewhere in the background). The basic structure of the functions is to use references (&VECTOR) as input to the functions, and pointers (*VECTOR) as outputs. Hope these are useful for someone. If not, I'm sure they will be some day. Feel free to point out mistakes, typos, or just if something isn't readable.

// Stores closest point to plane in *Closest

VOID ClosestPointOnPlane(VECTOR *Closest, VECTOR &v0, VECTOR &v1, VECTOR &v2, VECTOR &Normal, VECTOR &p)
{
	*Closest = p - v0;
	FLOAT Offset = D3DXVec3Dot(&Normal, Closest);
	*Closest -= (Normal * Offset);
	*Closest += v0;
}
// Stores closest point on line in *Closest

VOID ClosestPointOnLine(VECTOR *Closest, VECTOR &a, VECTOR &b, VECTOR &p)
{
	VECTOR c = p - a;
	VECTOR v = b - a;
	FLOAT d = D3DXVec3Length(&v);
	D3DXVec3Normalize(&v,&v);
	FLOAT t = D3DXVec3Dot(&v, &c);

	if(t < 0)
	{
		*Closest = a;
		return;
	}
	if(t > d)
	{
		*Closest = b;
		return;
	}

	*Closest = a + (v * t);
}
// Checks to make sure a point is on the same side of the line

// as the 3rd vertex. At least I think so :P

BOOL PIT_SameSide(VECTOR &p1, VECTOR &p2, VECTOR &a, VECTOR &b)
{
	VECTOR cp1,cp2;

	D3DXVec3Cross(&cp1, &(b-a), &(p1-a));
	D3DXVec3Cross(&cp2, &(b-a), &(p2-a));

	if(D3DXVec3Dot(&cp1, &cp2) >= 0.0f)
		return TRUE;

	return FALSE;
}
// Returns TRUE if a point on the triangle's plane is within the

// actual triangle

BOOL PointInTriangle(VECTOR &a, VECTOR &b, VECTOR &c, VECTOR &p)
{
	return (PIT_SameSide(p, a, b, c) && PIT_SameSide(p, b, a, c) &&	PIT_SameSide(p, c, a, b));
}
// Fills *Closest with closest point on the triangle to another

// point. Also returns distance to that point.

FLOAT ClosestPointOnTriangle(VECTOR *Closest, VECTOR &a, VECTOR &b, VECTOR &c, VECTOR &Normal, VECTOR &p)
{
	// Find Closest point on plane

	*Closest = p - a;
	FLOAT Offset = D3DXVec3Dot(&Normal, Closest);
	*Closest += a - (Normal * Offset);

	// See what to do next

	if(PointInTriangle(a,b,c, *Closest)) // Point is in triangle, so it's the closest point

	{
		return D3DXVec3Length(&(p - *Closest));
	}
	else
	{
		FLOAT Dist,CDist;
		VECTOR Close;

		ClosestPointOnLine(&Close, a, b, p);
		CDist = D3DXVec3Length(&(p - Close));
		*Closest = Close;

		ClosestPointOnLine(&Close, b, c, p);
		Dist = D3DXVec3Length(&(p - Close));
		if(Dist < CDist)
		{
			*Closest = Close;
			CDist = Dist;
		}

		ClosestPointOnLine(&Close, c, a, p);
		Dist = D3DXVec3Length(&(p - Close));
		if(Dist < CDist)
		{
			*Closest = Close;
			CDist = Dist;
		}

		return CDist;
	}
}
// Beware of this one, as I wrote it myself :P

VOID ClosestPointOnSphere(VECTOR *Closest, VECTOR &Position, FLOAT Radius, VECTOR &p)
{
	// Get Direction vector to sphere

	VECTOR Dir;
	D3DXVec3Normalize(&Dir, &(p - Position));

	// Closest point is just Sphere Position + (Direction * Radius)

	*Closest = Position + (Dir * Radius);
}
// I basically ripped this off from the SDK sample code. It

// casts a line from Origin in the Direction, returning TRUE

// if the line hits the triangle. "t" will tell you the

// distance from the Origin to the point on the triangle.

// "u" and "v" tell you the location on the triangle.

BOOL IntersectTriangle(VECTOR &Origin, VECTOR &Direction, VECTOR &v0, VECTOR &v1, VECTOR &v2, FLOAT *t, FLOAT *u, FLOAT *v)
{
	// Get Vectors for two edges that share vertex 0

	VECTOR Edge1 = v1 - v0;
	VECTOR Edge2 = v2 - v0;

	// Get Determinant to calculate U

	VECTOR PVec;
	D3DXVec3Cross(&PVec, &Direction, &Edge2);

	// if Determinant is near zero, ray lies in plane of triangle

	FLOAT Det = D3DXVec3Dot(&Edge1, &PVec);
	VECTOR TVec;
	if(Det > 0)
	{
		TVec = Origin - v0;
	}
	else
	{
		TVec = v0 - Origin;
		Det = -Det;
	}

	if(Det < 0.0001f)
		return FALSE;

	// Calculate U and test bounds

	*u = D3DXVec3Dot(&TVec, &PVec);
	if(*u < 0.0f || *u > Det)
		return FALSE;

	// Calculate V and test bounds

	VECTOR QVec;
	D3DXVec3Cross(&QVec, &TVec, &Edge1);
	*v = D3DXVec3Dot(&Direction, &QVec);
	if(*v < 0.0f || *u + *v > Det)
		return FALSE;

	// Calculate T and scale

	*t = D3DXVec3Dot(&Edge2, &QVec);
	FLOAT InvDet = 1.0f / Det;
	*t *= InvDet;
	*u *= InvDet;
	*v *= InvDet;

	return TRUE;
}

[edited by - Jiia on May 16, 2004 9:04:13 AM]