Tuesday April 16, 2024

 The Maths of 3D You can't have 3D without a little maths...

# Collision Detection

by bkenwright@xbdev.net

 Quick Code Dump View

## collision.cpp

```/***************************************************************************/
/*                                                                         */
/* File: collision.cpp                                                     */
/*                                                                         */
/* Author: Ben Kenwright                                                   */
/*                                                                         */
/* Email: bkenwright@xbdev.net                                             */
/*                                                                         */
/* URL: www.xbdev.net                                                      */
/*                                                                         */
/* Date: 26-08-05                                                          */
/*                                                                         */
/***************************************************************************/
/*
Various simple collision detection algorithms
*/
/***************************************************************************/

#include <d3dx9.h>					// DirectX Header File

#pragma comment(lib, "d3d9.lib")	// DirectX Library Files
#pragma comment(lib, "d3dx9.lib")

/***************************************************************************/
/*                                                                         */
/*  Name: TriangleLineCollision(..)                                        */
/*                                                                         */
/***************************************************************************/
/*                                                                         */
/* Probably the fastest method and uses barycentric coordinates, and is    */
/* the algorithm put forward by Moller and Trumbore                        */
/*                                                                         */
/***************************************************************************/

bool TriangleLineCollision( D3DXVECTOR3 p, D3DXVECTOR3 q,
D3DXVECTOR3 a, D3DXVECTOR3 b, D3DXVECTOR3 c,
D3DXVECTOR3* hit)
{
D3DXVECTOR3 ab = b - a;
D3DXVECTOR3 ac = c - a;
D3DXVECTOR3 qp = p - q;

D3DXVECTOR3 n;
D3DXVec3Cross(&n, &ac, &ab);

// First check if the line is parallel to the triangles plane
// and if so, we can never have a hit!
float d = D3DXVec3Dot(&qp, &n);
if (d<=0.0f)return false;

// Next compute the t intersection value with the triangles plane, so
// that we know that the triangle has actually intersected the plane!
D3DXVECTOR3 ap = p - a;
float t = D3DXVec3Dot(&ap, &n);
if (t<0.0f) return false;
if (t>d) return false;

// Finally, we need to determine if the intersection point is inside
// the triangle!  This part uses Barycentric coordinates
D3DXVECTOR3 e;
D3DXVec3Cross(&e,  &ap, &qp);
float v = D3DXVec3Dot(&ac, &e);
if (v<0.0f || v>d) return false;
float w =  - D3DXVec3Dot(&ab, &e);
if (w<0.0f || (v+w)>d) return false;

// If we got here, then we have a hit!!!

// If we don't have a null pointer for our hit Vector, then we have to
// return the point where we actually hit the triangle
if (hit)
{
float ood = 1.0f / d;
t *= ood;

D3DXVec3Normalize(&qp, &qp);
*hit = p - qp*t;
}

// Return true, we have a hit!
return true;
}// End TriangleLineCollision(..)

/***************************************************************************/
/*                                                                         */
/* Name: PointInTriangle(..)                                               */
/*                                                                         */
/***************************************************************************/
/*                                                                         */
/* Does what it says on the packet, not ever efficient, but simple to      */
/* to understand.  Basically, if we have a point on our triangles plane,   */
/* then in a clockwise direction we add up all the angles from our point   */
/* to the triangle edges, then if it is approx 360, then we are inside     */
/* the triangle, else we are outside it.                                   */
/*                                                                         */
/***************************************************************************/

inline bool PointInTriangle(D3DXVECTOR3& pntA,
D3DXVECTOR3& a, D3DXVECTOR3& b, D3DXVECTOR3& c)
{
double dAngle;
const float epsilon = 0.01f;
const float pi = 3.14f;

D3DXVECTOR3 vec0 = ( pntA - a );
D3DXVECTOR3 vec1 = ( pntA - b );
D3DXVECTOR3 vec2 = ( pntA - c );
D3DXVec3Normalize(&vec0, &vec0);
D3DXVec3Normalize(&vec1, &vec1);
D3DXVec3Normalize(&vec2, &vec2);

dAngle =
acos( D3DXVec3Dot(&vec0, &vec1) ) +
acos( D3DXVec3Dot(&vec1, &vec2) ) +
acos( D3DXVec3Dot(&vec2, &vec0) ) ;

if( fabs( dAngle - 2*pi ) < epsilon )
return true;
else
return false;
}// End PointInTriangle(..)

/***************************************************************************/
/*                                                                         */
/*  Name: DistRayPlane(..)                                                 */
/*                                                                         */
/***************************************************************************/
/*                                                                         */
/*  Find the distance between a ray and a plane.                           */
/*                                                                         */
/***************************************************************************/

inline float DistRayPlane(D3DXVECTOR3& rayOrigin,
D3DXVECTOR3& rayVector,
D3DXVECTOR3& planeNormal,
float planeD)
{
float cosAlpha;

cosAlpha = D3DXVec3Dot(&rayVector, &planeNormal);

// parallel to the plane (alpha=90)
if (cosAlpha==0) return -1.0f;

deltaD = planeD -  D3DXVec3Dot(&rayOrigin, &planeNormal);

}// End DistRayPlane(..)

/***************************************************************************/
/*                                                                         */
/*  Name: TriangleLineCollision2(..)                                       */
/*                                                                         */
/***************************************************************************/
/*                                                                         */
/*  Less than efficient version to determine if we have a ray segment that */
/*  intersects a triangle.  Basically, we determine if the line intersects */
/*  the plane, using the good old plane equation, then we use the above    */
/*  function to determine if that point is inside our outside the triangle */
/*                                                                         */
/***************************************************************************/

bool TriangleLineCollision2(D3DXVECTOR3 p, D3DXVECTOR3 q,
D3DXVECTOR3 a, D3DXVECTOR3 b, D3DXVECTOR3 c)
{
D3DXVECTOR3 v1 = a-b;
D3DXVECTOR3 v2 = b-c;

D3DXVECTOR3 pN;
D3DXVec3Cross(&pN, &v1, &v2);
D3DXVec3Normalize(&pN, &pN);

float pD = D3DXVec3Dot(&pN, &a);

D3DXVECTOR3 rayDir = q - p;
float rayLen = D3DXVec3Length(&rayDir);
D3DXVec3Normalize(&rayDir, &rayDir);

float len = DistRayPlane(p, rayDir, pN, pD);

if (len < 0 ) return false;

if (len > rayLen) return false;

D3DXVECTOR3 pointOnPlane = p + rayDir * len;

bool inTri = PointInTriangle(pointOnPlane,
a,b,c);

return inTri;

}// End RayTriangleCollision(..)

/***************************************************************************/
/*                                                                         */
/*  Name: RaySphere(..)                                                    */
/*                                                                         */
/***************************************************************************/
/*                                                                         */
/*  Its a simple algorithm to determine if a collision has occured, but    */
/*  it gets a little trickier when you have to dermine the collision point */
/*  on the spheres surface.                                                */
/*  The secret to solving this is pythagoras, and spheres radius!          */
/*                                                                         */
/***************************************************************************/

bool RaySphere( D3DXVECTOR3& ray0,		// Ray starting position
D3DXVECTOR3& rayDir,	// Ray direction
D3DXVECTOR3& sphereC,	// Centre of the Sphere
D3DXVECTOR3* hit )		// Hit point on sphere
{

// First we work out the closest point to the sphere centre, by projecting
// the ray onto it

D3DXVECTOR3 v0 = sphereC - ray0;

// Just incase, we need to normalize teh rayDir, incase it hasn't been
// passed in normalized
// Note!  This will modify the original as well!
D3DXVec3Normalize(&rayDir, &rayDir);

// Project v0 onto rayDir
float d = D3DXVec3Dot(&v0, &rayDir);

// We get the point which is closest to the spheres centre
D3DXVECTOR3 vP = ray0 + rayDir * d;

// Now if the length between this point and the spheres centre is less
// than the radius we have a hit!
float dist = D3DXVec3Length( &(sphereC - vP) );
if (  dist > sphereR )
{
// No intersection
return false;
}

// If hit vector is true, then we don't have to work out the hit point
// and can just return true
if (hit==NULL)
{
// We have a hit!
return true;
}

// Now this is where it gets ticky, we have to do lots of maths to
// solve for the hit point I think.
D3DXVECTOR3 hitP;

// We use pythagoras theorm to work out the spheres hit point with the ray,
// the secret to solving this, which isn't so obvious, as the vP is at
// at a right angle to our sphere centre, also the hit point is at a range
// of the spheres radius from the centre, so we can work out the distance
// along the ray from our vP point to the point which is the spheres radius
float pythag = sqrtf( sphereR*sphereR  - dist*dist);

float rayToSphere = D3DXVec3Length(&v0);

// Check if the ray start point is in front or behind the center
if ( D3DXVec3Dot(&v0, &rayDir) > 0 )
{
// Check if the ray origin is inside there sphere
if (rayToSphere > sphereR)
{
// Ray origin is outside there sphere, so we work out the length
// along the ray to our spheres surface hit point
float hitLen = d - pythag;

hitP = ray0 + rayDir*hitLen;
}
else
{
// Our rays origin is inside the sphere, so our hit point is the
// exit point of the ray, so we know the distance from our vP to
// the spheres surface, and hence we can work out the exit point.
hitP = vP + rayDir*(pythag);
}
}
else
{
if ( rayToSphere > sphereR )
{
// This is always tricky to see at first, but we can still have vP, but
// our ray origin is outside our sphere and its heading away from our sphere
return false;
}
else
{
// Our ray origin is inside the sphere, and we can now workout the exit
// hit point
hitP = vP + rayDir*(pythag);
}

}

*hit = hitP;

// We have a hit
return true;
}// End RaySphere(..)

/***************************************************************************/
/*                                                                         */
/*  Name: SphereSphereCollision(..)                                        */
/*                                                                         */
/*  Determine if two spheres have collided. Returns true is they are       */
/*  hitting each other, false if not.                                      */
/*                                                                         */
/***************************************************************************/

bool SphereSphereCollision(D3DXVECTOR3 & pA, float & rA,
D3DXVECTOR3 & pB, float & rB)
{
float lensq = (pA.x-pB.x)*(pA.x-pB.x) + (pA.y-pB.y)*(pA.y-pB.y) + (pA.z-pB.z)*(pA.z-pB.z);
float distsq = (rA+rB)*(rA+rB);

if (lensq < distsq)
{
// Collision has occured
return true;
}
// No collision has occured
return false;
}// End SphereSphereCollision(..)

/***************************************************************************/
/*                                                                         */
/*  Name: RayPlane(..)                                                     */
/*                                                                         */
/***************************************************************************/

bool RayPlane( D3DXVECTOR3 & ray0,   D3DXVECTOR3 & rayDir ,
D3DXVECTOR3 & plane0, D3DXVECTOR3 & planeN,
D3DXVECTOR3* hit,
D3DXVECTOR3* closestP)
{
// Just to check, make sure our values are normalized, makes things easier
D3DXVec3Normalize(&planeN, &planeN);
D3DXVec3Normalize(&rayDir, &rayDir);

// First lets check that the line and plane arn't parallel
if (D3DXVec3Dot( &rayDir, &planeN) == 0 )
{
// Plane is parallel to the plane, and hence can never intersect!
return false;
}

float d = D3DXVec3Dot( &plane0, &planeN );

float c = D3DXVec3Dot( &ray0, &planeN ) - d;

// Check that our ray is facing towards the plane!  As if our starting point
// is on one side of the plane, and is facing away from the plane, then it
// will never hit the plane!
if (c < 0)
{
return false;
}

// Check that we want to calculate this value
if (closestP)
{
D3DXVECTOR3 cP = ray0 - c * planeN;
*closestP = cP;
}

// Once we know the closest point, which is at a right angle to
// the plane, we can use the dot product to project our ray onto the
// plane and determine its hit point.
// Ray Equation => p = p0 + tV
// Plane Equation => p dot N - d = 0
// Putting Ray Eq into Plane Eq, we get:
// t = (p0 dot N) - d / (V dot N)

// c is (p0 dot N) - d, which we calculated above

// Check that we want to save this value
if (hit)
{
float t = c / D3DXVec3Dot(&planeN, &rayDir);
D3DXVECTOR3 hitP = ray0 - t * rayDir;
*hit = hitP;
}

return true;
}// End RayPlane(..)

/***************************************************************************/

/*
bool CheckPointInSphere(VECTOR3 pnt, VECTOR3 sc, float sr)
{
VECTOR3 q = pnt - sc;
float d = q.length();

if(d<= sr) return true;
return false;
}

// Collision Detection Between a Line and a Sphere
bool SphereRayCollision(VECTOR3 & sc, float & sr,
VECTOR3 & pA, VECTOR3 & pB)
{

VECTOR3 m = pA - sc;

VECTOR3 d = pB - pA;
float len = d.length();
d.normalize();

float c = (m.dot(m)) - (sr*sr);

float b = m.dot(d);

if (c>0.0f && b>0.0f) return false;

float discr = b*b - c;

if (discr < 0.0f) return false;

float t = -b - sqrtf(discr);

if (t<0.0f) t = 0.0f;

// q is the point on our ray that is hitting the spheres surface
VECTOR3 q = pA + t*d;

// Extra bit of code to check that the 'line' is
// hitting our sphere, and not just the ray ;)
if ( (q-pA).length() <= len )
{
return true;
}

// Our ray line has "NOT" hit the sphere
return false;
}
*/

```