www.xbdev.net xbdev - software development
Thursday October 17, 2019
home | about | contact | Donations

     
 

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;
    float deltaD;


    cosAlpha = D3DXVec3Dot(&rayVector, &planeNormal);

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


    deltaD = planeD -  D3DXVec3Dot(&rayOrigin, &planeNormal);
    
    return (deltaD/cosAlpha);
}// 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
			    float sphereR,			// Spheres Radius
			    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;
}
*/

 
 Visitor: 9534626  { 209.237.238.175 } Copyright (c) 2002-2017 xbdev.net - All rights reserved.
Designated tutorial and software are the property of their respective owners.